How does one turn a swerve drive?

Technical Programming
1

I’m interested purely theoretically, my team isn’t going to have a swerve next year.

Anyhow, I understand most of how swerve works (such that I could easily write crab drive code), but turning is giving me headaches. I can turn easily enough, but I want to move forward while turning… I achieved this with what, from a programming standpoint, is a mechanum in FTC but swerve is much harder.

I’m trying to find solutions for averaging polar coordinates, but at this rate I’m convinced that I’ll have to convert to Cartesian, average that, then go back. Which is ugly and probably slow (execution wise).

Please don’t give me actual code, this is a personal challenge :slight_smile: Pseudo-code is OK.

2

Basically, find the polar velocity vectors that each wheel would have to have while turning in place (for a square base each wheel would be 45 degrees offset) and then find the vector each wheel would have to drive straight in the direction you want without turning. Add these two vectors together for each wheel to get that wheel’s actual output. Note that if you just add a constant vector for the drive straight part, you will drive like a car, not turning while still going in a straight line. To solve this, have the drive vector vary direction according to the gyro (its direction will be gyro angle - 360 + direction you want to go).

3

Wait, you just add them? What happens when you end up with, say r=2.0 theta=45? Do you just clip the value? That seems like a very bad way to deal with it.

I have it as field-oriented drive already. I did field-oriented drive on a mechanum once.

4

Read your Private Messages.

5

I haven’t actually programmed a swerve drive, but the most sensible thing to do seems to be to re-scale the individual wheel vectors to the largest they can be whilst achieving a motion similar to the commanded motion (i.e., maintaining the proportion between the commanded linear/angular velocity).

6

Yeah I forgot to mention that. We scale all our vectors down from 1 if any are greater than 1. For example if we have four vectors with magnitudes 1, 1, 1, and 2, we would scale all the vectors down to 0.5, 0.5, 0.5, 1.

7

You can scale the magnitude by dividing by 2

8

But then driving straight will always result in 1/2 speed. Which is non-ideal.

The magnitude thing is a not-so-nice implementation for my current design though. I had the direction being calculated inside an instance of a class. I can still do that to some extent, but the numbers will spill out. I suppose it’s unavoidable.

9

Here’s an old whitepaper that should answer a lot of your questions, while still leaving with you with some work for your personal challenge :slight_smile:

10

Swerve drive is really interesting to me as a programmer. There’s so many different ways to move with it, but I was wondering the same thing. I also didn’t realize that moving in a straight line was as ‘simple’ as vector addition. ‘Just’ compensate for the rotation by subtracting gyro angle. Maybe if we ever do build a swerve drive train I will be prepared. And taking multivariable calc as a sophomore might actually come to good use.

*Notice the generous use of quotation marks…

11

If you want to write a swerve drive robot using Python, a PR for PyFRC was submitted that implements a physics(ish) model that allows you to drive a swerve robot around – you just have to provide the code to drive the motors and stuff.

12

Unfortunately the scaling problem is nearly unavoidable, and can throw off your driver in many cases. Some teams try to run the swerve at less than the top speed normally (say 80%) to give some headroom so that the scaling doesn’t affect translation speed too much for the driver.

13

Our team just had a successful season doing a swerve bot. What you are describing is called field centric driving. This is accomplished by calculating the commanded translation angle and modifying it by the gyro value. We convert to polar coordinates from the joystick then add the gyro value then back to Cartesian for swerve control.

14

Why would you convert to Cartesian? You need the vector to angle the wheels to the correct angle.

Side question: Is your guys’ code on GitHub somewhere? I enjoyed seeing your robot at state and would enjoy taking a look at the code.

15

We convert the X and Y from the joystick to a polar coordinates so that we can do field orientation by modifying the commanded angle by a gyro value. We convert back to Cartesian because we calculate our wheel angles and vectors separate X and Y. This made the math easier so that we could combine the commanded X and Y with the turning (omega) value. We could calculate the X and Y component of each wheel individually. Then we convert back to vectors for the wheels to command it to the correct position. If you want more info about swerve you can Email our Team at [email protected] and I would be happy to explain or send more code. You guys had a great gear cycler this year!

Our swerve code:

