Dependencies: mbed wave_player Servo 4DGL-uLCD-SE LSM9DS1_Library_cal HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 1:de52879ff5ec
- Parent:
- 0:808403b99108
- Child:
- 2:726636c1c83c
diff -r 808403b99108 -r de52879ff5ec main.cpp
--- a/main.cpp Tue Apr 28 20:15:24 2020 +0000
+++ b/main.cpp Wed Apr 29 08:33:11 2020 +0000
@@ -1,73 +1,65 @@
#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
+#include "rtos.h"
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 led1(p26);
DigitalOut led2(p19);
-
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
Servo dcmotorBottom(p21); // pwm
-Servo servoBottom(p22); // pwm
-Servo dcmotorBack(p23); // pwm
-
-// Setup to play wav file from SD Card
-AnalogOut DACout(p26);
+Servo dcmotorBack(p22); // pwm
+Servo servoBottom(p23); // pwm
+AnalogOut DACout(p18);
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);
+BusOut myleds(LED1,LED2,LED3,LED4);
+
+char bnum=0;//for blue
+Mutex my_mutex;
+int dist_flag = 0;
-void playSound(char * wav)
-{
- FILE *wave_file;
- wave_file = fopen(wav,"r");
- waver.play(wave_file);
- fclose(wave_file);
+void col_ESC() {
+ dcmotorBottom = 0.0;
+ dcmotorBack = 0.0;
+ wait(0.5); //ESC detects signal
+ //Required ESC Calibration/Arming sequence
+ //sends longest and shortest PWM pulse to learn and arm at power on
+ dcmotorBottom = 1.0; //send longest PWM
+ dcmotorBack = 1.0; //send longest PWM
+ wait(8);
+ dcmotorBottom = 0.0; //send shortest PWM
+ dcmotorBack = 0.0; //send shortest PWM
+ wait(8);
}
-void bottom_motor_on()
+ void dist(int distance)
{
- 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;
+ 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;
}
}
}
-
+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);
@@ -121,136 +113,142 @@
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;
+void IMU_fun(void const *args){//thread 1
+ x_c = 60;
+ y_c = 60;
+ 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);
+ 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);
- 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);
+ 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
- //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;
+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
+ 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)
+ {
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
+ 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?
- 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;
+ if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+ my_mutex.unlock();
+ myleds = bnum - '0'; //current button number will appear on LEDs
+
}
}
}
- }
- }
-}
\ No newline at end of file
+ }//for while(1)
+}//for int main()