![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
Diff: r5driver.cpp
- Revision:
- 44:d4207182bfc2
- Parent:
- 43:048c307bf8ac
- Child:
- 45:9a7ddd922706
diff -r 048c307bf8ac -r d4207182bfc2 r5driver.cpp --- a/r5driver.cpp Sat Apr 09 00:37:08 2016 +0000 +++ b/r5driver.cpp Sat Apr 09 03:29:24 2016 +0000 @@ -14,6 +14,7 @@ float distLocalL = 0.0; void reverseMove(StepperDrive &drive, float dist, float angle); +double calcHuntAdjust(double c, char side); void activate() { @@ -37,7 +38,7 @@ pc.printf("\nLong Range Sensors created"); bluetooth.printf("\nLong Range Sensors created"); - StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625, + StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, PTE22, 0, 10.0625, 8.1875, 700); // 8.4800 /* (serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, @@ -378,4 +379,25 @@ // restore original values for invertLeft and invertRight drive.setInvertLeft(!drive.getInvertLeft()); drive.setInvertRight(!drive.getInvertRight()); +} + +//inputs: double c, the distance measured by one of the servo sensors +// char side, 'l' or 'r' for left or right side servo sensor +//output: the angle in radians to adjust the center line of the bot to point at the peg +double calcHuntAdjust(double c, char side) +{ + const double theta = 3.14159/4; //scan servo angle in radians + const int k1 = 0.1; //distance from plane of the sensors to center of rotation + const int k2 = 0.1; //distance from sensors to center line + double a = c*sin(theta); + double b = sqrt(pow(c,2) - pow(a,2)); //sqrt(c^2 - a^2) + double A = a + k1; + double B; + + if(side == 'r') + B = b - k2; + else + B = k2 - b; + + return 3.14159/2 - atan(A/B); } \ No newline at end of file