Chief Delphi - Ether's swerve code for 2015 Control System

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Java (http://www.chiefdelphi.com/forums/forumdisplay.php?f=184)

- - Ether's swerve code for 2015 Control System (http://www.chiefdelphi.com/forums/showthread.php?t=137000)

Ether's swerve code for 2015 Control System

Hey there CD,
As an offseason project I took Ether's paper on calculating swerve angles and speeds 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

Code:

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.Voltage);

      RLdrive.changeControlMode(CANTalon.ControlMode.Voltage);

      FRdrive.changeControlMode(CANTalon.ControlMode.Voltage);

      RRdrive.changeControlMode(CANTalon.ControlMode.Voltage);

      FLsteer.changeControlMode(CANTalon.ControlMode.Position);

      RLsteer.changeControlMode(CANTalon.ControlMode.Position);

      FRsteer.changeControlMode(CANTalon.ControlMode.Position);

      RRsteer.changeControlMode(CANTalon.ControlMode.Position);



      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);

    }

    }

  }

Re: Ether's swerve code for 2015 Control System

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

Code:

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));

        }



}