Diff: main.cpp

Revision:

0:808403b99108

Child:

1:de52879ff5ec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 28 20:15:24 2020 +0000
@@ -0,0 +1,256 @@
+#include "mbed.h"
+#include "wave_player.h"
+#include "SDFileSystem.h"
+//#include "Motor.h"
+//#include "RGBLed.h"
+#include "Servo.h"
+#include "ultrasonic.h"
+#include "uLCD_4DGL.h"
+#include "LSM9DS1.h"
+
+#define PI 3.14159
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+int sonar_flag = 0; //1 = display distance, 0 = avoid colission
+float range = 0.0005; // for servo/motors calibration
+float position = 0.5; // for servo/motors calibration
+
+Serial pc(USBTX, USBRX);
+uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;
+//ultrasonic mu(p15, p16, .1, 1, &dist); //sonar
+DigitalOut led1(p18);
+DigitalOut led2(p19);
+
+Servo dcmotorBottom(p21); // pwm
+Servo servoBottom(p22); // pwm
+Servo dcmotorBack(p23); // pwm
+
+// Setup to play wav file from SD Card
+AnalogOut DACout(p26);
+wave_player waver(&DACout);
+SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
+ 
+// Setup for bluetooth
+Serial blue(p28,p27);
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+void playSound(char * wav)
+{
+    FILE *wave_file;
+    wave_file = fopen(wav,"r");
+    waver.play(wave_file);
+    fclose(wave_file);
+}
+
+void bottom_motor_on()
+{
+    for (float i=0; i<=0.8; i+=0.1) {
+        dcmotorBottom = i;
+        wait(0.1);
+    }
+}
+
+void dist(int distance) 
+{   if(sonar_flag ==1) { //display distance
+    //put code here to happen when the distance is changed
+    printf("Distance changed to %dmm\r\n", distance); //print to pc for debugging
+    uLCD.cls();
+    uLCD.printf("Distance changed to %dmm\r\n", distance); //print to uLCD
+    wait(2);
+    } else { //avoid colision: beep and stop motor
+        if(distance < 2){   
+        playSound("/sd/Beeping.wav");
+        dcmotorBack = 0.0;
+        }
+    }
+    
+}
+
+float printAttitude(float ax, float ay, float az, 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;
+    if (my == 0.0)
+        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;
+
+
+    // Convert everything from radians to degrees:
+    //heading *= 180.0 / PI;
+    pitch *= 180.0 / PI;
+    roll  *= 180.0 / PI;
+
+    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
+    pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    return heading;
+}
+
+void display_compass()
+{   
+    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));
+    float heading = 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));
+    double a = heading * 0.0174533; //deg to rad
+    x2 = x_c + r*cos(a);
+    y2 = y_c + r*sin(a);
+    uLCD.cls();
+    uLCD.circle(x_c, y_c, r, WHITE);
+    uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
+    wait(0.001);
+}
+
+
+int main()
+{   
+    // this part of the display compass script only needs to be set up once
+    int x_c = 60;
+    int y_c = 60;
+    int r = 30;
+    uLCD.circle(x_c, y_c, r, WHITE);
+    int z = 0;
+    double x2 = x_c + r*cos((double)z);
+    double y2 = x_c + r*sin((double)z);
+    uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
+    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);
+
+    //calibrate motors
+    dcmotorBack.calibrate(range, position);
+    dcmotorBottom.calibrate(range, position);
+    servoBottom.calibrate(range, position)
+    
+    bottom_motor_on(); // bottom motor always on
+    float servoAngle = 90.0; // ranges from 0 to 180 degrees where 90 is center
+    mu.startUpdates(); // start mesuring the distance
+    char bnum = 0;
+    char bhit = 0;
+    while(1) 
+    {   
+        sonar_flag = 0;
+        mu.checkDistance();
+        display_compass(); // this part of the compass script needs to be done repeatedly
+        servoBottom.position(servoAngle);
+        if (blue.getc() == '!') {
+            if (blue.getc() == 'B') { //button data packet
+                bnum = blue.getc(); //button number
+                bhit = blue.getc(); //1=hit, 0=release
+                if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+                    myled = bnum - '1'; //current button number will appear on LEDs
+                    switch (bnum) {
+                        case '1': //number button 1: speeds up motor
+                            if (bhit=='1') {
+                                //add hit code here
+                                motorSpeed += 0.2; // speed up
+                                myRGBled.write(0.0,1.0,0.0); //green
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '2': //number button 2: slows down motor
+                            if (bhit=='1') {
+                                //add hit code here
+                                motorSpeed -= 0.2; // speed down
+                                myRGBled.write(0.0,0.0,1.0); //blue
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '3': //number button 3: display distance from sonar
+                            if (bhit=='1') {
+                                //add hit code here   
+                                sonar_flag = 1; 
+                                mu.checkDistance();
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '4': //number button 4: play honking sound
+                            if (bhit=='1') {
+                                //add hit code here                              
+                                playSound("/sd/Honk.wav"); //write name of sound file
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '5': //button 5 up arrow: turn on bottom servo and back motor
+                            if (bhit=='1') {
+                                //add hit code here  
+                                for(float p=0; p<1.0; p += 0.1) {
+                                    dcmotorBack = p;
+                                    wait(0.2);
+                                }
+                                myled1 = 1;                             
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '6': //button 6 down arrow: switch it off (turn off back motor)
+                            if (bhit=='1') {
+                                //add hit code here
+                                dcmotorBack = 0.0;
+                                myled1 = 0;
+                                myled2 = 1;
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '7': //button 7 left arrow: turns servo leftwards
+                            if (bhit=='1') {
+                                //add hit code here
+                                if (servoAngle > 0.0) {
+                                    servoAngle -= 30; //turn servo left
+                                }
+                                myled3 = 1;
+                                myled4 = 0;
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        case '8': //button 8 right arrow: turns servo rightwards
+                            if (bhit=='1') {
+                                //add hit code here
+                                if (servoAngle < 180.0) {
+                                    servoAngle += 30.0; //turn servo right
+                                }
+                                myled4 = 1;
+                                myled3 = 0;
+                            } else {
+                                //add release code here
+                            }
+                            break;
+                        default:
+                            break;
+                    }
+                }
+            }
+        }
+    }
+}
\ No newline at end of file