Confusing Issues with Swerve

[Calasada]

So our team has been working off-season on programming swerve drive in Java. We already got help from here before but have run into a very puzzling problem that no one can figure out. We added efficient movement in the modules and that seemed to cause the problem. Two of the wheels, the front left and rear right, always go 180 degrees further when rotating for the time after a deploy. This causes the wheels to return to the forward heading causing the encoder to read 360 degrees. This screws up the efficiency code even further. We have no idea why it only affects 2 of the 4 modules, and the problem itself seems very strange. I'm happy to explain the problem further, I probably didn't explain it very well. Our entire code for the subsystem is here:

public class SwerveDriveBase extends Subsystem {
	
	private SwerveModule frMod;
	private SwerveModule flMod;
	private SwerveModule rrMod;
	private SwerveModule rlMod;
	
	private AHRS navSensor;
	
	private final double p = 0.05;    //0.015;
	private final double i = 0.0025;    //0.005;
	private final double d = 0.005;    //0.125;
	
	private boolean was = false;
	
	final private double ENCODER_TICKS_FOR_ADJUSTER_TRAVEL = 1;
	
	public class PIDOutputClass implements PIDOutput {
		private VictorSP motor;
		
		public PIDOutputClass(VictorSP motor) {
			this.motor = motor;
		}
		
		@Override
		public void pidWrite(double output) {
			motor.set(output);
		}
	}
	
	public class SwerveModule {
		
		VictorSP swivelMot;
		CANTalon driveMot;
		Encoder enc;
		PIDController pidCont;
		PIDOutputClass pidOut;
		
		public SwerveModule(
				VictorSP swivelMot,
				CANTalon driveMot,
				Encoder enc) {
			
			this.swivelMot = swivelMot;
			this.driveMot = driveMot;
			this.enc = enc;
			
			pidOut = new PIDOutputClass(
							swivelMot
						);
			
			pidCont = new PIDController(
							p, i, d,
							enc,
							pidOut
						);
			
			
			
			pidCont.setInputRange(0, 360);
			pidCont.setContinuous();
			
			enc.setDistancePerPulse(0.875);
			enc.setSamplesToAverage(127);
		}
		
		public void setModule(double angle, double speed) {
			
			if(angle < 0) {
				angle += 360;
			}
			
			double curAngle = getAngle();
	    	if(Math.abs(angle - curAngle) > 90 && Math.abs(angle - curAngle) < 270 && angle != 0) {
	    		angle = ((int)angle + 180)%360;
	    		speed = -speed;
	    	}	
	    	
	    	setAngle(angle);
	    	setSpeed(speed);
		}
		
		public void setAngle(double angle) {
			pidCont.enable();
			pidCont.setSetpoint(angle);
		}

		public void setSpeed(double speed) {
			driveMot.set(speed);
		}
		
		public void setSwivel(double speed) {
			swivelMot.set(speed);
		}

		public double getEncPercent() {
			return Math.abs(enc.getDistance() / ENCODER_TICKS_FOR_ADJUSTER_TRAVEL);
		}
		
		public double getAngle() {
			if (enc != null) {
				return (getEncPercent());
			} else {
				return -1.0;
			}
		}
		
		public void setBrake(boolean bool) {
			driveMot.enableBrakeMode(bool);
		}
		
	}
	
	public double cosDeg(double deg) {
		return Math.cos(Math.toRadians(deg));
	}
	
	public double sinDeg(double deg) {
		return Math.sin(Math.toRadians(deg));
	}
	
    public SwerveDriveBase() {
    	super();
    	
    	//Makes sure navX is on Robot, then instantiates it 
    	try {
	         navSensor = new AHRS(SPI.Port.kMXP);
	     } catch (RuntimeException ex ) {
	         DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
	     }
    	
    	frMod = new SwerveModule(
    					new VictorSP(RobotMap.FRONT_RIGHT_SWIVEL),
    					new CANTalon(RobotMap.FRONT_RIGHT_WHEEL),
    					new Encoder(new DigitalInput(6), 
    								new DigitalInput(7))
    				);
    	
    	flMod = new SwerveModule(
    					new VictorSP(RobotMap.FRONT_LEFT_SWIVEL),
    					new CANTalon(RobotMap.FRONT_LEFT_WHEEL),
    					new Encoder(new DigitalInput(0), 
    								new DigitalInput(1))
    				);
    	
    	rrMod = new SwerveModule(
    					new VictorSP(RobotMap.REAR_RIGHT_SWIVEL),
    					new CANTalon(RobotMap.REAR_RIGHT_WHEEL),
    					new Encoder(new DigitalInput(2), 
    								new DigitalInput(3))
    				);
    			
    	rlMod = new SwerveModule(
    					new VictorSP(RobotMap.REAR_LEFT_SWIVEL),
    					new CANTalon(RobotMap.REAR_LEFT_WHEEL),
    					new Encoder(new DigitalInput(4), 
    								new DigitalInput(5))
    				); // ):
    	
    }

    public void initDefaultCommand() {
        setDefaultCommand(new SwerveDrive());
    }
    
    //Small, simple tank drive method
    public void tankDrive(double leftValue, double rightValue) {
    	if (DriverStation.getInstance().isFMSAttached() && DriverStation.getInstance().getMatchTime() < 4) {
    		setRobotBrake(true);
    	} else {
    		setRobotBrake(true);
    	}
    	
    	frMod.setSpeed(rightValue);
    	rrMod.setSpeed(rightValue);
    	
    	flMod.setSpeed(leftValue);
    	rlMod.setSpeed(leftValue);
    }
    
