main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "Servo.h"
00004 #include "string"
00005 
00006 #define SERVO_MOVE 0.005
00007 
00008 DigitalOut myled(LED1);
00009 AnalogIn light(p18);
00010  
00011 RawSerial  pc(USBTX, USBRX);
00012 RawSerial  dev(p13,p14);
00013 DigitalOut l1(p11);
00014 DigitalOut l2(p12);
00015 DigitalOut l3(p10);
00016 DigitalOut led1(LED1);
00017 //DigitalOut led4(LED4);
00018  
00019 Motor m1(p21, p5, p6);
00020 Motor m2(p22, p7, p8);
00021 
00022 Servo horiz(p24);
00023 Servo vert(p23);
00024 
00025 Timeout flipper;
00026 
00027 void flip() {
00028     printf("Fire off\n");
00029     l1 = 0;
00030     l2 = 0;
00031     l3 = 0;
00032 }
00033 
00034 void dev_recv()
00035 {
00036     led1 = !led1;
00037     while(dev.readable()) {
00038         switch(dev.getc()) {
00039             case 'm': {
00040                 char xbuf[6];
00041                 char ybuf[6];
00042                 
00043                 int ind = 0;
00044                 char cur = dev.getc();
00045                 while(cur != ',') {
00046                     xbuf[ind++] = cur;
00047                     cur = dev.getc();
00048                 }
00049                 xbuf[ind] = NULL;
00050                 
00051                 ind = 0;
00052                 cur = dev.getc();
00053                 while(cur != '|') {
00054                     ybuf[ind++] = cur;
00055                     cur = dev.getc();
00056                 }
00057                 ybuf[ind] = NULL;
00058                 
00059                 float x, y;
00060                 x = (float)atof(xbuf);
00061                 y = (float)atof(ybuf);
00062                 
00063                 pc.printf("%f, %f\n",x , y);
00064                 
00065                 if (x >= 0.1) {
00066                     m1.speed( (y + x) );
00067                     m2.speed(y);
00068         //            pc.printf("GREATER THAN %f, %f\n",(y + x) , y);
00069                 } else if (x <= -0.1) {
00070                     m1.speed(y);
00071                     m2.speed( (y - x));
00072         //            pc.printf("LESS THAN %f, %f\n",y, (y - x));
00073                 } else {
00074                     m1.speed(y);
00075                     m2.speed(y);
00076         //            pc.printf("%.2f, %.2f\n",x, y);
00077                 }
00078             }
00079             case 's': {
00080                 switch(dev.getc()) {
00081                     case 'u': {
00082                         if (vert < 1) {
00083                             vert = vert + SERVO_MOVE;
00084                             printf("Servo Command: %f\n", vert.read());
00085                         }
00086                         break;
00087                     }
00088                     case 'd': {
00089                         if (vert > 0) {
00090                             vert = vert - SERVO_MOVE;
00091                             printf("Servo Command: %f\n", vert.read());
00092                         }
00093                         break;
00094                     }
00095                     case 'l': {
00096                         if (horiz < 5) {
00097                             horiz = horiz + SERVO_MOVE;
00098                         }
00099                         break;
00100                     }
00101                     case 'r': {
00102                         if (horiz > 0) {
00103                             horiz = horiz - SERVO_MOVE;
00104                         }
00105                         break;
00106                     }
00107                 }
00108                 break;
00109             }
00110             case 'f': {
00111                 printf("Fire\n");
00112                 l1 = 1;
00113                 l2 = 1;
00114                 l3 = 1;
00115                 flipper.attach(&flip, 60.0);
00116                 break;
00117             }
00118         }
00119     }
00120 }
00121  
00122 void pc_recv()
00123 {
00124 //    led4 = !led4;
00125     while(pc.readable()) {
00126         dev.putc(pc.getc());
00127     }
00128 }
00129  
00130 int main()
00131 {
00132     pc.baud(9600);
00133     dev.baud(9600);
00134     
00135  
00136     pc.attach(&pc_recv, Serial::RxIrq);
00137     dev.attach(&dev_recv, Serial::RxIrq);
00138     horiz = 0.5;
00139     vert = 0.5;
00140     while(1) {
00141         sleep();
00142     }
00143 }