main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "motordriver.h"
00004 #include "ultrasonic.h"
00005 #include <stdio.h>
00006 Serial pc(USBTX,USBRX);
00007 Serial blue(p13,p14);
00008 DigitalOut myled(LED1);      
00009 
00010 
00011 #define AUTOPILOT 10
00012 #define FORWARD 1
00013 #define REVERSE -1
00014 #define STOP 0
00015 
00016 int state = 0; //global variable stop state 
00017 int status1;
00018 int status2;
00019 int autoPilotLock;
00020 int distanceCenter;
00021 int distanceLeft;
00022 int distanceRight;
00023 Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake 
00024 Servo S(p24);
00025 
00026 
00027 void getNewState() {
00028     //Logic for AdaFruit App
00029     char bnum=0;
00030     char bhit=0;
00031     if (blue.readable()) {
00032     if (blue.getc()=='!') {
00033         if (blue.getc()=='B') { //button data packet
00034             bnum = blue.getc(); //button number
00035             bhit = blue.getc(); //1=hit, 0=release
00036             if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
00037                 myled = bnum - '0'; //current button number will appear on LEDs
00038                 switch (bnum) {
00039                     case '1': //AutoPilot Mode
00040                         if (bhit=='1') {
00041                             state = 10; //autopilot state
00042                         }
00043                         break;
00044                     case '5': //forward
00045                         if (bhit=='1') {
00046                             state = 1; //forward state
00047                         } else {
00048                             state = 0; //stop state
00049                         }
00050                         break;
00051                     case '6': //reverse
00052                         if (bhit=='1') {
00053                             state = -1; //reverse state
00054                         } else {
00055                             state = 0; //stop state
00056                         }
00057                         break;
00058                     case '7': //left
00059                         if (bhit=='1') {
00060                             S = S + 0.5; //turn left                                
00061                         }
00062                         break;
00063                     case '8': //right
00064                         if (bhit=='1') {
00065                             S = S - 0.5; //turn right
00066                         }
00067                         break;
00068                     }
00069                 }
00070             }
00071         }
00072     }
00073 }
00074 
00075 void autoPilot() {
00076     autoPilotLock = 0;
00077     if (distanceCenter >= 200 && autoPilotLock == 0) {
00078         M.speed(1.0);
00079         S = 0.5;
00080     }
00081     else if (distanceRight > distanceLeft) { //More space on right so turn right
00082         autoPilotLock = 1;
00083         M.speed(-1.0);
00084         S = 0;
00085         wait_ms(1000);
00086         autoPilotLock = 0;
00087     }
00088     else { //or turn left
00089         autoPilotLock = 1;
00090         M.speed(-1.0);
00091         S = 1.0;
00092         wait_ms(1000);
00093         autoPilotLock = 0;
00094     }
00095 }
00096 
00097 void dist1(int distance)
00098 {
00099     distanceCenter = distance;
00100 }
00101 
00102 void dist2(int distance)
00103 {
00104     distanceLeft = distance;
00105 }
00106 
00107 void dist3(int distance)
00108 {
00109     distanceRight = distance;
00110 }
00111 
00112 ultrasonic muCenter(p6, p7, .1, 1, &dist1); //sonar 1 initialization
00113 ultrasonic muLeft(p17, p18, .1, 1, &dist2); //sonar 2 initialization
00114 ultrasonic muRight(p15, p16, .1, 1, &dist3); //sonar 3 initialization
00115 
00116 
00117 
00118 int main() {
00119     //SONAR Initializations
00120     muCenter.startUpdates();//SONAR 1 starts measuring the distance 
00121     muLeft.startUpdates();//SONAR 2 starts measuring the distance 
00122     muRight.startUpdates();//SONAR 3 starts measuring the distance 
00123     while(1) { //main loop for motor control
00124         pc.printf("Center D=%ld mm\r\n", distanceCenter);
00125         pc.printf("Right D=%ld mm\r\n", distanceRight);
00126         pc.printf("Left D=%ld mm\r\n", distanceLeft);
00127         muCenter.checkDistance();  //SONAR measuring starts
00128         muLeft.checkDistance();
00129         muRight.checkDistance();
00130         getNewState();
00131         if (distanceCenter >= 250 && state == FORWARD) {
00132             M.speed(1.0);
00133         }
00134         else if (state == REVERSE) {
00135             M.speed(-1.0);
00136         }
00137         else if (state == AUTOPILOT) {
00138             autoPilot();
00139         }
00140         else {
00141             M.speed(0);
00142         }
00143     }
00144     
00145 }