    //Method for calculating and setting Speed and Angle of individual wheels given 3 movement inputs
    public void swerveDrive(double fbMot, double rlMot, double rotMot) {
    	//Swerve Math Taken from: https://www.chiefdelphi.com/media/papers/2426
    	
    	double curAngle = getNavSensor().getAngle();
    	double temp = fbMot*(cosDeg(curAngle)) + rlMot*(sinDeg(curAngle));
    	rlMot = (-fbMot*(sinDeg(curAngle)) + (rlMot*(cosDeg(curAngle))));
    	fbMot = temp;
    	
    	double L = 1.0;
    	double W = 1.0;
    	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) + (C*C));
    	double flSpd = Math.sqrt((B*B) + (D*D));
    	double rlSpd = Math.sqrt((A*A) + (D*D));
    	double rrSpd = Math.sqrt((A*A) + (C*C));
    	
    	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;
    	//I'm so sorry Jake
    	
    	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);
    	
    }
    
    //Returns navX sensor ?
    public AHRS getNavSensor() {
    	if (navSensor != null) {
    		return navSensor;
    	} else {
    		return null;
    	}
    }
    
    public void setRobotBrake(boolean bool) {
    	frMod.setBrake(bool);
    	flMod.setBrake(bool);
    	rrMod.setBrake(bool);
    	rlMod.setBrake(bool);
    }
    
}

and the efficiency test specifically is here:

double curAngle = getAngle();
if(Math.abs(angle - curAngle) > 90 && Math.abs(angle - curAngle) < 270 && angle != 0) {
      angle = ((int)angle + 180)%360;
      speed = -speed;
}

Thanks in advance for any help.

[kylelanman]

That code looks familiar

There is one addition on the end that is likely causing you problems.

&& angle != 0

In general everything works. If your commanded angle happens to be 0 then regardless of what your current angle is you skip the optimization. In the case of being optimized at 180 and then getting a 0 angle no rotation is required but due to the additional check it thinks a 180 degree rotation is required so it spins 180.

What problem were you trying to solve by adding the additional check?

[GeeTwo]

Originally Posted by Calasada

and the efficiency test specifically is here:

double curAngle = getAngle();
if(Math.abs(angle - curAngle) > 90 && Math.abs(angle - curAngle) < 270 && angle != 0) {
      angle = ((int)angle + 180)%360;
      speed = -speed;
}

Thanks in advance for any help.

Originally Posted by kylelanman

That code looks familiar

There is one addition on the end that is likely causing you problems.

&& angle != 0

In general everything works. If your commanded angle happens to be 0 then regardless of what your current angle is you skip the optimization. In the case of being optimized at 180 and then getting a 0 angle no rotation is required but due to the additional check it thinks a 180 degree rotation is required so it spins 180.

What problem were you trying to solve by adding the additional check?

I agree this appears to be the issue. Did you perhaps mean to specify speed!=0, so deal with the case where it just doesn't matter ?

[Calasada]

Originally Posted by kylelanman

That code looks familiar

There is one addition on the end that is likely causing you problems.

&& angle != 0

In general everything works. If your commanded angle happens to be 0 then regardless of what your current angle is you skip the optimization. In the case of being optimized at 180 and then getting a 0 angle no rotation is required but due to the additional check it thinks a 180 degree rotation is required so it spins 180.

What problem were you trying to solve by adding the additional check?

Hey again, thanks again for all your help. Your suggestion didn't seem to work, we just removed it and we got a new problem which i'm fairly certain the additional check was meant to fix: sometimes when the set angle is 0, it notices 180 is more efficient and moves there. But now when we set it to 90, 270, or something similar, the two problematic modules start flickering between efficient and not-efficient movement creating a sporadic movement, along with the aforementioned problem. The strangest thing about this to me is that it only occurs in the front left and rear right modules. Sorry if we're late, but any help is still appreciated.

[GeeTwo]

I went back and looked more closely at the calculation of the arguments to setModule() in swerveDrive(). The calculation of A B C and D seem to be the cartesian components of the desired motion at the center of each module. In all four cases, B is a component of the front wheels, and A of the rear. However, C and D seem to jump places, with C going to the right speeds and the left angles, and D going to the left speeds and right angles. I don't quite see how this would cause your observed problem, but it doesn't seem right.

[kylelanman]

Originally Posted by Calasada

Hey again, thanks again for all your help. Your suggestion didn't seem to work, we just removed it and we got a new problem which i'm fairly certain the additional check was meant to fix: sometimes when the set angle is 0, it notices 180 is more efficient and moves there. But now when we set it to 90, 270, or something similar, the two problematic modules start flickering between efficient and not-efficient movement creating a sporadic movement, along with the aforementioned problem. The strangest thing about this to me is that it only occurs in the front left and rear right modules. Sorry if we're late, but any help is still appreciated.

The two modules that are not working are diagonal from each other. I don't know your physical setup but is it possible that the steer motors or the encoder inputs needs to be reversed for those modules? You could plot the error of the PIDControllers. I suspect you will see the error increasing instead of decreasing as you approach your set point. It is possible that when you command 0 that it is actually turning the wrong direction but the controller is tuned and has a dead band such that the module will still stop even approaching the set point from the wrong direction at high speed when it loops around from 360 to 0. When you have the check in places it doesn't oscillate between 0 and 180. When the check is removed you are actually getting stuck at your max error of 180 and as you indicated switching between optimized and not optimized.

[Ether]

Originally Posted by Calasada

double frAng = Math.atan2(B, DC)*t;
double flAng = Math.atan2(B, CD)*t;
double rlAng = Math.atan2(A, CD)*t;
double rrAng = Math.atan2(A, DC)*t;

FTFY.

[Ether]

An easy way to test your swerve inverse kinematics code to confirm that it is doing the computations correctly is to feed it several [fwd,str,rcw] triplets and compare the resulting wheel speeds and angles to the values computed by this swerve calculator spreadsheet.