diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt index 5ac1cd55..d06ad12d 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -9,7 +9,8 @@ import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.PIDController +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.Radian @@ -36,7 +37,7 @@ class TargetAngleCommand( val targetAngle: () -> Angle ) : Command() { - private var thetaPID: PIDController> + private var thetaPID: ProfiledPIDController> val thetakP = LoggedTunableValue( "Pathfollow/thetaAmpkP", @@ -62,10 +63,13 @@ class TargetAngleCommand( addRequirements(drivetrain) thetaPID = - PIDController( + ProfiledPIDController( thetakP.get(), thetakI.get(), thetakD.get(), + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) if (!(RobotBase.isSimulation())) { @@ -75,10 +79,13 @@ class TargetAngleCommand( thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP, DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI, - DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } else { thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) @@ -86,10 +93,13 @@ class TargetAngleCommand( thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, - DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } @@ -97,7 +107,7 @@ class TargetAngleCommand( } override fun initialize() { - thetaPID.reset() // maybe do first for x? + thetaPID.reset(0.degrees) // maybe do first for x? /* if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index 4dadbafc..ad6ee390 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -9,7 +9,8 @@ import com.team4099.robot2023.util.DebugLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command -import org.team4099.lib.controller.PIDController +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.degrees @@ -35,7 +36,7 @@ class TargetNoteCommand( val limelight: LimelightVision ) : Command() { - private var thetaPID: PIDController> + private var thetaPID: ProfiledPIDController> val thetakP = LoggedTunableValue( "NoteAlignment/noteThetakP", @@ -61,10 +62,13 @@ class TargetNoteCommand( addRequirements(drivetrain) thetaPID = - PIDController( + ProfiledPIDController( thetakP.get(), thetakI.get(), thetakD.get(), + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) if (!(RobotBase.isSimulation())) { @@ -74,10 +78,13 @@ class TargetNoteCommand( thetakD.initDefault(DrivetrainConstants.PID.LIMELIGHT_THETA_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.LIMELIGHT_THETA_KP, DrivetrainConstants.PID.LIMELIGHT_THETA_KI, - DrivetrainConstants.PID.LIMELIGHT_THETA_KD + DrivetrainConstants.PID.LIMELIGHT_THETA_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } else { thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) @@ -85,10 +92,13 @@ class TargetNoteCommand( thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, - DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } @@ -96,10 +106,18 @@ class TargetNoteCommand( } override fun initialize() { - thetaPID.reset() + thetaPID.reset(0.degrees) if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { - thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get()) + thetaPID = + ProfiledPIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt index 7c125f42..95da2966 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetSpeakerCommand.kt @@ -12,7 +12,8 @@ import edu.wpi.first.math.filter.MedianFilter import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.PIDController +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.derived.Radian @@ -40,7 +41,7 @@ class TargetSpeakerCommand( val drivetrain: Drivetrain, val vision: Vision ) : Command() { - private var thetaPID: PIDController> + private var thetaPID: ProfiledPIDController> val thetakP = LoggedTunableValue( "Pathfollow/thetaAmpkP", @@ -70,10 +71,13 @@ class TargetSpeakerCommand( addRequirements(drivetrain) thetaPID = - PIDController( + ProfiledPIDController( thetakP.get(), thetakI.get(), thetakD.get(), + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) if (!(RobotBase.isSimulation())) { @@ -83,10 +87,13 @@ class TargetSpeakerCommand( thetakD.initDefault(DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.TELEOP_ALIGN_PID_KP, DrivetrainConstants.PID.TELEOP_ALIGN_PID_KI, - DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD + DrivetrainConstants.PID.TELEOP_ALIGN_PID_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } else { thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) @@ -94,10 +101,13 @@ class TargetSpeakerCommand( thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) thetaPID = - PIDController( + ProfiledPIDController( DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP, DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI, - DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD + DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD, + TrapezoidProfile.Constraints( + DrivetrainConstants.STEERING_VEL_MAX, DrivetrainConstants.STEERING_ACCEL_MAX + ) ) } @@ -105,7 +115,7 @@ class TargetSpeakerCommand( } override fun initialize() { - thetaPID.reset() // maybe do first for x? + thetaPID.reset(0.degrees) // maybe do first for x? angleMedianFilter.reset() /* if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) { diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 4ec9936f..cb482b45 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -69,7 +69,7 @@ object ControlBoard { val climbAlignLeft = Trigger { driver.dPadLeft } val climbAlignRight = Trigger { driver.dPadRight } - val targetSpeaker = Trigger { driver.xButton } // TODO: switch back to climbAlignLeft + val targetSpeaker = Trigger { driver.xButton } val climbAutoAlign = Trigger { driver.bButton } // week0 controls