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   *//
      }else if(Adjust < 1.0) {
      	Adjust *=1.05;     //* Ramp up the speed *//
      	if (Adjust > 1.0) Adjust = 1.0;
      }
      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.