Diff: main.cpp

Revision:

8:9d8999740604

Parent:

7:920cbfb28112

Child:

9:98ef1eee7ace

Child:

10:fefff533c442

--- a/main.cpp	Thu Sep 10 01:43:12 2015 +0000
+++ b/main.cpp	Mon Sep 14 04:51:25 2015 +0000
@@ -3,17 +3,18 @@
 #include "Motor.h"
 #include "PID.h"
 #include "QEI.h"
+#include "Servo.h"
 #define RATE 0.05
-BusOut air(p13,p14);
-DigitalOut out(p12);
-Serial conn(p28,p27);
+BusOut air(PC_10,PC_11);
+DigitalOut out(PC_12);
+Serial conn(PA_15,PB_7);
 Serial pc(USBTX,USBRX);
-Motor L(p21,p15,p16);
-Motor R(p22,p17,p18);
 PID Tp(50,40000,0,0.001);
 BusOut led(LED1,LED2,LED3,LED4);
-Motor ot(p23,p19,p20);
-QEI sensort(p29,p30,NC,624);
+Motor ot(PB_13,PB_3,PA_10);
+QEI sensort(PC_3,PC_0,NC,624);
+Servo L(PC_9);
+Servo R(PC_8); 
 Timeout ai;
 char read;
 int Rs = 0,Ls = 0;
@@ -30,6 +31,7 @@
 }
 int main()
 {
+    ot.setfrequency(60000);
     double tilt = 0,lo = 0,ro = 0;
     int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
     char tro = 0,tlo = 0;
@@ -65,8 +67,8 @@
         if (abs(ro) < 0.1){
             ro = 0;
         }
-        L.speed(lo);
-        R.speed(ro);
+        L.write((lo+1.0)/2.0);
+        R.write((ro+1.0)/2.0);
         ot.speed(Tp.compute());
         //pc.printf("%f %d\n\r",Tp.compute(),read);
         //pc.printf("%d-%d\r\n",tlo,tro);