Nivedha Ravi / Mbed 2 deprecated minesweeper

ECE 4180 Final Project, curiosity rover

Dependencies:   mbed wave_player Servo Motor SDFileSystem LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library

main.cpp@1:50d97a0b4c64, 2019-12-06 (annotated)

Committer:
nivedharavi
Date:
Fri Dec 06 19:54:32 2019 +0000
Revision:
1:50d97a0b4c64
Parent:
0:7bbc230e00d6 
ECE 4180 Final Project Minesweeper / Rover

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:7bbc230e00d6 1 // Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)
simon 0:7bbc230e00d6 2
simon 0:7bbc230e00d6 3 #include "mbed.h"
simon 0:7bbc230e00d6 4 #include "Motor.h"
nivedharavi 1:50d97a0b4c64 5 #include "LSM9DS1.h"
nivedharavi 1:50d97a0b4c64 6 #include "math.h"
nivedharavi 1:50d97a0b4c64 7 #define PI 3.14159
nivedharavi 1:50d97a0b4c64 8 #include "ultrasonic.h"
nivedharavi 1:50d97a0b4c64 9 #include "Servo.h"
nivedharavi 1:50d97a0b4c64 10 #include "SDFileSystem.h"
nivedharavi 1:50d97a0b4c64 11 #include "wave_player.h"
simon 0:7bbc230e00d6 12
nivedharavi 1:50d97a0b4c64 13 Servo myservo(p23);
simon 0:7bbc230e00d6 14
nivedharavi 1:50d97a0b4c64 15 Motor m1(p22, p17, p15); // pwmA, fwd, rev, can brake
nivedharavi 1:50d97a0b4c64 16 Motor m2(p21,p20,p19); //pwmB, fwd, rev,brake
nivedharavi 1:50d97a0b4c64 17 Serial bluemod(p13,p14);
nivedharavi 1:50d97a0b4c64 18 DigitalOut myLed(LED1);
nivedharavi 1:50d97a0b4c64 19 Serial pc(USBTX, USBRX); // usb serial for debugging
nivedharavi 1:50d97a0b4c64 20 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
nivedharavi 1:50d97a0b4c64 21 AnalogOut DACout(p18);
nivedharavi 1:50d97a0b4c64 22 wave_player waver(&DACout);
nivedharavi 1:50d97a0b4c64 23 // SDO_XM and SDO_G are pulled up, so our addresses are:
nivedharavi 1:50d97a0b4c64 24 #define LSM9DS0_XM_ADDR 0x1D
nivedharavi 1:50d97a0b4c64 25 #define LSM9DS0_G_ADDR 0x6B // Would be 0x6A if SDO_G is LOW
nivedharavi 1:50d97a0b4c64 26 float mx;
nivedharavi 1:50d97a0b4c64 27 float my;
nivedharavi 1:50d97a0b4c64 28 float heading_val = 0.0;
nivedharavi 1:50d97a0b4c64 29 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
nivedharavi 1:50d97a0b4c64 30 LSM9DS1 imu2(p28, p27, 0xD6, 0x3C);
nivedharavi 1:50d97a0b4c64 31 float p = 0;
nivedharavi 1:50d97a0b4c64 32 float calcHeading()
nivedharavi 1:50d97a0b4c64 33 {
nivedharavi 1:50d97a0b4c64 34 mx = -mx;
nivedharavi 1:50d97a0b4c64 35 float heading;
nivedharavi 1:50d97a0b4c64 36 if (my == 0.0)
nivedharavi 1:50d97a0b4c64 37 heading = (mx < 0.0) ? 180.0 : 0.0;
nivedharavi 1:50d97a0b4c64 38 else
nivedharavi 1:50d97a0b4c64 39 heading = atan2(mx, my)*360.0/(2.0*PI);
nivedharavi 1:50d97a0b4c64 40 heading -= DECLINATION; //correct for geo location
nivedharavi 1:50d97a0b4c64 41 if(heading>180.0) heading = heading - 360.0;
nivedharavi 1:50d97a0b4c64 42 else if(heading<-180.0) heading = 360.0 + heading;
nivedharavi 1:50d97a0b4c64 43 else if(heading<0.0) heading = 360.0 + heading;
nivedharavi 1:50d97a0b4c64 44 return heading;
nivedharavi 1:50d97a0b4c64 45 }
nivedharavi 1:50d97a0b4c64 46
nivedharavi 1:50d97a0b4c64 47
nivedharavi 1:50d97a0b4c64 48 float updateHeading(){
nivedharavi 1:50d97a0b4c64 49 imu2.readMag();
nivedharavi 1:50d97a0b4c64 50 mx=imu2.mx;
nivedharavi 1:50d97a0b4c64 51 my=imu2.my;
nivedharavi 1:50d97a0b4c64 52 heading_val = calcHeading();
nivedharavi 1:50d97a0b4c64 53 pc.printf("%2f\r\n",abs(heading_val));
nivedharavi 1:50d97a0b4c64 54 return abs(heading_val);
nivedharavi 1:50d97a0b4c64 55
nivedharavi 1:50d97a0b4c64 56 }
nivedharavi 1:50d97a0b4c64 57
nivedharavi 1:50d97a0b4c64 58 void playSounds(){
nivedharavi 1:50d97a0b4c64 59 FILE *wave_file;
nivedharavi 1:50d97a0b4c64 60 wave_file=fopen("/sd/blaster-firing.wav","r");
nivedharavi 1:50d97a0b4c64 61 waver.play(wave_file);
nivedharavi 1:50d97a0b4c64 62 fclose(wave_file);
nivedharavi 1:50d97a0b4c64 63
nivedharavi 1:50d97a0b4c64 64 }
nivedharavi 1:50d97a0b4c64 65
simon 0:7bbc230e00d6 66 int main() {
nivedharavi 1:50d97a0b4c64 67 imu2.begin();
nivedharavi 1:50d97a0b4c64 68 if (!imu2.begin()) {
nivedharavi 1:50d97a0b4c64 69 pc.printf("Failed to communicate with LSM9DS1.\n");
simon 0:7bbc230e00d6 70 }
nivedharavi 1:50d97a0b4c64 71 imu2.calibrate(1);
nivedharavi 1:50d97a0b4c64 72 wait(2);
nivedharavi 1:50d97a0b4c64 73 m1.speed(0.25);
nivedharavi 1:50d97a0b4c64 74 m2.speed(-0.25);
nivedharavi 1:50d97a0b4c64 75 imu2.calibrateMag(0);
nivedharavi 1:50d97a0b4c64 76 m1.speed(0.0);
nivedharavi 1:50d97a0b4c64 77 m2.speed(0.0);
nivedharavi 1:50d97a0b4c64 78
nivedharavi 1:50d97a0b4c64 79
nivedharavi 1:50d97a0b4c64 80 while(1){
nivedharavi 1:50d97a0b4c64 81 myLed = 0;
nivedharavi 1:50d97a0b4c64 82 wait(0.2);
nivedharavi 1:50d97a0b4c64 83 imu2.readMag();
nivedharavi 1:50d97a0b4c64 84 mx=imu2.mx;
nivedharavi 1:50d97a0b4c64 85 my=imu2.my;
nivedharavi 1:50d97a0b4c64 86 heading_val = calcHeading();
nivedharavi 1:50d97a0b4c64 87 heading_val = updateHeading();
nivedharavi 1:50d97a0b4c64 88 myservo.calibrate();
nivedharavi 1:50d97a0b4c64 89 if(bluemod.readable()){
nivedharavi 1:50d97a0b4c64 90 float speed;
nivedharavi 1:50d97a0b4c64 91 myLed = 1;
nivedharavi 1:50d97a0b4c64 92 char bnum=0;
nivedharavi 1:50d97a0b4c64 93 char bhit = 0;
nivedharavi 1:50d97a0b4c64 94 if (bluemod.getc()=='!') {
nivedharavi 1:50d97a0b4c64 95 if (bluemod.getc()=='B') { //button data
nivedharavi 1:50d97a0b4c64 96 bnum = bluemod.getc(); //button number
nivedharavi 1:50d97a0b4c64 97 bhit = bluemod.getc();
nivedharavi 1:50d97a0b4c64 98 switch(bnum){
nivedharavi 1:50d97a0b4c64 99 case '1': // move forward
nivedharavi 1:50d97a0b4c64 100 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 101 speed=0.3;
nivedharavi 1:50d97a0b4c64 102 playSounds();
nivedharavi 1:50d97a0b4c64 103 }
nivedharavi 1:50d97a0b4c64 104 break;
nivedharavi 1:50d97a0b4c64 105 case '2': // break
nivedharavi 1:50d97a0b4c64 106 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 107 speed=0;
nivedharavi 1:50d97a0b4c64 108 }
nivedharavi 1:50d97a0b4c64 109 break;
nivedharavi 1:50d97a0b4c64 110 case '3': //reverse
nivedharavi 1:50d97a0b4c64 111 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 112 speed=-0.3;
nivedharavi 1:50d97a0b4c64 113 }
nivedharavi 1:50d97a0b4c64 114 break;
nivedharavi 1:50d97a0b4c64 115 case '4': //raise flag
nivedharavi 1:50d97a0b4c64 116 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 117 playSounds(); //play sound before raising flag
nivedharavi 1:50d97a0b4c64 118 myLed=1;
nivedharavi 1:50d97a0b4c64 119 myservo.write(0);
nivedharavi 1:50d97a0b4c64 120 wait(1);
nivedharavi 1:50d97a0b4c64 121 myLed =0;
nivedharavi 1:50d97a0b4c64 122 myservo.write(.05);
nivedharavi 1:50d97a0b4c64 123 wait(2);
nivedharavi 1:50d97a0b4c64 124 myLed = 1;
nivedharavi 1:50d97a0b4c64 125 m1.speed(0);
nivedharavi 1:50d97a0b4c64 126 m2.speed(0);
nivedharavi 1:50d97a0b4c64 127 playSounds(); //play sound after flag is raised
nivedharavi 1:50d97a0b4c64 128 }
nivedharavi 1:50d97a0b4c64 129 break;
nivedharavi 1:50d97a0b4c64 130 case '5': //north
nivedharavi 1:50d97a0b4c64 131 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 132 while(heading_val>5.0 && heading_val<355.0){
nivedharavi 1:50d97a0b4c64 133 heading_val = updateHeading();
nivedharavi 1:50d97a0b4c64 134 m1.speed(0.3);
nivedharavi 1:50d97a0b4c64 135 m2.speed(-0.3);
nivedharavi 1:50d97a0b4c64 136 }
nivedharavi 1:50d97a0b4c64 137 playSounds();
nivedharavi 1:50d97a0b4c64 138 m1.speed(0);
nivedharavi 1:50d97a0b4c64 139 m2.speed(0);
nivedharavi 1:50d97a0b4c64 140 }
nivedharavi 1:50d97a0b4c64 141 break;
nivedharavi 1:50d97a0b4c64 142 case '6': //south: 175->185
nivedharavi 1:50d97a0b4c64 143 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 144 while(!(heading_val<185.0 && heading_val>175.0)){
nivedharavi 1:50d97a0b4c64 145 heading_val = updateHeading();
nivedharavi 1:50d97a0b4c64 146 m1.speed(0.3);
nivedharavi 1:50d97a0b4c64 147 m2.speed(-0.3);
nivedharavi 1:50d97a0b4c64 148 }
nivedharavi 1:50d97a0b4c64 149 playSounds();
nivedharavi 1:50d97a0b4c64 150 m1.speed(0);
nivedharavi 1:50d97a0b4c64 151 m2.speed(0);
nivedharavi 1:50d97a0b4c64 152 }
nivedharavi 1:50d97a0b4c64 153 break;
nivedharavi 1:50d97a0b4c64 154 case '8': //east
nivedharavi 1:50d97a0b4c64 155 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 156 while(!(heading_val<95.0 && heading_val>85.0)){
nivedharavi 1:50d97a0b4c64 157 heading_val = updateHeading();
nivedharavi 1:50d97a0b4c64 158 m1.speed(0.3);
nivedharavi 1:50d97a0b4c64 159 m2.speed(-0.3);
nivedharavi 1:50d97a0b4c64 160 }
nivedharavi 1:50d97a0b4c64 161 playSounds();
nivedharavi 1:50d97a0b4c64 162 m1.speed(0);
nivedharavi 1:50d97a0b4c64 163 m2.speed(0);
nivedharavi 1:50d97a0b4c64 164 }
nivedharavi 1:50d97a0b4c64 165 break;
nivedharavi 1:50d97a0b4c64 166 case '7': //west
nivedharavi 1:50d97a0b4c64 167 if(bhit=='1'){
nivedharavi 1:50d97a0b4c64 168 while(!(heading_val<275.0 && heading_val>265.0)){
nivedharavi 1:50d97a0b4c64 169 heading_val = updateHeading();
nivedharavi 1:50d97a0b4c64 170 m1.speed(0.3);
nivedharavi 1:50d97a0b4c64 171 m2.speed(-0.3);
nivedharavi 1:50d97a0b4c64 172 }
nivedharavi 1:50d97a0b4c64 173 playSounds();//play sound when direction is found
nivedharavi 1:50d97a0b4c64 174 m1.speed(0);
nivedharavi 1:50d97a0b4c64 175 m2.speed(0);
nivedharavi 1:50d97a0b4c64 176 }
nivedharavi 1:50d97a0b4c64 177 break;
nivedharavi 1:50d97a0b4c64 178 default: break;
nivedharavi 1:50d97a0b4c64 179 }//close case
nivedharavi 1:50d97a0b4c64 180 } //close B
nivedharavi 1:50d97a0b4c64 181 } //!
nivedharavi 1:50d97a0b4c64 182 m1.speed(speed);
nivedharavi 1:50d97a0b4c64 183 m2.speed(speed);
nivedharavi 1:50d97a0b4c64 184 }//close readable
nivedharavi 1:50d97a0b4c64 185 } // close while
nivedharavi 1:50d97a0b4c64 186 }//main
nivedharavi 1:50d97a0b4c64 187