Advertisement
Guest User

Untitled

a guest
Apr 19th, 2018
89
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.86 KB | None | 0 0
  1. //function to turn a specific angle
  2. void turnAngle (int angle) {
  3. //convert input from degrees to radians
  4. int rad=(angle*pi)/180.0;
  5.  
  6. //calculate arc length using a radius of 136mm to a turning spot
  7. //in between the two back wheels
  8. int s=136*rad;
  9.  
  10. // Initialise distance controller variables
  11. float currentPos, disError, u;
  12. int Pwr = 0;
  13. // Initialise turn controller variables
  14. float Kp_turn = 0.2;
  15. int encError = 0;
  16.  
  17. setSensor(LeftEnc, 0);
  18. setSensor(RightEnc, 0);
  19.  
  20. //condition to make the robot turn counterclockwise when input angle is positive
  21.  
  22. do{
  23. int wheelencodercount = readSensor(LeftEnc);
  24. currentPos = readWheelEncoder(wheelencodercount);
  25.  
  26. disError = s - currentPos;
  27. u = Kp_turn*disError;
  28. Pwr = (int)saturate(u,-50,50);
  29.  
  30. motorPower(LeftMotor, Pwr);
  31. motorPower(RightMotor, -Pwr);
  32. delay(100);
  33. }while(currentPos<s);
  34. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement