# Problem with coding inertial sensor

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

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

}

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

Does the robot ever stop?

1 Like

Try

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

3 Likes

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.

3 Likes

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

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.

2 Likes

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
{
{
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
{
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);

}
``````