Problem with coding inertial sensor

Programming Support VEX Coding Studio Tech Support
1

I’m trying to code a turn function for autonomous where it will turn to a certain degree. However, it turns over 360 degrees whenever I test it. Any feedback would be greatly appreciated.

Here is what is in void autonomous

iSensor.setHeading(0, vex::degrees);
turn(“right”, 90, 225);

Here is the code for the function,

void turn(std::string dir, double angle, double speed){

//double dSpeed = (360 * speed)/(wheelCircumference);
if (dir.compare(“left”) == 0){
frontLeft.setVelocity(-speed, vex::velocityUnits::dps);
frontRight.setVelocity(speed, vex::velocityUnits::dps);
}
if (dir.compare(“right”) == 0){
frontLeft.setVelocity(speed, vex::velocityUnits::dps);
frontRight.setVelocity(-speed, vex::velocityUnits::dps);
}

frontLeft.spin(vex::directionType::fwd);
frontRight.spin(vex::directionType::rev);

while(iSensor.heading()!=angle){
vex::task::sleep(1);
}

frontLeft.stop();
frontRight.stop();
}

2

Does the robot ever stop?

3

Try
while(iSensor.heading()<=angle){

You are trying for an EXACT match, and that will most likely never happen

4

If the robot doesnt stop, youre overshooting the desired angle. I would restructure the code to be something like this:

while(gyroAngle > angle) {
 turnleft();
}
while(gyroAngle < angle) {
 turnRight();
}
stop();

I would use .rotation(degrees) instead of.heading(degrees) because .heading will underflow to 360 and overflow to 0. .rotation will be able to go negative and past 360 degrees, making it easier to use.

5

I tried that before and it eventually stopped, but it turned over 360 degrees and it’s not always consistent.

6

Unfortunately this is what happens with such simple control, it’s hard to minimize overshoot without significantly slowing down the turn speed.

I would highly suggest adding P or even PD control to be able to turn without overshoot without having to really slow down your turn speed.

7

Our main coder has developed a PID loop based function to make our robot turn more precise. The robot turns at the degree angle, but sometimes it doesn’t turn in the direction we want it to go.

Here’s our turn function:

void turn(double heading, int direction, double multiplier) //Heading will always be between 0 and 359.99 degrees (if turning counterclockwise, heading is measured counterclockwise and vice versa), direction 0 is clockwise and direction 1 is counterclockwise
{
 double kp, kd;
 double value = 1;
 double lastError, error, derivative, speed;
 
 
  kp = value; //determined through testing, reduces time taken but also increases overshoot
  kd = value; //determined through testing, increases smoothness and decreases overshoot, yet shouldn’t get too large

if(direction == 1) //If turning clockwise/right
{
  //iSensor.setHeading(0.01, vex::degrees);
	//lastError = heading - iSensor.heading(); //The initial error for the derivative
  lastError = heading - iSensor.heading(); //The initial error for the derivative
	while(std::abs(heading - iSensor.heading()) > 5) //While the desired and inertial sensor headings are different
 	{
    		error = heading - iSensor.heading(); //The number of degrees needed until the desired heading is reached
   	  	derivative = error - lastError; //The change in error
    		lastError = error; //The last error used in the derivative
    		speed = (kp*error + kd*derivative); //Motor speeds
    		map(speed, 0, kp*360, 0, multiplier*100); //Scale speed from 0-(max speed) to 0-100 for percent motor speed (maximum error is 360 degrees)
	    	frontRight.spin(vex::directionType::fwd, speed, vex::velocityUnits::pct); //Front right motor is reversed
      	frontLeft.spin(vex::directionType::fwd, speed, vex::velocityUnits::pct);
	}
}
else if(direction == 0) //If turning counterclockwise/left
{
	//lastError = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //Initial remapped error for the derivative
  lastError = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //Initial remapped error for the derivative
	while(std::abs(heading - map(iSensor.heading(), 0, 359.99, 359.99, 0)) > 5) //While the desired and inertial sensor headings are different (heading orientation measurement reversed)
 	{
    		error = heading - map(iSensor.heading(), 0, 359.99, 359.99, 0); //The number of degrees needed until the desired (remapped) heading is reached
   		derivative = error - lastError; //The change in error
    		lastError = error; //The last error used in the derivative
    		speed = (kp*error + kd*derivative); //Motor speeds
    		map(speed, 0, kp*360, 0, multiplier*100); //Scale speed from 0-(max speed) to 0-100 for percent motor speed (maximum error is 360 degrees)
		frontRight.spin(vex::directionType::rev, speed, vex::velocityUnits::pct); //Front right motor is reversed
      		frontLeft.spin(vex::directionType::rev, speed, vex::velocityUnits::pct);
	}
}

frontLeft.stop(vex::brakeType::hold);
frontRight.stop(vex::brakeType::hold);

}

Some observations we have made:
*Near the 0/360 degree mark, if it’s pointing near the 0 to 90 degree area, it turns clockwise. If it’s pointing near the 270 to 360 degree area, it turns counter-clockwise.

The main thing we want to fix is to ensure that it turns only clockwise or counter-clockwise when told to. Any suggestions would be great.

And thank you to all who have help suggest some solutions before.