main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 #define BUFF_SIZE 200
00005 
00006 
00007 Serial pc(USBTX, USBRX);
00008 DigitalOut myled(LED1);
00009 DigitalOut m (LED4);
00010 InterruptIn Msg(USBRX,PullUp);
00011 m3pi pi;
00012 int flag;
00013 char clavier;
00014 char buffer[BUFF_SIZE];
00015 
00016 void readBattery()
00017 {
00018     char s[500];
00019     char* s2 = "Battery:%f V\n";
00020     sprintf(s, s2, pi.battery());
00021     for (int i=0; i<strlen(s); i++) {
00022         pc.putc(s[i]);
00023     }
00024     pc.printf("\r");
00025 }
00026 
00027 void readSensors()
00028 {
00029     char s[500];
00030     char* s2 = "Sensors:(%u,%u,%u,%u,%u) \n";
00031     unsigned short val[5];
00032     pi.calibrated_sensors(val);
00033     sprintf(s, s2, val[0],val[1],val[2],val[3],val[4]);
00034     for (int i=0; i<strlen(s); i++) {
00035         pc.putc(s[i]);
00036     }
00037     pc.printf("\r");
00038 }
00039 
00040 void Interrupt()
00041 {
00042     //printf("%c",pc.getc());
00043     //Command(pc.getc());
00044     flag=1;
00045     m=1;
00046 }
00047 
00048 void Command(char c){
00049     switch (c) {
00050         case 'z':
00051             pi.forward(0.5);
00052             wait(0.5);
00053             pi.stop();
00054             break;
00055         case 'q':
00056             pi.left(0.5);
00057             wait(0.5);
00058             pi.stop();
00059             break;
00060         case 's':
00061             pi.backward(0.5);
00062             wait(0.5);
00063             pi.stop();
00064             break;
00065         case 'd':
00066             pi.right(0.5);
00067             wait(0.5);
00068             pi.stop();
00069             break;
00070         case 'b':
00071             readBattery();
00072             break;
00073         case 'c':
00074             readSensors();
00075             break;
00076     }
00077 }
00078 
00079 int main()
00080 {
00081     pc.attach(&Interrupt);
00082     pc.baud(9600);
00083     pi.sensor_auto_calibrate();
00084     //clavier='a';
00085 
00086     //flag = 0;
00087     while(1) {
00088         ///BLINK LEDS
00089         myled = 1;
00090         wait(0.1);
00091         myled = 0;
00092         wait(0.1);
00093         
00094         ///GET INPUT
00095         int i = 0;
00096         while (pc.readable()) {
00097             if (i < BUFF_SIZE)
00098                 buffer[i++] = pc.getc();
00099         }
00100         while (i > 0) {
00101             Command(buffer[i]);
00102             pi.cls();
00103             pi.locate(0,0);
00104             pi.print(&buffer[i], 1);
00105             i--;
00106         }
00107 
00108         /*
00109         if(flag) {
00110             clavier+=1;
00111             pi.print(&clavier,1);
00112             //clavier = pc.getc();
00113             
00114             switch (clavier) {
00115                 case 'z':
00116                     pi.forward(0.5);
00117                     wait(0.5);
00118                     pi.stop();
00119                     break;
00120                 case 'q':
00121                     pi.left(0.5);
00122                     wait(0.5);
00123                     pi.stop();
00124                     break;
00125                 case 's':
00126                     pi.backward(0.5);
00127                     wait(0.5);
00128                     pi.stop();
00129                     break;
00130                 case 'd':
00131                     pi.right(0.5);
00132                     wait(0.5);
00133                     pi.stop();
00134                     break;
00135                 case 'b':
00136                     readBattery();
00137                     break;
00138                 case 'c':
00139                     readSensors();
00140                     break;
00141             }
00142             flag=0;
00143         }
00144         */
00145     }
00146 }
00147