Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/feeder' into feeder
Browse files Browse the repository at this point in the history
  • Loading branch information
lakelandspark committed Jan 17, 2024
2 parents f30fe02 + 64ff6b3 commit d5e1c2a
Showing 1 changed file with 65 additions and 53 deletions.
118 changes: 65 additions & 53 deletions src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<Radian, Volt>(wristkS.get(), wristlkV.get(), wristkA.get())


private var wristFeedforward: SimpleMotorFeedforward<Meter, Volt>

private var WristFeedforward: SimpleMotorFeedforward<Meter, Volt>(kS,kV,kA)
// >>>>>>> 829a07586e50bf71782342ae9623c560237b502b

/*
private val wristflywheelkP =
LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
private val wristflywheelkI =
Expand All @@ -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)
}
Expand All @@ -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)
}



Expand All @@ -141,6 +155,4 @@ fun periodic(){
}
}

}


}

0 comments on commit d5e1c2a

Please sign in to comment.