[tomy]

We trying to manipulate the kp so that we turn slow at the beginning then increase the speed of rotation when we approach the angle we want. It works kinda of just spins way to fast at the beginning of the rotate. Suggestions?

	
if(stage_one_complete){
		gyro.Reset();
		while(IsAutonomous() && IsEnabled() ){
			float angle = gyro.GetAngle();

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

			myRobot.MecanumDrive_Cartesian( 0, 0, ((angle + 82) * kp), 0);
			timer ++;
			Wait (0.004);

			if(timer > 400){
				myRobot.MecanumDrive_Cartesian(0, 0, 0, 0);
				stage_one_complete = false;
				stage_two_complete = true;
				break;
			}
		}
	}