Go to Post They had a well made scouting form on the bottom was a check box with nothing next to it, when asked what the box was for they said. "Oh thats the jerk box" Since then one of our team goals is "Stay out of the jerk box" - Gary Stearns [more]
Home
Go Back   Chief Delphi > Technical > Programming > Java
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
Reply
Thread Tools Rate Thread Display Modes
  #1   Spotlight this post!  
Unread 07-18-2018, 01:30 PM
JohnFogarty's Avatar
JohnFogarty JohnFogarty is offline
R I P Georgia Dome
FRC #1102 (M'Aiken Magic & Team FUN)
Team Role: Mentor
 
Join Date: Aug 2009
Rookie Year: 2006
Location: Augusta, GA
Posts: 1,904
JohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond reputeJohnFogarty has a reputation beyond repute
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.

Code:
//*************************************************************************//
//*                                                                       *//
//*    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       *//
}
__________________
John Fogarty - 13 Years in FIRST - http://cceti.com
FLL 1102 -> FTC 3864/1102 | FRC 1102 -> FRC 1772 -> FRC 4901 -> FRC 5632 | FTC 11444 -> FRC 1102 | VRC 5817
Head Coach - FRC Team 1102 M'Aiken Magic
"By teaching students the power of STEM, we're M'Aiken Magic happen!"
2010 FTC World Championship Winner - FTC Team 3864 Driver/Captain
Reply With Quote
  #2   Spotlight this post!  
Unread 07-18-2018, 01:40 PM
ahartnet's Avatar
ahartnet ahartnet is offline
Registered User
AKA: Andrew Hartnett
FRC #5414 (Pearadox)
Team Role: Mentor
 
Join Date: Jan 2011
Rookie Year: 2005
Location: Houston, Texas
Posts: 357
ahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond reputeahartnet has a reputation beyond repute
Re: Autonomous Movement

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/Worlds20...veEncDist.java
__________________
Team 5414 Pearadox, Mentor (2015-Present) | Website | Facebook
Team 2936 Gatorzillas, Mentor (2011-2014)
Team 1646 Precision Guessworks, Mentor (2006-2008)
Team 451 The Cat Attack, Student Alumni (2005)
Reply With Quote
  #3   Spotlight this post!  
Unread 07-26-2018, 04:28 PM
gerthworm's Avatar
gerthworm gerthworm is offline
Making the 1's and 0's
FRC #1736 (Robot Casserole)
Team Role: Mentor
 
Join Date: Jan 2015
Rookie Year: 2015
Location: Peoria, IL
Posts: 593
gerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond reputegerthworm has a reputation beyond repute
Re: Autonomous Movement

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.
Reply With Quote
Reply


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 01:29 AM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2018, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi