4180 Hover Vehicle Project / Mbed 2 deprecated hoverboad_project

Dependencies:   mbed wave_player Servo 4DGL-uLCD-SE LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library

main.cpp@0:808403b99108, 2020-04-28 (annotated)

Committer:
msafwan3
Date:
Tue Apr 28 20:15:24 2020 +0000
Revision:
0:808403b99108
Child:
1:de52879ff5ec
project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
msafwan3 0:808403b99108 1 #include "mbed.h"
msafwan3 0:808403b99108 2 #include "wave_player.h"
msafwan3 0:808403b99108 3 #include "SDFileSystem.h"
msafwan3 0:808403b99108 4 //#include "Motor.h"
msafwan3 0:808403b99108 5 //#include "RGBLed.h"
msafwan3 0:808403b99108 6 #include "Servo.h"
msafwan3 0:808403b99108 7 #include "ultrasonic.h"
msafwan3 0:808403b99108 8 #include "uLCD_4DGL.h"
msafwan3 0:808403b99108 9 #include "LSM9DS1.h"
msafwan3 0:808403b99108 10
msafwan3 0:808403b99108 11 #define PI 3.14159
msafwan3 0:808403b99108 12 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
msafwan3 0:808403b99108 13 int sonar_flag = 0; //1 = display distance, 0 = avoid colission
msafwan3 0:808403b99108 14 float range = 0.0005; // for servo/motors calibration
msafwan3 0:808403b99108 15 float position = 0.5; // for servo/motors calibration
msafwan3 0:808403b99108 16
msafwan3 0:808403b99108 17 Serial pc(USBTX, USBRX);
msafwan3 0:808403b99108 18 uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;
msafwan3 0:808403b99108 19 //ultrasonic mu(p15, p16, .1, 1, &dist); //sonar
msafwan3 0:808403b99108 20 DigitalOut led1(p18);
msafwan3 0:808403b99108 21 DigitalOut led2(p19);
msafwan3 0:808403b99108 22
msafwan3 0:808403b99108 23 Servo dcmotorBottom(p21); // pwm
msafwan3 0:808403b99108 24 Servo servoBottom(p22); // pwm
msafwan3 0:808403b99108 25 Servo dcmotorBack(p23); // pwm
msafwan3 0:808403b99108 26
msafwan3 0:808403b99108 27 // Setup to play wav file from SD Card
msafwan3 0:808403b99108 28 AnalogOut DACout(p26);
msafwan3 0:808403b99108 29 wave_player waver(&DACout);
msafwan3 0:808403b99108 30 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
msafwan3 0:808403b99108 31
msafwan3 0:808403b99108 32 // Setup for bluetooth
msafwan3 0:808403b99108 33 Serial blue(p28,p27);
msafwan3 0:808403b99108 34 DigitalOut myled1(LED1);
msafwan3 0:808403b99108 35 DigitalOut myled2(LED2);
msafwan3 0:808403b99108 36 DigitalOut myled3(LED3);
msafwan3 0:808403b99108 37 DigitalOut myled4(LED4);
msafwan3 0:808403b99108 38
msafwan3 0:808403b99108 39 void playSound(char * wav)
msafwan3 0:808403b99108 40 {
msafwan3 0:808403b99108 41 FILE *wave_file;
msafwan3 0:808403b99108 42 wave_file = fopen(wav,"r");
msafwan3 0:808403b99108 43 waver.play(wave_file);
msafwan3 0:808403b99108 44 fclose(wave_file);
msafwan3 0:808403b99108 45 }
msafwan3 0:808403b99108 46
msafwan3 0:808403b99108 47 void bottom_motor_on()
msafwan3 0:808403b99108 48 {
msafwan3 0:808403b99108 49 for (float i=0; i<=0.8; i+=0.1) {
msafwan3 0:808403b99108 50 dcmotorBottom = i;
msafwan3 0:808403b99108 51 wait(0.1);
msafwan3 0:808403b99108 52 }
msafwan3 0:808403b99108 53 }
msafwan3 0:808403b99108 54
msafwan3 0:808403b99108 55 void dist(int distance)
msafwan3 0:808403b99108 56 { if(sonar_flag ==1) { //display distance
msafwan3 0:808403b99108 57 //put code here to happen when the distance is changed
msafwan3 0:808403b99108 58 printf("Distance changed to %dmm\r\n", distance); //print to pc for debugging
msafwan3 0:808403b99108 59 uLCD.cls();
msafwan3 0:808403b99108 60 uLCD.printf("Distance changed to %dmm\r\n", distance); //print to uLCD
msafwan3 0:808403b99108 61 wait(2);
msafwan3 0:808403b99108 62 } else { //avoid colision: beep and stop motor
msafwan3 0:808403b99108 63 if(distance < 2){
msafwan3 0:808403b99108 64 playSound("/sd/Beeping.wav");
msafwan3 0:808403b99108 65 dcmotorBack = 0.0;
msafwan3 0:808403b99108 66 }
msafwan3 0:808403b99108 67 }
msafwan3 0:808403b99108 68
msafwan3 0:808403b99108 69 }
msafwan3 0:808403b99108 70
msafwan3 0:808403b99108 71 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
msafwan3 0:808403b99108 72 {
msafwan3 0:808403b99108 73 float roll = atan2(ay, az);
msafwan3 0:808403b99108 74 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
msafwan3 0:808403b99108 75 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
msafwan3 0:808403b99108 76 mx = -mx;
msafwan3 0:808403b99108 77 float heading;
msafwan3 0:808403b99108 78 if (my == 0.0)
msafwan3 0:808403b99108 79 heading = (mx < 0.0) ? 180.0 : 0.0;
msafwan3 0:808403b99108 80 else
msafwan3 0:808403b99108 81 heading = atan2(mx, my)*360.0/(2.0*PI);
msafwan3 0:808403b99108 82 //pc.printf("heading atan=%f \n\r",heading);
msafwan3 0:808403b99108 83 heading -= DECLINATION; //correct for geo location
msafwan3 0:808403b99108 84 if(heading>180.0) heading = heading - 360.0;
msafwan3 0:808403b99108 85 else if(heading<-180.0) heading = 360.0 + heading;
msafwan3 0:808403b99108 86 else if(heading<0.0) heading = 360.0 + heading;
msafwan3 0:808403b99108 87
msafwan3 0:808403b99108 88
msafwan3 0:808403b99108 89 // Convert everything from radians to degrees:
msafwan3 0:808403b99108 90 //heading *= 180.0 / PI;
msafwan3 0:808403b99108 91 pitch *= 180.0 / PI;
msafwan3 0:808403b99108 92 roll *= 180.0 / PI;
msafwan3 0:808403b99108 93
msafwan3 0:808403b99108 94 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
msafwan3 0:808403b99108 95 pc.printf("Magnetic Heading: %f degress\n\r",heading);
msafwan3 0:808403b99108 96 return heading;
msafwan3 0:808403b99108 97 }
msafwan3 0:808403b99108 98
msafwan3 0:808403b99108 99 void display_compass()
msafwan3 0:808403b99108 100 {
msafwan3 0:808403b99108 101 while(!IMU.tempAvailable());
msafwan3 0:808403b99108 102 IMU.readTemp();
msafwan3 0:808403b99108 103 while(!IMU.magAvailable(X_AXIS));
msafwan3 0:808403b99108 104 IMU.readMag();
msafwan3 0:808403b99108 105 while(!IMU.accelAvailable());
msafwan3 0:808403b99108 106 IMU.readAccel();
msafwan3 0:808403b99108 107 while(!IMU.gyroAvailable());
msafwan3 0:808403b99108 108 IMU.readGyro();
msafwan3 0:808403b99108 109 pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
msafwan3 0:808403b99108 110 pc.printf(" X axis Y axis Z axis\n\r");
msafwan3 0:808403b99108 111 pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
msafwan3 0:808403b99108 112 pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
msafwan3 0:808403b99108 113 pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
msafwan3 0:808403b99108 114 float heading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
msafwan3 0:808403b99108 115 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
msafwan3 0:808403b99108 116 double a = heading * 0.0174533; //deg to rad
msafwan3 0:808403b99108 117 x2 = x_c + r*cos(a);
msafwan3 0:808403b99108 118 y2 = y_c + r*sin(a);
msafwan3 0:808403b99108 119 uLCD.cls();
msafwan3 0:808403b99108 120 uLCD.circle(x_c, y_c, r, WHITE);
msafwan3 0:808403b99108 121 uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
msafwan3 0:808403b99108 122 wait(0.001);
msafwan3 0:808403b99108 123 }
msafwan3 0:808403b99108 124
msafwan3 0:808403b99108 125
msafwan3 0:808403b99108 126 int main()
msafwan3 0:808403b99108 127 {
msafwan3 0:808403b99108 128 // this part of the display compass script only needs to be set up once
msafwan3 0:808403b99108 129 int x_c = 60;
msafwan3 0:808403b99108 130 int y_c = 60;
msafwan3 0:808403b99108 131 int r = 30;
msafwan3 0:808403b99108 132 uLCD.circle(x_c, y_c, r, WHITE);
msafwan3 0:808403b99108 133 int z = 0;
msafwan3 0:808403b99108 134 double x2 = x_c + r*cos((double)z);
msafwan3 0:808403b99108 135 double y2 = x_c + r*sin((double)z);
msafwan3 0:808403b99108 136 uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
msafwan3 0:808403b99108 137 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
msafwan3 0:808403b99108 138 IMU.begin();
msafwan3 0:808403b99108 139 if (!IMU.begin()) {
msafwan3 0:808403b99108 140 pc.printf("Failed to communicate with LSM9DS1.\n");
msafwan3 0:808403b99108 141 }
msafwan3 0:808403b99108 142 IMU.calibrate(1);
msafwan3 0:808403b99108 143 IMU.calibrateMag(0);
msafwan3 0:808403b99108 144
msafwan3 0:808403b99108 145 //calibrate motors
msafwan3 0:808403b99108 146 dcmotorBack.calibrate(range, position);
msafwan3 0:808403b99108 147 dcmotorBottom.calibrate(range, position);
msafwan3 0:808403b99108 148 servoBottom.calibrate(range, position)
msafwan3 0:808403b99108 149
msafwan3 0:808403b99108 150 bottom_motor_on(); // bottom motor always on
msafwan3 0:808403b99108 151 float servoAngle = 90.0; // ranges from 0 to 180 degrees where 90 is center
msafwan3 0:808403b99108 152 mu.startUpdates(); // start mesuring the distance
msafwan3 0:808403b99108 153 char bnum = 0;
msafwan3 0:808403b99108 154 char bhit = 0;
msafwan3 0:808403b99108 155 while(1)
msafwan3 0:808403b99108 156 {
msafwan3 0:808403b99108 157 sonar_flag = 0;
msafwan3 0:808403b99108 158 mu.checkDistance();
msafwan3 0:808403b99108 159 display_compass(); // this part of the compass script needs to be done repeatedly
msafwan3 0:808403b99108 160 servoBottom.position(servoAngle);
msafwan3 0:808403b99108 161 if (blue.getc() == '!') {
msafwan3 0:808403b99108 162 if (blue.getc() == 'B') { //button data packet
msafwan3 0:808403b99108 163 bnum = blue.getc(); //button number
msafwan3 0:808403b99108 164 bhit = blue.getc(); //1=hit, 0=release
msafwan3 0:808403b99108 165 if (blue.getc() == char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
msafwan3 0:808403b99108 166 myled = bnum - '1'; //current button number will appear on LEDs
msafwan3 0:808403b99108 167 switch (bnum) {
msafwan3 0:808403b99108 168 case '1': //number button 1: speeds up motor
msafwan3 0:808403b99108 169 if (bhit=='1') {
msafwan3 0:808403b99108 170 //add hit code here
msafwan3 0:808403b99108 171 motorSpeed += 0.2; // speed up
msafwan3 0:808403b99108 172 myRGBled.write(0.0,1.0,0.0); //green
msafwan3 0:808403b99108 173 } else {
msafwan3 0:808403b99108 174 //add release code here
msafwan3 0:808403b99108 175 }
msafwan3 0:808403b99108 176 break;
msafwan3 0:808403b99108 177 case '2': //number button 2: slows down motor
msafwan3 0:808403b99108 178 if (bhit=='1') {
msafwan3 0:808403b99108 179 //add hit code here
msafwan3 0:808403b99108 180 motorSpeed -= 0.2; // speed down
msafwan3 0:808403b99108 181 myRGBled.write(0.0,0.0,1.0); //blue
msafwan3 0:808403b99108 182 } else {
msafwan3 0:808403b99108 183 //add release code here
msafwan3 0:808403b99108 184 }
msafwan3 0:808403b99108 185 break;
msafwan3 0:808403b99108 186 case '3': //number button 3: display distance from sonar
msafwan3 0:808403b99108 187 if (bhit=='1') {
msafwan3 0:808403b99108 188 //add hit code here
msafwan3 0:808403b99108 189 sonar_flag = 1;
msafwan3 0:808403b99108 190 mu.checkDistance();
msafwan3 0:808403b99108 191 } else {
msafwan3 0:808403b99108 192 //add release code here
msafwan3 0:808403b99108 193 }
msafwan3 0:808403b99108 194 break;
msafwan3 0:808403b99108 195 case '4': //number button 4: play honking sound
msafwan3 0:808403b99108 196 if (bhit=='1') {
msafwan3 0:808403b99108 197 //add hit code here
msafwan3 0:808403b99108 198 playSound("/sd/Honk.wav"); //write name of sound file
msafwan3 0:808403b99108 199 } else {
msafwan3 0:808403b99108 200 //add release code here
msafwan3 0:808403b99108 201 }
msafwan3 0:808403b99108 202 break;
msafwan3 0:808403b99108 203 case '5': //button 5 up arrow: turn on bottom servo and back motor
msafwan3 0:808403b99108 204 if (bhit=='1') {
msafwan3 0:808403b99108 205 //add hit code here
msafwan3 0:808403b99108 206 for(float p=0; p<1.0; p += 0.1) {
msafwan3 0:808403b99108 207 dcmotorBack = p;
msafwan3 0:808403b99108 208 wait(0.2);
msafwan3 0:808403b99108 209 }
msafwan3 0:808403b99108 210 myled1 = 1;
msafwan3 0:808403b99108 211 } else {
msafwan3 0:808403b99108 212 //add release code here
msafwan3 0:808403b99108 213 }
msafwan3 0:808403b99108 214 break;
msafwan3 0:808403b99108 215 case '6': //button 6 down arrow: switch it off (turn off back motor)
msafwan3 0:808403b99108 216 if (bhit=='1') {
msafwan3 0:808403b99108 217 //add hit code here
msafwan3 0:808403b99108 218 dcmotorBack = 0.0;
msafwan3 0:808403b99108 219 myled1 = 0;
msafwan3 0:808403b99108 220 myled2 = 1;
msafwan3 0:808403b99108 221 } else {
msafwan3 0:808403b99108 222 //add release code here
msafwan3 0:808403b99108 223 }
msafwan3 0:808403b99108 224 break;
msafwan3 0:808403b99108 225 case '7': //button 7 left arrow: turns servo leftwards
msafwan3 0:808403b99108 226 if (bhit=='1') {
msafwan3 0:808403b99108 227 //add hit code here
msafwan3 0:808403b99108 228 if (servoAngle > 0.0) {
msafwan3 0:808403b99108 229 servoAngle -= 30; //turn servo left
msafwan3 0:808403b99108 230 }
msafwan3 0:808403b99108 231 myled3 = 1;
msafwan3 0:808403b99108 232 myled4 = 0;
msafwan3 0:808403b99108 233 } else {
msafwan3 0:808403b99108 234 //add release code here
msafwan3 0:808403b99108 235 }
msafwan3 0:808403b99108 236 break;
msafwan3 0:808403b99108 237 case '8': //button 8 right arrow: turns servo rightwards
msafwan3 0:808403b99108 238 if (bhit=='1') {
msafwan3 0:808403b99108 239 //add hit code here
msafwan3 0:808403b99108 240 if (servoAngle < 180.0) {
msafwan3 0:808403b99108 241 servoAngle += 30.0; //turn servo right
msafwan3 0:808403b99108 242 }
msafwan3 0:808403b99108 243 myled4 = 1;
msafwan3 0:808403b99108 244 myled3 = 0;
msafwan3 0:808403b99108 245 } else {
msafwan3 0:808403b99108 246 //add release code here
msafwan3 0:808403b99108 247 }
msafwan3 0:808403b99108 248 break;
msafwan3 0:808403b99108 249 default:
msafwan3 0:808403b99108 250 break;
msafwan3 0:808403b99108 251 }
msafwan3 0:808403b99108 252 }
msafwan3 0:808403b99108 253 }
msafwan3 0:808403b99108 254 }
msafwan3 0:808403b99108 255 }
msafwan3 0:808403b99108 256 }