StingerBot Project

Dependencies:   mbed HMC6352 mbed-rtos

Files at this revision

API Documentation at this revision

Comitter:
Strikewolf
Date:
Tue Apr 22 23:16:32 2014 +0000
Parent:
1:41cee26b35cc
Commit message:
Complete initial working RobotControl drive functions using the honeywell compass module. Also added in emergency stop function.

Changed in this revision

RobotControl.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
diff -r 41cee26b35cc -r 56eb726bdb0d RobotControl.h
--- 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();
+}
diff -r 41cee26b35cc -r 56eb726bdb0d main.cpp
--- a/main.cpp	Fri Apr 11 17:54:21 2014 +0000
+++ b/main.cpp	Tue Apr 22 23:16:32 2014 +0000
@@ -1,13 +1,48 @@
 #include "mbed.h"
 #include "RobotControl.h"
+#include "rtos.h"
+
+#define ENTERCALIB 0x43
+#define EXITCALIB 0x45
+
+InterruptIn sw(p30);
+
+void StopISR()
+{
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    exit(1);
+}
+
+void InitCompass() {
+    //Init compass
+    compass.setReset();
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
+    //Calibration level - Garrus
+    //Rotate robot one full rotation during this section
+    pc.printf("Begin Calibration...\r\n");
+    compass.setCalibrationMode(ENTERCALIB);
+    wait(10);
+    compass.setCalibrationMode(EXITCALIB);
+    
+    //Sample a few to clean out buffer
+    compass.sample();
+    compass.sample();
+    compass.sample();
+}
 
 int main() {
-    //Init compass
-    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
-    wait(2);
-    //centerTurnLeft(25);
-    stop();
+    //Emergency stop mechanism
+    sw.rise(&StopISR);
+
+    //Setup mode and perform calibration
+    InitCompass();
+    
+    compassDriveStraight(0.7, false, 20000);
     
     while(1) {
+        float sample = compass.sample() / 10;
+        pc.printf("%f\r\n", sample);
     }
 }
diff -r 41cee26b35cc -r 56eb726bdb0d mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Apr 22 23:16:32 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#4ef72665e2c8