### Author Topic: Getting the Swerve Drive to do continuous circles  (Read 4079 times)

#### Brayden L

• Guest ##### Getting the Swerve Drive to do continuous circles
« on: January 20, 2017, 08:39:43 PM »
Here is the pseudo code for how I got around the 360 degree limit on the swerve drive modules (aka: after 359 degrees it would go the long way to get to 1 degree)

Set a variable to the current encoder position (I'm going to refer to it as variable A)

Set that variable to itself divided by 1657 (the number of encoder counts per rotation)

Set a temporary variable to the next setpoint (aka the temp variable for now)

Round variable A down to the nearest whole number, and add it to the temp variable (but don't actually change variable A) (In Java, you can just cast to an Integer for this step.)

Set the encoder value to itself mod 1. (mod = modulus, which is a % sign in java)

If the next setpoint (not the temp variable) - the encoder value is greater than 0.5, then subtract 1 from the temp variable.

If the next setpoint (again not the temp variable) - the encoder value is less than -0.5, then add 1 to the temp variable.

Set the setpoint on the talon to the temp value

enable the talon.

Here is the code in Java:

double encPos1=cANTalon1.getEncPosition();
encPos1/=1657;
double temp1 = setPoint1;
temp1+=(int)encPos1;
encPos1= encPos1%1;

if ((setPoint1 - encPos1) > 0.5){
temp1-=1;

}
if ((setPoint1 - encPos1) < -0.5){
temp1+=1;
}

cANTalon1.set(temp1);
cANTalon1.enable();

#### Brayden L

• Guest ##### Re: Getting the Swerve Drive to do continuous circles
« Reply #1 on: January 22, 2017, 03:00:59 PM »
Update: I had both x axis inputs from the joysticks reversed, so I made them not reversed (I believe that the rest of the programming teams already did this, or never reversed them to begin with). I also reversed the output of the motor and the encoder. The algorithm also needs to change a bit, which can be done by making the encoder position negative. (Aka, after the first line, set the encoder position to itself times -1)

#### Louis L

• Hero Member
•     • Posts: 569 ##### Re: Getting the Swerve Drive to do continuous circles
« Reply #2 on: January 31, 2017, 12:53:47 PM »
For further clarification, the number 1657 comes from
7 x 71 / 1.2 x 4
Details are:

7 ticks of the encoder per rotation of the motor shaft
X
71 : 1 reduction in gearbox. This means that for every one rotation of the gearbox output shaft, there are 7x71=497 ticks of the encoder
/
1.2 due to the 48:40 gearing between the steering motor and the wheel mount. So for every full 360 degree rotation of the wheel mount, the gearbox only rotates 1/1.2 times (less than 1 full turn). So there are 497 / 1.2 = 414-1/6 ticks per rotation.
X
4 because the motor controller is operating in 4x mode. Quad encoders simply output two pulse trains (A and B). The controller is responsible for taking these pulse trains to increment and decrement a counter. 4X mode gives the most resolution and effectively causes the count to quadruple for every tick of the input count.

7 X 71 / 1.2 X 4 = 1656-2/3.  This is rounded up to 1657 if the interface requires integers but it does lead to errors over time if the right conditions are present.