Mbed project to control a hoverboard
Dependencies: mbed wave_player Servo 4DGL-uLCD-SE LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 2:726636c1c83c
- Parent:
- 1:de52879ff5ec
--- a/main.cpp Wed Apr 29 08:33:11 2020 +0000
+++ b/main.cpp Wed Apr 29 22:40:56 2020 +0000
@@ -5,27 +5,64 @@
#include "ultrasonic.h"
#include "uLCD_4DGL.h"
#include "LSM9DS1.h"
-#include "rtos.h"
+
+volatile bool global_running = true; //1 = display distance, 0 = avoid collision
+float range = 0.0005; // for servo/motors calibration
+float position = 0.5; // for servo/motors calibration
+unsigned int frame_limiter = 0; // Limits the framerate of the lcd
Serial pc(USBTX, USBRX);
uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
-DigitalOut led1(p26);
-DigitalOut led2(p19);
-LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+
Servo dcmotorBottom(p21); // pwm
+Servo servoBottom(p23); // pwm
Servo dcmotorBack(p22); // pwm
-Servo servoBottom(p23); // pwm
+
+// Setup to play wav file from SD Card
AnalogOut DACout(p18);
wave_player waver(&DACout);
SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
+
+// Setup for bluetooth
Serial blue(p28,p27);
-BusOut myleds(LED1,LED2,LED3,LED4);
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+DigitalOut exled1(p19);
+DigitalOut exled2(p26);
+
-char bnum=0;//for blue
-Mutex my_mutex;
-int dist_flag = 0;
+// Setup for IMU
+#define PI 3.14159
+// Earth's magnetic field varies by location. Add or subtract
+// a declination to get a more accurate heading. Calculate
+// your's here:
+// http://www.ngdc.noaa.gov/geomag-web/#declination
+#define DECLINATION -4.94*PI/180 // Declination (radians) in Atlanta,GA.
+// Calculate heading.
+// Heading calculations taken from this app note:
+// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
+float calculateHeading(float mx, float my)
+{
+// touchy trig stuff to use arctan to get compass heading
+ mx = -mx;
+ float heading;
+ if (my == 0.0)
+ heading = (mx < 0.0) ? PI : 0.0;
+ else
+ heading = atan2(mx, my);
+ //pc.printf("heading atan=%f \n\r",heading);
+ heading -= DECLINATION; //correct for geo location
+ if(heading>PI) heading = heading - 2*PI;
+ else if(heading<-PI) heading = 2*PI + heading;
+ else if(heading<0.0) heading = 2*PI + heading;
+ return heading;
+}
+// ESC/ Brushless DC Motor setup
void col_ESC() {
dcmotorBottom = 0.0;
dcmotorBack = 0.0;
@@ -40,215 +77,229 @@
wait(8);
}
- void dist(int distance)
-{
- while(1){
- //put code here to execute when the distance has changed
- printf("Distance %d mm\r\n", distance);
- if(distance <50){
- dist_flag=1;
+
+void dist(int distance)
+{
+ // Set global running flag based on distance to sonar
+ if(distance < 100){
+ global_running = false;
+ }
+ else{
+ if (global_running == false){
+ global_running = true;
+ // Wait 2 seconds before starting again
+ //wait(2);
}
}
+ // If there is an obstruction, print the distance until it is clear
+ if(!global_running){
+ // Only update once every 4 loops
+ if (frame_limiter % 2 == 0){
+ uLCD.cls();
+ uLCD.printf("Distance:\r\n%dmm\r\n", distance); //print to uLCD
+ }
+ frame_limiter++;
+ }
+
}
-ultrasonic mu(p15, p16, .1, 1, &dist); //sonar
-#define PI 3.14159
-#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
-double x2;
-double y2;
-int x_c;
-int y_c;
-int r;
-int z;
-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;
-
+ultrasonic mu(p15, p16, .1, 1, &dist);
+//have updates every .1 seconds and a timeout after 1
+//second, and call dist when the distance changes
- // 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);
+void honk(){
+ FILE *wave_file;
+ wave_file=fopen("/sd/honk.wav","r");
+ waver.play(wave_file);
+ fclose(wave_file);
+ myled1=!myled1;
+ myled2=!myled2;
+ myled3=!myled3;
+ myled4=!myled4;
}
-void IMU_fun(void const *args){//thread 1
- x_c = 60;
- y_c = 60;
- r = 30;
- uLCD.circle(x_c, y_c, r, WHITE);
- z = 0;
- x2 = x_c + r*cos((double)z);
- y2 = x_c + r*sin((double)z);
- uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
-
- IMU.begin();
- if (!IMU.begin()) {
- pc.printf("Failed to communicate with LSM9DS1.\n");
- }
- IMU.calibrate(1);
- IMU.calibrateMag(0);
- while(1){
- if(dist_flag==0){
- display_compass();
- }
-
- }
-}
-void dcmotorBottom_fun(void const *args){//thread2
- while (1) {
- if(dist_flag ==0){
- for (float p=0.0; p<=0.9; p += 0.05) { //Throttle up slowly to full throttle
- dcmotorBottom = p;
- wait(1.0);
- }
- }
- if(dist_flag ==1){
- for (float p=0.9; p>=0.0; p -= 0.05) { //Throttle down slowly from full throttle
- dcmotorBottom = p;
- wait(1.0);
- }
- }
- }
-}
-void dist_flag1(void const *args){//thread3
- while(1) {
- if(dist_flag==1){
- for(int i=0; i<4; ++i) {
- //open wav file and play it
- FILE *wave_file;
- printf("\n\n\nHello, wave world!\n");
- wave_file=fopen("/sd/beep.wav","r");
- waver.play(wave_file);
- fclose(wave_file);
- Thread::wait(1925);
- }
- led1 =!led1;
- led2 =!led2;
- wait(0.25);
- }
-
-
- }//while
-}//fun
-
-void Blue_control(void const *args){//thread4
- float servo_p = 0.5;
- while(1) {
- if(bnum=='1'){
- dist_flag =0;
- }
- if(dist_flag==0){
- if(bnum =='2'){
- for(int i=0; i<4; ++i) {
- //open wav file and play it
- FILE *wave_file;
- printf("\n\n\nHello, wave world!\n");
- wave_file=fopen("/sd/honk.wav","r");
- waver.play(wave_file);
- fclose(wave_file);
- Thread::wait(1925);
- }
- }
- if(bnum=='3'){
- led1 =!led1;
- led2 =!led2;
- }
- if(bnum=='5'){
- for (float p=0.0; p<=0.3; p += 0.025) { //Throttle up slowly to full throttle
- dcmotorBack = p;
- wait(1.0);
- }
- }
- if(bnum=='6'){
- for (float p=0.3; p>=0.3; p -= 0.025) { //Throttle dowm slowly from full throttle
- dcmotorBack = p;
- wait(1.0);
- }
- }
- if(bnum=='7' && servo_p>0.0){
- servo_p = servo_p+0.05;
- servoBottom = servo_p;
- }
- if(bnum=='8' && servo_p<1.0){
- servo_p = servo_p-0.05;
- servoBottom = servo_p;
- }
- }
-
- }//while(1)
-}//func
-
-
-int main()//thread5
-{
- mu.startUpdates();//start measuring the distance
+int main()
+{
+ uLCD.reset();
+ //calibrate motors
+ servoBottom.calibrate(range, position);
col_ESC();
- led1=0;
- led2=0;
- Thread t1(IMU_fun); //start thread1
- Thread t2(dcmotorBottom_fun); //start thread2
- Thread t3(dist_flag1); //start thread2
- Thread t4(Blue_control); //start thread2
- char bhit=0;//for blue
- while(1)
- {
+
+ Ticker honk_int;
+
+ char bnum = 0;
+ char bhit = 0;
+ float speed = 0.3;
+ uLCD.baudrate(BAUD_3000000); //jack up baud rate to max for fast display
+ wait(0.5);
+
+ IMU.begin();
+ if (!IMU.begin()) {
+ // red circle of death
+ uLCD.circle(63,63, 51, RED);
+ }
+ IMU.calibrate(1);
+ uLCD.cls();
+ uLCD.printf("Calibrating:\r\nSpin Me!\r\n"); //print to uLCD
+ IMU.calibrateMag(0);
+ uLCD.cls();
+
+ int x=0;
+ int y=50;
+ mu.startUpdates();//start measuring the distance
+ bool start_motor = true;
+ bool moving = false;
+
+
+ while(1)
+ {
+ // Check for collision
mu.checkDistance();
- my_mutex.lock();
- 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?
- my_mutex.unlock();
- myleds = bnum - '0'; //current button number will appear on LEDs
-
- }
+ // If the motor is stopped and there are no collisions, start the motor
+ if (global_running && start_motor){
+ for (float p=0.0; p<=0.9; p += 0.1) { //Throttle up slowly to full throttle
+ dcmotorBottom = p;
+ wait(0.2);
+ }
+ // Draw the circle for the compass on the LCD
+ uLCD.cls();
+ uLCD.circle(63,63, 51, WHITE);
+ start_motor = false;
+
+ }
+ if (global_running){
+ // Update the compass reading
+ // Only update once every 150000 loops
+ if (frame_limiter % 100000 == 0){
+ if(IMU.magAvailable(X_AXIS)){
+ IMU.readMag();
+ int prev_x = x;
+ int prev_y = y;
+ float heading = calculateHeading(IMU.mx, IMU.my);
+ x = 63+50*sin(heading+PI)+ 0.5;
+ y = 63+50*cos(heading+PI)+ 0.5;
+ uLCD.line(63, 63, prev_x, prev_y, BLACK);
+ uLCD.line(63, 63, x, y, RED);
+ uLCD.printf("%.2f\r", heading*180/PI);
}
}
- }//for while(1)
-}//for int main()
+ frame_limiter++;
+ // Read bluetooth controller input
+ if (blue.readable()){
+ 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?
+ switch (bnum) {
+ case '1': //number button 1: Increase max speed
+ if (bhit=='1') {
+ //add hit code here
+ if(speed < 0.3){
+ speed = speed + 0.06;
+ }
+ } else {
+ // When button is released, set rear motor to max speed
+ // if it is currently running
+ if(moving) dcmotorBack = speed;
+ }
+ break;
+ case '2': //number button 2: Decrease max speed
+ if (bhit=='1') {
+ //add hit code here
+ if(speed > 0.06){
+ speed = speed - 0.06;
+ }
+ } else {
+ // When button is released, set rear motor to max speed
+ // if it is currently running
+ if(moving) dcmotorBack = speed;
+ }
+ break;
+ case '3': //number button 3: light the external leds
+ if (bhit=='1') {
+ exled1 = !exled1;
+ exled2 = !exled2;
+ } else {
+ //add release code here
+ }
+ break;
+ case '4': //number button 4: play honking sound
+ if (bhit=='1') {
+ honk();
+ } 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.0; p<=speed; p += 0.06) { //Throttle up slowly to full throttle
+ dcmotorBack = p;
+ wait(0.2);
+ }
+ moving = true;
+
+ } else {
+ //add release code here
+ dcmotorBack = 0.0;
+ moving = false;
+ }
+ break;
+ case '6': //button 6 down arrow: switch it off (turn off back motor)
+ if (bhit=='1') {
+
+ } else {
+ //add release code here
+ }
+ break;
+ case '7': //button 7 left arrow: turns servo leftwards
+ if (bhit=='1') {
+ servoBottom = 1.0;
+ } else {
+ //add release code here
+ servoBottom = 0.5;
+ }
+ break;
+ case '8': //button 8 right arrow: turns servo rightwards
+ if (bhit=='1') {
+ servoBottom = 0.0;
+ } else {
+ //add release code here
+ servoBottom = 0.5;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }// End of bluetooth controller loop
+ }
+ }
+ else
+ {
+ // Collision detection: Stop all motors and honk until obstruction is cleared
+ dcmotorBack = 0.0;
+ dcmotorBottom = 0.0;
+ servoBottom = 0.5;
+ start_motor = true;
+ moving = false;
+ unsigned int honk_limiter = 0;
+ while(!global_running){
+ // Check if the obstruction is clear
+ mu.checkDistance();
+ // Honk every once in a while
+ if(honk_limiter % 6000000 == 0){
+ honk();
+ }
+ honk_limiter++;
+
+ }
+
+ }
+
+ }
+}
\ No newline at end of file