Hey here’s a link for our autonomous run of 28 points which was the highscore at the VEX IQ Robotics World Championship
28 points auton 
If you have any questions or want to see the code pls ask
Hope You enjoy,
Arav.
P.S. Sorry for the music and bright light 
Congrats on the high score! I can’t watch it right now b/c I’m in school, but I will when I get home. Keep up the good work!
Sure, it’s a bit messy and not fully commented. Top bit is just think award presentation
any improvements or tips for next yr would be appreciated.
#pragma config(Sensor, port1, bumper, sensorVexIQ_Touch)
#pragma config(Sensor, port9, gyro, sensorVexIQ_Gyro)
#pragma config(Motor, motor2, rightDrive, tmotorVexIQ, PIDControl, reversed, driveRight, encoder)
#pragma config(Motor, motor3, armMotorRight, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor4, armMotorLeft, tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor5, leftDrive, tmotorVexIQ, PIDControl, driveLeft, encoder)
#pragma config(Motor, motor7, NeedleRight, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor12, NeedleLeft, tmotorVexIQ, PIDControl, encoder)
/*
1. choosing a language - robotC Graphical, robotC or Modkit
2. Using robotC natural language commands
3. 3D printing and makig different field elements
4. 12 point autonomous
5. X points
4. Field Friction Problem
5. Field Fricion Solution
6. 20 point autonomous
7. Touch LED button usage.
8. 21 points
9. Field Friction Problem
10. Battery Life Problem
11. Speed Problem
10. Gyro Usage.
11. Inertia Problem
12. Wheel + Arm Erosion
13. Fixing Gyro Drift - Discarding Small change
14. Decelaration Alignment Program
15. SSF
16. Minimum Speed Plus Threshold
17. Gyro Calibration vs Discarding small Change
18. Ranking different angles in terms of accuracy needed
19. Dead Zone Driver Control
20. Toggle Driver Control
21. Nationals Error
22. Judges Talk
23. Worlds Program Strategy
24. PID theory research
25. Proportional
26. Integral
27. Derivative
28. Tuning
29. Mixing Straight and Forwards
*/
float error, prevError, integral, derivative, distanceSet, distanceMoved, power, stopT, threshold2,
angleError, anglePrevError, angleIntegral, angleDerivative, angleSet, angleMoved, anglePower, threshold, var, override, Otime, maxSpeed;
float kP = 5; //tuning for proportional
float kI = 0; //tuning for integral
float kD = 0; //tuning for derivative
float AkP = 4; //tuning for angular proportional
float AkI = 0; //tuning for angular integral
float AkD = 20; // tuning for angular derivative
/////////////////////////////////////////////Forward PID
task goStraightPID() // PID for going straight
{
threshold2 = 10;
resetMotorEncoder(rightDrive); //resets motor encoder
resetMotorEncoder(leftDrive);
while (true){
distanceMoved = (getMotorEncoder(leftDrive) + getMotorEncoder(rightDrive))/2;
// finds the average of the encoders and makes distance moved that number
error = distanceSet - distanceMoved; //finds the error to the target
// as the error gets less the power will also be reduced
// if the robot oevrshoots distanceMoved will be higher than distance set and thus will correct itself
integral = integral + error; // the integral adds up all the error and previous error to create a huge value
//this means that if the robot is experiencing any resitant e.g going over the blue line the integral will make the speed
//sligly larger
if (error == 0 || error > distanceSet){ // this means that if the robot overshoots and only has to move a tiny bit the
//intergral won't raise the speed too much
integral = 0;
}
derivative = error - prevError; // the derivative will look at the rate of change and actually apply
//an opposite force incase the robot is deviating from normal.
prevError = error; //used in the derivative
power = (error*kP) + (integral*kI) + (derivative*kD); //set the output power to all three parts of PID with tuning variables
if (abs(power) > maxSpeed && var == 1){power = (power/abs(power))*maxSpeed;} // as we're using a gyro keeep straight program
// the speed has to be less than 100 so the gyro can change the angle with small changes as the value is'nt huge
if (distanceMoved > (distanceSet-threshold2) && distanceMoved < (distanceSet + threshold2)){power = 0;}
wait1Msec(15); //as we're using the gyro this threshold means the straight commmands don't have to be too accurate
//which is helpful due to little deviations going forward because of the gyro changing the speed
}
}
/////////////////////////////////////////Turn PID
task turnPID() // same PID comments as above
{
while (true){
angleMoved = getGyroDegrees(gyro);
angleError = angleSet - angleMoved;
angleIntegral = angleIntegral + angleError;
if (angleError == 0 || angleError > angleSet){
angleIntegral = 0;
}
angleDerivative = angleError - anglePrevError;
anglePrevError = angleError;
anglePower = (angleError*AkP) + (angleIntegral*AkI) + (angleDerivative*AkD);
if (angleMoved > (angleSet-threshold) && angleMoved < (angleSet + threshold)){anglePower = 0;}
//to increase speed we added a threshold meaning the angles can be out by a few degrees depending
//on how accurate the angle has to be
setMotor(leftDrive, power - anglePower); // sets the motor speed to straight powe from above and adds
setMotor(rightDrive, power + anglePower); //the gyro corrections
wait1Msec(15);
}
}
task end (){
sleep(Otime);
override = 1;
}
///////////////////////////////////Calibration of Gyro
void iqCalibrateGyro() // Calibrates gyro to fasctory precision
{
short count = 20;
startGyroCalibration(gyro, gyroCalibrateSamples512);
// by resetting the gyro's internal flag
wait1Msec(100);
eraseDisplay();
// wait for calibration to finish or 2 seconds, whichever is longer
while(getGyroCalibrationFlag(gyro) || (count-- > 0))
{
displayTextLine(1, "Calibrating... %02d", count/10);
wait1Msec(100);
}
eraseDisplay();
// reset so this is 0 heading
resetGyro(gyro);
displayTextLine(3, "Gyro Value is: %d", getGyroHeading(gyro));
}
//////////////////////////////////////////Forward Function
void straight(float a, float g, float gobo){
threshold = 0;
override = 0;
distanceSet = a;
var = gobo;
angleSet = g;
//forward(0.5, seconds, 90);
startTask(goStraightPID);
startTask(turnPID);
stopT=0;
while(stopT != 5 && override == 0){
if(power == 0) //makes sure power = 0 before moving on :)
{
stopT = stopT+1;
sleep(5);
}
else{
stopT = 0;
}
}
stopTask(goStraightPID);
stopT = 0;
}
///////////////////////////////////////////////////////////
void custom(){
threshold = 0;
var = 1;
distanceSet = 950;
startTask(goStraightPID);
startTask(turnPID);
angleSet = -8;
sleep(750);
angleSet = 20;
sleep(150);
angleSet = 60;
sleep(750);
angleSet = 70;
sleep(150);
angleSet = 80;
/*sleep(150);
angleSet = 60;
sleep(150);
angleSet= 70;
sleep(150);
angleSet = 90;*/
stopT=0;
while(stopT != 5){
if(power == 0)
{
stopT = stopT+1;
sleep(5);
}
else{
stopT = 0;
}
}
stopTask(goStraightPID);
stopT = 0;
}
///////////////////////////////////////////Turn Function
void turn(float b){
power = 0;
threshold = 1.5;
angleSet = b;
startTask(turnPID);
stopT=0;
while(stopT != 8 && override == 0){
if(anglePower == 0)
{
stopT = stopT+1;
sleep(5);
}
else {
stopT = 0;
}
}
stopT = 0;
stopTask(turnPID);
}
///////////////////////////////////////////Needle Functions
void NLeftUp(bool c){
if (c == true)
{
setMotor(NeedleLeft, 127);
}
}
void NLeftDown(bool d){
if (d == true)
{
setMotor(NeedleLeft, -127);
}
}
void NRightUp(bool e){
if (e == true)
{
setMotor(NeedleRight, 127);
}
}
void NRightDown(bool f){
if (f == true)
{
setMotor(NeedleRight, -127);
}
}
void NBothUp(bool fish){
if (fish == true)
{
setMultipleMotors(100, NeedleLeft,NeedleRight);
}
}
void NBothDown(bool bobby){
if (bobby == true)
{
setMultipleMotors(-100, NeedleLeft,NeedleRight);
}
}
//////////////////////////////////////////////
void arm(float h, float j)
{
resetMotorEncoder(armMotorLeft);
resetMotorEncoder(armMotorRight);
setMotorTarget(armMotorLeft, h, j);
setMotorTarget(armMotorRight, h, j);
}
///////////////////////////////
void change(float P, float I, float D)
{
AkP = P;
AkI = I;
AkD = D;
}
task main(){
maxSpeed = 95;
NBothUp(true);
sleep(600);
stopMultipleMotors(NeedleLeft, NeedleRight);
backward(0.3, seconds, 100);
arm(-400, -100);
sleep(2000);
iqCalibrateGyro();
arm(300, 100);
change(2,0 , 5);
waitUntil(getBumperValue(bumper) == 1);
turn(68);
change(4, 0 ,15);
straight(810, 68,1);
arm(380, 100);
sleep(450);
straight(200, 0,1);
custom();
// straight(550, -35, 1);
// straight(430, 65, 1);
Otime = 66;
startTask(end);
straight(5000, 90, 1);
sleep(200);
straight(-210, 90, 1);
kP = 5;
arm(-130, -50);
sleep(500);
straight(-100, 90, 1);
arm(-400, -100);
straight(370, 273, 1);
arm(700, 100);
straight(770, 263, 1);
NLeftDown(true);
sleep(200);
straight(-80, 225, 1);
straight(220, 225, 1);
resetMotorEncoder(NeedleRight);
setMotorTarget(NeedleRight, -200, -127);
kP = 10;
sleep(200);
stopMotor(NeedleLeft);
straight(-475, 245, 1);
stopMultipleMotors(NeedleLeft, NeedleRight);
straight(-700, 267, 1);
change(10,0,15);
straight(200, 70, 1);
change(6,0,15);
//sleep(300);
NBothUp(true);
Otime = 900;
startTask(end);
straight(1000, 78, 1);
change(4,0,15);
straight(-270, 90, 1);
arm(-250, -80);
sleep(300);
straight(60, 90, 1);
arm(-80, -60);
sleep(100);
straight(-150, 90, 1);
arm(470, 100);
sleep(150);
straight(300, 212, 1);
arm(200, 100);
sleep(250);
straight(-150, 212, 1);
change(2,0 , 5);
turn(75);
change(4, 0, 15);
straight(215, 76, 1);
sleep(100);
arm(-430, -100);
sleep(460);
straight(-200, 76, 1);
stopTask(turnPID);
sleep(200);
stopMultipleMotors(leftDrive,rightDrive);
arm(275, 100);
/// second program
waitUntil(getBumperValue(bumper) == 1);
backward(0.2, seconds, 100);
resetGyro(gyro);
//arm(400, 100);
straight(440, -30,1);
arm(200, 100);
sleep(200);
straight(-160, -30,1);
straight(100, -90,1);
straight(350, -82,1);
NRightDown(true);
sleep(200);
kP = 10;
change(10, 0 , 15);
straight(-330, -86,1);
straight(340, -60,1);
straight(30, -98,1);
sleep(200);
NLeftDown(true);
sleep(200);
change(6, 0 , 15);
straight(-30, -101, 1);
change(15, 0 , 50);
turn(-48);
change(6, 0 , 15);
straight(-375, -49, 1); // 230 works okay if jsut going straight
change(15, 0 , 50);
arm(-480, -100);
turn(-91);
change(6, 0 , 15);
stopMultipleMotors(NeedleLeft, NeedleRight);
straight(-1255, -91, 1);
change(15, 0 , 50);
turn(-116);
change(6, 0 , 15);
straight(-680, -116, 1);
change(9, 0 , 50);
turn(-180);
change(6, 0 , 15);
straight(210, -180,1);
NBothUp(true);
change(4,0,15);
Otime= 780;
startTask(end);
straight(3000, -180,1);
straight(-170, -180, 1);
arm(-210, -60);
sleep(200);
straight(-150, -180, 1);
stopTask(turnPID);
sleep(200);
stopMultipleMotors(leftDrive,rightDrive);
arm(-200, 100);
waitUntil(getBumperValue(bumper) == 1);
backward(0.2, seconds, 100);
resetGyro(gyro);
turn(10);
straight(530, 10, 1);
arm(410, 100);
sleep(100);
straight(-390, 0, 1);
straight(420, 105, 1);
sleep(100);
arm(-130, 60);
sleep(250);
straight(-150, 110, 1);
stopTask(turnPID);
sleep(200);
stopMultipleMotors(leftDrive,rightDrive);
arm(700, 100);
waitUntil(getBumperValue(bumper) == 1);
straight(800,5, 1);
arm(-1700, 100);
NBothDown(true);
sleep(200);
stopMultipleMotors(leftDrive,rightDrive);
sleep(333333);
}I’ve watched it now, that’s incredible. What kind of sensors did you use? Distance/ultrasonic? color sensor? (to sense the blag line on the field.)
Thanks!
Color Sensors and Ultrasonic sensors are really fragile and our school didn’t have any that worked so haven’t been able to test much . We did get a new color sensor before the UK nationals but I’m pretty sure that it was broken or very inaccurate too as it was just giving random values and definitely couldn’t detect the difference between black and white. I’m also guessing they would be quite hard to use accurately as they would change according to outside environment so you may have to make some kind of calibration program to use effectively
We only use a gyro sensor which we calibrate to factory precision (thanks to this forum post) at the start of every code to minimize zero level drift. To create 100 percent accurate turnings we used a PD loop for the gyro as although you can predict inertia and make angles less , if you want to get tit accurate at a very high speed the only way (i think 
) was some form of deceleration
Hope this helps
Arav (1565B) - Glitch 3.0