Diff: main.cpp

Revision:

1:50d97a0b4c64

Parent:

0:7bbc230e00d6

--- a/main.cpp	Tue Nov 23 16:19:19 2010 +0000
+++ b/main.cpp	Fri Dec 06 19:54:32 2019 +0000
@@ -2,12 +2,186 @@
 
 #include "mbed.h"
 #include "Motor.h"
+#include "LSM9DS1.h"
+#include "math.h"
+#define PI 3.14159
+#include "ultrasonic.h"
+#include "Servo.h"
+#include "SDFileSystem.h"
+#include "wave_player.h"
 
-Motor m(p23, p6, p5); // pwm, fwd, rev
+Servo myservo(p23);
 
+Motor m1(p22, p17, p15); // pwmA, fwd, rev, can brake
+Motor m2(p21,p20,p19); //pwmB, fwd, rev,brake
+Serial bluemod(p13,p14);
+DigitalOut myLed(LED1);
+Serial pc(USBTX, USBRX); // usb serial for debugging
+SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
+AnalogOut DACout(p18);
+wave_player waver(&DACout);
+// SDO_XM and SDO_G are pulled up, so our addresses are:
+#define LSM9DS0_XM_ADDR  0x1D 
+#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
+float mx;
+float my;
+float heading_val = 0.0;
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+LSM9DS1 imu2(p28, p27, 0xD6, 0x3C);
+float p = 0;
+float calcHeading()
+{
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    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;
+    return heading;
+}
+
+
+float updateHeading(){
+        imu2.readMag();
+        mx=imu2.mx;
+        my=imu2.my;
+        heading_val = calcHeading();
+        pc.printf("%2f\r\n",abs(heading_val));
+        return abs(heading_val);
+  
+  }
+
+void playSounds(){
+    FILE *wave_file;
+    wave_file=fopen("/sd/blaster-firing.wav","r");
+    waver.play(wave_file);
+    fclose(wave_file);    
+    
+}
+    
 int main() {
-    for (float s= -1.0; s < 1.0 ; s += 0.01) {
-       m.speed(s); 
-       wait(0.02);
+    imu2.begin();
+    if (!imu2.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
     }
-}
+    imu2.calibrate(1);
+    wait(2);
+    m1.speed(0.25);
+    m2.speed(-0.25);
+    imu2.calibrateMag(0);
+    m1.speed(0.0);
+    m2.speed(0.0);
+    
+    
+    while(1){ 
+        myLed = 0;
+        wait(0.2);
+        imu2.readMag();
+        mx=imu2.mx;
+        my=imu2.my;
+        heading_val = calcHeading();
+        heading_val = updateHeading();
+        myservo.calibrate();
+        if(bluemod.readable()){
+                    float speed;
+                    myLed = 1;
+                    char bnum=0;
+                    char bhit = 0;
+                     if (bluemod.getc()=='!') {
+                        if (bluemod.getc()=='B') { //button data
+                            bnum = bluemod.getc(); //button number
+                            bhit  = bluemod.getc();
+                            switch(bnum){
+                                case '1': // move forward
+                                    if(bhit=='1'){
+                                        speed=0.3;
+                                        playSounds();
+                                        }
+                                    break;
+                                case '2': // break
+                                    if(bhit=='1'){
+                                            speed=0;
+                                        }
+                                    break;
+                                case '3':  //reverse
+                                    if(bhit=='1'){
+                                            speed=-0.3;
+                                        }
+                                    break;
+                                case '4': //raise flag
+                                    if(bhit=='1'){
+                                        playSounds(); //play sound before raising flag
+                                        myLed=1;
+                                        myservo.write(0);
+                                        wait(1);
+                                        myLed =0;
+                                        myservo.write(.05);
+                                        wait(2);
+                                        myLed = 1;
+                                        m1.speed(0);
+                                        m2.speed(0);
+                                        playSounds();  //play sound after flag is raised
+                                        }
+                                    break;
+                                case '5':  //north
+                                    if(bhit=='1'){
+                                        while(heading_val>5.0 && heading_val<355.0){
+                                            heading_val = updateHeading();
+                                            m1.speed(0.3);
+                                            m2.speed(-0.3);
+                                            }
+                                        playSounds();
+                                        m1.speed(0);
+                                        m2.speed(0);
+                                        }
+                                    break;
+                                case '6': //south: 175->185
+                                    if(bhit=='1'){
+                                       while(!(heading_val<185.0 && heading_val>175.0)){
+                                            heading_val = updateHeading();
+                                            m1.speed(0.3);
+                                            m2.speed(-0.3);
+                                            }
+                                        playSounds();
+                                        m1.speed(0);
+                                        m2.speed(0);
+                                        }
+                                    break;
+                                case '8': //east
+                                    if(bhit=='1'){
+                                       while(!(heading_val<95.0 && heading_val>85.0)){
+                                            heading_val = updateHeading();
+                                            m1.speed(0.3);
+                                            m2.speed(-0.3);
+                                            }
+                                        playSounds();
+                                        m1.speed(0);
+                                        m2.speed(0);
+                                        }
+                                    break;
+                                 case '7': //west
+                                    if(bhit=='1'){
+                                       while(!(heading_val<275.0 && heading_val>265.0)){
+                                            heading_val = updateHeading();
+                                            m1.speed(0.3);
+                                            m2.speed(-0.3);
+                                            }
+                                        playSounds();//play sound when direction is found
+                                        m1.speed(0);
+                                        m2.speed(0);
+                                        }
+                                    break;
+                            default: break;    
+                            }//close case
+                        } //close B
+                } //!
+        m1.speed(speed);
+        m2.speed(speed);
+        }//close readable
+    }  // close while
+}//main
+