diff --git a/src/main/java/frc/robot/hardware/MotorController.java b/src/main/java/frc/robot/hardware/MotorController.java index d98174b2..0619a089 100644 --- a/src/main/java/frc/robot/hardware/MotorController.java +++ b/src/main/java/frc/robot/hardware/MotorController.java @@ -1,10 +1,13 @@ 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; +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 +101,26 @@ 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){DriverStation.reportError("An Error has occured in constructMotor(MotorConfig config) with config " + config.getID(), 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; //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) 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 7a2c9072..08d30841 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -174,10 +174,9 @@ public ArmSubsystem() { baseMotor2 = MotorController.constructMotor(MotorConfig.ArmBase2); elbowMotor = MotorController.constructMotor(MotorConfig.ArmElbow); - baseMotor2.follow(baseMotor); - - baseMotor.enableVoltageCompensation(11); - elbowMotor.enableVoltageCompensation(11); + 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); @@ -432,13 +431,11 @@ public Command stowArmParallel() { } public void coastBase() { - baseMotor.setIdleMode(IdleMode.kCoast); - baseMotor2.setIdleMode(IdleMode.kCoast); + MotorController.errorCheck(baseMotor.setIdleMode(IdleMode.kCoast)); } public void brakeBase() { - baseMotor.setIdleMode(IdleMode.kBrake); - baseMotor2.setIdleMode(IdleMode.kBrake); + 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 aeaf8b84..ae911e73 100644 --- a/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java @@ -92,13 +92,13 @@ 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); + 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 - rightPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition); - leftPIDController.setReference(kDeployedPosition, CANSparkMax.ControlType.kPosition); + 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 556f7f0a..783f2170 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; @@ -71,6 +72,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; @@ -82,11 +85,11 @@ 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); + + MotorController.errorCheck(driveEncoder.setPositionConversionFactor(kDriveEncoderRotFactor)); + MotorController.errorCheck(driveEncoder.setVelocityConversionFactor(kDriveEncoderRPMFactor)); + MotorController.errorCheck(turningEncoder.setPositionConversionFactor(kTurningEncoderRotFactor)); + MotorController.errorCheck(turningEncoder.setVelocityConversionFactor(kTurningEncoderRPMFactor)); simDriveMotor = new FlywheelSim( LinearSystemId.identifyVelocitySystem(2, 1.24), //TODO: Update with real SysID @@ -103,7 +106,7 @@ public SwerveModule(MotorConfig driveMotorConfig, MotorConfig turningMotorConfig simTurningEncoder = new RelativeEncoderSim(turningMotor); turningPIDController = turningMotor.getPIDController(); - turningPIDController.setP(kPTurning); + MotorController.errorCheck(turningPIDController.setP(kPTurning)); simTurningPIDController = new PIDController(turningPIDController.getP(), turningPIDController.getI(), turningPIDController.getD()); simTurningPIDController.enableContinuousInput(-Math.PI, Math.PI); @@ -143,8 +146,8 @@ public double getTurningVelocity() { } public void resetEncoders() { - driveEncoder.setPosition(0); - turningEncoder.setPosition(getAbsoluteTurningPosition()); + MotorController.errorCheck(driveEncoder.setPosition(0)); + MotorController.errorCheck(turningEncoder.setPosition(getAbsoluteTurningPosition())); if(Robot.isReal()){ absoluteEncoder.close(); } @@ -167,8 +170,7 @@ 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); - + MotorController.errorCheck(turningPIDController.setReference(turningSetpoint, ControlType.kPosition)); SmartDashboard.putString("Swerve[" + ID + "] state", desiredState.toString()); desiredAngleLog.append(desiredState.angle.getRadians()); desiredSpeedLog.append(desiredState.speedMetersPerSecond); @@ -178,9 +180,9 @@ 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); + MotorController.errorCheck(turningPIDController.setReference(Math.PI/4, ControlType.kPosition)); } else{ - turningPIDController.setReference(-Math.PI/4, ControlType.kPosition); + MotorController.errorCheck(turningPIDController.setReference(-Math.PI/4, ControlType.kPosition)); } } @@ -190,13 +192,13 @@ public void stop() { } public void coast() { - driveMotor.setIdleMode(IdleMode.kCoast); - turningMotor.setIdleMode(IdleMode.kCoast); + MotorController.errorCheck(driveMotor.setIdleMode(IdleMode.kCoast)); + MotorController.errorCheck(turningMotor.setIdleMode(IdleMode.kCoast)); } public void brake() { - driveMotor.setIdleMode(IdleMode.kBrake); - turningMotor.setIdleMode(IdleMode.kBrake); + MotorController.errorCheck(driveMotor.setIdleMode(IdleMode.kBrake)); + MotorController.errorCheck(turningMotor.setIdleMode(IdleMode.kBrake)); } @Override @@ -207,11 +209,10 @@ public void simulationPeriodic() { simDriveMotor.update(Robot.kDefaultPeriod); simTurningMotor.update(Robot.kDefaultPeriod); - simDriveEncoder.setPosition(simDriveEncoder.getPosition() + simDriveMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod); - simDriveEncoder.setSimVelocity(simDriveMotor.getAngularVelocityRadPerSec()); - - simTurningEncoder.setPosition(simTurningEncoder.getPosition() + simTurningMotor.getAngularVelocityRadPerSec() * Robot.kDefaultPeriod); - simTurningEncoder.setSimVelocity(simTurningMotor.getAngularVelocityRadPerSec()); + 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)); }