Gyro PID Code for Autonomous Turns Help

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

I’ve been writing code to use the gyro and JPearman’s gyro library to make precise turns for autonomous and programming skills.
The code is supposed to use a baseline power level of 30 and use the gyro for a proportional addition.
However, when I tested the code the robot over shoots and then just keeps spinning forever. I am stumped.

When I was testing, as I rotated the gyro counterclockwise, the values increase from 0-360 and then rollover.
So setting the target to 270 should do a 90 degree right turn if the robot starts at zero.


#pragma config(Sensor, in6,    gyro,      		 sensorGyro)
#pragma config(Motor,  port1,           driveRB,       tmotorVex393HighSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           driveRF,       tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           driveLF,       tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          driveLB,       tmotorVex393HighSpeed_HBridge, openLoop)

#include "gyroLib2.c"	// JPearman's Gyro Library

float gyro_kp = 0.3;	// Gyro proportionality constant
float gyroTarget;		// Target gyro angle
int 	gyroPower = 0; 	// Claw power level
float gyroCurrent;	// Current robot angle
float gyroError;		// Difference between Target and Current
int gyroPower_R = 0;
int gyroPower_L = 0;

void gyroPIDTurn(float angle)
{
	gyroTarget = angle;
	do
	{
		gyroCurrent = theGyro.angle;	// Use JPearman's Gyro Library to filter and scale gyro readings

		if(abs(gyroTarget - gyroCurrent) < abs(gyroTarget - 360 - gyroCurrent) )
			gyroError = gyroTarget - gyroCurrent; // No zero cross

		else
			gyroError = gyroTarget - 360 - gyroCurrent; // Zero cross

		gyroPower	= gyro_kp * gyroError;			// Calculate power level

		if(gyroPower > 127)
			gyroPower = 127;
		if(gyroPower < -127)
			gyroPower = -127;

		gyroPower_L = -(gyroPower) - (30 * sgn(gyroPower));
		motor[driveLF] = motor[driveLB] = gyroPower_L;
		
		gyroPower_R =  (gyroPower) + (30 * sgn(gyroPower));
		motor[driveRF] = motor[driveRB]  = gyroPower_R;
		
	}
	while (abs(gyroError) > 1 );
	motor[driveLF] = motor[driveLB] = 0;
	motor[driveRF] = motor[driveRB] = 0;
	gyroPower = 0;
	gyroPower_L = 0;
	gyroPower_R = 0;
}

task main()
{
	GyroInit();
	wait1Msec(2000);
	gyroPIDTurn(270);
	wait1Msec(1000);
	gyroPIDTurn(180);
}


gyroLib2.zip (2.61 KB)

#2

Have you checked to see that spinning in X direction is actually reducing your %error? with RobotC if you get it backwards, your values will just climb infinitely and you will either accelerate until you hit max velocity or you will immediately hit max velocity and stay there. I would add a few lines similar to this:

 if(gyroCurrent>360)
gyrocurrent = gyrocurrent-360;
else if(gyroCurrent<-360)
gyrocurrent = gyrocurrent+360;

This would make sure your gyro values stayed between -360 and 360, which may be easier to debug with. I haven’t personally used this gyro library, so I don’t know if this code will actually work or if it will take some modification.

#3

+1 to this! Usually the culprit.

#4