Diff: main.cpp

Revision:

3:d2c733b52600

Parent:

2:74c543a0a671

Child:

4:646562d80dc2

diff -r 74c543a0a671 -r d2c733b52600 main.cpp
--- a/main.cpp	Thu Aug 06 14:24:04 2015 +0000
+++ b/main.cpp	Wed Sep 02 00:19:34 2015 +0000
@@ -1,60 +1,106 @@
-#define pi 3.141592
+#define pi 3.141593
 #include "mbed.h"
-#include "rtos.h"
-#include "QEI.h"
 #include "Motor.h"
-#include "HMC6352.h"
-BusOut air(p15,p16);
-Serial conn(p28,p27);
+#include "PID.h"
+#include "QEI.h"
+#define RATE 0.01
+BusOut air(p13,p14);
+DigitalOut out(p12);
+Serial conn(p9,p10);
 Serial pc(USBTX,USBRX);
-BusOut mypaul(p17,p18);
-BusIn sw(p24,p25);
-Motor od(p21,p19,p20);
-Motor ot(p22,p12,p11);
+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);
+BusOut led(LED1,LED2,LED3,LED4);
+Motor ot(p21,p19,p20);
 QEI sensort(p29,p30,NC,624);
-HMC6352 sensord(p9,p10);
+QEI ls(p27,p28,NC,624);
+QEI rs(p25,p26,NC,624);
+Timeout ai;
+Ticker getsp;
 char read;
+int Rs = 0,Ls = 0;
+int i = 0;
+void getspeed()
+{
+    Rs =rs.getPulses();
+    Ls =ls.getPulses();
+    rs.reset();
+    ls.reset();
+}
+void zero(){ 
+    air = 0;
+    i = 0;
+    out = 1;
+}
+void rev(){
+    air = 2;
+    ai.attach(&zero,1.0);
+    out = 0;
+}
 int main()
 {
-    //sensord.setOpMode(HMC6352_CONTINUOUS,1,20);
-    double direct = 0,tilt = 0,cdirect = 0,adirect = 0,tdirect = 0,ttilt =0,itilt=0;
-    //tdirect = sensord.sample()/10.0;
-    sw.mode(PullUp);
+    double tilt = 0;
+    int8_t ttilt = 0;
+    Lp.setInputLimits(-3000,3000);
+    Rp.setInputLimits(-3000,3000);
+    Tp.setInputLimits(-10000,10000);
+    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);
     while(1) {
-        if(conn.readable()) {
+        if(conn.getc() == 255) {
             read = conn.getc();
+            ttilt = conn.getc();
         }
-        air = read%4;
-        mypaul = (read>>2)%4;
-        if((read>>4)%2 || 0/*sw == 2*/) {
-            //tdirect++;
-            od.speed(0.3);
-        } else if((read>>5)%2 ||0 /*sw == 1*/) {
-            //tdirect--;
-            od.speed(-0.3);
-        } else {
-            od.speed(0);
+        if((read>>2)%2 && i == 0){
+            air = 1;
+            ai.attach(&rev,1.0);
+            i = 1;
+            led = 1;
         }
-        if((read>>6)%2|| sw == 2 ) {
-            //ttilt+=pi/180.0*0.01;
-            ot.speed(0.3);
-        } else if((read>>7)%2 || sw == 1) {
-            //ttilt-=pi/180.0*0.01;
-            ot.speed(-0.3);
-        } else {
-            ot.speed(0);
+        if((read>>3)%2){
+            Lp.setSetPoint(100);
+            Rp.setSetPoint(100);
+        }else
+        if((read>>4)%2){
+            Lp.setSetPoint(-100);
+            Rp.setSetPoint(-100);
+            led = 2;
+        }else
+        if((read>>5)%2){
+            Lp.setSetPoint(-100);
+            Rp.setSetPoint(100);
+            led = 4;
+        }else
+        if((read>>6)%2){
+            Lp.setSetPoint(100);
+            Rp.setSetPoint(-100);
+            led = 8;
+        }else{
+            Lp.setSetPoint(0);
+            Rp.setSetPoint(0);
+            led = 0;
         }
-        if(tdirect >= 180 ||tdirect <= -180) {
-            //tdirect *= -1;
-        }
-        //cdirect = sensord.sample()/10.0;
-        direct = cdirect+540-tdirect;
-        direct = int(direct+0.5)%360;
-        direct -= 180;
-        //tilt = sensort.getPulses()/360.0/25.0*pi;
-        pc.printf("%d\n\r",read);
-        //od.speed(cdirect);
-        //ot.speed((ttilt-tilt)*1.0+itilt*0.01);
-        itilt += ttilt-tilt;
+        Tp.setSetPoint(ttilt);
+        tilt = sensort.getPulses();
+        //tilt = tilt*24.0/500.0*pi/360.0;
+        conn.putc(int8_t(tilt));
+        Lp.setProcessValue(Ls);
+        Rp.setProcessValue(Rs);
+        Tp.setProcessValue(tilt);
+        L.speed(Lp.compute());
+        R.speed(Rp.compute());
+        ot.speed(Tp.compute());
+        pc.printf("%f\n\r",Lp.compute());
     }
 }