main.cpp

00001 #include "Motor.h"
00002 #include "mbed.h"
00003 #include "emic2.h"
00004 #include "XNucleo53L0A1.h"
00005 #include <stdio.h>
00006 #include "rtos.h"
00007 
00008 
00009 Serial pc(USBTX, USBRX);
00010 DigitalOut shdn(p26);
00011 AnalogOut led(p18);
00012 //I2C sensor pins
00013 #define VL53L0_I2C_SDA   p28
00014 #define VL53L0_I2C_SCL   p27
00015 
00016 
00017 static XNucleo53L0A1 *board=NULL;
00018 emic2 myTTS(p13, p14);
00019 Serial xbee1(p9, p10);
00020 DigitalOut rst1(p11);
00021 
00022 Motor B(p23, p6, p5); // pwm, fwd, rev, can brake, right
00023 Motor A(p24, p7, p8); // left motor
00024 
00025 char input;
00026 int counter;
00027 int direction;
00028 int main() {
00029     //initialize distance sensor
00030     int status;
00031     uint32_t distance;
00032     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00033     /* creates the 53L0A1 expansion board singleton obj */
00034     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
00035     shdn = 0; //must reset sensor for an mbed reset to work
00036     wait(0.1);
00037     shdn = 1;
00038     wait(0.1);
00039     /* init the 53L0A1 board with default values */
00040     status = board->init_board();
00041     while (status) {
00042         pc.printf("Failed to init board! \r\n");
00043         status = board->init_board();
00044     }
00045     
00046     
00047     
00048     counter = 0;
00049     // reset the xbees (at least 200ns)
00050     rst1 = 0;
00051     wait_ms(1); 
00052     rst1 = 1;
00053     wait_ms(1); 
00054     input = 'I';
00055     
00056     A.speed(0);
00057     B.speed(0);
00058     wait(2);
00059     direction = 1;
00060     while(1) {
00061         if(xbee1.readable()) {
00062             input = xbee1.getc();
00063             wait(0.2);
00064         }
00065         status = board->sensor_centre->get_distance(&distance);
00066         if (status == VL53L0X_ERROR_NONE) {
00067             pc.printf("D=%ld mm\r\n", distance);
00068             
00069         }
00070         
00071         while (distance < 200) {
00072                 direction = rand()*100;
00073                 if (direction > 50)
00074                 {
00075                     A.speed(-0.5);
00076                     B.speed(-0.5);
00077                     led = 0;
00078                     wait(0.1);
00079                 } else
00080                 {
00081                     
00082                     A.speed(0.5);
00083                     B.speed(0.5);
00084                     led = 1;
00085                     wait(0.1);
00086                 }
00087                 status = board->sensor_centre->get_distance(&distance);
00088                 led = 0;
00089                 A.speed(0);
00090                 B.speed(0);
00091         }
00092         if (input == 'A')
00093         {
00094             //myTTS.speakf("S");//Speak command starts with "S"
00095             //myTTS.speakf("forward");
00096             //myTTS.speakf("\r"); //marks end of speak command
00097             //myTTS.ready();
00098             A.speed(-0.5);
00099             B.speed(0.5);
00100             wait(0.2);
00101         } else if (input == 'B')
00102         {
00103             //myTTS.speakf("S");//Speak command starts with "S"
00104             //myTTS.speakf("back");
00105             //myTTS.speakf("\r"); //marks end of speak command
00106             //myTTS.ready();
00107             A.speed(0.5);
00108             B.speed(-0.5);
00109             wait(0.2);
00110         } else if (input == 'C')
00111         {
00112             myTTS.speakf("S");//Speak command starts with "S"
00113             myTTS.speakf("left");
00114             myTTS.speakf("\r"); //marks end of speak command
00115             myTTS.ready();
00116             A.speed(0.5);
00117             B.speed(0.5);
00118             wait(0.2);
00119         } else if (input == 'D')
00120         {
00121             myTTS.speakf("S");//Speak command starts with "S"
00122             myTTS.speakf("right");
00123             myTTS.speakf("\r"); //marks end of speak command
00124             myTTS.ready();
00125             A.speed(-0.5);
00126             B.speed(-0.5);
00127             wait(0.2);
00128         } else if (input == 'E')
00129         {
00130             myTTS.speakf("S");//Speak command starts with "S"
00131             myTTS.speakf("stop");
00132             myTTS.speakf("\r"); //marks end of speak command
00133             myTTS.ready();
00134             A.speed(0);
00135             B.speed(0);
00136             wait(0.2);
00137         }
00138     }
00139 }