diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 2bf0c01d..eec37766 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inSeconds @@ -18,13 +19,22 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward + private val wristkS = + LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) + ) + private val wristlkV = + LoggedTunableValue( + "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + ) + private val wristkA = + LoggedTunableValue( + "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }) + ) + val flywheelFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) + - private var wristFeedforward: SimpleMotorFeedforward - private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) -// >>>>>>> 829a07586e50bf71782342ae9623c560237b502b -/* private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) private val wristflywheelkI = @@ -34,14 +44,14 @@ class Shooter (val io: ShooterIO){ private val wristflywheelkD = LoggedTunableValue( "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - )*/ + ) var currentState = ShooterStates.UNINITIALIZED //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts - /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ - io.setflywheelVoltage(appliedVoltage) - }*/ + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setflywheelVoltage(appliedVoltage) + }*/ fun setWristVoltage(appliedVoltage: ElectricalPotential){ io.setWristVoltage(appliedVoltage) } @@ -64,59 +74,63 @@ class Shooter (val io: ShooterIO){ ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, //ShooterConstants.FEEDER_INIT_VOLTAGE - ) + ) private var wristProfile = TrapezoidProfile( - TrapezoidProfile.Constraints, + wristConstraints, TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) -fun periodic(){ - io.updateInputs(inputs) - var nextState = currentState - when (currentState) { - ShooterStates.UNINITIALIZED -> { - nextState = ShooterStates.ZERO - } - ShooterStates.ZERO ->{ - } - - ShooterStates.OPEN_LOOP ->{ - /* - setflywheelVoltage(flywheelTargetVoltage) - lastflywheelRunTime = Clock.fpgaTime -*/ - setWristVoltage(wristTargetVoltage) - lastWristRunTime = Clock.fpgaTime - - /* setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime*/ - if (isZeroed == true ){ + fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + ShooterStates.UNINITIALIZED -> { + nextState = fromRequestToState(currentRequest) + } + ShooterStates.ZERO ->{ nextState = fromRequestToState(currentRequest) } - } + ShooterStates.OPEN_LOOP ->{ + /* + setflywheelVoltage(flywheelTargetVoltage) + lastflywheelRunTime = Clock.fpgaTime + */ + setWristVoltage(wristTargetVoltage) + lastWristRunTime = Clock.fpgaTime + + /* setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime*/ + if (isZeroed == true ){ + nextState = fromRequestToState(currentRequest) + } + nextState = fromRequestToState(currentRequest) - ShooterStates.TARGETING_POSITION ->{ - - if (wristPositionTarget!=lastWristPositionTarget){ - val preProfileGenerate = Clock.fpgaTime - wristProfile = TrapezoidProfile( - TrapezoidProfile.Constraints, - TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), - TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) - ) - val postProfileGenerate = Clock.fpgaTime - Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) - timeProfileGeneratedAt = Clock.fpgaTime - lastWristPositionTarget = wristPositionTarget } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO fix this error - Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) - } + + ShooterStates.TARGETING_POSITION ->{ + + if (wristPositionTarget!=lastWristPositionTarget){ + val preProfileGenerate = Clock.fpgaTime + //TODO figure out how to implment feedforward here. + wristProfile = TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + ) + val postProfileGenerate = Clock.fpgaTime + Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) + timeProfileGeneratedAt = Clock.fpgaTime + lastWristPositionTarget = wristPositionTarget + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + setWristPosition(wristProfile.calculate(timeElapsed)) + //TODO fix this error + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) + } @@ -141,6 +155,4 @@ fun periodic(){ } } -} - - +} \ No newline at end of file