Ether's swerve code for 2015 Control System

[EmileH]

Hey there CD,
As an offseason project I took Ether's paper on calculating swerve angles and speeds (http://www.chiefdelphi.com/media/papers/download/3028) and turned it into an all-in-one java class for driving using a single 3-axis joystick. This was something I threw together in a few hours today and haven't really tested at all (it's my schools vacation week so it'll be a while before I can test it and we don't even have a swerve base to use) but it's something you can use as a starting point for your swerve. I am 100% sure that there is something more that teams/users would need to do before this code is ready for competition, again only a starting point.

Github repository (http://bit.ly/1EgwCJB)

package org.usfirst.frc.team3467.robot;


import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.SampleRobot;

/**
* This is a short sample program demonstrating how to use the Talon SRX over
* CAN to run a closed-loop PID controller with an analog potentiometer.
*/
public class Robot extends SampleRobot {

CANTalon FLdrive, FLsteer, RLdrive, RLsteer, FRdrive, FRsteer, RRdrive, RRsteer;
Joystick driveJoy;
Gyro gyro;

public Robot() {
FLdrive = new CANTalon(1); // Initialize the CanTalonSRX on device 1.
FLsteer = new CANTalon(2);
RLdrive = new CANTalon(3);
RLsteer = new CANTalon(4);
FRdrive = new CANTalon(5);
FRsteer = new CANTalon(6);
RRdrive = new CANTalon(7);
RRsteer = new CANTalon(8);
driveJoy = new Joystick(1);

FLdrive.changeControlMode(CANTalon.ControlMode.Vol tage);
RLdrive.changeControlMode(CANTalon.ControlMode.Vol tage);
FRdrive.changeControlMode(CANTalon.ControlMode.Vol tage);
RRdrive.changeControlMode(CANTalon.ControlMode.Vol tage);
FLsteer.changeControlMode(CANTalon.ControlMode.Pos ition);
RLsteer.changeControlMode(CANTalon.ControlMode.Pos ition);
FRsteer.changeControlMode(CANTalon.ControlMode.Pos ition);
RRsteer.changeControlMode(CANTalon.ControlMode.Pos ition);

FLsteer.setFeedbackDevice(CANTalon.FeedbackDevice. AnalogPot);
RLsteer.setFeedbackDevice(CANTalon.FeedbackDevice. AnalogPot);
FRsteer.setFeedbackDevice(CANTalon.FeedbackDevice. AnalogPot);
RRsteer.setFeedbackDevice(CANTalon.FeedbackDevice. AnalogPot);
FLdrive.setFeedbackDevice(CANTalon.FeedbackDevice. QuadEncoder);
RLdrive.setFeedbackDevice(CANTalon.FeedbackDevice. QuadEncoder);
FRdrive.setFeedbackDevice(CANTalon.FeedbackDevice. QuadEncoder);
RRdrive.setFeedbackDevice(CANTalon.FeedbackDevice. QuadEncoder);

FLsteer.setPID(1, 0, 0);
RLsteer.setPID(1, 0, 0);
FRsteer.setPID(1, 0, 0);
RRsteer.setPID(1, 0, 0);
}

public void operatorControl() {
while (isOperatorControl() && isEnabled()) {

double fwd;
double str;
double rcw;
double temp;
double fwd2;
double str2;
double wheelbase;
double trackwidth;
double r;
double a;
double b;
double c;
double d;
double frs, fls, rls, rrs; //Front Right, Front Left, Rear Left, Rear Right Wheel Speeds, respectively
double fra, fla, rla, rra; //Wheel Angles
double max;
fwd = driveJoy.getY();
str = driveJoy.getX();
rcw = driveJoy.getZ();

temp = (fwd*Math.cos(gyro.getAngle())) + str*Math.sin(gyro.getAngle());
str2 = (-fwd*Math.sin(gyro.getAngle())) + str*Math.cos(gyro.getAngle());
fwd2 = temp;

wheelbase = 30; //length of drivebase
trackwidth = 24; //width of drivebase
r = Math.sqrt((wheelbase*wheelbase) + (trackwidth*trackwidth));

a = str2 - rcw * (wheelbase/r);
b = str2 + rcw * (wheelbase/r);
c = fwd2 - rcw * (trackwidth/r);
d = fwd2 + rcw * (trackwidth/r);

frs = Math.sqrt(b*b + c*c);
fls = Math.sqrt(b*b + d*d);
rls = Math.sqrt(a*a + d*d);
rrs = Math.sqrt(a*a + c*c);

fra = Math.atan2(b,c) * 180/Math.PI;
fla = Math.atan2(b,d) * 180/Math.PI;
rra = Math.atan2(a,d) * 180/Math.PI;
rla = Math.atan2(a,c) * 180/Math.PI;

//Because the CANTalon position control uses values from 0 - 1023
//for potentiometer ranges, we must modify the wheel angles to
//compenstate for this. To do this add 180 to the wheel angle to
//get a 0-360 range then multiply by 1023/360 which equals 2.8444...

FRsteer.set((fra+180) * (1023/360));
FLsteer.set((fla+180) * (1023/360));
RLsteer.set((rla+180) * (1023/360));
RRsteer.set((rra+180) * (1023/360));

//Normalize wheel speeds as stated in Ether's document
max = frs;
if(fls>max){
max = fls;
}
if(rls>max){
max = rls;
}
if(rrs>max){
max = rrs;
}
if(max>1){
frs/=max;
fls/=max;
rrs/=max;
rls/=max;
}
//Wheel speeds are now 0-1. Not -1 to +1.
//Multiply wheel speeds by 12 for voltage control mode
FRdrive.set(frs*12);
FLdrive.set(fls*12);
RRdrive.set(rrs*12);
RLdrive.set(rls*12);
}
}
}

[EmileH]

Also created a Simulator today to test this. Just paste into eclipse/nb and go.

package org.usfirst.frc.team3467.robot;

import javax.swing.JOptionPane;

public class Simulator {
/**
* This program is based off of Ether's swerve drive kinematics and
* algorithms. I take no credit for the calculations used in this code.
* @param args
*/
public static void main(String[] args) {
double fwd;
double str;
double rcw;
double getAngle;
double temp;
double fwd2;
double str2;
double wheelbase;
double trackwidth;
double r;
double a;
double b;
double c;
double d;
double frs, fls, rls, rrs; //Front Right, Front Left, Rear Left, Rear Right Wheel Speeds, respectively
double fra, fla, rla, rra; //Wheel Angles
double max;
String getfwd = JOptionPane.showInputDialog("Please Input negated Y joystick value");
String getstr = JOptionPane.showInputDialog("Please Input X joystick value (-1 to 1)");
String getrcw = JOptionPane.showInputDialog("Please Input Rotational Joy Value (-1 to 1)");
String getgyro = JOptionPane.showInputDialog("Please Input Gyro Angle (0-360)");
fwd = Double.parseDouble(getfwd);
str = Double.parseDouble(getstr);
rcw = Double.parseDouble(getrcw);
getAngle = Double.parseDouble(getgyro);

temp = (fwd*Math.cos(getAngle)) + str*Math.sin(getAngle);
str2 = (-fwd*Math.sin(getAngle)) + str*Math.cos(getAngle);
fwd2 = temp;

wheelbase = 30; //length of drivebase
trackwidth = 24; //width of drivebase
r = Math.sqrt((wheelbase*wheelbase) + (trackwidth*trackwidth));

a = str2 - rcw * (wheelbase/r);
b = str2 + rcw * (wheelbase/r);
c = fwd2 - rcw * (trackwidth/r);
d = fwd2 + rcw * (trackwidth/r);

frs = Math.sqrt(b*b + c*c);
fls = Math.sqrt(b*b + d*d);
rls = Math.sqrt(a*a + d*d);
rrs = Math.sqrt(a*a + c*c);

fra = Math.atan2(b,c) * 180/Math.PI;
fla = Math.atan2(b,d) * 180/Math.PI;
rra = Math.atan2(a,d) * 180/Math.PI;
rla = Math.atan2(a,c) * 180/Math.PI;

// we must modify the wheel angles to
//compenstate for this. To do this add 180 to the wheel angle to
//get a 0-360 range

System.out.println("Front Right Wheel Angle" + ((fra)));
System.out.println("Front Left Wheel Angle" + ((fla)));
System.out.println("Rear Right Wheel Angle" + ((rla)));
System.out.println("Rear Left Wheel Angle" + ((rra)));

//Normalize wheel speeds as stated in Ether's document
max = frs;
if(fls>max){
max = fls;
}
if(rls>max){
max = rls;
}
if(rrs>max){
max = rrs;
}
if(max>1){
frs/=max;
fls/=max;
rrs/=max;
rls/=max;
}
//Wheel speeds are now 0-1. Not -1 to +1.
//Multiply wheel speeds by 12 for voltage control mode
System.out.println("Front Right Wheel Speed"+ (frs*12));
System.out.println("Front Left Wheel Speed" + (fls*12));
System.out.println("Rear Right Wheel Speed" + (rrs*12));
System.out.println("Rear Left Wheel Speed" + (rls*12));
}

}