Diff: main.cpp

Revision:

0:1f5a1332637b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 21 04:03:52 2017 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "Servo.h"
+//Reads in joystick positions from controller and drives the motors. 
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+AnalogIn jx1(p16);
+AnalogIn jy1(p15);
+//AnalogIn jx2
+//AnalogIn jy2
+float mot_x = 0.5;
+float mot_y = 0.5;
+float cam_x = 0;
+float cam_y = 0;
+
+char buf[32];
+int idx = 0;
+
+Serial pc(p9,p10);
+//Serial pc(USBTX, USBRX);
+
+Motor A(p21, p20, p19);  //pwm, inB, inA
+Motor B(p22, p17, p12); //pwm, inA, inB
+
+
+int main()
+{
+    pc.baud(115200);
+    while(1) {
+            int x = pc.getc();
+            
+            if(x == -1) continue;
+            
+            //while(!pc.readable());
+            buf[idx] = (char)x; 
+            
+            if(buf[idx] == '\r' || buf[idx] == '\n'){
+                sscanf(buf, "%f %f %f %f", &mot_x, &mot_y, &cam_x, &cam_y);
+                A.speed(mot_x);
+                B.speed(mot_y); 
+                idx = 0;
+            }else{
+                idx++;
+            }
+    }
+}
\ No newline at end of file