Diff: RobotControl.h

Revision:

3:6a7620e9abd9

Parent:

2:56eb726bdb0d

--- a/RobotControl.h	Tue Apr 22 23:16:32 2014 +0000
+++ b/RobotControl.h	Fri Apr 25 23:06:47 2014 +0000
@@ -1,9 +1,14 @@
 //*******************************************
 //* Robot motor control and drive functions *
 //*******************************************
-
+ 
 #include "HMC6352.h"
+#include "PositionSystem.h"
+#include "SonarSystem.h"
+#include <math.h>
 
+#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
+ 
 //Radio Calibration Signals - Need to implement calibration routine...
 #define CHAN3_MAX 2060  //Throttle Hi
 #define CHAN3_MIN 1150  //Throttle Lo
@@ -11,13 +16,10 @@
 #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(p22);  //Channel A
 PwmOut leftMotorPWM(p21);   //Channel B
@@ -26,18 +28,16 @@
 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();
-    }
+float sampleCompass(int sample_size) {
+    float c = 0;
+    for(int i = 0; i < sample_size; i++)
+        c += compass.sample() / 10.0;
+    c = c / (double) sample_size;
+    return c;
 }
-
+ 
 //Non-feedback corrected 'dumb' driving
-void setDriveStraight(float speed, bool reverse) {
+void setMove(float speed, bool reverse) {
     //Set speed
     rightMotorPWM = speed;
     leftMotorPWM = speed;
@@ -48,7 +48,7 @@
     rightMotor1 = (!reverse) ? 0 : 1;
     rightMotor2 = (!reverse) ? 1 : 0;
 }
-
+ 
 void setTurnLeft(float speed) {
     //Set speed
     rightMotorPWM = speed;
@@ -60,7 +60,7 @@
     rightMotor1 = 1;
     rightMotor2 = 0;
 }
-
+ 
 void setTurnRight(float speed) {
     //Set speed
     rightMotorPWM = speed;
@@ -72,7 +72,7 @@
     rightMotor1 = 0;
     rightMotor2 = 1;
 }
-
+ 
 //Stop with braking
 void stop() {
     rightMotorPWM = 0;
@@ -83,130 +83,85 @@
     rightMotor2 = 0;
 }
 
-//Turn left about the center
-void centerTurnLeft(int delta_degrees) {
-    prepCompass();
-    
-    float initial = (compass.sample() / 10);
-    float target = initial - delta_degrees;
-    double sample;
-    bool rollover = false;
+// calibrate 90 degree turns
+void turnCalibrate() {
     
-    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;
-    }
+    // tell positioning system to set the turn
+    // calibration variable, PTURN_RIGHT
+    PFlag = PFLAG_CALIB;
+        
+    // start turning hardcoded speed PTURN_SPEED
+    setTurnRight(PTURN_SPEED);
+    
+    // wait until ROTATE_90_TIME has passed. This should
+    // be the time to complete a 90 degree turn
+    wait_ms(ROTATE_90_TIME);
     stop();
+    
+    // done calibrating, turn positioning system back on
+    PFlag = PFLAG_ON;
 }
 
+// -------------------------------------------------------------------
+ 
 //Turn right about the center
-void centerTurnRight(int delta_degrees)
+void turnRight(int delta_degrees)
 {
-    prepCompass();
+    pc.printf("Turnleft: \n\r");
+    
+    // turn positioning system off during turns since
+    // no x and y movements
+    PFlag = PFLAG_OFF;
     
-    float initial = (compass.sample() / 10);
-    float target = initial + delta_degrees;
-    double sample;
-    bool rollover = false;
+    // store new orientation after this turn executes
+    t_position += 90;
+    t_position %= 360;   
+    
+    // initialize turn variables
+    turnInit();
     
-    if (target > 360) {
-        target -= 360;
-        rollover = true;
+    // start turning right
+    setTurnRight(PTURN_SPEED);
+    
+    // read mouse sensor until 90 degress has completed
+    while(pturn_x > PTURN_RIGHT) {
+        pc.printf("%f, %f\n\r", pturn_x, pturn_y);
     }
-
-    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 turning
     stop();
+    
+    // turn positioning system back on
+    PFlag = PFLAG_ON;
 }
 
 //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);
+void move(float speed, bool reverse) {
+
+    // begin moving
+    setMove(speed, reverse);
     
-    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);
-        }
+    // blocking call
+    while(1) {
+  
+        // read all sonar sensors
+        float s1 = sonar1.read() * 100;
+        float s2 = sonar2.read() * 100;
+        float s3 = sonar3.read() * 100;
+            
+        // check if obstacle is near and adjust 
+        if(s2 < SONAR_STOP) {
+            pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3);
+            if(s1 >= SONAR_STOP) {
+               //turnLeft(90);
+            } else if(s3 >= SONAR_STOP) {
+               turnRight(90);
+            } else {
+               pc.printf("IM STUCK HALP\n\t");
+            }
+        }  
+          
     }
-    stop();
-}
+ }
+
+ 
\ No newline at end of file