void swerve(float x,float y,float w)
{
SmartDashboard::PutNumber(“swerve x”,x);
SmartDashboard::PutNumber(“swerve y”,y);
SmartDashboard::PutNumber(“swerve w”,w);

	if(w <= .003 && w >= -.003 ){
		w = 0;
	}
	//Sets distance in inches between center of robot and pod
	float x1 = 23.5/2.0;
	float y2 =26.0/2.0;
	//Sets x and y components of wheels
	float V1x = x + w * x1;
	float V1y = y - w * y2;
	float V2x = x  - w * x1;
	float V2y = y - w * y2;
	float V3x = x - w * x1;
	float V3y = y + w * y2;
	float V4x = x + w * x1;
	float V4y = y + w * y2;
	//converts to polar coords
	float Theta1 = getTheta(x,y,w,V1x,V1y,0);
	float Theta2 = getTheta(x,y,w,V2x,V2y,1);
	float Theta3 = getTheta(x,y,w,V3x,V3y,2);
	float Theta4 = getTheta(x,y,w,V4x,V4y,3);
	float V1 = sqrt(V1x*V1x+V1y*V1y);
	float V2 = sqrt(V2x*V2x+V2y*V2y);
	float V3 = sqrt(V3x*V3x+V3y*V3y);
	float V4 = sqrt(V4x*V4x+V4y*V4y);
	//Get encoder from 0 to 2 pi
	float encAngle1 = fmod((((enc1.Get()	-offset[0])/497.0) *(2*3.141596)) * (32.0/36.0),	(3.14159265*2));
	float encAngle2 = fmod((((enc2.Get()	-offset[1])/497.0) *(2*3.141596)) * (32.0/36.0),	(3.14159265*2));
	float encAngle3 = fmod((((enc3.Get() 	-offset[2])/497.0) *(2*3.141596)) * (32.0/36.0),	(3.14159265*2));
	float encAngle4 = fmod((((enc4.Get()	-offset[3])/497.0) *(2*3.141596)) * (32.0/36.0),	(3.14159265*2));
	//when controller is 0 keep last angle
	lastError1=error1;
	lastError2=error2;
	lastError3=error3;
	lastError4=error4;
	error1=getError(Theta1,encAngle1,0);
	error2=getError(Theta2,encAngle2,1);
	error3=getError(Theta3,encAngle3,2);
	error4=getError(Theta4,encAngle4,3);
	Accum1=Accum1+error1;
	Accum2=Accum2+error2;
	Accum3=Accum3+error3;
	Accum4=Accum4+error4;
	
	//Calculate each pod rotation commands
	float aw1Setpoint = pidAngle(error1,Accum1,lastError1);
	float aw2Setpoint = pidAngle(error2,Accum2,lastError2);
	float aw3Setpoint = pidAngle(error3,Accum3,lastError3);
	float aw4Setpoint = pidAngle(error4,Accum4,lastError4);
	
	//Calculate each wheel speed command
	float V1Setpoint = V1 * reverse[0];
	float V2Setpoint = V2 * reverse[1];
	float V3Setpoint = V3 * reverse[2];
	float V4Setpoint = V4 * reverse[3];
	//Brownout Protection
	float maxTotalPower = 6.0;		
	float allWheelDerate = 1.0;
									
	float totalWheelCommand = 
			fabs(aw1Setpoint) + 
			fabs(aw2Setpoint) + 
			fabs(aw3Setpoint) + 
			fabs(aw4Setpoint) +
			fabs(V1Setpoint) +
			fabs(V2Setpoint) +
			fabs(V4Setpoint) +
			fabs(V4Setpoint) ;
	if(totalWheelCommand > maxTotalPower){
		allWheelDerate = maxTotalPower / totalWheelCommand;
	}else{
		allWheelDerate = 1.0;
		
	}
		
	
	
	//sets the position of the wheels
	aw1.Set(aw1Setpoint*allWheelDerate);
	aw2.Set(aw2Setpoint*allWheelDerate);
	aw3.Set(aw3Setpoint*allWheelDerate);
	aw4.Set(aw4Setpoint*allWheelDerate);
	
	//sets the speed of the wheels
	w1.Set(V1Setpoint*allWheelDerate);
	w2.Set(V2Setpoint*allWheelDerate);
	w3.Set(V3Setpoint*allWheelDerate);
	w4.Set(V4Setpoint*allWheelDerate);