Gyro P

#1

How can I make a Gyro P (P in PID) to make my robot go straight an include and encoder P to go forward and backwards. I am really confused and need some help with it.

#2

Are you trying to build just a P loop or an entire PID loop?
Also, what coding software are you using? PROS, ROBOTC, etc.?

#3

I am using RobotC and I am trying to make a P loop for it

#4

Making something from scratch, here it goes (You may have to change things here and there to fit your robot):


int kPdistance=0.2; //Decrease or increase this to tune the distance
int kPturning=0.05; //Decrease or increase this to tune the turning
int encodertarget = 1000; //1 degree of turning for the wheel = 1 Sensorvalue (Applies to Shaft Encoders)
int gyrotarget = 900; //1 degree for turning = sensorValue of 10, so 900 = 90 degrees
while(1==1){
motor[DriveLeft]=( ((sensorValue[encoder] - encodertarget) * kPdistance) - ((sensorValue[gyro] - gyrotarget) * kPturning) );
motor[DriveRight]=( ((sensorValue[encoder] - encodertarget) * kPdistance) + ((sensorValue[gyro] - gyrotarget) * kPturning) );
}

Try to tinker this to fit your needs, and if you have any questions go ahead and ask!
Hopefully this helps :slight_smile:

  • [TVA]Connor
#5

I’d suggest you limit the contribution from the position PID. The code posted above would initially saturate the motors (commanded speed of 200), so the heading compensation would have no effect (200-20 is still full speed ahead).
You can either budget 80 for position and 40 for gyro error, or you could do some smarter math that would still allow one motor go full speed when the position controller thinks it could still rush, while only slowing down the motor that’s ahead (heading wise).

#6

Ohhhh I see… Let me fix that :slight_smile:


int kPdistance=0.2; //Decrease or increase this to tune the distance
int kPturning=0.05; //Decrease or increase this to tune the turning
int encodertarget = 1000; //1 degree of turning for the wheel = 1 Sensorvalue (Applies to Shaft Encoders)
int gyrotarget = 900; //1 degree for turning = sensorValue of 10, so 900 = 90 degrees
int CalcD;
int CalcT;

while(1==1){

//Calculates the motor power for Distance and Turning//
CalcD = (sensorValue[encoder] - encodertarget) * kPdistance; //Nothing should need to be changed here//
CalcT = (sensorValue[gyro] - gyrotarget) * kPturning);
/////////////////////////////////////////////

//Limits Contribution Of Distance//
if (CalcD>80){
CalcD=80;
}
else if(CalcD<-80){
CalcD=-80;
}
////////////////////////

//Limits Contribution Of Turning//
if (CalcT>40){
CalcT=40;
}
else if(CalcT<-40){
CalcT=-40;
}
////////////////////////

//Combine The Values To One Product//
motor[DriveLeft]= CalcD - CalcT;//The way these values are added and subtracted may need to be changed
motor[DriveRight]= CalcD + CalcT;
/////////////////////////////////////////////////////
}

Would this work?

#7

Ya that does what he described.
Couple notes,
please indent properly so its a lot easier to read
Also this is a simple way to do the 2 if statements

if(abs(calc)>80){
	calc = sgn(calc) * 80;
}
#8

Watch out, in both of these you define ints that are not integers.

#9

O SHOOT. Yeah, they should be floats xD
My apologies. I am learning so much from you guys and it’s doubling with helping someone else as well. Thank you :slight_smile:


float kPdistance=0.2; //Decrease or increase this to tune the distance
float kPturning=0.05; //Decrease or increase this to tune the turning
int encodertarget = 1000; //1 degree of turning for the wheel = 1 Sensorvalue (Applies to Shaft Encoders)
int gyrotarget = 900; //1 degree for turning = sensorValue of 10, so 900 = 90 degrees
int CalcD;
int CalcT;

while(1==1){

	//Calculates the motor power for Distance and Turning//
	CalcD = (encodertarget - sensorValue[encoder]) * kPdistance; //Nothing should need to be changed here//
	CalcT = (gyrotarget - sensorValue[gyro]) * kPturning;
	/////////////////////////////////////////////

	//Limits Contribution Of Distance//
	if(abs(CalcD)>80){
		CalcD = sgn(CalcD) * 80;
	}
	////////////////////////

	//Limits Contribution Of Turning//
	if(abs(CalcT)>40){
		CalcT = sgn(CalcT) * 40;
	}
	////////////////////////

	//Combine The Values To One Product//
	motor[DriveLeft]= CalcD - CalcT;//The way these values are added and subtracted may need to be changed
	motor[DriveRight]= CalcD + CalcT;
	/////////////////////////////////////////////////////
}
#10

Really minor things. I think people usually do “target - sensor” so it’ll be positive when it’s not there yet and going forward to get there. Also, there was an extra parenthesis in CalcT = (sensorValue[gyro] - gyrotarget) * kPturning);.

#11

I rather teach a functional approach. Such as:


int inRange(int low, int val, int high) {
    return max(low, min(val, high));
}
int limit(int val, int bounds) {
    return inRange(-bounds, val, bounds);
}

you can then use it inline for shorter, yet better readable code - best code should be fully readable without (elaborate) comments.


motor[DriveLeft]= limit(CalcD, 80) - limit(CalcT,40);

Still, this is the easier approach that doesn’t provide full speed. The math for full speed version would go like this (still reigning in the gyro contribution somewhat):


int driveL = limit(CalcD, 127) - limit(CalcT, 60);
int driveR = limit(CalcD, 127) + limit(CalcT, 60);
int factor = min(127, max(driveL, driveR));
motor[DriveLeft] = 127 * driveL / factor;
motor[DriveRight] = 127 * driveR / factor;

In case the math approach makes it actually harder for you to read, the behavior is as follows:
as long as both driveL and driveR are under 127, we take no correction, since factor stays 127 and we multiply by 127/127, i.e. unity. But if either of them goes over 127, we multiply both sides by 127/<something higher than 127>, with the effect of scaling both sides by the same factor, with the higher side ending up 127 and the lower side less than that.

All of the above is moot without also taking the MC29 non-linearity into consideration, e.g. by using true speed mapping array/function.

#12

Thank you, I edited my last post to make the changes you suggested. I also forgot to remove some parenthesis so I did that as well.
Thank you :slight_smile:

#13

I never knew there was a limit function. Interesting, that will help me out so much while coding :smiley:

#14

There is one… as soon as you write one. If you can think it up, you can code it.

#15

Thank you for your help. It helped a lot!