Diff: main.cpp

Revision:

1:4a490de4c2a4

Parent:

0:e693d5bf0a25

Child:

3:f41855c51aaf

--- a/main.cpp	Wed Feb 03 18:47:07 2016 +0000
+++ b/main.cpp	Thu Nov 03 14:07:48 2016 +0000
@@ -1,4 +1,6 @@
 #include "mbed.h"
+#include "motordriver.h"    //Library to drive motors
+#include "PID.h"            //PID library for distance control
 #include "LSM9DS1.h"
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
@@ -7,17 +9,32 @@
 // http://www.ngdc.noaa.gov/geomag-web/#declination
 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
-DigitalOut myled(LED1);
+Motor LeftM(p21, p22, p23,1); // pwm, fwd, rev
+Motor RightM(p26, p25, p24,1);
+
+InterruptIn rhes(p15);
+InterruptIn lhes(p16);
+
+PID leftPid(1.0, 0.0, 0.0,0.01);  //Kc, Ti, Td
+PID rightPid(1.0, 0.0, 0.0,0.01); //Kc, Ti, Td
+
 Serial pc(USBTX, USBRX);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+
+int countl = 0, countr = 0;
+    
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+float getHeading(float mx, float my, float mz)
 {
-    float roll = atan2(ay, az);
-    float pitch = atan2(-ax, sqrt(ay * ay + az * az));
 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
     mx = -mx;
     float heading;
@@ -25,55 +42,153 @@
         heading = (mx < 0.0) ? 180.0 : 0.0;
     else
         heading = atan2(mx, my)*360.0/(2.0*PI);
-    //pc.printf("heading atan=%f \n\r",heading);
     heading -= DECLINATION; //correct for geo location
     if(heading>180.0) heading = heading - 360.0;
     else if(heading<-180.0) heading = 360.0 + heading;
     else if(heading<0.0) heading = 360.0  + heading;
-
+    heading = fabs(heading);
+    
+    return heading;
+}
 
-    // Convert everything from radians to degrees:
-    //heading *= 180.0 / PI;
-    pitch *= 180.0 / PI;
-    roll  *= 180.0 / PI;
+float getHead(){
+    while(!IMU.magAvailable(X_AXIS));
+        IMU.readMag();
+    return getHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+}
 
-    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
-    pc.printf("Magnetic Heading: %f degress\n\r",heading);
+void leftM_count() {
+    countl++;
+}
+void rightM_count() {
+    countr++;
 }
 
+void move(float dist, int dir = 1){
+    led1 = 1;
+    leftPid.setInputLimits(0, 1000);
+    leftPid.setOutputLimits(0.0, 0.9);
+    leftPid.setMode(AUTO_MODE);
+    rightPid.setInputLimits(0, 1000);
+    rightPid.setOutputLimits(0.0, 0.9);
+    rightPid.setMode(AUTO_MODE);
+    
+    int leftPulses      = 0;
+    int leftPrevPulses  = 0;
+    float leftVelocity  = 0.0;
+    int rightPulses     = 0;
+    int rightPrevPulses = 0;
+    float rightVelocity = 0.0;
+    
+    wait(1);
+    
+    leftPid.setSetPoint(750);
+    rightPid.setSetPoint(750);
+    
+    while ((leftPulses < dist) || (rightPulses < dist)) {
+        leftPulses = countl;
+        leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
+        leftPrevPulses = leftPulses;
+        leftPid.setProcessValue(leftVelocity);
+        LeftM.speed(leftPid.compute()*dir);
+        
+        rightPulses = countr;
+        rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
+        rightPrevPulses = rightPulses;
+        rightPid.setProcessValue(rightVelocity);
+        RightM.speed(rightPid.compute()*dir);
+        
+        wait(0.01);
+    }
+    countl = 0;
+    countr = 0;
+    RightM.speed(0);
+    LeftM.speed(0);
+    led1 = 0;
+    wait(1);
+}
 
-
+void turn(int direction){
+    led2 = 1;
+    float head = 0, oldHead = 0,newHead = 0;
+    oldHead = getHead();
+    head = oldHead;
+    //printf("Old Heading: %f\n\r", oldHead);
+    if (direction == 1){
+        newHead = oldHead + 90;
+        if (newHead > 360)
+            newHead -= 360;
+    }
+    else{
+        newHead = oldHead - 90;
+        if (newHead < 0)
+            newHead += 360;
+    }
+    //printf("New Heading: %f\n\r", newHead);
+    
+    while ((head < newHead - 1) || (head > newHead + 1)){
+        if ((head < newHead && (newHead - head < 180)) || (head - newHead > 180)){
+            LeftM.speed(0.5);
+            RightM.speed(-0.5);
+            head = getHead();
+            //printf("heading: %f\n\r", head);
+            wait(0.01);
+        }
+        else if ((head > newHead && (head-newHead < 180)) || (newHead - head > 180)){
+            LeftM.speed(-0.5);
+            RightM.speed(0.5);
+            head = getHead();
+            //printf("heading: %f\n\r", head);
+            wait(0.01);
+        }
+    }
+    LeftM.speed(0);
+    RightM.speed(0);
+    led2 = 0;
+    countl = 0;
+    countr = 0;
+    wait(1);
+}
 
 int main()
 {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+    lhes.mode(PullUp);
+    rhes.mode(PullUp);
+    lhes.rise(&leftM_count);
+    rhes.rise(&rightM_count);
     IMU.begin();
     if (!IMU.begin()) {
-        pc.printf("Failed to communicate with LSM9DS1.\n");
+        led1 = 1;
+        led2 = 1;
+        led3 = 1;
     }
     IMU.calibrate(1);
+    
+    led3 = 1;
     IMU.calibrateMag(0);
-    while(1) {
-        while(!IMU.tempAvailable());
-        IMU.readTemp();
-        while(!IMU.magAvailable(X_AXIS));
-        IMU.readMag();
-        while(!IMU.accelAvailable());
-        IMU.readAccel();
-        while(!IMU.gyroAvailable());
-        IMU.readGyro();
-        pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
-        pc.printf("        X axis    Y axis    Z axis\n\r");
-        pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
-        pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
-        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
-                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
+    led3 = 0;
+    wait(2);
+    /*
+    while (1){
+        pc.printf("Heading: %f\n\r", getHead());
+        wait (1);
     }
+    */
+    move(500);
+    wait(0.5);
+    turn(1);
+    wait(0.5);
+    
+    move(500);
+    wait(0.5);
+    turn(1);
+    wait(0.5);
+    
+    move(500);
+    wait(0.5);
+    turn(1);
+    wait(0.5);
+    
+    move(500);
 }