Chaser for 4180 Project
Dependencies: Motor Servo mbed
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 }
Generated on Mon Jul 18 2022 01:23:31 by 1.7.2