diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index cebea9e9..aa6f0fa6 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -13,12 +13,14 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator +import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo import com.team4099.robot2023.subsystems.feeder.FeederIOSim import com.team4099.robot2023.subsystems.flywheel.Flywheel +import com.team4099.robot2023.subsystems.flywheel.FlywheelIO import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon import com.team4099.robot2023.subsystems.intake.Intake @@ -80,7 +82,7 @@ object RobotContainer { limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(ElevatorIONEO) + elevator = Elevator(object: ElevatorIO {}) flywheel = Flywheel(FlywheelIOTalon) wrist = Wrist(WristIOTalon) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index bad9aaaa..f6427e2e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -53,8 +53,8 @@ object Constants { } object Tuning { - const val TUNING_MODE = false - const val DEBUGING_MODE = false + const val TUNING_MODE = true + const val DEBUGING_MODE = true const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 4598ced7..400586e6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -84,10 +84,10 @@ object DrivetrainConstants { val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds - val FRONT_LEFT_MODULE_ZERO = 3.285.radians + PI.radians // good - val FRONT_RIGHT_MODULE_ZERO = 2.91.radians + PI.radians // good - val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians // good - val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians // good + val FRONT_LEFT_MODULE_ZERO = 0.19.radians // good + val FRONT_RIGHT_MODULE_ZERO = 6.016.radians // good + val BACK_LEFT_MODULE_ZERO = 2.538.radians // good + val BACK_RIGHT_MODULE_ZERO = 1.25.radians // good val STEERING_COMPENSATION_VOLTAGE = 10.volts val DRIVE_COMPENSATION_VOLTAGE = 12.volts @@ -184,4 +184,4 @@ object DrivetrainConstants { val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index cb60870f..f6a63c4f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -31,17 +31,12 @@ object WristConstants { val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 69.0 / 20.0 val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO = - 5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / (69.0 / 20.0) + 5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 18.0 * 48.0 / 18.0 * 1.0 / (69.0 / 20.0) val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = ( - 97.72227856659904.degrees - 35.degrees + 1.90.degrees - - 0.55.degrees - - 0.6.degrees - // add to drop angle - 1.degrees - - 0.5.degrees - - 96.3.degrees + -34.5.degrees + 3.95.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared @@ -59,31 +54,31 @@ object WristConstants { val WRIST_ZERO_SIM_OFFSET = 27.5.degrees - val MAX_WRIST_VELOCITY = 300.degrees.perSecond - val MAX_WRIST_ACCELERATION = 1500.degrees.perSecond.perSecond + val MAX_WRIST_VELOCITY = 400.degrees.perSecond + val MAX_WRIST_ACCELERATION = 400.degrees.perSecond.perSecond val HARDSTOP_OFFSET = 47.degrees object PID { val ARBITRARY_FEEDFORWARD = 0.03.volts - val REAL_KP: ProportionalGain = 0.79.volts / 1.0.degrees + val REAL_KP: ProportionalGain = 0.515.volts / 1.0.degrees val REAL_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val REAL_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + val REAL_KD: DerivativeGain = 0.03.volts / 1.0.degrees.perSecond val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 3.0.degrees.perSecond - val FIRST_STAGE_KP: ProportionalGain = 0.9.volts / 1.0.degrees + val FIRST_STAGE_KP: ProportionalGain = 0.35.volts / 1.0.degrees val FIRST_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val FIRST_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + val FIRST_STAGE_KD: DerivativeGain = 0.03.volts / 1.0.degrees.perSecond - val SECOND_STAGE_POS_SWITCH_THRESHOLD = 1.0.degrees + val SECOND_STAGE_POS_SWITCH_THRESHOLD = 0.8.degrees val SECOND_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond - val SECOND_STAGE_KP: ProportionalGain = 3.volts / 1.0.degrees + val SECOND_STAGE_KP: ProportionalGain = 1.8.volts / 1.0.degrees val SECOND_STAGE_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) - val SECOND_STAGE_KD: DerivativeGain = 0.0.volts / 1.0.degrees.perSecond + val SECOND_STAGE_KD: DerivativeGain = 0.075.volts / 1.0.degrees.perSecond val SIM_KP: ProportionalGain = 10.volts / 1.0.degrees val SIM_KI: IntegralGain = 0.0.volts / (1.0.degrees * 1.0.seconds) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index e695222e..8cd621ce 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -80,7 +80,7 @@ class SwerveModuleIOTalon( private val potentiometerOutput: Double get() { - return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) { + return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) { potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * PI } else { 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI @@ -221,9 +221,9 @@ class SwerveModuleIOTalon( Logger.recordOutput( "$label/drivePosition", driveFalcon.position.value * - (PI) * - DrivetrainConstants.WHEEL_DIAMETER.inMeters * - DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO + (PI) * + DrivetrainConstants.WHEEL_DIAMETER.inMeters * + DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO ) Logger.recordOutput("$label/drivePositionUnits", driveSensor.position.inMeters) inputs.drivePosition = driveSensor.position @@ -341,9 +341,11 @@ class SwerveModuleIOTalon( override fun zeroSteering(isInAutonomous: Boolean) { steeringFalcon.setPosition( steeringSensor.positionToRawUnits( - if (label != Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) + if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME){ + (potentiometerOutput.radians - zeroOffset) + } else { (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) - else (potentiometerOutput.radians - zeroOffset) + } ) ) @@ -437,4 +439,4 @@ class SwerveModuleIOTalon( driveFalcon.setControl(DutyCycleOut(input.inVolts / 12.0)) } } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index c2f50ed1..e9cdba1c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -379,7 +379,9 @@ class Wrist(val io: WristIO) : SubsystemBase() { } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt val setPoint: TrapezoidProfile.State = wristProfile.calculate(timeElapsed) + setWristPosition(setPoint) + Logger.recordOutput("Wrist/completedMotionProfile", wristProfile.isFinished(timeElapsed)) Logger.recordOutput("Wrist/travelingUp", travelingUp) nextState = fromRequestToState(currentRequest) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt index 814d739a..b09b6984 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -148,6 +148,10 @@ object WristIOTalon : WristIO { wristConfiguration.Slot2.kD = wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KD) + wristConfiguration.Voltage.PeakForwardVoltage = 3.0 + wristConfiguration.Voltage.PeakReverseVoltage = -3.0 + + wristConfiguration.CurrentLimits.SupplyCurrentLimit = WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes wristConfiguration.CurrentLimits.SupplyCurrentThreshold =