Skip to content

Commit 047ff0e

Browse files
TaimoriokaTaimorioka
committed
Changes from 11/30, some of this was for testing and should not be pushed to main
1 parent eeea816 commit 047ff0e

File tree

6 files changed

+92
-57
lines changed

6 files changed

+92
-57
lines changed

‎src/main/kotlin/com/frcteam3636/bunnybots2025/Robot.kt‎

Lines changed: 33 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ import com.frcteam3636.bunnybots2025.subsystems.intake.Intake.Position
1111
import com.frcteam3636.bunnybots2025.subsystems.shooter.Shooter
1212
import com.frcteam3636.bunnybots2025.subsystems.shooter.Target
1313
import com.frcteam3636.bunnybots2025.subsystems.shooter.zooTranslation
14+
import com.frcteam3636.bunnybots2025.utils.math.seconds
1415
import com.frcteam3636.version.BUILD_DATE
1516
import com.frcteam3636.version.DIRTY
1617
import com.frcteam3636.version.GIT_BRANCH
@@ -245,7 +246,7 @@ object Robot : LoggedRobot() {
245246
Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft.hid, joystickRight.hid)
246247
Shooter.Flywheels.defaultCommand = Shooter.Flywheels.idle()
247248
Shooter.Pivot.defaultCommand = Shooter.Pivot.moveToActiveTarget()
248-
Intake.defaultCommand = Intake.setPivotPosition(Intake.Position.Stowed)
249+
// Intake.defaultCommand = Intake.setPivotPosition(Intake.Position.Stowed)
249250
// (The button with the yellow tape on it)
250251
joystickLeft.button(8).onTrue(Commands.runOnce({
251252
println("Zeroing gyro.")
@@ -288,6 +289,23 @@ object Robot : LoggedRobot() {
288289
)
289290

290291
controller.leftTrigger().whileTrue(Intake.bulldoze())
292+
controller.rightTrigger().whileTrue(
293+
Commands.parallel(
294+
Shooter.Flywheels.shoot(),
295+
Commands.sequence(
296+
Commands.waitUntil(Shooter.Flywheels.atDesiredVelocity),
297+
Commands.waitUntil(Shooter.Pivot.atDesiredPosition),
298+
Commands.waitTime(0.5.seconds),
299+
Commands.parallel(
300+
Shooter.Feeder.feed()
301+
)
302+
),
303+
Commands.sequence(
304+
Shooter.Pivot.setTarget(Target.PETTINGZOO),
305+
Shooter.Pivot.moveToActiveTarget()
306+
)
307+
)
308+
)
291309

292310
controller.a().onTrue(Shooter.Pivot.setTarget(Target.STOWED))
293311
controller.b().onTrue(Shooter.Pivot.setTarget(Target.PETTINGZOO))
@@ -311,20 +329,20 @@ object Robot : LoggedRobot() {
311329
controllerDev.x().whileTrue(Shooter.Flywheels.sysIdDynamic(SysIdRoutine.Direction.kReverse))
312330

313331

314-
controller.rightTrigger()
315-
.whileTrue(Commands.parallel(
316-
Shooter.Flywheels.shoot(),
317-
Commands.sequence(
318-
Commands.waitUntil(Shooter.Flywheels.atDesiredVelocity),
319-
Commands.waitUntil(Shooter.Pivot.atDesiredPosition),
320-
Commands.parallel(
321-
Shooter.Feeder.feed(),
322-
Indexer.index()
323-
)
324-
)
325-
))
326-
.onTrue(Shooter.Pivot.setTarget(Target.TUNING))
327-
.onFalse(Shooter.Pivot.setTarget(Target.STOWED))
332+
// controller.rightTrigger()
333+
// .whileTrue(Commands.parallel(
334+
// Shooter.Flywheels.shoot(),
335+
// Commands.sequence(
336+
// Commands.waitUntil(Shooter.Flywheels.atDesiredVelocity),
337+
// Commands.waitUntil(Shooter.Pivot.atDesiredPosition),
338+
// Commands.parallel(
339+
// Shooter.Feeder.feed(),
340+
// Indexer.index()
341+
// )
342+
// )
343+
// ))
344+
// .onTrue(Shooter.Pivot.setTarget(Target.TUNING))
345+
// .onFalse(Shooter.Pivot.setTarget(Target.STOWED))
328346
// .onFalse(Shooter.Pivot.setTarget(Target.STOWED))
329347
// controllerDev.leftTrigger()
330348
// .onTrue(Shooter.Pivot.setTarget(Target.AIM))

‎src/main/kotlin/com/frcteam3636/bunnybots2025/subsystems/indexer/Indexer.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ object Indexer : Subsystem {
2626
fun index(): Command =
2727
startEnd(
2828
{
29-
io.setIndexerSpeed(0.7)
29+
io.setIndexerSpeed(0.4)
3030
},
3131
{
3232
io.setIndexerSpeed(0.0)

‎src/main/kotlin/com/frcteam3636/bunnybots2025/subsystems/intake/Intake.kt‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -44,11 +44,10 @@ object Intake : Subsystem {
4444
intakeAngleLigament.angle = inputs.pivotPosition.inDegrees() + 90.0
4545
Logger.recordOutput("Intake/Pivot/Mechanism", mechanism)
4646

47-
// the extra 5 degrees is to account for encoder noise
48-
// if ((inputs.pivotPosition > 120.degrees || inputs.pivotPosition < (-75).degrees) && !inputs.pivotDisabled && Robot.isEnabled) {
49-
// io.disablePivot()
50-
// pivotDisabledAlert.set(true)
51-
// }
47+
if ((inputs.pivotPosition > 110.degrees || inputs.pivotPosition < (-85).degrees) && !inputs.pivotDisabled && Robot.isEnabled) {
48+
io.disablePivot()
49+
pivotDisabledAlert.set(true)
50+
}
5251
}
5352

5453
fun setPivotPosition(position: Position): Command = (
@@ -95,8 +94,9 @@ object Intake : Subsystem {
9594
)
9695

9796
enum class Position(val angle: Angle) {
98-
Stowed((-3.5).radians),
99-
Deployed((-1.0).radians)
97+
// Stowed((-40).degrees),
98+
Stowed(0.degrees),
99+
Deployed(90.degrees)
100100
}
101101

102102
val signals: Array<BaseStatusSignal>

‎src/main/kotlin/com/frcteam3636/bunnybots2025/subsystems/intake/IntakeIO.kt‎

Lines changed: 18 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@ package com.frcteam3636.bunnybots2025.subsystems.intake
22

33
import com.ctre.phoenix6.BaseStatusSignal
44
import com.ctre.phoenix6.configs.CANcoderConfiguration
5+
import com.ctre.phoenix6.configs.ExternalFeedbackConfigs
56
import com.ctre.phoenix6.configs.TalonFXConfiguration
67
import com.ctre.phoenix6.controls.MotionMagicVoltage
78
import com.ctre.phoenix6.controls.NeutralOut
@@ -24,6 +25,7 @@ import edu.wpi.first.units.measure.Angle
2425
import edu.wpi.first.wpilibj.Timer
2526
import edu.wpi.first.wpilibj.simulation.DCMotorSim
2627
import org.team9432.annotation.Logged
28+
import kotlin.apply
2729

2830
@Logged
2931
open class IntakeInputs {
@@ -88,6 +90,9 @@ class IntakeIOReal : IntakeIO {
8890
MagnetOffset = ENCODER_MAGNET_OFFSET
8991
AbsoluteSensorDiscontinuityPoint = ABSOLUTE_SENSOR_DISCONTINUITY_POINT
9092
}
93+
ExternalFeedbackConfigs().apply {
94+
SensorToMechanismRatio = ENCODER_TO_PIVOT_GEAR_RATIO
95+
}
9196
})
9297
}
9398
}
@@ -136,24 +141,24 @@ class IntakeIOReal : IntakeIO {
136141
override val signals: Array<BaseStatusSignal>
137142
get() = arrayOf(positionSignal, currentSignal, velocitySignal, temperatureSignal, positionReferenceSignal)
138143

139-
override fun disablePivot() {
140-
pivotDisabled = true
141-
// this causes a sizeable loop overrun, but I'm willing to do this
142-
// to prevent the robot from tearing itself apart
143-
if (!brakeModeEnabled)
144-
setBrakeMode(true)
145-
intakePivotMotor.setControl(NeutralOut())
146-
}
144+
// override fun disablePivot() {
145+
// pivotDisabled = true
146+
// // this causes a sizeable loop overrun, but I'm willing to do this
147+
// // to prevent the robot from tearing itself apart
148+
// if (!brakeModeEnabled)
149+
// setBrakeMode(true)
150+
// intakePivotMotor.setControl(NeutralOut())
151+
// }
147152

148153
companion object Constants {
149-
val PID_GAINS = PIDGains(25.0, 0.0, 0.0)
150-
val PROFILE_CRUISE_VELOCITY = 15.0.rotationsPerSecond
151-
val PROFILE_ACCELERATION = 2.5.rotationsPerSecondPerSecond
154+
val PID_GAINS = PIDGains(20.0, 0.0, 0.0)
155+
val PROFILE_CRUISE_VELOCITY = 20.0.rotationsPerSecond
156+
val PROFILE_ACCELERATION = (6.7 / 2.0).rotationsPerSecondPerSecond
152157
const val PROFILE_JERK = 0.0
153-
const val ENCODER_MAGNET_OFFSET = -0.134521
158+
const val ENCODER_MAGNET_OFFSET = -0.17529
154159
const val ENCODER_TO_PIVOT_GEAR_RATIO = 2.25
155160
const val MOTOR_TO_ENCODER_GEAR_RATIO = 4.0
156-
const val ABSOLUTE_SENSOR_DISCONTINUITY_POINT = 0.8
161+
const val ABSOLUTE_SENSOR_DISCONTINUITY_POINT = 0.16
157162
}
158163
}
159164

‎src/main/kotlin/com/frcteam3636/bunnybots2025/subsystems/shooter/FlywheelIO.kt‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ open class FlywheelInputs {
3939
interface FlywheelIO {
4040
fun setSpeed(upperPercent: Double, lowerPercent: Double)
4141
fun setVoltage(voltage: Voltage)
42-
fun setVelocity(velocity: AngularVelocity)
42+
fun setVelocity(upperVelocity: AngularVelocity, lowerVelocity: AngularVelocity)
4343
fun updateInputs(inputs: FlywheelInputs)
4444

4545
val signals: Array<BaseStatusSignal>
@@ -106,15 +106,15 @@ class FlywheelIOReal : FlywheelIO {
106106
lowerShooterMotor.setVoltage(voltage)
107107
}
108108

109-
override fun setVelocity(velocity: AngularVelocity) {
110-
val upperFFOutput = upperFFController.calculate(velocity.inRPM())
111-
val lowerFFOutput = lowerFFController.calculate(velocity.inRPM())
109+
override fun setVelocity(upperVelocity: AngularVelocity, lowerVelocity: AngularVelocity) {
110+
val upperFFOutput = upperFFController.calculate(upperVelocity.inRPM())
111+
val lowerFFOutput = lowerFFController.calculate(lowerVelocity.inRPM())
112112
upperShooterMotor.closedLoopController.setReference(
113-
velocity.inRPM(), SparkBase.ControlType.kVelocity,
113+
upperVelocity.inRPM(), SparkBase.ControlType.kVelocity,
114114
ClosedLoopSlot.kSlot0, upperFFOutput
115115
)
116116
lowerShooterMotor.closedLoopController.setReference(
117-
velocity.inRPM(), SparkBase.ControlType.kVelocity,
117+
lowerVelocity.inRPM(), SparkBase.ControlType.kVelocity,
118118
ClosedLoopSlot.kSlot0, lowerFFOutput
119119
)
120120
}
@@ -163,8 +163,8 @@ class FlywheelIOSim : FlywheelIO {
163163
lowerFlywheelMotor.inputVoltage = voltage.inVolts()
164164
}
165165

166-
override fun setVelocity(velocity: AngularVelocity) {
167-
upperFlywheelMotor.setAngularVelocity(velocity.inRadiansPerSecond())
168-
lowerFlywheelMotor.setAngularVelocity(velocity.inRadiansPerSecond())
166+
override fun setVelocity(upperVelocity: AngularVelocity, lowerVelocity: AngularVelocity) {
167+
upperFlywheelMotor.setAngularVelocity(upperVelocity.inRadiansPerSecond())
168+
lowerFlywheelMotor.setAngularVelocity(upperVelocity.inRadiansPerSecond())
169169
}
170170
}

‎src/main/kotlin/com/frcteam3636/bunnybots2025/subsystems/shooter/Shooter.kt‎

Lines changed: 24 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@ import com.ctre.phoenix6.BaseStatusSignal
44
import com.frcteam3636.bunnybots2025.Robot
55
import com.frcteam3636.bunnybots2025.subsystems.drivetrain.Drivetrain
66
import com.frcteam3636.bunnybots2025.subsystems.drivetrain.FIELD_LAYOUT
7-
import com.frcteam3636.bunnybots2025.subsystems.shooter.Shooter.Flywheels.setpoint
87
import com.frcteam3636.bunnybots2025.subsystems.shooter.Shooter.Flywheels.speedInterpolationTable
98
import com.frcteam3636.bunnybots2025.subsystems.shooter.Shooter.Pivot.angleInterpolationTable
109
import com.frcteam3636.bunnybots2025.utils.math.*
@@ -21,6 +20,7 @@ import edu.wpi.first.wpilibj.DriverStation
2120
import edu.wpi.first.wpilibj.util.Color
2221
import edu.wpi.first.wpilibj.util.Color8Bit
2322
import edu.wpi.first.wpilibj2.command.Command
23+
import edu.wpi.first.wpilibj2.command.Commands
2424
import edu.wpi.first.wpilibj2.command.Subsystem
2525
import edu.wpi.first.wpilibj2.command.button.Trigger
2626
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
@@ -37,7 +37,8 @@ object Shooter {
3737
Robot.Model.SIMULATION -> FlywheelIOSim()
3838
}
3939

40-
private var setpoint = RadiansPerSecond.zero()!!
40+
private var upperSetpoint = RadiansPerSecond.zero()!!
41+
private var lowerSetpoint = RadiansPerSecond.zero()!!
4142

4243
val isDetected: Trigger =
4344
Trigger {
@@ -46,7 +47,7 @@ object Shooter {
4647

4748
val atDesiredVelocity =
4849
Trigger {
49-
val velocityDifference = inputs.topVelocity - setpoint
50+
val velocityDifference = inputs.topVelocity - upperSetpoint
5051
Logger.recordOutput("Shooter/Flywheels/Velocity Difference", velocityDifference)
5152
Logger.recordOutput(
5253
"Shooter/Flywheels/At Desired Velocity",
@@ -95,24 +96,35 @@ object Shooter {
9596

9697
Logger.processInputs("Shooter/Flywheels", inputs)
9798

98-
Logger.recordOutput("Shooter/Flywheels/Setpoint", setpoint.inRPM())
99-
io.setVelocity(setpoint) // move this into commands?
99+
Logger.recordOutput("Shooter/Flywheels/upperSetpoint", upperSetpoint.inRPM())
100+
Logger.recordOutput("Shooter/Flywheels/lowerSetpoint", lowerSetpoint.inRPM())
101+
io.setVelocity(upperSetpoint, lowerSetpoint) // move this into commands?
100102
}
101103

102104
fun idle(): Command =
103105
startEnd(
104106
{
105-
setpoint = 1.radiansPerSecond
107+
upperSetpoint = 1.radiansPerSecond
108+
lowerSetpoint = 1.radiansPerSecond
106109
},
107110
{
108-
setpoint = 0.radiansPerSecond
111+
upperSetpoint = 0.radiansPerSecond
112+
lowerSetpoint = 0.radiansPerSecond
109113
}
110114
)
111115

112116
fun shoot(): Command =
113-
run {
114-
setpoint = Pivot.target.profile.getVelocity()
115-
}
117+
//TODO: Fix
118+
startEnd(
119+
{
120+
upperSetpoint = 500.radiansPerSecond
121+
lowerSetpoint = 500.radiansPerSecond
122+
},
123+
{
124+
upperSetpoint = 0.radiansPerSecond
125+
lowerSetpoint = 0.radiansPerSecond
126+
}
127+
)
116128

117129
val signals: Array<BaseStatusSignal>
118130
get() = io.signals
@@ -202,7 +214,7 @@ object Shooter {
202214
fun feed(interruptBehavior: Command.InterruptionBehavior = Command.InterruptionBehavior.kCancelSelf): Command =
203215
startEnd(
204216
{
205-
io.setSpeed(0.7)
217+
io.setSpeed(1.0)
206218
},
207219
{
208220
io.setSpeed(0.0)
@@ -256,7 +268,7 @@ enum class Target(val profile: ShooterProfile) {
256268
PETTINGZOO(
257269
ShooterProfile(
258270
{
259-
45.degrees
271+
30.degrees
260272
},
261273
{
262274
1000.rpm

0 commit comments

Comments
 (0)