Skip to content
This repository was archived by the owner on May 11, 2025. It is now read-only.

Feature/error check #84

Open
kennyL07 wants to merge 10 commits into
base: develop
Choose a base branch
from
Open

Feature/error check #84

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
5a1de69
First commit
kennyL07 Mar 22, 2023
d9fb931
Give errorCode name with report
kennyL07 Mar 22, 2023
cd897a7
Remove Double checks
kennyL07 Mar 22, 2023
d0f8f5b
Remove double ErrorCheck
kennyL07 Mar 22, 2023
952e1e3
Add multipe checks for sections in SwerveModule
kennyL07 Mar 22, 2023
a1a17bb
Add more info, check codes seperately
kennyL07 Mar 30, 2023
b1168d1
Addressed PR changes
calvinwt Apr 4, 2023
eed3c36
removed redundant int variable
calvinwt Apr 4, 2023
c423dd1
Addressed further PR changes
calvinwt Apr 4, 2023
0bc69cc
Trimmed down error checks to a single method call, allowing for easie…
calvinwt May 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 16 additions & 7 deletions src/main/java/frc/robot/hardware/MotorController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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;
Copy link
Copy Markdown
Contributor

@zainmfjavaid zainmfjavaid Mar 22, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A comment here might help for clarity

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);}
}
}
13 changes: 5 additions & 8 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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() {
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/subsystems/BuddyBalanceSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
43 changes: 22 additions & 21 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand All @@ -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);

Expand Down Expand Up @@ -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();
}
Expand All @@ -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);
Expand All @@ -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));
}
}

Expand All @@ -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
Expand All @@ -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));
}
Expand Down