Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //function to turn a specific angle
- void turnAngle (int angle) {
- //convert input from degrees to radians
- int rad=(angle*pi)/180.0;
- //calculate arc length using a radius of 136mm to a turning spot
- //in between the two back wheels
- int s=136*rad;
- // Initialise distance controller variables
- float currentPos, disError, u;
- int Pwr = 0;
- // Initialise turn controller variables
- float Kp_turn = 0.2;
- int encError = 0;
- setSensor(LeftEnc, 0);
- setSensor(RightEnc, 0);
- //condition to make the robot turn counterclockwise when input angle is positive
- do{
- int wheelencodercount = readSensor(LeftEnc);
- currentPos = readWheelEncoder(wheelencodercount);
- disError = s - currentPos;
- u = Kp_turn*disError;
- Pwr = (int)saturate(u,-50,50);
- motorPower(LeftMotor, Pwr);
- motorPower(RightMotor, -Pwr);
- delay(100);
- }while(currentPos<s);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement