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]
 Chief Delphi > Java Autonomous Movement
 User Name Remember Me? Password
 CD-Media CD-Spy
 portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 Thread Tools Rate Thread Display Modes
#1
07-18-2018, 01:30 PM
 JohnFogarty 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
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
#2
07-18-2018, 01:40 PM
 ahartnet Registered User AKA: Andrew Hartnett FRC #5414 (Pearadox) Team Role: Mentor Join Date: Jan 2011 Rookie Year: 2005 Location: Houston, Texas Posts: 357
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)
#3
07-26-2018, 04:28 PM
 gerthworm 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
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.

 Thread Tools Display Modes Rate This Thread Linear Mode Rate This Thread: 5 : Excellent 4 : Good 3 : Average 2 : Bad 1 : Terrible

 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 User Control Panel Private Messages Subscriptions Who's Online Search Forums Forums Home Announcements     User Announcements FIRST     General Forum         FIRST E-Mail Blast Archive     Rumor Mill     Career     Robot Showcase Technical     Technical Discussion     Robotics Education and Curriculum     Motors     Electrical         CAN     Programming         NI LabVIEW         C/C++         Java         Python     Control System         FRC Control System         Sensors     Pneumatics     Kit & Additional Hardware     CAD         Inventor         SolidWorks         Creo     IT / Communications         3D Animation and Competition         Website Design/Showcase         Videography and Photography         Computer Graphics     National Instruments LabVIEW and Data Acquisition         LabView and Data Acquisition Competition     Unsung FIRST Heroes     Awards         Chairman's Award     Rules/Strategy         Scouting         You Make The Call     Team Organization         Fundraising         Starting New Teams         Finding A Team         College Teams     Championship Event     Regional Competitions     District Events     Off-Season Events     Thanks and/or Congrats     FRC Game Design     OCCRA         OCCRA Q&A         OCCRA Programming Other     Chit-Chat         Games/Trivia             Fantasy FIRST     Car Nack's Corner     College & University Education     Dean Kamen's Inventions     FIRST-related Organizations         Western Region Robotics Forum         Southern California Regional Robotics Forum         The Blue Alliance             Video Archives     FIRST In the News...     FIRST Lego League         Lego Mindstorm Discussion     FIRST Tech Challenge     VEX         VEX Robotics Competition         VEX IQ     Televised Robotics     Math and Science         NASA Discussion ChiefDelphi.com Website     CD Forum Support     Extra Discussion

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

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

 -- English (12 hour) -- English (24 hour) Contact Us - Chief Delphi - Rules - Archive - Top

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