4180 Hover Vehicle Project / Mbed 2 deprecated hoverboad_project

Mbed project to control a hoverboard

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

main.cpp@1:de52879ff5ec, 2020-04-29 (annotated)

Committer:
msafwan3
Date:
Wed Apr 29 08:33:11 2020 +0000
Revision:
1:de52879ff5ec
Parent:
0:808403b99108
Child:
2:726636c1c83c
updated code using rtos

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 "Servo.h"
msafwan3 0:808403b99108 5 #include "ultrasonic.h"
msafwan3 0:808403b99108 6 #include "uLCD_4DGL.h"
msafwan3 0:808403b99108 7 #include "LSM9DS1.h"
msafwan3 1:de52879ff5ec 8 #include "rtos.h"
msafwan3 0:808403b99108 9
msafwan3 0:808403b99108 10 Serial pc(USBTX, USBRX);
msafwan3 0:808403b99108 11 uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;
msafwan3 1:de52879ff5ec 12
msafwan3 1:de52879ff5ec 13 DigitalOut led1(p26);
msafwan3 0:808403b99108 14 DigitalOut led2(p19);
msafwan3 1:de52879ff5ec 15 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
msafwan3 0:808403b99108 16 Servo dcmotorBottom(p21); // pwm
msafwan3 1:de52879ff5ec 17 Servo dcmotorBack(p22); // pwm
msafwan3 1:de52879ff5ec 18 Servo servoBottom(p23); // pwm
msafwan3 1:de52879ff5ec 19 AnalogOut DACout(p18);
msafwan3 0:808403b99108 20 wave_player waver(&DACout);
msafwan3 0:808403b99108 21 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
msafwan3 0:808403b99108 22 Serial blue(p28,p27);
msafwan3 1:de52879ff5ec 23 BusOut myleds(LED1,LED2,LED3,LED4);
msafwan3 1:de52879ff5ec 24
msafwan3 1:de52879ff5ec 25 char bnum=0;//for blue
msafwan3 1:de52879ff5ec 26 Mutex my_mutex;
msafwan3 1:de52879ff5ec 27 int dist_flag = 0;
msafwan3 0:808403b99108 28
msafwan3 1:de52879ff5ec 29 void col_ESC() {
msafwan3 1:de52879ff5ec 30 dcmotorBottom = 0.0;
msafwan3 1:de52879ff5ec 31 dcmotorBack = 0.0;
msafwan3 1:de52879ff5ec 32 wait(0.5); //ESC detects signal
msafwan3 1:de52879ff5ec 33 //Required ESC Calibration/Arming sequence
msafwan3 1:de52879ff5ec 34 //sends longest and shortest PWM pulse to learn and arm at power on
msafwan3 1:de52879ff5ec 35 dcmotorBottom = 1.0; //send longest PWM
msafwan3 1:de52879ff5ec 36 dcmotorBack = 1.0; //send longest PWM
msafwan3 1:de52879ff5ec 37 wait(8);
msafwan3 1:de52879ff5ec 38 dcmotorBottom = 0.0; //send shortest PWM
msafwan3 1:de52879ff5ec 39 dcmotorBack = 0.0; //send shortest PWM
msafwan3 1:de52879ff5ec 40 wait(8);
msafwan3 0:808403b99108 41 }
msafwan3 0:808403b99108 42
msafwan3 1:de52879ff5ec 43 void dist(int distance)
msafwan3 0:808403b99108 44 {
msafwan3 1:de52879ff5ec 45 while(1){
msafwan3 1:de52879ff5ec 46 //put code here to execute when the distance has changed
msafwan3 1:de52879ff5ec 47 printf("Distance %d mm\r\n", distance);
msafwan3 1:de52879ff5ec 48 if(distance <50){
msafwan3 1:de52879ff5ec 49 dist_flag=1;
msafwan3 0:808403b99108 50 }
msafwan3 0:808403b99108 51 }
msafwan3 0:808403b99108 52
msafwan3 0:808403b99108 53 }
msafwan3 1:de52879ff5ec 54 ultrasonic mu(p15, p16, .1, 1, &dist); //sonar
msafwan3 1:de52879ff5ec 55 #define PI 3.14159
msafwan3 1:de52879ff5ec 56 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
msafwan3 1:de52879ff5ec 57 double x2;
msafwan3 1:de52879ff5ec 58 double y2;
msafwan3 1:de52879ff5ec 59 int x_c;
msafwan3 1:de52879ff5ec 60 int y_c;
msafwan3 1:de52879ff5ec 61 int r;
msafwan3 1:de52879ff5ec 62 int z;
msafwan3 0:808403b99108 63 float printAttitude(float ax, float ay, float az, float mx, float my, float mz)
msafwan3 0:808403b99108 64 {
msafwan3 0:808403b99108 65 float roll = atan2(ay, az);
msafwan3 0:808403b99108 66 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
msafwan3 0:808403b99108 67 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
msafwan3 0:808403b99108 68 mx = -mx;
msafwan3 0:808403b99108 69 float heading;
msafwan3 0:808403b99108 70 if (my == 0.0)
msafwan3 0:808403b99108 71 heading = (mx < 0.0) ? 180.0 : 0.0;
msafwan3 0:808403b99108 72 else
msafwan3 0:808403b99108 73 heading = atan2(mx, my)*360.0/(2.0*PI);
msafwan3 0:808403b99108 74 //pc.printf("heading atan=%f \n\r",heading);
msafwan3 0:808403b99108 75 heading -= DECLINATION; //correct for geo location
msafwan3 0:808403b99108 76 if(heading>180.0) heading = heading - 360.0;
msafwan3 0:808403b99108 77 else if(heading<-180.0) heading = 360.0 + heading;
msafwan3 0:808403b99108 78 else if(heading<0.0) heading = 360.0 + heading;
msafwan3 0:808403b99108 79
msafwan3 0:808403b99108 80
msafwan3 0:808403b99108 81 // Convert everything from radians to degrees:
msafwan3 0:808403b99108 82 //heading *= 180.0 / PI;
msafwan3 0:808403b99108 83 pitch *= 180.0 / PI;
msafwan3 0:808403b99108 84 roll *= 180.0 / PI;
msafwan3 0:808403b99108 85
msafwan3 0:808403b99108 86 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
msafwan3 0:808403b99108 87 pc.printf("Magnetic Heading: %f degress\n\r",heading);
msafwan3 0:808403b99108 88 return heading;
msafwan3 0:808403b99108 89 }
msafwan3 0:808403b99108 90
msafwan3 0:808403b99108 91 void display_compass()
msafwan3 0:808403b99108 92 {
msafwan3 0:808403b99108 93 while(!IMU.tempAvailable());
msafwan3 0:808403b99108 94 IMU.readTemp();
msafwan3 0:808403b99108 95 while(!IMU.magAvailable(X_AXIS));
msafwan3 0:808403b99108 96 IMU.readMag();
msafwan3 0:808403b99108 97 while(!IMU.accelAvailable());
msafwan3 0:808403b99108 98 IMU.readAccel();
msafwan3 0:808403b99108 99 while(!IMU.gyroAvailable());
msafwan3 0:808403b99108 100 IMU.readGyro();
msafwan3 0:808403b99108 101 pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
msafwan3 0:808403b99108 102 pc.printf(" X axis Y axis Z axis\n\r");
msafwan3 0:808403b99108 103 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 104 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 105 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 106 float heading = printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
msafwan3 0:808403b99108 107 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
msafwan3 0:808403b99108 108 double a = heading * 0.0174533; //deg to rad
msafwan3 0:808403b99108 109 x2 = x_c + r*cos(a);
msafwan3 0:808403b99108 110 y2 = y_c + r*sin(a);
msafwan3 0:808403b99108 111 uLCD.cls();
msafwan3 0:808403b99108 112 uLCD.circle(x_c, y_c, r, WHITE);
msafwan3 0:808403b99108 113 uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
msafwan3 0:808403b99108 114 wait(0.001);
msafwan3 0:808403b99108 115 }
msafwan3 1:de52879ff5ec 116 void IMU_fun(void const *args){//thread 1
msafwan3 1:de52879ff5ec 117 x_c = 60;
msafwan3 1:de52879ff5ec 118 y_c = 60;
msafwan3 1:de52879ff5ec 119 r = 30;
msafwan3 0:808403b99108 120 uLCD.circle(x_c, y_c, r, WHITE);
msafwan3 1:de52879ff5ec 121 z = 0;
msafwan3 1:de52879ff5ec 122 x2 = x_c + r*cos((double)z);
msafwan3 1:de52879ff5ec 123 y2 = x_c + r*sin((double)z);
msafwan3 0:808403b99108 124 uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
msafwan3 1:de52879ff5ec 125
msafwan3 0:808403b99108 126 IMU.begin();
msafwan3 0:808403b99108 127 if (!IMU.begin()) {
msafwan3 0:808403b99108 128 pc.printf("Failed to communicate with LSM9DS1.\n");
msafwan3 0:808403b99108 129 }
msafwan3 0:808403b99108 130 IMU.calibrate(1);
msafwan3 0:808403b99108 131 IMU.calibrateMag(0);
msafwan3 1:de52879ff5ec 132 while(1){
msafwan3 1:de52879ff5ec 133 if(dist_flag==0){
msafwan3 1:de52879ff5ec 134 display_compass();
msafwan3 1:de52879ff5ec 135 }
msafwan3 1:de52879ff5ec 136
msafwan3 1:de52879ff5ec 137 }
msafwan3 1:de52879ff5ec 138 }
msafwan3 1:de52879ff5ec 139 void dcmotorBottom_fun(void const *args){//thread2
msafwan3 1:de52879ff5ec 140 while (1) {
msafwan3 1:de52879ff5ec 141 if(dist_flag ==0){
msafwan3 1:de52879ff5ec 142 for (float p=0.0; p<=0.9; p += 0.05) { //Throttle up slowly to full throttle
msafwan3 1:de52879ff5ec 143 dcmotorBottom = p;
msafwan3 1:de52879ff5ec 144 wait(1.0);
msafwan3 1:de52879ff5ec 145 }
msafwan3 1:de52879ff5ec 146 }
msafwan3 1:de52879ff5ec 147 if(dist_flag ==1){
msafwan3 1:de52879ff5ec 148 for (float p=0.9; p>=0.0; p -= 0.05) { //Throttle down slowly from full throttle
msafwan3 1:de52879ff5ec 149 dcmotorBottom = p;
msafwan3 1:de52879ff5ec 150 wait(1.0);
msafwan3 1:de52879ff5ec 151 }
msafwan3 1:de52879ff5ec 152 }
msafwan3 1:de52879ff5ec 153 }
msafwan3 1:de52879ff5ec 154 }
msafwan3 1:de52879ff5ec 155 void dist_flag1(void const *args){//thread3
msafwan3 1:de52879ff5ec 156 while(1) {
msafwan3 1:de52879ff5ec 157 if(dist_flag==1){
msafwan3 1:de52879ff5ec 158 for(int i=0; i<4; ++i) {
msafwan3 1:de52879ff5ec 159 //open wav file and play it
msafwan3 1:de52879ff5ec 160 FILE *wave_file;
msafwan3 1:de52879ff5ec 161 printf("\n\n\nHello, wave world!\n");
msafwan3 1:de52879ff5ec 162 wave_file=fopen("/sd/beep.wav","r");
msafwan3 1:de52879ff5ec 163 waver.play(wave_file);
msafwan3 1:de52879ff5ec 164 fclose(wave_file);
msafwan3 1:de52879ff5ec 165 Thread::wait(1925);
msafwan3 1:de52879ff5ec 166 }
msafwan3 1:de52879ff5ec 167 led1 =!led1;
msafwan3 1:de52879ff5ec 168 led2 =!led2;
msafwan3 1:de52879ff5ec 169 wait(0.25);
msafwan3 1:de52879ff5ec 170 }
msafwan3 1:de52879ff5ec 171
msafwan3 1:de52879ff5ec 172
msafwan3 1:de52879ff5ec 173 }//while
msafwan3 1:de52879ff5ec 174 }//fun
msafwan3 0:808403b99108 175
msafwan3 1:de52879ff5ec 176 void Blue_control(void const *args){//thread4
msafwan3 1:de52879ff5ec 177 float servo_p = 0.5;
msafwan3 1:de52879ff5ec 178 while(1) {
msafwan3 1:de52879ff5ec 179 if(bnum=='1'){
msafwan3 1:de52879ff5ec 180 dist_flag =0;
msafwan3 1:de52879ff5ec 181 }
msafwan3 1:de52879ff5ec 182 if(dist_flag==0){
msafwan3 1:de52879ff5ec 183 if(bnum =='2'){
msafwan3 1:de52879ff5ec 184 for(int i=0; i<4; ++i) {
msafwan3 1:de52879ff5ec 185 //open wav file and play it
msafwan3 1:de52879ff5ec 186 FILE *wave_file;
msafwan3 1:de52879ff5ec 187 printf("\n\n\nHello, wave world!\n");
msafwan3 1:de52879ff5ec 188 wave_file=fopen("/sd/honk.wav","r");
msafwan3 1:de52879ff5ec 189 waver.play(wave_file);
msafwan3 1:de52879ff5ec 190 fclose(wave_file);
msafwan3 1:de52879ff5ec 191 Thread::wait(1925);
msafwan3 1:de52879ff5ec 192 }
msafwan3 1:de52879ff5ec 193 }
msafwan3 1:de52879ff5ec 194 if(bnum=='3'){
msafwan3 1:de52879ff5ec 195 led1 =!led1;
msafwan3 1:de52879ff5ec 196 led2 =!led2;
msafwan3 1:de52879ff5ec 197 }
msafwan3 1:de52879ff5ec 198 if(bnum=='5'){
msafwan3 1:de52879ff5ec 199 for (float p=0.0; p<=0.3; p += 0.025) { //Throttle up slowly to full throttle
msafwan3 1:de52879ff5ec 200 dcmotorBack = p;
msafwan3 1:de52879ff5ec 201 wait(1.0);
msafwan3 1:de52879ff5ec 202 }
msafwan3 1:de52879ff5ec 203 }
msafwan3 1:de52879ff5ec 204 if(bnum=='6'){
msafwan3 1:de52879ff5ec 205 for (float p=0.3; p>=0.3; p -= 0.025) { //Throttle dowm slowly from full throttle
msafwan3 1:de52879ff5ec 206 dcmotorBack = p;
msafwan3 1:de52879ff5ec 207 wait(1.0);
msafwan3 1:de52879ff5ec 208 }
msafwan3 1:de52879ff5ec 209 }
msafwan3 1:de52879ff5ec 210 if(bnum=='7' && servo_p>0.0){
msafwan3 1:de52879ff5ec 211 servo_p = servo_p+0.05;
msafwan3 1:de52879ff5ec 212 servoBottom = servo_p;
msafwan3 1:de52879ff5ec 213 }
msafwan3 1:de52879ff5ec 214 if(bnum=='8' && servo_p<1.0){
msafwan3 1:de52879ff5ec 215 servo_p = servo_p-0.05;
msafwan3 1:de52879ff5ec 216 servoBottom = servo_p;
msafwan3 1:de52879ff5ec 217 }
msafwan3 1:de52879ff5ec 218 }
msafwan3 1:de52879ff5ec 219
msafwan3 1:de52879ff5ec 220 }//while(1)
msafwan3 1:de52879ff5ec 221 }//func
msafwan3 1:de52879ff5ec 222
msafwan3 1:de52879ff5ec 223
msafwan3 1:de52879ff5ec 224
msafwan3 1:de52879ff5ec 225
msafwan3 1:de52879ff5ec 226
msafwan3 1:de52879ff5ec 227 int main()//thread5
msafwan3 1:de52879ff5ec 228 {
msafwan3 1:de52879ff5ec 229 mu.startUpdates();//start measuring the distance
msafwan3 1:de52879ff5ec 230 col_ESC();
msafwan3 1:de52879ff5ec 231 led1=0;
msafwan3 1:de52879ff5ec 232 led2=0;
msafwan3 1:de52879ff5ec 233 Thread t1(IMU_fun); //start thread1
msafwan3 1:de52879ff5ec 234 Thread t2(dcmotorBottom_fun); //start thread2
msafwan3 1:de52879ff5ec 235 Thread t3(dist_flag1); //start thread2
msafwan3 1:de52879ff5ec 236 Thread t4(Blue_control); //start thread2
msafwan3 1:de52879ff5ec 237 char bhit=0;//for blue
msafwan3 1:de52879ff5ec 238 while(1)
msafwan3 1:de52879ff5ec 239 {
msafwan3 0:808403b99108 240 mu.checkDistance();
msafwan3 1:de52879ff5ec 241 my_mutex.lock();
msafwan3 1:de52879ff5ec 242 if (blue.getc()=='!') {
msafwan3 1:de52879ff5ec 243 if (blue.getc()=='B') { //button data packet
msafwan3 0:808403b99108 244 bnum = blue.getc(); //button number
msafwan3 0:808403b99108 245 bhit = blue.getc(); //1=hit, 0=release
msafwan3 1:de52879ff5ec 246 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
msafwan3 1:de52879ff5ec 247 my_mutex.unlock();
msafwan3 1:de52879ff5ec 248 myleds = bnum - '0'; //current button number will appear on LEDs
msafwan3 1:de52879ff5ec 249
msafwan3 0:808403b99108 250 }
msafwan3 0:808403b99108 251 }
msafwan3 0:808403b99108 252 }
msafwan3 1:de52879ff5ec 253 }//for while(1)
msafwan3 1:de52879ff5ec 254 }//for int main()