Diff: main.cpp

Revision:

10:fefff533c442

Parent:

8:9d8999740604

--- a/main.cpp	Mon Sep 14 04:51:25 2015 +0000
+++ b/main.cpp	Wed Sep 16 08:33:58 2015 +0000
@@ -5,12 +5,12 @@
 #include "QEI.h"
 #include "Servo.h"
 #define RATE 0.05
+DigitalOut led(LED1);
 BusOut air(PC_10,PC_11);
 DigitalOut out(PC_12);
-Serial conn(PA_15,PB_7);
-Serial pc(USBTX,USBRX);
+Serial conn(NC,PA_12);
+Serial pc(SERIAL_TX, SERIAL_RX);
 PID Tp(50,40000,0,0.001);
-BusOut led(LED1,LED2,LED3,LED4);
 Motor ot(PB_13,PB_3,PA_10);
 QEI sensort(PC_3,PC_0,NC,624);
 Servo L(PC_9);
@@ -31,16 +31,20 @@
 }
 int main()
 {
-    ot.setfrequency(60000);
+    pc.printf("connected\n\r");
+    //conn.baud(115200);
+    //ot.setfrequency(60000);
     double tilt = 0,lo = 0,ro = 0;
     int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
     char tro = 0,tlo = 0;
     Tp.setInputLimits(-45,45);
     Tp.setOutputLimits(-0.9,0.9);
     Tp.setMode(1);
+    led=1;
     Tp.setBias(0.0);
     while(1) {
-        if(conn.getc() == 255) {
+        if(conn.getc() == 0xFF) {
+            led = !led;
             tmpread = conn.getc();
             tmpttilt = conn.getc();
             tro = conn.getc();
@@ -48,8 +52,8 @@
             if(tmpread^tmpttilt^tro^tlo == conn.getc()){
                 ttilt = tmpttilt;
                 read = tmpread;
-                ro = (tro-127)/127.0*0.9;
-                lo = (tlo-127)/127.0*0.9;
+                ro = tro;
+                lo = tlo;
             }
         }
         if((read>>2)%2 && i == 0){
@@ -67,11 +71,10 @@
         if (abs(ro) < 0.1){
             ro = 0;
         }
-        L.write((lo+1.0)/2.0);
-        R.write((ro+1.0)/2.0);
+        L= (lo-127)/127.0;
+        R=(ro-127)/127.0;
         ot.speed(Tp.compute());
         //pc.printf("%f %d\n\r",Tp.compute(),read);
-        //pc.printf("%d-%d\r\n",tlo,tro);
-        wait_ms(1);
+        pc.printf("%d-%f\r\n",read,lo);
     }
 }