Gyro Negative Target Problem

Hello,

I am using a gyro for the first time for making turns in autonomous. The code works perfectly when the target value is positive. I run into problems when the target value (Degrees) is negative.

int GyroLimit = 30;
int MSR; //Motor speed right side
int MSL; //Motor speed Left side

void GyroTurn(int Degrees){

while(1){
          //less than target

	if(abs(SensorValue[Gyro]) <= (Degrees / 2)){
	
          //if far from target go fast
		MSR = 75;
                    MSL = -75;

		}else if(abs(SensorValue[Gyro]) > (Degrees / 2) && abs(SensorValue[Gyro]) < Degrees){

            //once the robot gets closer slow down
		MSR = 30;
                    MSL = -30;


	       }else if(abs(SensorValue[Gyro]) > (Degrees - GyroLimit) && abs(SensorValue[Gyro]) < (Degrees + GyroLimit)){

            //stop if within target range
		MSR = 0;
                    MSL = 0;
	}


          //greater than target

	if(abs(SensorValue[Gyro]) > (Degrees * 1.5)){

          //if far from target go fast
		MSR = -75;
                    MSL = 75;

		}else if(abs(SensorValue[Gyro]) < (Degrees * 1.5) && abs(SensorValue[Gyro]) > Degrees){

           //once the robot gets closer slow down
		MSR = -30;
                    MSL = 30;

	       }else if(abs(SensorValue[Gyro]) > (Degrees - GyroLimit) && abs(SensorValue[Gyro]) < (Degrees + GyroLimit)){
		MSR = 0;
                    MSL = 0;
	}

	Checks();

	motor[RightFront] = MSR;
	motor[RightRear] = MSR;
	motor[LeftRear]= MSL;
	motor[LeftFront] = MSL;
}

}

I’m probably just over thinking it. Any help would be greatly appreciated.

Thanks in advance,

Kelton

Your functions won’t work for high number of degrees as it will always be a big value. What you want to compare against is how far away you are from your target value and adjust the motor speed accordingly.

Check this olide but goodie. It uses a arm value while you use a gyro.
https://vexforum.com/t/simple-p-controller-for-arm-position/23699/1

Proportional constants for rotation is generally a much smaller number than an arm or straight driving Kp constants. Two reasons you have 3600 as your range and it is much easier to get a robot spinning since you have four motors working together.

You will have to make some adjustments using abs() for the gyro sensor like you have but then use the sign of the value via the function sgn() to get that + or - sign back.

So first you figure out how far you are from your target value. Let’s set our target variable to +90 degrees. So that value would be +900 in the Gyro. We have a value of target which will be in tenths of a degree like your gyro reads. We have the error which is how far away you are, the Kp is the proportional constant which is the multiplying factor to the motor drive which we will have the variable pidDrive to hold for us. The bigger the Kp, the greater the motor power is the further away from target.

You had a step change for being so far away, Kp is a linear line of power the further away you are.

int target=900;
int error=0;
float Kp = 0.2;
int pidDrive = 0;

In your while loop, get the error.


error = target - SensorValue[Gyro];

So now set the motor power to the proportional constant times the error. You will slow down as you approach. If Kp is too high you will go past, but don’t worry, the function will try and pull you back. If Kp is too low you could never reach your goal or never get the robot started.

So let’s set a motor power in PidDrive value to give us more push the further away from target we are.


       pidDrive = Kp * error;

Now Kp and the error can cause the value sent to the motor to be wicked high, so let’s trim that up to what the motor likes.


       // limit drive
        if( pidDrive > 127 )
            pidDrive = 127;
        if( pidDrive < (-127) )
            pidDrive = (-127);

Finally set the motors to the value of the rotational spin.


        motor[RightFront] = pidDrive ;

Multiply some pidDrive values by -1 to get the robot to spin versus fighting each other…

There is another trick when your sensor starts reading 350 degrees instead of -10 degrees. Once you get this we’ll apply that trick to fool the Gyro values.

Once nail this control down, you can move on to other control types like full on PID. A rotational PID is wicked cool and stops on a dime.

Thank you for your help it makes a lot more sense now. The robot now can turn in either direction.

void GyroTurn(int GyroTarget){

float GyroKp = 0.12; 
float GyroKd = 0.5; 

int GyroLast_error;
int GyroDerivative;

	while(1){

		//P
		GyroError = (GyroTarget - SensorValue[Gyro]);

		// D
		GyroDerivative = GyroError - GyroLast_error;
		GyroLast_error = GyroError;

		pidDrive = ((GyroKp * GyroError) + (GyroKd * GyroDerivative));

                //made it turn slower to try and eliminate overshoot with a full battery.
		if(pidDrive < -60){

			pidDrive = -60;
		}
		if(pidDrive > 60){

			pidDrive = 60;
		}

		motor[RightFront] = (pidDrive *  1);
		motor[RightRear] =  (pidDrive *  1);
		motor[LeftRear]=    (pidDrive *  -1);
		motor[LeftFront] =  (pidDrive *  -1);
	}
	motor[RightFront] = 0;
	motor[RightRear] =  0;
	motor[LeftRear]=    0;
	motor[LeftFront] =  0;

}

You inspired me to add in the derivative thanks to jpearmans simple PID code https://vexforum.com/t/simple-pid-for-easyc-and-convex/28399/1 I was able to. It seems to help accuracy.

One problem is I still have is when I have a full battery it still tends to over shoot the target when the battery starts to become more dead it is much better. I can’t use a half dead battery at competition. Would adding Intergral help this problem (the person here didn’t use intergral https://vexforum.com/t/qcc2-gyro-and-pid-control-code/29292/1) or do I just have to do a bit more tuning?

The second problem is how do I break out of the loop once I am close to the target value. I did it like this

		if(pidDrive > -3 && pidDrive < 3){

			break;
		}

and it worked but it contributes to the drift/overshoot since the drive speed is no longer being calculated. What is the best way to break the loop so I can continue to the next loop in autonomous?

Thanks,

Kelton

Adding integral could help. I am not sure why a full battery acts so weird for you.

So now you have the loop, you can turn this into a background task and use a variable to turn it on or off or start and stop the task. This way you can control your shooter while turning. (speed it up, maintain speed, etc)

You can also make the drive forward/backward PID as a section too. Trying to turn and get to a specific distance at the same time is a different algorithm. So for now I would stick with a PID turn and a PID drive section. Then set the drive motors all at once with whichever you came up with.

You could also turn code below this into a function. Most common name is int LimitMotor(int input_value) and use a global of #define as your limiting value you have at 60 right now.

                //made it turn slower to try and eliminate overshoot with a full battery.
		if(pidDrive < -60){

			pidDrive = -60;
		}
		if(pidDrive > 60){

			pidDrive = 60;
		}