Hi all,
Our team’s Java swerve drive code only moves one set of motors, even though we are using two different axes on a Xbox Controller to move the drive and angle motors respectively. This code used to work normally, but now it doesn’t.
I’ll paste code in below.
Subsystems (SwerveDriveBase.java)
package frc.robot.subsystems;
import frc.robot.RobotMap;
import frc.robot.commands.SwerveDrive;
import frc.robot.commands.SwerveDrive.ControlType;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
* Subsystem used to control the Swerve Drive
*
*
*
*/
public class SwerveDriveBase extends Subsystem {
public SwerveModule frMod;
public SwerveModule flMod;
public SwerveModule rrMod;
public SwerveModule rlMod;
private final double kp = 0;
private final double ki = 0.000;
private final double kd = 0.000;
/**
*
* Class used to set the swivel motor to the value calculated by the PID controller
*
*/
public class PIDOutputClass implements PIDOutput {
private WPI_TalonSRX swivelMot;
public PIDOutputClass(WPI_TalonSRX swivelMot) {
this.swivelMot = swivelMot;
}
@Override
public void pidWrite(double output) {
swivelMot.set(output);
}
}
/**
*
* Class used to make a swerve module
*
*/
public class SwerveModule {
WPI_TalonSRX driveMot;
WPI_TalonSRX swivelMot;
AnalogInput analogInput;
PIDOutputClass pidOut;
PIDController pidCont;
double lastAngle;
/**
* Constructs the swerve module
* @param swivelMot The motor that controls the module's rotation
* @param driveMot The motor that controls the module's drive speed
* @param analogInput Encoder that tracks the angle of the swivel motor
*/
public SwerveModule(
WPI_TalonSRX swivelMot,
WPI_TalonSRX driveMot,
AnalogInput analogInput
) {
this.driveMot = driveMot;
this.swivelMot = swivelMot;
this.analogInput = analogInput;
pidOut = new PIDOutputClass(swivelMot);
pidCont = new PIDController(
kp, ki, kd
);
}
/**
*
* Method that sets a module's angle of the module and the speed of the drive wheel
* @param angle Angle of the module
* @param speed Speed of the drive wheel
*/
public void setModule(double angle, double speed) {
//Resets the encoder if it gets too high or low not needed so deleted since analog 0 to 5 volts only
double curAngle = getAngle();
//Makes the module go to the opposite angle and go backwards when it's quicker than turning all the way around
if(Math.abs(angle - curAngle) > 90 && Math.abs(angle - curAngle) < 270 && angle != 0) {
angle = (angle + 180)%360;
speed = -speed;
}
//Makes it so when you stop moving it doesn't reset the angle to 0, but leaves it where it was
if(speed == 0) {
setAngle(lastAngle);
setSpeed(speed);
} else {
setAngle(angle);
setSpeed(speed);
lastAngle = angle;
}
//System.out.println(curAngle);
//System.out.println(angle);
}
//Sets the module to a specific angle
public void setAngle(double angle) {
//pidCont.enable();
pidCont.setSetpoint(((angle)/360)*5);
}
//Sets the drive motor's speed
public void setSpeed(double speed) {
driveMot.set(speed);
}
//Sets the Swivel Motor's speed
public void setSwivel(double speed) {
swivelMot.set(speed);
}
//Returns where the Encoder is
public double getEncPercent() {
return analogInput.getVoltage();
}
//Gets the current angle of the module
public double getAngle() {
if (analogInput != null) {
return ((getEncPercent()/5)*360);
} else {
return -1;
}
}
}
//Returns the cosine of an angle in radians
public double cosDeg(double deg) {
return Math.cos(Math.toRadians(deg));
}
//Returns the sine of an angle in radians
public double sinDeg(double deg) {
return Math.sin(Math.toRadians(deg));
}
/**
* Constructs the swerve modules, each with a VictorSP, a CANTalon, and an Encoder
*/
public SwerveDriveBase() {
super();
//Creates the front left Swerve Module
flMod = new SwerveModule(
new WPI_TalonSRX(RobotMap.frontLeftSwivel),
new WPI_TalonSRX(RobotMap.frontLeftDrive),
new AnalogInput(RobotMap.FRONT_LEFT_ABSOLUTE) ) ;
//Creates the rear left Swerve Module
rlMod = new SwerveModule(
new WPI_TalonSRX(RobotMap.REAR_LEFT_SWIVEL),
new WPI_TalonSRX(RobotMap.backLeftDrive),
new AnalogInput(RobotMap.REAR_LEFT_ABSOLUTE)
)
;
//Creates the front right Swerve Module
frMod = new SwerveModule(
new WPI_TalonSRX(RobotMap.FRONT_RIGHT_SWIVEL),
new WPI_TalonSRX(RobotMap.frontRightDrive),
new AnalogInput(RobotMap.FRONT_RIGHT_ABSOLUTE)
);
//Creates the rear right Swerve Module
rrMod = new SwerveModule(
new WPI_TalonSRX(RobotMap.REAR_RIGHT_SWIVEL),
new WPI_TalonSRX(RobotMap.backRightDrive),
new AnalogInput(RobotMap.REAR_RIGHT_ABSOLUTE)
)
; // ):
}
//Initiates SwerveDrive as the Default Command
public void initDefaultCommand() {
setDefaultCommand(new SwerveDrive(ControlType.CONTROLLER));
}
/**
* Method for calculating and setting Speed and Angle of individual wheels given 3 movement inputs
*
*
* @param fbMot Forward/Backward Motion
* @param rlMot Right/Left Motion
* @param rotMot Rotation Motion
* @param fieldOriented If it's field oriented or not
*/
public void swerveDrive(double fbMot, double rlMot, double rotMot, boolean fieldOriented) {
double L = 23;
double W = 23;
double R = Math.sqrt((L*L) + (W*W));
double A = rlMot - rotMot*(L/R);
double B = rlMot + rotMot*(L/R);
double C = fbMot - rotMot*(W/R);
double D = fbMot + rotMot*(W/R);
double frSpd = Math.sqrt((B*B) + (D*D));
double flSpd = Math.sqrt((B*B) + (C*C));
double rlSpd = Math.sqrt((A*A) + (C*C));
double rrSpd = Math.sqrt((A*A) + (D*D));
double t = 180/Math.PI;
double frAng = Math.atan2(B, D)*t;
double flAng = Math.atan2(B, C)*t;
double rlAng = Math.atan2(A, C)*t;
double rrAng = Math.atan2(A, D)*t;
double max = frSpd;
if(max < flSpd) max = flSpd;
if(max < rlSpd) max = rlSpd;
if(max < rrSpd) max = rrSpd;
if(max > 1) {
frSpd /= max;
flSpd /= max;
rlSpd /= max;
rrSpd /= max;
}
//Set Wheel Speeds and Angles
frMod.setModule(frAng, frSpd);
flMod.setModule(flAng, flSpd);
rrMod.setModule(rrAng, rrSpd);
rlMod.setModule(rlAng, rlSpd);
System.out.println("FrAngle: "+frAng);
System.out.println("FrSpeed: "+frSpd);
System.out.println("FlAngle: "+flAng);
System.out.println("FlSpeed: "+flSpd);
System.out.println("rrAngle: "+rrAng);
System.out.println("rrSpeed: "+rrSpd);
System.out.println("rlAngle: "+rlAng);
System.out.println("rlSpeed: "+rlSpd);
}
}
Commands
SwerveDrive.java
package frc.robot.commands;
import edu.wpi.first.wpilibj.command.Command;
import frc.robot.OI;
import frc.robot.Robot;
public class SwerveDrive extends Command {
//Enum used to set the way in which swerve is controlled
public enum ControlType {
CONTROLLER(1, 0, 4, 5, 4, 3, 6),
JOYSTICK(1, 0, 2, 1, 2, 4, 6);
int fbAxis;
int rlAxis;
int rotAxis;
int doubleSpeed;
int centerGyro;
int zeroModules;
int dockingMode;
/**
* Constructs the variables based on control type
* @param fbAxis Forward/Backward axis
* @param rlAxis Right/Left axis
* @param rotAxis Rotation axis
*/
private ControlType(int fbAxis, int rlAxis, int rotAxis, int doubleSpeed,
int centerGyro, int zeroModules, int dockingMode) {
this.fbAxis = fbAxis;
this.rlAxis = rlAxis;
this.rotAxis = rotAxis;
this.doubleSpeed = doubleSpeed;
this.centerGyro = centerGyro;
this.zeroModules = zeroModules;
this.dockingMode = dockingMode;
}
public double getFBAxis() {
return OI.xBoxController.getRawAxis(fbAxis);
}
public double getRLAxis() {
return OI.xBoxController.getRawAxis(rlAxis);
}
public double getRotAxis() {
return OI.xBoxController.getRawAxis(rotAxis);
}
public boolean getDoubleSpeedButton() {
return OI.xBoxController.getRawButton(doubleSpeed);
}
public boolean getCenterGyroButton() {
return OI.xBoxController.getRawButton(centerGyro);
}
public boolean getZeroModulesButton() {
return OI.xBoxController.getRawButton(zeroModules);
}
public boolean getDockingModeButton() {
return OI.xBoxController.getRawButton(dockingMode);
}
}
private ControlType controlType;
//Makes SwerveDrive require the subsystem swerveBase
public SwerveDrive(ControlType controlType) {
requires(Robot.swerveBase);
this.controlType = controlType;
}
// Called just before this Command runs the first time
protected void initialize() {
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
//Sets input for swerveDrive method as input from controller stick axes. Note: FBValue is negative as required by doc linked to in swerveDrive method
Double fbValue = (controlType.getFBAxis())/2;
Double rlValue = (controlType.getRLAxis())/2;
Double rotValue = (controlType.getRotAxis())/2;
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return false;
}
// Called once after isFinished returns true
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}