Diff: RobotControl.h

Revision:

2:56eb726bdb0d

Parent:

1:41cee26b35cc

Child:

3:6a7620e9abd9

--- a/RobotControl.h	Fri Apr 11 17:54:21 2014 +0000
+++ b/RobotControl.h	Tue Apr 22 23:16:32 2014 +0000
@@ -4,17 +4,38 @@
 
 #include "HMC6352.h"
 
+//Radio Calibration Signals - Need to implement calibration routine...
+#define CHAN3_MAX 2060  //Throttle Hi
+#define CHAN3_MIN 1150  //Throttle Lo
+#define CHAN2_MAX 1260  //Elevator Hi
+#define CHAN2_MIN 2290  //Elevator Lo
+#define CHAN1_MAX 2160  //Rudder Hi
+#define CHAN1_MIN 1210  //Rudder Lo
+
+//Debug
+Serial pc(USBTX, USBRX);
+
 //Gyro
 HMC6352 compass(p28, p27);
 
 //H-bridge
-PwmOut rightMotorPWM(p21);  //Channel A
-PwmOut leftMotorPWM(p22);   //Channel B
+PwmOut rightMotorPWM(p22);  //Channel A
+PwmOut leftMotorPWM(p21);   //Channel B
 DigitalOut leftMotor1(p23);
 DigitalOut leftMotor2(p24);
 DigitalOut rightMotor1(p25);
 DigitalOut rightMotor2(p26);
 
+//Compass sampling functions - TBA if compass keeps freaking out
+int prev_sample, cur_sample;
+void prepCompass() {
+    int i;
+    for (i = 0; i <= 4; i++) {
+        //Chew through a few samples, the compass seems to return "stale" values if unsampled for awhile
+        compass.sample();
+    }
+}
+
 //Non-feedback corrected 'dumb' driving
 void setDriveStraight(float speed, bool reverse) {
     //Set speed
@@ -28,6 +49,30 @@
     rightMotor2 = (!reverse) ? 1 : 0;
 }
 
+void setTurnLeft(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+void setTurnRight(float speed) {
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+    
+    //Set motor function
+    leftMotor1 = 1;
+    leftMotor2 = 0;
+    rightMotor1 = 0;
+    rightMotor2 = 1;
+}
+
 //Stop with braking
 void stop() {
     rightMotorPWM = 0;
@@ -38,21 +83,130 @@
     rightMotor2 = 0;
 }
 
-//These functions are not completed yet...
-void gyroDriveStraight(float speed, bool reverse);
+//Turn left about the center
 void centerTurnLeft(int delta_degrees) {
-    double initial = compass.sample() / 10;
-    while ((compass.sample() / 10) > initial - delta_degrees){
-        //Set speed
-        rightMotorPWM = 0.8;
-        leftMotorPWM = 0.8;
-        leftMotor1 = 1;
-        leftMotor2 = 0;
-        rightMotor1 = 1;
-        rightMotor1 = 0; 
+    prepCompass();
+    
+    float initial = (compass.sample() / 10);
+    float target = initial - delta_degrees;
+    double sample;
+    bool rollover = false;
+    
+    if (target < 0) {
+        target += 360;
+        rollover = true;
+    }
+
+    pc.printf("Init: %f Target: %f\r\n", initial, target);
+
+    //Take care of 360 degree rollover
+    if(rollover) {
+        pc.printf("begin  rollover \r\n");
+        while(true) {
+            setTurnLeft(0.6);
+            sample = compass.sample() / 10;
+            wait(0.02);
+            if (sample > 345)
+                break;
+        }
+        pc.printf("rollover complete\r\n");
+    }
+
+    //Continue to turn to target
+    while (true) {
+        setTurnLeft(0.6);
+        sample = compass.sample()/10;
+        wait(0.02);
+        if(sample < target)
+            break;
     }
     stop();
 }
-void centerTurnRight(int delta_degrees);
-void driveTurnLeft(int delta_degrees);
-void driveTurnRight(int delta_degrees);
+
+//Turn right about the center
+void centerTurnRight(int delta_degrees)
+{
+    prepCompass();
+    
+    float initial = (compass.sample() / 10);
+    float target = initial + delta_degrees;
+    double sample;
+    bool rollover = false;
+    
+    if (target > 360) {
+        target -= 360;
+        rollover = true;
+    }
+
+    pc.printf("Init: %f Target: %f\r\n", initial, target);
+
+    //Take care of 360 degree rollover
+    if(rollover) {
+        pc.printf("begin  rollover \r\n");
+        
+        while(true) {
+            setTurnRight(0.6);
+            sample = compass.sample() / 10;
+            wait(0.02);
+            if (sample > 0 && sample < 10)
+                break;
+        }
+        pc.printf("rollover complete\r\n");
+    }
+
+    //Continue to turn to target
+    while (true) {
+        setTurnRight(0.6);
+        sample = compass.sample()/10;
+        wait(0.02);
+        if(sample > target)
+            break;
+    }
+    stop();
+}
+
+//Drive straight with compass correction
+void compassDriveStraight(float speed, bool reverse, int ms) {
+    Timer timer;
+    double sample;
+    
+    setDriveStraight(speed, reverse);
+    wait(0.2);
+    //really hacky way to compensate for motor EM interference
+    prepCompass();
+    float initial = compass.sample() / 10;
+    
+    //Really hacky way to deal with 360 rollover
+    if (initial + 6 > 360)
+        centerTurnLeft(7);
+    if (initial - 6 < 0)
+        centerTurnRight(7);
+    
+    timer.start();
+    while (timer.read_ms() < ms) {
+        sample = compass.sample() / 10;
+        pc.printf("%f :::", sample);
+        
+        if (sample > initial + 3) {
+            pc.printf("Steer Left\r\n");
+            leftMotorPWM = speed - 0.1;
+            rightMotorPWM = speed + 0.1;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+        } else if (sample < initial - 3) {
+            pc.printf("Steer Right \r\n");
+            leftMotorPWM = speed + 0.1;
+            rightMotorPWM = speed - 0.1;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+        } else {
+            pc.printf("Straight\r\n");
+            setDriveStraight(speed, reverse);
+        }
+    }
+    stop();
+}