Diff: main.cpp

Revision:

0:3ec03802c957

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 26 14:27:52 2019 +0000
@@ -0,0 +1,226 @@
+#include "mbed.h"
+#include "ultrasonic.h"
+#include "motordriver.h"
+#include "rtos.h"
+
+#define STOP_DISTANCE 250
+#define RADAR_SPEED 0.5
+#define RADAR_WAIT 0.07
+#define pi 3.14159265f
+
+/* LEDS TO SHOW WHICH MODE ROBOT IS IN */
+DigitalOut led_left(LED1);
+DigitalOut led_center_left(LED2);
+DigitalOut led_center_right(LED3);
+DigitalOut led_right(LED4);
+
+AnalogIn lightSensor(p20);
+DigitalOut headlights(p30);
+
+Serial hc05_slave(p9,p10);
+Serial pc(USBTX, USBRX);
+
+Motor LeftMotor(p21,p15,p16,1); //left motor
+Motor RightMotor(p22,p17,p19,1); //right motor
+
+volatile int mode = 0;
+
+volatile float Lspeed_rx = 0;
+volatile float Rspeed_rx = 0;
+volatile float Fspeed_rx = 0;
+volatile float Bspeed_rx = 0;
+
+void collision_protocol (int distance);
+
+ultrasonic usSensor(p6, p5, 0.050f, 1, &collision_protocol);
+
+void collision_protocol (int distance) {
+    
+    if ((distance < STOP_DISTANCE) && (distance > 50)) {
+        mode = 1;
+        
+        LeftMotor.speed(0); RightMotor.speed(0);
+        Timer ack_timer;
+        /* pc.printf("TOO CLOSE\r\n");
+        pc.printf("\r\nListening on Bluetooth...\r\n"); */
+    
+        float angle;
+        
+        hc05_slave.printf("rXX1,0.101,0.101");   /* SEND MAGIC COMMAND TO CONTROLLER */
+        /* pc.printf("\r\nSENT\r\n"); */
+        
+        LeftMotor.speed(-1); RightMotor.speed(-1);
+        Thread::wait(400);                /* BACK UP TO AVOID BEING SET OFF AGAIN */
+        LeftMotor.speed(0); RightMotor.speed(0);
+        
+        int x = 0; int dist = 0;
+        int idx; char *ack;
+        char response_buffer[25];
+        memset(response_buffer, NULL, 25);                           /* ZERO INIT */
+        ack_timer.start();
+        while(x < 60) {
+            
+            idx = 0; ack = NULL;                               /* ACK --> NULLPTR */
+            memset(response_buffer, NULL, 25);                    /* RESET BUFFER */
+            
+            while(hc05_slave.readable() && idx < 25){     /* FILL RESPONSE BUFFER */
+                response_buffer[idx] = hc05_slave.getc();
+                idx++;
+            }  
+                
+            /* pc.printf("\r\nRESPONSE: %s\r\n", response_buffer); */
+        
+            ack = strstr(response_buffer, "_ack_");                   /* FIND ACK */
+        
+            if(ack != NULL) {                                     /* ACK RECIEVED */
+                pc.printf("Acknowledged - Sending reading %d\r\n",x);
+                angle = ((2*pi)/60) * x;    /* add pi/2 */
+                dist = usSensor.getCurrentDistance(); 
+                if(dist >= 5000) { dist = 4999; }
+                /* pc.printf("1,%.3f,%.3f\r\n", (((float) dist)/500), angle); */
+                hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle);
+                
+                LeftMotor.speed(RADAR_SPEED); RightMotor.speed(-1*RADAR_SPEED);
+                Thread::wait(RADAR_WAIT * 1000);    /* TURN TO NEXT READING ANGLE */
+                LeftMotor.speed(0); RightMotor.speed(0);
+                
+                ack_timer.reset(); x++;
+            } else {                                              /* WAIT FOR ACK */
+                pc.printf("Waiting for ack...\r\n");
+                
+                if(ack_timer.read_ms() > 2000) {  /* RETRANSMIT IF ACK NOT RECV'D */
+                    hc05_slave.printf("r%02d1,%02.3f,%.3f", x,(((float) dist)/500), angle);
+                    ack_timer.reset(); /* pc.printf("Retransmitting...\r\n"); */
+                } led_center_left = led_center_right = 1;
+            }      
+            led_center_left = led_center_right = 1;
+            Thread::wait(100);
+            led_center_left = led_center_right = 0;
+        } mode = 0;                                                /* RESET MODE */
+    }
+}
+
+void coll_start () {
+    
+    usSensor.startUpdates();
+    Thread::wait(2000);
+    while(1) {
+        if (!mode) {
+            usSensor.checkDistance();
+            /* pc.printf("%d", dist); */
+            Thread::wait(50);
+        }
+    }
+}
+
+void headlight_switch () {
+    
+    headlights = 0;
+    
+    while (1) {
+        if (lightSensor.read() < 0.09) {
+            headlights = 1;
+        } else if (lightSensor > 0.105) {
+            headlights = 0;
+        }
+    }
+}
+
+void read_speeds () {
+    
+    char speed_buffer[11];
+    char Lspeed_buff[5];
+    char Rspeed_buff[5];
+    char Fspeed_buff[5];
+    char Bspeed_buff[5];
+    char c;
+    
+    while (1) {
+        
+        if (hc05_slave.readable() && !mode) {
+            
+            memset(speed_buffer, NULL, 11);
+            
+            c = hc05_slave.getc();
+            if((c == 'v') || (c == 'h')) {
+                int i = 0;
+                while (i < 11){
+                    if(hc05_slave.readable()) {
+                        speed_buffer[i] = hc05_slave.getc();
+                        i++;
+                    }
+                }
+                
+                if (c == 'h') {
+                    
+                    for (int j = 0; j < 5; j++) {
+                        Lspeed_buff[j] = speed_buffer[j];
+                    }
+                
+                    for (int j = 0; j < 5; j++) {
+                        Rspeed_buff[j] = speed_buffer[j+6];
+                    }
+                    
+                    Lspeed_rx = atof(Lspeed_buff);
+                    Rspeed_rx = atof(Rspeed_buff);
+                    
+                } else if (c == 'v') {
+                
+                    for (int j = 0; j < 5; j++) {
+                        Fspeed_buff[j] = speed_buffer[j];
+                    }
+                    
+                    for (int j = 0; j < 5; j++) {
+                        Bspeed_buff[j] = speed_buffer[j+6];
+                    }
+                    
+                    Fspeed_rx = atof(Fspeed_buff);
+                    Bspeed_rx = atof(Bspeed_buff);
+                    
+                }
+                
+                if (Fspeed_rx > 0.0f) {
+                    
+                    //pc.printf("forward\r\n");
+                    //pc.printf("LF: %f, RF: %f\r\n", (Fspeed_rx * 0.5f + Rspeed_rx * 0.4), Fspeed_rx * 0.5f + Lspeed_rx * 0.4f);
+                    LeftMotor.speed(Fspeed_rx * 0.6f + Rspeed_rx * 0.4f);
+                    RightMotor.speed(Fspeed_rx * 0.6f + Lspeed_rx * 0.4f);
+                    
+                } else if (Bspeed_rx > 0.0f) {
+                    
+                    //pc.printf("reverse\r\n");
+                    //pc.printf("LF: %f, RF: %f\r\n", 0-(Bspeed_rx * 0.5f + Rspeed_rx * 0.4), 0-(Bspeed_rx * 0.5f + Lspeed_rx * 0.4f));
+                    LeftMotor.speed(0-(Bspeed_rx * 0.5f + Rspeed_rx * 0.4f));
+                    RightMotor.speed(0-(Bspeed_rx * 0.5f + Lspeed_rx * 0.4f));
+                    
+                } else {
+                    
+                    //pc.printf("turn only\r\n");
+                    //pc.printf("LF: %f, RF: %f\r\n", Rspeed_rx * 0.4f, Lspeed_rx * 0.4f);
+                    LeftMotor.speed(Rspeed_rx * 0.4f);
+                    RightMotor.speed(Lspeed_rx * 0.4f);
+                    
+                }
+            }
+        }
+    }
+}
+
+int main() {
+    
+    hc05_slave.baud(115200);
+    pc.baud(115200);
+    
+    Thread thread2(&read_speeds);
+    Thread thread3(&coll_start);
+    Thread thread4(&headlight_switch);
+    
+    while(1) {
+        if(!mode) {
+            led_left = led_right = 1;
+            wait(0.2);
+            led_left = led_right = 0;
+            wait(0.2);
+        }
+    }
+}