Diff: main.cpp

Revision:

4:646562d80dc2

Parent:

3:d2c733b52600

Child:

5:4b462b9cb255

--- a/main.cpp	Wed Sep 02 00:19:34 2015 +0000
+++ b/main.cpp	Thu Sep 03 02:37:51 2015 +0000
@@ -3,18 +3,18 @@
 #include "Motor.h"
 #include "PID.h"
 #include "QEI.h"
-#define RATE 0.01
+#define RATE 0.05
 BusOut air(p13,p14);
 DigitalOut out(p12);
 Serial conn(p9,p10);
 Serial pc(USBTX,USBRX);
-Motor L(p22,p15,p16);
-Motor R(p23,p17,p18);
-PID Lp(1.0,0,0,RATE);
-PID Rp(1.0,0,0,RATE);
-PID Tp(1.0,0,0,RATE);
+Motor L(p21,p15,p16);
+Motor R(p22,p17,p18);
+PID Lp(4.7,6,0,RATE);
+PID Rp(4.7,6,0,RATE);
+PID Tp(50,40000,0,0.001);
 BusOut led(LED1,LED2,LED3,LED4);
-Motor ot(p21,p19,p20);
+Motor ot(p23,p19,p20);
 QEI sensort(p29,p30,NC,624);
 QEI ls(p27,p28,NC,624);
 QEI rs(p25,p26,NC,624);
@@ -25,8 +25,8 @@
 int i = 0;
 void getspeed()
 {
-    Rs =rs.getPulses();
-    Ls =ls.getPulses();
+    Rs =rs.getPulses()*2;
+    Ls =ls.getPulses()*2;
     rs.reset();
     ls.reset();
 }
@@ -42,25 +42,29 @@
 }
 int main()
 {
-    double tilt = 0;
-    int8_t ttilt = 0;
+    double tilt = 0,lo = 0,ro = 0;
+    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0;
     Lp.setInputLimits(-3000,3000);
     Rp.setInputLimits(-3000,3000);
-    Tp.setInputLimits(-10000,10000);
+    Tp.setInputLimits(-45,45);
     Lp.setOutputLimits(-0.9,0.9);
     Rp.setOutputLimits(-0.9,0.9);
     Tp.setOutputLimits(-0.9,0.9);
     Lp.setMode(1);
     Rp.setMode(1);
     Tp.setMode(1);
-    Lp.setBias(0);
-    Rp.setBias(0);
-    Tp.setBias(0);
-    getsp.attach(&getspeed,0.1);
+    Lp.setBias(0.0);
+    Rp.setBias(0.0);
+    Tp.setBias(0.0);
+    getsp.attach(&getspeed,0.05);
     while(1) {
         if(conn.getc() == 255) {
-            read = conn.getc();
-            ttilt = conn.getc();
+            tmpread = conn.getc();
+            tmpttilt = conn.getc();
+            if(tmpread^tmpttilt == conn.getc()){
+                ttilt = tmpttilt;
+                read = tmpread;
+            }
         }
         if((read>>2)%2 && i == 0){
             air = 1;
@@ -69,22 +73,22 @@
             led = 1;
         }
         if((read>>3)%2){
-            Lp.setSetPoint(100);
-            Rp.setSetPoint(100);
+            Lp.setSetPoint(500);
+            Rp.setSetPoint(500);
         }else
         if((read>>4)%2){
-            Lp.setSetPoint(-100);
-            Rp.setSetPoint(-100);
+            Lp.setSetPoint(-500);
+            Rp.setSetPoint(-500);
             led = 2;
         }else
         if((read>>5)%2){
-            Lp.setSetPoint(-100);
-            Rp.setSetPoint(100);
+            Lp.setSetPoint(-500);
+            Rp.setSetPoint(500);
             led = 4;
         }else
         if((read>>6)%2){
-            Lp.setSetPoint(100);
-            Rp.setSetPoint(-100);
+            Lp.setSetPoint(500);
+            Rp.setSetPoint(-500);
             led = 8;
         }else{
             Lp.setSetPoint(0);
@@ -92,15 +96,23 @@
             led = 0;
         }
         Tp.setSetPoint(ttilt);
-        tilt = sensort.getPulses();
-        //tilt = tilt*24.0/500.0*pi/360.0;
-        conn.putc(int8_t(tilt));
+        tilt = double(sensort.getPulses());
+        tilt = tilt*61/5128.0;
         Lp.setProcessValue(Ls);
         Rp.setProcessValue(Rs);
         Tp.setProcessValue(tilt);
-        L.speed(Lp.compute());
-        R.speed(Rp.compute());
+        lo = Lp.compute();
+        ro = Rp.compute();
+        if (abs(lo) < 0.1){
+            lo = 0;
+        }
+        if (abs(ro) < 0.1){
+            ro = 0;
+        }
+        L.speed(lo);
+        R.speed(ro);
         ot.speed(Tp.compute());
-        pc.printf("%f\n\r",Lp.compute());
+        pc.printf("%d\n\r",ttilt);
+        wait_ms(1);
     }
 }