Revision 3:6a7620e9abd9, committed 2014-04-25

Comitter:

ganeshsubram1

Date:

Fri Apr 25 23:06:47 2014 +0000

Parent:

2:56eb726bdb0d

Child:

4:b2a30c313297

Commit message:

first commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PositionSystem.h	Fri Apr 25 23:06:47 2014 +0000
@@ -0,0 +1,77 @@
+#include "USBHostMouse.h"
+#include "HMC6352.h"
+
+#define PFLAG_ON 0
+#define PFLAG_OFF 1
+#define PFLAG_CALIB 2
+#define PTURN_SPEED (0.5)
+
+float PTURN_RIGHT = 0;
+
+extern Serial pc(USBTX, USBRX);
+
+// updates xy position if on, does nothing if off
+extern char PFlag = PFLAG_ON;
+
+// whenever a turn is complete, this should store 
+// the degrees facing (0, 90, 180, 270) in system
+extern int t_position = 0;
+
+// variables to keep track of coordinate position
+float x_position = 0;
+float y_position = 0;
+
+// variables to keep track of movement during turning
+float pturn_x = 0;
+float pturn_y = 0;
+
+/* mouse event handler */
+void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) {
+    
+    // calculate new position 
+    if(PFlag == PFLAG_ON) {
+        
+        // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
+        float temp = y_mickey / 232.6;
+        
+        // determine direction we are facing and add to that direction
+        if(t_position == 0)
+            y_position += temp;
+        else if(t_position == 90)
+            x_position += temp;
+        else if(t_position == 180)
+            y_position -= temp;
+        else
+            x_position -= temp;
+    } else if(PFlag == PFLAG_CALIB) {    
+        PTURN_RIGHT += x_mickey / 232.6;
+    } else {
+        pturn_x += x_mickey / 232.6;
+        pturn_y += y_mickey / 232.6;
+    }
+}
+
+// intialize x and y variables for turning
+void turnInit() {
+    pturn_x = 0;
+    pturn_y = 0;
+}
+    
+/* positioning system thread function */
+void PositionSystemMain(void const *) {
+    
+    USBHostMouse mouse;
+    
+    while(1) {
+        // try to connect a USB mouse
+        while(!mouse.connect())
+            Thread::wait(500);
+    
+        // when connected, attach handler called on mouse event
+        mouse.attachEvent(onMouseEvent);
+        
+        // wait until the mouse is disconnected
+        while(mouse.connected())
+            Thread::wait(500);
+    }
+}
\ No newline at end of file

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SonarSystem.h	Fri Apr 25 23:06:47 2014 +0000
@@ -0,0 +1,8 @@
+#include "mbed.h"
+
+#define SONAR_STOP (1.8)
+
+AnalogIn sonar1(p16); // FL
+AnalogIn sonar2(p17); // FC
+AnalogIn sonar3(p15); // FR
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHost.lib	Fri Apr 25 23:06:47 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/USBHost/#b010bddab593

--- a/main.cpp	Tue Apr 22 23:16:32 2014 +0000
+++ b/main.cpp	Fri Apr 25 23:06:47 2014 +0000
@@ -1,48 +1,41 @@
 #include "mbed.h"
 #include "RobotControl.h"
 #include "rtos.h"
-
+ 
 #define ENTERCALIB 0x43
 #define EXITCALIB 0x45
 
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+DigitalOut rst1(p12);
+
+Serial xbee1(p13, p14);
+ 
 InterruptIn sw(p30);
-
+ 
 void StopISR()
 {
     rightMotorPWM = 0;
     leftMotorPWM = 0;
     exit(1);
 }
-
-void InitCompass() {
-    //Init compass
-    compass.setReset();
-    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ 
+int main() {
     
-    //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() {
     //Emergency stop mechanism
     sw.rise(&StopISR);
-
-    //Setup mode and perform calibration
-    InitCompass();
     
-    compassDriveStraight(0.7, false, 20000);
+    // start positioning system
+    Thread position(PositionSystemMain, NULL, osPriorityNormal, 256 * 4);
+    wait(3); // wait for the mouse sensor to boot up
+    
+    // calibrate 90 degree turns
+    turnCalibrate();
     
-    while(1) {
-        float sample = compass.sample() / 10;
-        pc.printf("%f\r\n", sample);
-    }
-}
+    // PTURN_RIGHT is the measured x value for a 90 degree turn
+    pc.printf("PTURN_RIGHT: %f\n\r", PTURN_RIGHT); 
+    wait(1);
+    
+    // try a 90 degree turn with the measured PTURN_RIGHT value
+    turnRight(90);
+}
\ No newline at end of file