Diff: main.cpp

Revision:

0:31128b41490c

Child:

1:5bf45482441d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 19 04:20:24 2015 +0000
@@ -0,0 +1,291 @@
+#include "mbed.h"
+#include "PinDetect.h"
+#include "uLCD_4DGL.h"
+#include "math.h"
+#include "LSM9DS0.h"
+#include "Motor.h"
+#define PI 3.14159265
+#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
+
+// refresh time. set to 500 for part 2 and 50 for part 4
+#define REFRESH_TIME_MS 500
+Serial pc(USBTX, USBRX);
+// Verify that the pin assignments below match your breadboard
+LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
+Motor mA(p21, p23, p24); // pwm, fwd, rev LEFT
+Motor mB(p22, p25, p26); // pwm, fwd, rev RIGHT
+//speed: 10.44 cm/s
+// use class to setup pushbuttons pins
+PinDetect pb1(p16);
+PinDetect pb2(p17);
+PinDetect pb3(p18);
+PinDetect pb4(p19);
+
+DigitalOut myLED1(LED1);
+DigitalOut myLED2(LED2);
+DigitalOut myLED3(LED3);
+DigitalOut myLED4(LED4);
+// use class to setup the  Color LCD
+uLCD_4DGL uLCD(p28, p27, p29); // create a global uLCD object
+
+//Timer class
+Timer timer;
+
+//Mode settting
+
+enum Modetype { SETX = 0, SETY, GO };
+Modetype mode1 = SETX;
+int settingMode = 0;
+//Variable
+int xCoord = 0;
+int yCoord = 0;
+float distance = 0;
+float angle = 0;
+float seconds = 0;
+int startDegree = 0;
+int turningDegree = 0;
+int finalDegree = 0;
+int heading = 0;
+int degree = 0;
+int initialHeading = 0;
+int state = 0;
+int difference = 0;
+void setup()
+{
+    uLCD.cls();
+    uint16_t status = imu.begin();
+//Make sure communication is working
+}
+
+//When pb1 push increment the coordinate
+void pb1_hit_callback (void)
+{
+    if (mode1 == SETX)
+        xCoord++;
+    else if (mode1 == SETY)
+        yCoord++;
+    
+    myLED1 = 1;
+    myLED2 = 0;
+    myLED3 = 0;
+    myLED4 = 0;
+}
+
+//When pb2 push decrement the coordinate
+void pb2_hit_callback (void)
+{
+    if (mode1 == SETX)
+        xCoord--;
+    else if (mode1 == SETY)
+        yCoord--;
+        
+    myLED2 = 1;
+    myLED1 = 0;
+    myLED3 = 0;
+    myLED4 = 0;
+    
+}
+
+//When pb3 push increment the mode
+void pb3_hit_callback (void)
+{
+    mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3);
+    myLED3 = 1;
+    myLED1 = 0;
+    myLED2 = 0;
+    myLED4 = 0;
+}
+
+//When pb4 push decrement the mode
+void pb4_hit_callback (void)
+{
+    if (mode1 == SETY)
+    {
+        mode1 = SETX;   
+    }
+    myLED4 = 1;
+    myLED1 = 0;
+    myLED2 = 0;
+    myLED3 = 0;
+}
+
+void turnRight(void)
+{
+    imu.readMag();
+    myLED3 = 1;
+    uLCD.locate(1,5);
+    uLCD.printf("Turning Right \n");
+    mA.speed(0.2);
+    mB.speed(-0.2);
+    heading = static_cast<int> (imu.calcHeading());
+    uLCD.printf("H : %d \n",heading);
+    degree =   heading - initialHeading;
+    degree = turningDegree - degree;
+    uLCD.printf("Degree = %d \n",degree);
+    uLCD.printf("Heading = %d \n",heading);
+}
+
+void turnLeft(void)
+{
+    imu.readMag();
+    myLED3 = 1;
+    uLCD.locate(1,5);
+    uLCD.printf("Turning Left \n");
+    mA.speed(-0.2);
+    mB.speed(0.2);
+    heading = static_cast<int> (imu.calcHeading());
+    uLCD.printf("H : %d \n",heading);
+    degree =   heading - initialHeading;
+    degree = turningDegree - degree;
+    uLCD.printf("Degree = %d \n",degree);
+    uLCD.printf("Heading = %d \n",heading);
+}
+
+
+int main() 
+{
+    uLCD.cls();
+    // Use internal pullups for the three pushbuttons
+    pb1.mode(PullUp);
+    pb2.mode(PullUp);
+    pb3.mode(PullUp);
+    pb4.mode(PullUp);
+    // Delay for initial pullup to take effect
+    wait(.01);
+    // Setup Interrupt callback functions for a pb hit
+    pb1.attach_deasserted(&pb1_hit_callback);
+    pb2.attach_deasserted(&pb2_hit_callback);
+    pb3.attach_deasserted(&pb3_hit_callback);
+    pb4.attach_deasserted(&pb4_hit_callback);
+    // Start sampling pb inputs using interrupts
+    pb1.setSampleFrequency();
+    pb2.setSampleFrequency();
+    pb3.setSampleFrequency();
+    pb4.setSampleFrequency();
+    mode1 = SETX;
+    setup();
+    
+    xCoord = 0;
+    yCoord = 0;
+    state = 1;
+        //Setting mode
+        while(settingMode == 0)
+        {
+            uLCD.locate(1,2);
+            if (mode1 == SETX)
+            {
+                uLCD.printf("Setting X coordinate");    
+            }
+            else if (mode1 == SETY)
+            {
+                uLCD.printf("Setting Y Coordinate");   
+            }
+            else if (mode1 == GO)
+            {
+                uLCD.printf("Running Now");   
+            }    
+            uLCD.locate(4,5);
+            uLCD.printf("X-Coord: %d \n\r", xCoord);
+            uLCD.locate(4,7);
+            uLCD.printf("Y-Coord: %d \n\r", yCoord);            
+            
+            if (mode1 == GO)
+            {
+                distance =  xCoord*xCoord + yCoord*yCoord;
+                distance = sqrt(distance);
+                float totalDistance = distance * 30.48;
+                seconds = totalDistance / 23.18 * 1000;
+                imu.readMag();
+                initialHeading = static_cast<int>(imu.calcHeading());
+                angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
+                finalDegree = initialHeading + angle;
+                uLCD.locate(4,9);
+                uLCD.printf("Distance: %5.2F Feet \n\r", distance);
+                uLCD.locate(4,11);
+                uLCD.printf("Turning: %5.2F Degree \n\r", angle);
+                
+                settingMode = 1;
+            }
+                
+        } //end first while
+        wait(2);
+        //Turning mode
+        while(settingMode == 1)
+        {
+            uLCD.cls();
+            uLCD.printf("Setting mode 1");
+            imu.readMag();
+            initialHeading = static_cast<int> (imu.calcHeading());
+            uLCD.locate(1,3);
+            uLCD.printf("Initial : %d \n",initialHeading);
+            
+            startDegree = heading;
+            turningDegree = angle;
+            degree =  static_cast<int> (imu.calcHeading()) - initialHeading;
+            
+           if (turningDegree > 0)
+           {
+                while (degree  > -2 && degree < 180)
+                {              
+                    turnRight();
+                    wait(0.2);
+                    mA.speed(0);
+                    mB.speed(0);  
+                } 
+                settingMode = 2;
+
+            }
+            else
+            {
+                while (degree < 2 && degree > -180 )
+                {
+                    turnLeft();
+                    wait(0.2);
+                    mA.speed(0);
+                    mB.speed(0);  
+                }
+                settingMode = 2;
+            }                                                  
+        }
+        
+            wait(2);
+            imu.readMag();
+            initialHeading = static_cast<int> (imu.calcHeading());
+            timer.start();
+            while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
+            {
+                uLCD.locate(1,7);
+                uLCD.printf("Time : %d \n",timer.read_ms());
+                mA.speed(0.4);
+                mB.speed(0.4);
+                imu.readMag();
+                heading = static_cast<int> (imu.calcHeading());
+                difference = heading - initialHeading;
+                uLCD.locate(1,3);
+                uLCD.printf("Heading : %d \n",heading);
+                if( difference <-2)
+                {
+                            mA.speed(0.5);
+                            mB.speed(0.4);
+                            imu.readMag();
+                            heading = static_cast<int> (imu.calcHeading());
+                            difference = heading - finalDegree;
+                            uLCD.locate(1,3);
+                            uLCD.printf("Heading : %d \n",heading);
+                }
+                 if(difference >2)
+                {
+                            mA.speed(0.4);
+                            mB.speed(0.5);
+                            imu.readMag();
+                            heading = static_cast<int> (imu.calcHeading());
+                            difference = heading - finalDegree;
+                            uLCD.locate(1,3);
+                            uLCD.printf("Heading : %d \n",heading);
+                }                                    
+            }
+            mA.speed(0);
+            mB.speed(0);
+        
+}