# Autonomous Movement

Hello!

Our team has been trying to work with some fairly simple command/method based programming techniques for writing autonomous code. We wanted to use this to help our programmers be able to write simple, procedural autonomous programs.

We built a method for basic drive-train movement, and I’ve been having issues with it for a while now. I can’t get the robot to drive straight over longer distances, I always end up with it veering to the left/right. We have a encoder driven PID algorithm implemented and a master-slave relationship between each side of the drive-train to correct for error as the robot moves, but for some reason I can’t get the code to correct enough to end up traveling perfectly straight.

//*************************************************************************//
//*                                                                       *//
//*    Defines Drive Ratio versus Motor Speed, Wheel Size,                *//
//*    Encoder Counts per motor rev. Arithmetic gives us Encoder          *//
//*    Counts per inch. Counts per degree is trial and error              *//
//*    Counts per second is used to develop timeout values using the      *//
//*    lowest motor speed @ full torque at full power.                    *//
//*                                                                       *//
//*************************************************************************//
static final int    GO    = 1;         //* Type of Move,Forward or Back *//
static final int    SPIN  = 2;         //* Type of Move, Both sides Opp *//
static final int    SWEEP = 3;         //* Type of Move, Strafe         *//
static final int    PIVOTL= 4;         //* Type of Pivot - Left fixed   *//
static final int    PIVOTR= 5;         //* Type of Pivot - Right Fixed  *//
static final int    DIAGF = 6;         //* Go diagonal Forward          *//
static final int    DIAGB = 7;         //* Go diagonal Backward         *//
static final int    MINLOOPCOUNT= 100; //* Minimum Loop Count allowed   *//
static final double MINMOVE     = 4.0; //* Minimum move amount 4"       *//
static final double MINSPIN     = 5.0; //* Minimum spin amount 5 Degrees*//
static final double MINSWEEP    = 6.0; //* Minimum sweep amount 6"      *//
static final double MINDIAG     = 6.0; //* Minimum sweep amount 6"      *//
static final double MINPIVOT    = 5.0; //* Minimum Pivot 5 degrees      *//
static final double MAXPIDPOWER = .85; //* Max power allowed for PID    *//
static final double MAXPOWER   = 1.00; //* Max power allowed            *//
static final double MINPOWER   = 0.20; //* Minimum move power required  *//
static final double SAMPLERATE = 0.05; //* 20 Times per second          *//
static final double kP =   0.3;        //* Used to keep slave on track  *//
static final double kI =   0.0;        //* Used for PID                 *//
static final double kD =   0.4;        //* Used for PID                 *//
static final double WHEEL_CIRCUM = 18.85;//* Wheel Circumference-inches *//
static final double ENC_COUNTS = 90.0;  //* Counts per rev VEX Quad Encoder *//
static final double COUNTS_PER_INCH=ENC_COUNTS/WHEEL_CIRCUM; //* 4.77   *//
static final double COUNTS_PER_SECOND = 471.0; //* At Max Speed         *//
static final double SPIN_COUNTS_PER_DEGREE_LEFT  = 1.75;//* Increment   *//
static final double SPIN_COUNTS_PER_DEGREE_RIGHT = 1.80;//* Increment   *//
static final double SWEEP_COUNTS_PER_INCH_LEFT   = 8.0;//* Increment    *//
static final double SWEEP_COUNTS_PER_INCH_RIGHT  = 8.0;//* Increment    *//
static final double DIAG_COUNTS_PER_INCH_LEFT    = 8.1;//* Increment    *//
static final double DIAG_COUNTS_PER_INCH_RIGHT   = 8.1;//* Increment    *//
static final double PIVOT_COUNTS_PER_DEGREE_LEFT = 4.0;//* Increment    *//
static final double PIVOT_COUNTS_PER_DEGREE_RIGHT= 4.0;//* Increment    *//
static final double MILESTONE_STEP_1             = .60;//* First Step   *//
static final double MILESTONE_STEP_2             = .70;//* Second Step  *//
static final double REDUCE_SPEED_STEP_1          = .60;//* First Step   *//
static final double REDUCE_SPEED_STEP_2          = .50;//* Second Step  *//
//*************************************************************************//
//*   Move Routine: Caller supplies Type (GO/SPIN/SWEEP) ,                *//
//*   distance/degrees/distance +(Forward/Clockwise/Right) or -(Backward) *//
//*          and actual max power (0.2 to 1.0) to be used                 *//
//*  Left Side drive is considered master, will make right motors follow  *//
//*************************************************************************//
void RobotMove(int type, double amount, double power)
{
System.out.println(auto_state);
if(auto_state == false)
return;
int    IntCounts, Totalcounts;   //* Encoder values for moves         *//
int    Timeout, LoopCounter;     //* Calculate value for timeout      *//
int    MasterEncoder, SlaveEncoder; //* Encoder values                *//
int    PartialCounts1, PartialCounts2;//*Partial count to reduce speed*//
int    Error      = 0;          //* Used for PID                      *//
double FactorLF   = -1.0;       //* Motor Direction,Left Side Reversed*//
double FactorLR   = -1.0;       //* Motor Direction,Left Side Reversed*//
double FactorRF   =  1.0;       //* Motor Direction                   *//
double FactorRR   =  1.0;       //* Motor Direction                   *//
double SlavePower, MasterPower, Counts; //* Motor Power and Counts    *//
double Adjust = 0.5;  //* Adjust factor for Soft start & slowing down *//
if (power < MINPOWER) return;
MasterPower = power;            //* Use given power                   *//
if(MasterPower > MAXPIDPOWER) MasterPower = MAXPIDPOWER;//* For PID   *//
SlavePower  =  MasterPower;     //*Already converted to actual power  *//
switch (type) {
case(GO):
if (Math.abs(amount) < MINMOVE)return;   //* Must move enough  *//
if (amount < 0) {
FactorLF =  1.0;      //* Reverse all of the motors         *//
FactorLR =  1.0;
FactorRF = -1.0;
FactorRR = -1.0;
Counts = -amount * COUNTS_PER_INCH;  //* Backward move     *//
}else {
Counts = amount * COUNTS_PER_INCH;   //* Forward move      *//
}
break;
case (SPIN):      //* Must be a SPIN Type then                    *//
if (Math.abs(amount) < MINSPIN)return;   //* Must move enough  *//
if(amount < 0) {  //* Spin to left                            *//
Counts   = -amount * SPIN_COUNTS_PER_DEGREE_LEFT;
FactorLF = 1.0;    //* Reverse left side motors            *//
FactorLR = 1.0;
}else{               //* Spin to Right                       *//
Counts   = amount * SPIN_COUNTS_PER_DEGREE_RIGHT;
FactorRF = -1.0;   //* Reverse Right side motors           *//
FactorRR = -1.0;
}
break;
case (SWEEP):             //* Sweep to left or right              *//
if (Math.abs(amount) < MINSWEEP)return; //* Must move enough  *//
if(amount < 0) {      //* Sweep to left                       *//
Counts = -amount * SWEEP_COUNTS_PER_INCH_LEFT;
FactorLF =  1.0;   //* Reverse LF & RR motors              *//
FactorRR = -1.0;
}else{               //* Sweep to Right                      *//
Counts = amount * SWEEP_COUNTS_PER_INCH_RIGHT;
FactorLR =  1.0;  //* Reverse LR & RF motors              *//
FactorRF = -1.0;
}
break;
default:
return;    // Unknown type of move
}
PartialCounts2 =(int)(Counts * MILESTONE_STEP_2); //* @ 70% slow it   *//
PartialCounts1 =(int)(Counts * MILESTONE_STEP_1); //* @ 60% slow it   *//
Timeout= (int) ((Counts/COUNTS_PER_SECOND) /(MasterPower*SAMPLERATE));
if (Timeout < MINLOOPCOUNT) Timeout = MINLOOPCOUNT; //* Minimum loops *//
IntCounts = (int) Counts;        //* Integer value for the count      *//
Totalcounts = 0; LoopCounter = 0; //* Clear starting values           *//
if((Math.abs(amount) < 20.0)|| (power < 0.5)|| type == SWEEP)Adjust = 1.0;//*Too Small*//
LeftFrontDriveValue = FactorLF * MasterPower * Adjust; //* Motor Power*//
RightFrontDriveValue= FactorRF * SlavePower  * Adjust; //* Motor Power*//
LeftRearDriveValue  = FactorLR * MasterPower * Adjust; //* Motor Power*//
RightRearDriveValue = FactorRR * SlavePower  * Adjust; //* Motor Power*//
LeftEncoder.reset();             //* Clear the encoders               *//
RightEncoder.reset();            //* Clear the encoders               *//
LeftFront.set(LeftFrontDriveValue);      //* Set Motors Power         *//
RightFront.set(RightFrontDriveValue);    //* Set Motors Power         *//
LeftRear.set(LeftRearDriveValue);        //* Set Motors Power         *//
RightRear.set(RightRearDriveValue);      //* Set Motors Power         *//
Timer.delay(0.2);                //* Time for motors to get started   *//
while ((LoopCounter < Timeout) && auto_state == true){
if(auto_state == false)
return;
//* This waits for motors to reach desired position and then jumps out    *//
MasterEncoder = Math.abs (LeftEncoder.get());
SlaveEncoder  = Math.abs (RightEncoder.get());
Totalcounts += MasterEncoder;
if (Totalcounts >= IntCounts) break;
if (Totalcounts >= PartialCounts1) {
Adjust = REDUCE_SPEED_STEP_1; //* Reduce speed   *//
Adjust *=1.05;     //* Ramp up the speed *//
}
if (Totalcounts >= PartialCounts2) Adjust = REDUCE_SPEED_STEP_2;
Error = MasterEncoder - SlaveEncoder;
// Adjust the Slave power by a factor of the error percentage -
SlavePower += (((double)Error/(double)MasterEncoder) * kP);
if(SlavePower > 1.0) SlavePower = 1.0;
if(SlavePower < (.85 * MasterPower)) SlavePower = MasterPower*.85;
LeftFrontDriveValue  = FactorLF * MasterPower * Adjust;
LeftRearDriveValue   = FactorLR * MasterPower * Adjust;
RightFrontDriveValue = FactorRF * SlavePower  * Adjust;
RightRearDriveValue  = FactorRR * SlavePower  * Adjust;
LeftFront.set(LeftFrontDriveValue);      //* Set Motors Power       *//
RightFront.set(RightFrontDriveValue);
LeftRear.set(LeftRearDriveValue);
RightRear.set(RightRearDriveValue);
RightEncoder.reset();               //* Clear the encoders          *//
LeftEncoder.reset();                //* Clear the encoders          *//
LoopCounter++;                      //* Increment the loop counter  *//
Timer.delay(SAMPLERATE);            //* Set Sample Rate             *//
}
LeftFront.set(0.0);                   //* Stop Motors                 *//
LeftRear.set(0.0);                    //* Stop Motors                 *//
RightFront.set(0.0);                  //* Stop Motors                 *//
RightRear.set(0.0);                   //* Stop Motors                 *//
Timer.delay(0.2);                   //* Time for them to stop       *//
}

I dont know that we’ve ever gotten a robot to drive straight without looking at a gyro. Too many variables: backlash in gears/chain, wheels wearing down, etc.

Here’s code from our 2017 robot for driving forward and using the initial heading of a gyro to maintain direction: https://github.com/Pearadox/Worlds2017robot/blob/master/src/org/usfirst/frc/team5414/robot/commands/DriveEncDist.java

I concur - even some of the best-built drivetrains will drift over a long distance. There are some techniques to attempting to correct for things to happen, but in all honesty the far better answer is to invest in a gyro.