28 points autonomous run

#1

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 :slight_smile:

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 :joy::joy:

8 Likes

#2

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!

1 Like

#3

Simply Amazing. Great job.

1 Like

#4

Incredible run, well done! Would you be able to post your code for that?

1 Like

#5

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);
}
2 Likes

#6

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.)

0 Likes

#7

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 :thinking:) was some form of deceleration

Hope this helps
Arav (1565B) - Glitch 3.0

2 Likes

#8

Amazing run! Thanks for sharing the video and code.

0 Likes