From 5a1de6976a90b5e266b2adf16636ea55fb7dfd35 Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Wed, 22 Mar 2023 17:39:49 -0500 Subject: [PATCH 01/10] First commit --- .../frc/robot/hardware/MotorController.java | 20 +++++--- .../frc/robot/subsystems/ArmSubsystem.java | 19 ++++--- .../subsystems/BuddyBalanceSubsystem.java | 15 ++++-- .../frc/robot/subsystems/SwerveModule.java | 49 ++++++++++++------- 4 files changed, 67 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/hardware/MotorController.java b/src/main/java/frc/robot/hardware/MotorController.java index d98174b2..c712df6e 100644 --- a/src/main/java/frc/robot/hardware/MotorController.java +++ b/src/main/java/frc/robot/hardware/MotorController.java @@ -5,6 +5,8 @@ import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.DriverStation; + public class MotorController { public static final class MotorDefaults{ //Constants to use as default values for Motor Controllers @@ -98,20 +100,24 @@ public boolean getReversed(){ public static CANSparkMax constructMotor(MotorConfig config){ CANSparkMax motor = new CANSparkMax(config.getID(), MotorType.kBrushless); - motor.restoreFactoryDefaults(); - motor.setSmartCurrentLimit(config.getCurrentLimit()); - motor.setIdleMode(config.getIdleMode()); - motor.setOpenLoopRampRate(config.getOpenLoopRampRate()); + int errorCode = motor.restoreFactoryDefaults().value; + errorCode += motor.setSmartCurrentLimit(config.getCurrentLimit()).value; + errorCode += motor.setIdleMode(config.getIdleMode()).value; + errorCode += motor.setOpenLoopRampRate(config.getOpenLoopRampRate()).value; motor.setInverted(config.getReversed()); + if (errorCode != 0){ + if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config)", null);} + } return motor; } public static CANSparkMax constructMotor(MotorConfig config, double[] PIDArray){ CANSparkMax motor = constructMotor(config); SparkMaxPIDController motorPIDcontroller = motor.getPIDController(); - motorPIDcontroller.setP(PIDArray[0]); - motorPIDcontroller.setI(PIDArray[1]); - motorPIDcontroller.setD(PIDArray[2]); + int errorCode = motorPIDcontroller.setP(PIDArray[0]).value; + errorCode += motorPIDcontroller.setI(PIDArray[1]).value; + errorCode += motorPIDcontroller.setD(PIDArray[2]).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config, double[] PIDArray)", null);} return motor; } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 7a2c9072..fc8396ed 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -174,10 +174,13 @@ public ArmSubsystem() { baseMotor2 = MotorController.constructMotor(MotorConfig.ArmBase2); elbowMotor = MotorController.constructMotor(MotorConfig.ArmElbow); - baseMotor2.follow(baseMotor); + int errorCode = baseMotor2.follow(baseMotor).value; + + errorCode += baseMotor.enableVoltageCompensation(11).value; + errorCode += elbowMotor.enableVoltageCompensation(11).value; + + if(errorCode != 0){DriverStation.reportError("An Error has occured in ArmSubsystem()", null);} - baseMotor.enableVoltageCompensation(11); - elbowMotor.enableVoltageCompensation(11); baseAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmBase); elbowAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmElbow); @@ -432,13 +435,15 @@ public Command stowArmParallel() { } public void coastBase() { - baseMotor.setIdleMode(IdleMode.kCoast); - baseMotor2.setIdleMode(IdleMode.kCoast); + int errorCode = baseMotor.setIdleMode(IdleMode.kCoast).value; + errorCode += baseMotor2.setIdleMode(IdleMode.kCoast).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in coastBase()", null);} } public void brakeBase() { - baseMotor.setIdleMode(IdleMode.kBrake); - baseMotor2.setIdleMode(IdleMode.kBrake); + int errorCode = baseMotor.setIdleMode(IdleMode.kBrake).value; + errorCode += baseMotor2.setIdleMode(IdleMode.kBrake).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in brakeBase()", null);} } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java index aeaf8b84..bfb6424e 100644 --- a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -92,13 +93,19 @@ public void deployBuddyBalance() { } public void retrieveBuddy() { // Used to pick up the buddy robot while the lift is already underneath it - rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition); - leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition); + int errorCode = rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; + errorCode += leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; + if (errorCode != 0){ + if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + } } public void releaseBuddy() { // Used to set down the robot - rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition); - leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition); + int errorCode = rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; + errorCode += leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; + if (errorCode != 0){ + if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + } } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 556f7f0a..5ae8eff8 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -21,6 +21,7 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -83,10 +84,10 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig driveEncoder = this.driveMotor.getEncoder(); turningEncoder = this.turningMotor.getEncoder(); - driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor); - driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor); - turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor); - turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor); + int errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; + errorCode += driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor).value; + errorCode += turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor).value; + errorCode += turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor).value; simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID @@ -103,7 +104,7 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig simTurningEncoder = new RelativeEncoderSim(turningMotor); turningPIDController = turningMotor.getPIDController(); - turningPIDController.setP(kPTurning); + errorCode += turningPIDController.setP(kPTurning).value; simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -113,6 +114,8 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig actualSpeedLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/setSpeed"); //Logs actual speed in meters per second desiredAngleLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/setAngle"); //Logs desired angle in radians actualAngleLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/actualAngle"); //Logs actual relative angle in radians + + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule", null);} } public SwerveModulePosition getPosition() { @@ -143,11 +146,14 @@ public double getTurningVelocity() { } public void resetEncoders() { - driveEncoder.setPosition(0); - turningEncoder.setPosition(getAbsoluteTurningPosition()); + int errorCode = driveEncoder.setPosition(0).value; + errorCode += turningEncoder.setPosition(getAbsoluteTurningPosition()).value; if(Robot.isReal()){ absoluteEncoder.close(); } + if(errorCode != 0){ + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() Code:" + errorCode, null);} + } } public SwerveModuleState getState() { @@ -167,7 +173,9 @@ public void setDesiredState(SwerveModuleState desiredState) { desiredState = SwerveModuleState.optimize(desiredState, getState().angle); driveMotor.set(desiredState.speedMetersPerSecond / SwerveSubsystem.kPhysicalMaxSpeed); turningSetpoint = calculateSetpoint(desiredState.angle.getRadians()); - turningPIDController.setReference(turningSetpoint, ControlType.kPosition); + int errorCode = turningPIDController.setReference(turningSetpoint, ControlType.kPosition).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in setDesiredState() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + SmartDashboard.putString("Swerve[" + ID + "] state", desiredState.toString()); desiredAngleLog.append(desiredState.angle.getRadians()); @@ -178,9 +186,11 @@ public void park(boolean reversed) { stop(); if(reversed){ //not using calculateSetpoint because these are less than one full rotation - turningPIDController.setReference(Math.PI/4, ControlType.kPosition); + int errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} } else{ - turningPIDController.setReference(-Math.PI/4, ControlType.kPosition); + int errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} } } @@ -190,13 +200,15 @@ public void stop() { } public void coast() { - driveMotor.setIdleMode(IdleMode.kCoast); - turningMotor.setIdleMode(IdleMode.kCoast); + int errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; + errorCode += turningMotor.setIdleMode(IdleMode.kCoast).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.coast() Code:" + errorCode, null);} } public void brake() { - driveMotor.setIdleMode(IdleMode.kBrake); - turningMotor.setIdleMode(IdleMode.kBrake); + int errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; + errorCode += turningMotor.setIdleMode(IdleMode.kBrake).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() during .setIdleMode() Code:" + errorCode, null);} } @Override @@ -207,13 +219,14 @@ public void simulationPeriodic() { simDriveMotor.update(Robot.kDefaultPeriod); simTurningMotor.update(Robot.kDefaultPeriod); - simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod); - simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()); + int errorCode = simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value; + errorCode += simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value; - simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod); - simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()); + errorCode += simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value; + errorCode += simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value; turningMotor.set(simTurningPIDController.calculate(getTurningPosition(), turningSetpoint)); + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule simulationPeriodic", null);} } @Override From d9fb931bbd308d17344d26f3cbe257c3bf306258 Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Wed, 22 Mar 2023 17:52:34 -0500 Subject: [PATCH 02/10] Give errorCode name with report --- .../frc/robot/subsystems/BuddyBalanceSubsystem.java | 5 +++-- .../java/frc/robot/subsystems/SwerveModule.java | 13 +++++++------ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java index bfb6424e..9cd22a69 100644 --- a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java @@ -18,6 +18,7 @@ import frc.robot.classes.TunableNumber; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -96,7 +97,7 @@ public void retrieveBuddy() { // Used to pick up the buddy robot while the lift int errorCode = rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; errorCode += leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; if (errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } @@ -104,7 +105,7 @@ public void releaseBuddy() { // Used to set down the robot int errorCode = rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; errorCode += leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; if (errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 5ae8eff8..133f5505 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -6,6 +6,7 @@ import com.ctre.phoenix.sensors.WPI_CANCoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -152,7 +153,7 @@ public void resetEncoders() { absoluteEncoder.close(); } if(errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } @@ -174,7 +175,7 @@ public void setDesiredState(SwerveModuleState desiredState) { driveMotor.set(desiredState.speedMetersPerSecond / SwerveSubsystem.kPhysicalMaxSpeed); turningSetpoint = calculateSetpoint(desiredState.angle.getRadians()); int errorCode = turningPIDController.setReference(turningSetpoint, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in setDesiredState() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in setDesiredState() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} SmartDashboard.putString("Swerve[" + ID + "] state", desiredState.toString()); @@ -187,10 +188,10 @@ public void park(boolean reversed) { if(reversed){ //not using calculateSetpoint because these are less than one full rotation int errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } else{ int errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } @@ -202,13 +203,13 @@ public void stop() { public void coast() { int errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; errorCode += turningMotor.setIdleMode(IdleMode.kCoast).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.coast() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.coast() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } public void brake() { int errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; errorCode += turningMotor.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() during .setIdleMode() Code:" + errorCode, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() during .setIdleMode() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } @Override From cd897a72e033758bb9d06255990f959207ef63e1 Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Wed, 22 Mar 2023 18:16:38 -0500 Subject: [PATCH 03/10] Remove Double checks --- .../java/frc/robot/subsystems/BuddyBalanceSubsystem.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java index 9cd22a69..1867aeba 100644 --- a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java @@ -96,17 +96,13 @@ public void deployBuddyBalance() { public void retrieveBuddy() { // Used to pick up the buddy robot while the lift is already underneath it int errorCode = rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; errorCode += leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; - if (errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} - } + if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } public void releaseBuddy() { // Used to set down the robot int errorCode = rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; errorCode += leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; - if (errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} - } + if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } @Override From d0f8f5bd197a9d66168fb77b20f9e096281cecb1 Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Wed, 22 Mar 2023 18:16:44 -0500 Subject: [PATCH 04/10] Remove double ErrorCheck --- src/main/java/frc/robot/hardware/MotorController.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/hardware/MotorController.java b/src/main/java/frc/robot/hardware/MotorController.java index c712df6e..b1531abe 100644 --- a/src/main/java/frc/robot/hardware/MotorController.java +++ b/src/main/java/frc/robot/hardware/MotorController.java @@ -105,16 +105,14 @@ public static CANSparkMax constructMotor(MotorConfig config){ errorCode += motor.setIdleMode(config.getIdleMode()).value; errorCode += motor.setOpenLoopRampRate(config.getOpenLoopRampRate()).value; motor.setInverted(config.getReversed()); - if (errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config)", null);} - } + if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config)", null);} return motor; } public static CANSparkMax constructMotor(MotorConfig config, double[] PIDArray){ CANSparkMax motor = constructMotor(config); SparkMaxPIDController motorPIDcontroller = motor.getPIDController(); - int errorCode = motorPIDcontroller.setP(PIDArray[0]).value; + int errorCode = motorPIDcontroller.setP(PIDArray[0]).value; //Gets REVLibError code value (Is 0 if no error occured) errorCode += motorPIDcontroller.setI(PIDArray[1]).value; errorCode += motorPIDcontroller.setD(PIDArray[2]).value; if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config, double[] PIDArray)", null);} From 952e1e342e282e040b3ba065df0f3747fd13c87a Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Wed, 22 Mar 2023 18:20:48 -0500 Subject: [PATCH 05/10] Add multipe checks for sections in SwerveModule --- src/main/java/frc/robot/subsystems/SwerveModule.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 133f5505..2e087316 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -87,9 +87,12 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig int errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; errorCode += driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder", null);} + errorCode = 0; errorCode += turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor).value; errorCode += turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor).value; - + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder", null);} + errorCode = 0; simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID DCMotor.getNEO(1), @@ -106,6 +109,7 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig turningPIDController = turningMotor.getPIDController(); errorCode += turningPIDController.setP(kPTurning).value; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningPIDController", null);} simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -115,8 +119,6 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig actualSpeedLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/setSpeed"); //Logs actual speed in meters per second desiredAngleLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/setAngle"); //Logs desired angle in radians actualAngleLog = new DoubleLogEntry(datalog, "/swerve/" + ID +"/actualAngle"); //Logs actual relative angle in radians - - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule", null);} } public SwerveModulePosition getPosition() { From a1a17bb24de2fdc4c9b8964e2665ea847e96c7b2 Mon Sep 17 00:00:00 2001 From: Kennyson Le Date: Thu, 30 Mar 2023 18:35:34 -0500 Subject: [PATCH 06/10] Add more info, check codes seperately --- .../frc/robot/hardware/MotorController.java | 4 +-- .../frc/robot/subsystems/ArmSubsystem.java | 10 ++----- .../frc/robot/subsystems/SwerveModule.java | 28 +++++++++---------- 3 files changed, 19 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/hardware/MotorController.java b/src/main/java/frc/robot/hardware/MotorController.java index b1531abe..2112a796 100644 --- a/src/main/java/frc/robot/hardware/MotorController.java +++ b/src/main/java/frc/robot/hardware/MotorController.java @@ -105,7 +105,7 @@ public static CANSparkMax constructMotor(MotorConfig config){ errorCode += motor.setIdleMode(config.getIdleMode()).value; errorCode += motor.setOpenLoopRampRate(config.getOpenLoopRampRate()).value; motor.setInverted(config.getReversed()); - if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config)", null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config) with config " + config.getID(), null);} return motor; } @@ -115,7 +115,7 @@ public static CANSparkMax constructMotor(MotorConfig config, double[] PIDArray){ int errorCode = motorPIDcontroller.setP(PIDArray[0]).value; //Gets REVLibError code value (Is 0 if no error occured) errorCode += motorPIDcontroller.setI(PIDArray[1]).value; errorCode += motorPIDcontroller.setD(PIDArray[2]).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config, double[] PIDArray)", null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config, double[] PIDArray) with motor " + motor.getDeviceId() + " whilst setting PID", null);} return motor; } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index fc8396ed..6dcb0506 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -174,13 +174,9 @@ public ArmSubsystem() { baseMotor2 = MotorController.constructMotor(MotorConfig.ArmBase2); elbowMotor = MotorController.constructMotor(MotorConfig.ArmElbow); - int errorCode = baseMotor2.follow(baseMotor).value; - - errorCode += baseMotor.enableVoltageCompensation(11).value; - errorCode += elbowMotor.enableVoltageCompensation(11).value; - - if(errorCode != 0){DriverStation.reportError("An Error has occured in ArmSubsystem()", null);} - + if(baseMotor2.follow(baseMotor).value != 0){DriverStation.reportError("An Error has occured with baseMotor2.follow(baseMotor)", null);}; + if(baseMotor.enableVoltageCompensation(11).value != 0){DriverStation.reportError("An Error has occured with baseMotor.enableVoltageCompensation", null);}; + if(elbowMotor.enableVoltageCompensation(11).value != 0){DriverStation.reportError("An Error has occured with elbowMotor.enableVoltageCompensation", null);}; baseAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmBase); elbowAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmElbow); diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 2e087316..8775df14 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -87,12 +87,11 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig int errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; errorCode += driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder", null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder with module " + ID, null);} errorCode = 0; errorCode += turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor).value; errorCode += turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder", null);} - errorCode = 0; + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder with module " + ID, null);} simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID DCMotor.getNEO(1), @@ -108,8 +107,9 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig simTurningEncoder = new RelativeEncoderSim(turningMotor); turningPIDController = turningMotor.getPIDController(); + errorCode = 0; errorCode += turningPIDController.setP(kPTurning).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningPIDController", null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningPIDController with module " + ID, null);} simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -155,7 +155,7 @@ public void resetEncoders() { absoluteEncoder.close(); } if(errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() with module " + ID, null);} } } @@ -186,13 +186,14 @@ public void setDesiredState(SwerveModuleState desiredState) { } public void park(boolean reversed) { + int errorCode; stop(); if(reversed){ //not using calculateSetpoint because these are less than one full rotation - int errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; + errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } else{ - int errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; + errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } @@ -211,25 +212,24 @@ public void coast() { public void brake() { int errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; errorCode += turningMotor.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() during .setIdleMode() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() with .setIdleMode()", null);} } @Override public void simulationPeriodic() { + int errorCode; simDriveMotor.setInputVoltage(driveMotor.get() * RobotController.getBatteryVoltage()); simTurningMotor.setInputVoltage(turningMotor.get() * RobotController.getBatteryVoltage()); simDriveMotor.update(Robot.kDefaultPeriod); simTurningMotor.update(Robot.kDefaultPeriod); - int errorCode = simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value; - errorCode += simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value; - - errorCode += simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value; - errorCode += simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value; + if(simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simDriveEncoder.setPosition during simulationPeriodic", null);}; + if(simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simDriveEncoder.setSimVelocity during simulationPeriodic", null);}; + if(simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simTurningEncoder.setPosition during simulationPeriodic", null);}; + if(simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occured in SwerveModule with simTurningEncoder.setSimVelocity during simulationPeriodic", null); errorCode = 0;} turningMotor.set(simTurningPIDController.calculate(getTurningPosition(), turningSetpoint)); - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule simulationPeriodic", null);} } @Override From b1168d117ee573992a0f6d80034816c675efaf27 Mon Sep 17 00:00:00 2001 From: Calvin Tucker Date: Tue, 4 Apr 2023 17:41:50 -0500 Subject: [PATCH 07/10] Addressed PR changes --- .../frc/robot/subsystems/SwerveModule.java | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 8775df14..ef0cfefa 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -73,6 +73,8 @@ public class SwerveModule extends SubsystemBase { private final String ID; + private int errorCode; + public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig, EncoderConfig absoluteEncoderConfig, String ID) { this.ID = ID; @@ -85,13 +87,13 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig driveEncoder = this.driveMotor.getEncoder(); turningEncoder = this.turningMotor.getEncoder(); - int errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; + errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; errorCode += driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder with module " + ID, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder at module " + ID, null);} errorCode = 0; errorCode += turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor).value; errorCode += turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder with module " + ID, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder at module " + ID, null);} simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID DCMotor.getNEO(1), @@ -109,7 +111,7 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig turningPIDController = turningMotor.getPIDController(); errorCode = 0; errorCode += turningPIDController.setP(kPTurning).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningPIDController with module " + ID, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule turningPIDController at module " + ID, null);} simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -155,7 +157,7 @@ public void resetEncoders() { absoluteEncoder.close(); } if(errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() with module " + ID, null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() at module " + ID, null);} } } @@ -177,7 +179,7 @@ public void setDesiredState(SwerveModuleState desiredState) { driveMotor.set(desiredState.speedMetersPerSecond / SwerveSubsystem.kPhysicalMaxSpeed); turningSetpoint = calculateSetpoint(desiredState.angle.getRadians()); int errorCode = turningPIDController.setReference(turningSetpoint, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in setDesiredState() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in setDesiredState() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} SmartDashboard.putString("Swerve[" + ID + "] state", desiredState.toString()); @@ -191,10 +193,10 @@ public void park(boolean reversed) { if(reversed){ //not using calculateSetpoint because these are less than one full rotation errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.park() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } else{ errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.park() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.park() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} } } @@ -206,13 +208,14 @@ public void stop() { public void coast() { int errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; errorCode += turningMotor.setIdleMode(IdleMode.kCoast).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule.coast() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.coast() at module" + ID + " Code:" + REVLibError.fromInt(errorCode).toString(), null);} } public void brake() { int errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; errorCode += turningMotor.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured inn SwerveModule.brake() with .setIdleMode()", null);} + if(errorCode != 0){DriverStation.reportError("An Error has occurred inn SwerveModule.brake() at module" + ID + " with .setIdleMode()", null);} } @Override @@ -224,10 +227,10 @@ public void simulationPeriodic() { simDriveMotor.update(Robot.kDefaultPeriod); simTurningMotor.update(Robot.kDefaultPeriod); - if(simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simDriveEncoder.setPosition during simulationPeriodic", null);}; - if(simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simDriveEncoder.setSimVelocity during simulationPeriodic", null);}; - if(simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule whilst simTurningEncoder.setPosition during simulationPeriodic", null);}; - if(simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occured in SwerveModule with simTurningEncoder.setSimVelocity during simulationPeriodic", null); errorCode = 0;} + if(simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule with at" + ID + " whilst simDriveEncoder.setPosition during simulationPeriodic", null);}; + if(simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occurred in SwerveModule at module" + ID + " whilst simDriveEncoder.setSimVelocity during simulationPeriodic", null);}; + if(simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule at module" + ID + " whilst simTurningEncoder.setPosition during simulationPeriodic", null);}; + if(simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occurred in SwerveModule at module" + ID + " with simTurningEncoder.setSimVelocity during simulationPeriodic", null); errorCode = 0;} turningMotor.set(simTurningPIDController.calculate(getTurningPosition(), turningSetpoint)); } From eed3c3653dd15a6b80b687260da3d470cd02a236 Mon Sep 17 00:00:00 2001 From: Calvin Tucker Date: Tue, 4 Apr 2023 17:43:50 -0500 Subject: [PATCH 08/10] removed redundant int variable --- src/main/java/frc/robot/subsystems/SwerveModule.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index ef0cfefa..30563dca 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -220,7 +220,6 @@ public void brake() { @Override public void simulationPeriodic() { - int errorCode; simDriveMotor.setInputVoltage(driveMotor.get() * RobotController.getBatteryVoltage()); simTurningMotor.setInputVoltage(turningMotor.get() * RobotController.getBatteryVoltage()); From c423dd1723e3888ce610e366f32dc0b78649334e Mon Sep 17 00:00:00 2001 From: Calvin Tucker Date: Tue, 4 Apr 2023 17:49:01 -0500 Subject: [PATCH 09/10] Addressed further PR changes --- .../java/frc/robot/subsystems/SwerveModule.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 30563dca..3f3cb079 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -206,15 +206,18 @@ public void stop() { } public void coast() { - int errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; - errorCode += turningMotor.setIdleMode(IdleMode.kCoast).value; - + errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; + if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.coast() at module" + ID + " Code:" + REVLibError.fromInt(errorCode).toString(), null);} + + errorCode = turningMotor.setIdleMode(IdleMode.kCoast).value; if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.coast() at module" + ID + " Code:" + REVLibError.fromInt(errorCode).toString(), null);} } public void brake() { - int errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; - errorCode += turningMotor.setIdleMode(IdleMode.kBrake).value; + errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; + if(errorCode != 0){DriverStation.reportError("An Error has occurred inn SwerveModule.brake() at module" + ID + " with .setIdleMode()", null);} + + errorCode = turningMotor.setIdleMode(IdleMode.kBrake).value; if(errorCode != 0){DriverStation.reportError("An Error has occurred inn SwerveModule.brake() at module" + ID + " with .setIdleMode()", null);} } From 0bc69ccb5d3e50905dc64c0e45f803bec9ac557d Mon Sep 17 00:00:00 2001 From: Calvin Tucker Date: Thu, 11 May 2023 17:45:08 -0500 Subject: [PATCH 10/10] Trimmed down error checks to a single method call, allowing for easier addition of error checks --- .../frc/robot/hardware/MotorController.java | 5 ++ .../frc/robot/subsystems/ArmSubsystem.java | 14 ++--- .../subsystems/BuddyBalanceSubsystem.java | 12 ++-- .../frc/robot/subsystems/SwerveModule.java | 58 ++++++------------- 4 files changed, 33 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/hardware/MotorController.java b/src/main/java/frc/robot/hardware/MotorController.java index 2112a796..0619a089 100644 --- a/src/main/java/frc/robot/hardware/MotorController.java +++ b/src/main/java/frc/robot/hardware/MotorController.java @@ -1,6 +1,7 @@ package frc.robot.hardware; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -118,4 +119,8 @@ public static CANSparkMax constructMotor(MotorConfig config, double[] PIDArray){ if(errorCode != 0){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config, double[] PIDArray) with motor " + motor.getDeviceId() + " whilst setting PID", null);} return motor; } + + public static void errorCheck(REVLibError error) { + if(error != REVLibError.kOk){DriverStation.reportError("An error has occured! ", true);} + } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 6dcb0506..08d30841 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -174,9 +174,9 @@ public ArmSubsystem() { baseMotor2 = MotorController.constructMotor(MotorConfig.ArmBase2); elbowMotor = MotorController.constructMotor(MotorConfig.ArmElbow); - if(baseMotor2.follow(baseMotor).value != 0){DriverStation.reportError("An Error has occured with baseMotor2.follow(baseMotor)", null);}; - if(baseMotor.enableVoltageCompensation(11).value != 0){DriverStation.reportError("An Error has occured with baseMotor.enableVoltageCompensation", null);}; - if(elbowMotor.enableVoltageCompensation(11).value != 0){DriverStation.reportError("An Error has occured with elbowMotor.enableVoltageCompensation", null);}; + MotorController.errorCheck(baseMotor2.follow(baseMotor)); + MotorController.errorCheck(baseMotor.enableVoltageCompensation(11)); + MotorController.errorCheck(elbowMotor.enableVoltageCompensation(11)); baseAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmBase); elbowAbsoluteEncoder = AbsoluteEncoder.constructREVEncoder(EncoderConfig.ArmElbow); @@ -431,15 +431,11 @@ public Command stowArmParallel() { } public void coastBase() { - int errorCode = baseMotor.setIdleMode(IdleMode.kCoast).value; - errorCode += baseMotor2.setIdleMode(IdleMode.kCoast).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in coastBase()", null);} + MotorController.errorCheck(baseMotor.setIdleMode(IdleMode.kCoast)); } public void brakeBase() { - int errorCode = baseMotor.setIdleMode(IdleMode.kBrake).value; - errorCode += baseMotor2.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in brakeBase()", null);} + MotorController.errorCheck(baseMotor.setIdleMode(IdleMode.kBrake)); } public void stop() { diff --git a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java index 1867aeba..ae911e73 100644 --- a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -18,7 +17,6 @@ import frc.robot.classes.TunableNumber; import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -94,15 +92,13 @@ public void deployBuddyBalance() { } public void retrieveBuddy() { // Used to pick up the buddy robot while the lift is already underneath it - int errorCode = rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; - errorCode += leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in retrieveBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + MotorController.errorCheck(rightPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition)); + MotorController.errorCheck(leftPIDController.setReference(kBalancedPosition, CANSparkMax.ControlType.kPosition)); } public void releaseBuddy() { // Used to set down the robot - int errorCode = rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; - errorCode += leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in releaseBuddy() for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + MotorController.errorCheck(rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition)); + MotorController.errorCheck(leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition)); } @Override diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 3f3cb079..783f2170 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -6,7 +6,6 @@ import com.ctre.phoenix.sensors.WPI_CANCoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -86,14 +85,12 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig driveEncoder = this.driveMotor.getEncoder(); turningEncoder = this.turningMotor.getEncoder(); + + MotorController.errorCheck(driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor)); + MotorController.errorCheck(driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor)); + MotorController.errorCheck(turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor)); + MotorController.errorCheck(turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor)); - errorCode = driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor).value; - errorCode += driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule driveEncoder at module " + ID, null);} - errorCode = 0; - errorCode += turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor).value; - errorCode += turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor).value; - if(errorCode != 0){DriverStation.reportError("An Error has occured in SwerveModule turningEncoder at module " + ID, null);} simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID DCMotor.getNEO(1), @@ -109,9 +106,7 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig simTurningEncoder = new RelativeEncoderSim(turningMotor); turningPIDController = turningMotor.getPIDController(); - errorCode = 0; - errorCode += turningPIDController.setP(kPTurning).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule turningPIDController at module " + ID, null);} + MotorController.errorCheck(turningPIDController.setP(kPTurning)); simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -151,14 +146,11 @@ public double getTurningVelocity() { } public void resetEncoders() { - int errorCode = driveEncoder.setPosition(0).value; - errorCode += turningEncoder.setPosition(getAbsoluteTurningPosition()).value; + MotorController.errorCheck(driveEncoder.setPosition(0)); + MotorController.errorCheck(turningEncoder.setPosition(getAbsoluteTurningPosition())); if(Robot.isReal()){ absoluteEncoder.close(); } - if(errorCode != 0){ - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.resetEncoders() at module " + ID, null);} - } } public SwerveModuleState getState() { @@ -178,25 +170,19 @@ public void setDesiredState(SwerveModuleState desiredState) { desiredState = SwerveModuleState.optimize(desiredState, getState().angle); driveMotor.set(desiredState.speedMetersPerSecond / SwerveSubsystem.kPhysicalMaxSpeed); turningSetpoint = calculateSetpoint(desiredState.angle.getRadians()); - int errorCode = turningPIDController.setReference(turningSetpoint, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in setDesiredState() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} - - + MotorController.errorCheck(turningPIDController.setReference(turningSetpoint, ControlType.kPosition)); SmartDashboard.putString("Swerve[" + ID + "] state", desiredState.toString()); desiredAngleLog.append(desiredState.angle.getRadians()); desiredSpeedLog.append(desiredState.speedMetersPerSecond); } public void park(boolean reversed) { - int errorCode; stop(); if(reversed){ //not using calculateSetpoint because these are less than one full rotation - errorCode = turningPIDController.setReference(Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.park() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + MotorController.errorCheck(turningPIDController.setReference(Math.PI/4, ControlType.kPosition)); } else{ - errorCode = turningPIDController.setReference(-Math.PI/4, ControlType.kPosition).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.park() at module" + ID + " for SparkMaxPIDController.setReference() Code:" + REVLibError.fromInt(errorCode).toString(), null);} + MotorController.errorCheck(turningPIDController.setReference(-Math.PI/4, ControlType.kPosition)); } } @@ -206,19 +192,13 @@ public void stop() { } public void coast() { - errorCode = driveMotor.setIdleMode(IdleMode.kCoast).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.coast() at module" + ID + " Code:" + REVLibError.fromInt(errorCode).toString(), null);} - - errorCode = turningMotor.setIdleMode(IdleMode.kCoast).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred in SwerveModule.coast() at module" + ID + " Code:" + REVLibError.fromInt(errorCode).toString(), null);} + MotorController.errorCheck(driveMotor.setIdleMode(IdleMode.kCoast)); + MotorController.errorCheck(turningMotor.setIdleMode(IdleMode.kCoast)); } public void brake() { - errorCode = driveMotor.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred inn SwerveModule.brake() at module" + ID + " with .setIdleMode()", null);} - - errorCode = turningMotor.setIdleMode(IdleMode.kBrake).value; - if(errorCode != 0){DriverStation.reportError("An Error has occurred inn SwerveModule.brake() at module" + ID + " with .setIdleMode()", null);} + MotorController.errorCheck(driveMotor.setIdleMode(IdleMode.kBrake)); + MotorController.errorCheck(turningMotor.setIdleMode(IdleMode.kBrake)); } @Override @@ -229,10 +209,10 @@ public void simulationPeriodic() { simDriveMotor.update(Robot.kDefaultPeriod); simTurningMotor.update(Robot.kDefaultPeriod); - if(simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule with at" + ID + " whilst simDriveEncoder.setPosition during simulationPeriodic", null);}; - if(simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occurred in SwerveModule at module" + ID + " whilst simDriveEncoder.setSimVelocity during simulationPeriodic", null);}; - if(simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod).value != 0){DriverStation.reportError("An Error has occured in SwerveModule at module" + ID + " whilst simTurningEncoder.setPosition during simulationPeriodic", null);}; - if(simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()).value != 0){DriverStation.reportError("An Error has occurred in SwerveModule at module" + ID + " with simTurningEncoder.setSimVelocity during simulationPeriodic", null); errorCode = 0;} + MotorController.errorCheck(simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod)); + MotorController.errorCheck(simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec())); + MotorController.errorCheck(simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod)); + MotorController.errorCheck(simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec())); turningMotor.set(simTurningPIDController.calculate(getTurningPosition(), turningSetpoint)); }