[tomy]

Yes we are using a sampleRobot. The code works just not as expected. We want to have to robot turn and correct itself. If we were to put an else code something like:

else if (angle == 70) {
    myRobot.MecanumDrive_Cartesian(0, 0, 0, 0);
}

Then as soon as the the gyro hit 70 degrees it would stop moving. If the robot rotated to fast, which it is, it would not be able to correct itself.

We are trying to make the robot correct itself because the robot turns to fast. That's the point of changing the KP. If the angle is high KP should be low such that the robot turns slow. As the angle approaches the set angle we change KP to a bigger number such that the robot moves fast enough to turn it.

Also to note when we rotate the robot it returns a negative value. If we were to say:

                        if(angle > - 70){
				kp = 0.005;
			}else if(angle < - 70){
				kp = 0.02;
			}

myRobot.MecanumDrive_Cartesian( 0, 0, ((angle - 82) * kp), 0);

The robot rotated the wrong way.