Diff: r5driver.cpp

Revision:

44:d4207182bfc2

Parent:

43:048c307bf8ac

Child:

45:9a7ddd922706

--- 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