main.cpp

00001 #include "mbed.h"
00002 #include "PinDetect.h"
00003 #include "uLCD_4DGL.h"
00004 #include "math.h"
00005 #include "LSM9DS0.h"
00006 #include "Motor.h"
00007 #define PI 3.14159265
00008 #define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
00009 #define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
00010 
00011 // refresh time. set to 500 for part 2 and 50 for part 4
00012 #define REFRESH_TIME_MS 500
00013 Serial pc(USBTX, USBRX);
00014 // Verify that the pin assignments below match your breadboard
00015 LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
00016 Motor mA(p21, p23, p24); // pwm, fwd, rev LEFT
00017 Motor mB(p22, p25, p26); // pwm, fwd, rev RIGHT
00018 //speed: 10.44 cm/s
00019 // use class to setup pushbuttons pins
00020 PinDetect pb1(p16);
00021 PinDetect pb2(p17);
00022 PinDetect pb3(p18);
00023 PinDetect pb4(p19);
00024 
00025 DigitalOut myLED1(LED1);
00026 DigitalOut myLED2(LED2);
00027 DigitalOut myLED3(LED3);
00028 DigitalOut myLED4(LED4);
00029 // use class to setup the  Color LCD
00030 uLCD_4DGL uLCD(p28, p27, p29); // create a global uLCD object
00031 
00032 //Timer class
00033 Timer timer;
00034 
00035 //Mode settting
00036 
00037 enum Modetype { SETX = 0, SETY, GO };
00038 Modetype mode1 = SETX;
00039 int settingMode = 0;
00040 //Variable
00041 int xCoord = 0;
00042 int yCoord = 0;
00043 float distance = 0;
00044 float angle = 0;
00045 float seconds = 0;
00046 int startDegree = 0;
00047 int turningDegree = 0;
00048 int finalDegree = 0;
00049 int heading = 0;
00050 int degree = 0;
00051 int initialHeading = 0;
00052 int state = 0;
00053 int difference = 0;
00054 int counter = 0;
00055 void setup()
00056 {
00057     uLCD.cls();
00058     uint16_t status = imu.begin();
00059 //Make sure communication is working
00060 }
00061 
00062 //When pb1 push increment the coordinate
00063 void pb1_hit_callback (void)
00064 {
00065     if (mode1 == SETX)
00066         xCoord++;
00067     else if (mode1 == SETY)
00068         yCoord++;
00069     
00070     myLED1 = 1;
00071     myLED2 = 0;
00072     myLED3 = 0;
00073     myLED4 = 0;
00074 }
00075 
00076 //When pb2 push decrement the coordinate
00077 void pb2_hit_callback (void)
00078 {
00079     if (mode1 == SETX)
00080         xCoord--;
00081     else if (mode1 == SETY)
00082         yCoord--;
00083         
00084     myLED2 = 1;
00085     myLED1 = 0;
00086     myLED3 = 0;
00087     myLED4 = 0;
00088     
00089 }
00090 
00091 //When pb3 push increment the mode
00092 void pb3_hit_callback (void)
00093 {
00094     mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3);
00095     myLED3 = 1;
00096     myLED1 = 0;
00097     myLED2 = 0;
00098     myLED4 = 0;
00099 }
00100 
00101 //When pb4 push decrement the mode
00102 void pb4_hit_callback (void)
00103 {
00104     if (mode1 == SETY)
00105     {
00106         mode1 = SETX;   
00107     }
00108     myLED4 = 1;
00109     myLED1 = 0;
00110     myLED2 = 0;
00111     myLED3 = 0;
00112 }
00113 
00114 void turnRight(void)
00115 {
00116     imu.readMag();
00117     myLED3 = 1;
00118 //    uLCD.locate(1,5);
00119 //    uLCD.printf("Turning Right \n");
00120     mA.speed(0.25);
00121     mB.speed(-0.25);
00122     heading = static_cast<int> (imu.calcHeading());
00123 //    uLCD.printf("H : %d \n",heading);
00124     degree =   heading - initialHeading;
00125     degree = turningDegree - degree;
00126     uLCD.locate(1,5);
00127     uLCD.printf("Degree Left = %d \n",degree);
00128     uLCD.printf("Heading = %d \n",heading);
00129 }
00130 
00131 void turnLeft(void)
00132 {
00133     imu.readMag();
00134     myLED3 = 1;
00135 //    uLCD.locate(1,5);
00136 //    uLCD.printf("Turning Left \n");
00137     mA.speed(-0.25);
00138     mB.speed(0.25);
00139     heading = static_cast<int> (imu.calcHeading());
00140 //    uLCD.printf("H : %d \n",heading);
00141     degree =   heading - initialHeading;
00142     degree = turningDegree - degree;
00143     uLCD.locate(1,5);
00144     uLCD.printf("Degree Left = %d \n",degree);
00145     uLCD.printf("Heading = %d \n",heading);
00146 }
00147 
00148 
00149 int main() 
00150 {
00151     uLCD.cls();
00152     // Use internal pullups for the three pushbuttons
00153     pb1.mode(PullUp);
00154     pb2.mode(PullUp);
00155     pb3.mode(PullUp);
00156     pb4.mode(PullUp);
00157     // Delay for initial pullup to take effect
00158     wait(.01);
00159     // Setup Interrupt callback functions for a pb hit
00160     pb1.attach_deasserted(&pb1_hit_callback);
00161     pb2.attach_deasserted(&pb2_hit_callback);
00162     pb3.attach_deasserted(&pb3_hit_callback);
00163     pb4.attach_deasserted(&pb4_hit_callback);
00164     // Start sampling pb inputs using interrupts
00165     pb1.setSampleFrequency();
00166     pb2.setSampleFrequency();
00167     pb3.setSampleFrequency();
00168     pb4.setSampleFrequency();
00169     mode1 = SETX;
00170     setup();
00171     
00172     xCoord = 0;
00173     yCoord = 0;
00174     state = 1;
00175         //Setting mode
00176         while(settingMode == 0)
00177         {
00178             uLCD.locate(1,1);
00179             if (mode1 == SETX)
00180             {
00181                 uLCD.printf("Setting X Coord");    
00182             }
00183             else if (mode1 == SETY)
00184             {
00185                 uLCD.printf("Setting Y Coord");   
00186             }
00187             else if (mode1 == GO)
00188             {
00189                 uLCD.printf("Running Now");   
00190             }    
00191             uLCD.locate(4,5);
00192             uLCD.printf("X-Coord: %d \n\r", xCoord);
00193             uLCD.locate(4,7);
00194             uLCD.printf("Y-Coord: %d \n\r", yCoord);            
00195             
00196             if (mode1 == GO)
00197             {
00198                 distance =  xCoord*xCoord + yCoord*yCoord;
00199                 distance = sqrt(distance);
00200                 float totalDistance = distance * 30.48;
00201                 seconds = totalDistance / 23.18 * 1000;
00202                 imu.readMag();
00203                 initialHeading = static_cast<int>(imu.calcHeading());
00204                 angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
00205                 finalDegree = initialHeading + angle;
00206                 uLCD.locate(1,9);
00207                 uLCD.printf("Distance: %5.2F Feet \n\r", distance);
00208                 uLCD.locate(1,11);
00209                 uLCD.printf("Turning: %5.2F Degree \n\r", angle);
00210                 
00211                 settingMode = 1;
00212             }
00213                 
00214         } //end first while
00215         wait(2);
00216         //Turning mode
00217         while(settingMode == 1)
00218         {
00219             uLCD.cls();
00220 //            uLCD.printf("Setting mode 1");
00221             imu.readMag();
00222             initialHeading = static_cast<int> (imu.calcHeading());
00223             uLCD.locate(1,3);
00224             uLCD.printf("Initial : %d \n",initialHeading);
00225             
00226             startDegree = heading;
00227             turningDegree = angle;
00228             degree =  static_cast<int> (imu.calcHeading()) - initialHeading;
00229             
00230            if (turningDegree > 0)
00231            {
00232                 while (degree  > -2 && degree < 180)
00233                 {              
00234                     turnRight();
00235                     
00236                     mA.speed(0);
00237                     mB.speed(0);  
00238                     wait(0.4);
00239                 } 
00240                 settingMode = 2;
00241 
00242             }
00243             else
00244             {
00245                 while (degree < 2 && degree > -180 )
00246                 {
00247                     turnLeft();
00248                     
00249                     mA.speed(0);
00250                     mB.speed(0);
00251                     wait(0.4);  
00252                 }
00253                 settingMode = 2;
00254             }                                                  
00255         }
00256         
00257             wait(2);
00258             imu.readMag();
00259             initialHeading = static_cast<int> (imu.calcHeading());
00260             timer.start();
00261             uLCD.cls();
00262             while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
00263             {
00264             
00265                 
00266 //                uLCD.printf("Time : %d \n",timer.read_ms());
00267                 mA.speed(0.4);
00268                 mB.speed(0.4);
00269                 imu.readMag();
00270                 heading = static_cast<int> (imu.calcHeading());
00271                 difference = heading - finalDegree;
00272                 uLCD.locate(5,6);
00273                 uLCD.printf("Go!");
00274                 while( difference <-2 &&timer.read_ms() < static_cast<int>(seconds) )
00275                 {
00276                             
00277                             mA.speed(0.5);
00278                             mB.speed(0.4);
00279                             imu.readMag();
00280                             heading = static_cast<int> (imu.calcHeading());
00281                             difference = heading - finalDegree;
00282                             uLCD.locate(1,3);
00283                             uLCD.printf("Difference : %d \n",difference);
00284                             uLCD.locate(1,4);
00285                             uLCD.printf("Turning Right \n");
00286                             uLCD.locate(1,5);
00287                             uLCD.printf("Counter = %d \n",counter);
00288                             counter++;
00289 //                            wait(0.2);
00290                 }
00291                  while(difference >2 && timer.read_ms() < static_cast<int>(seconds))
00292                 {
00293                             mA.speed(0.4);
00294                             mB.speed(0.5);
00295                             imu.readMag();
00296                             heading = static_cast<int> (imu.calcHeading());
00297                             difference = heading - finalDegree;
00298                             uLCD.locate(1,3);
00299                             uLCD.printf("Difference : %d \n",difference);
00300                             uLCD.locate(1,4);
00301                             uLCD.printf("Turning Left \n");
00302                             uLCD.locate(1,5);
00303                             uLCD.printf("Counter = %d \n",counter);
00304                             counter++;
00305 //                            wait(0.2);
00306                 }                                    
00307             }
00308             mA.speed(0);
00309             mB.speed(0);
00310         
00311 }