Diff: main.cpp

Revision:

22:a834fd13fcfd

Parent:

21:5ca943882097

Child:

23:bfe8c3791f1f

--- a/main.cpp	Sun Oct 18 01:17:08 2015 +0000
+++ b/main.cpp	Mon Oct 19 08:23:12 2015 +0000
@@ -4,6 +4,7 @@
 #include "PID.h"
 #include "QEI.h"
 #include "Servo.h"
+#include "DebounceIn.h"
 #define RATE 0.05
 #define shoottime 0.5
 BusOut air(p5,p6);
@@ -18,20 +19,21 @@
 PID Tp(50,4000000,0,0.001);
 //PID Tp(4.,40000,0,0.001);
 BusOut led(p8,p13,p14,p24);
-Motor ot(p23,p19,p20);
+Motor ot(p23,p20,p19);
 QEI sensort(p29,p30,NC,624);
 //Motor sup1(p21,p15,p16);
 //Motor sup2(p22,p17,p18);
 SoftPWM sup1p(p21);
 BusOut sup1d(p15,p16);
 SoftPWM sup2p(p22);
-BusOut sup2d(p17,p18);
-DigitalIn limit1(p11,PullUp);
-DigitalIn limit2(p12,PullUp);
+BusOut sup2d(p18,p17);
+DebounceIn limit1(p11);
+DebounceIn limit2(p12);
 Timeout ai;
 char read;
 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
 int i = 0;
+int old_limit1 = 0,old_limit2 = 0,new_limit1 = 0,new_limit2 = 0,count1 = 0,count2 = 0,target_count1 = 0,target_count2 = 0,supply_seq1 = 0,supply_seq2 = 0;
 void sup1_speed(float speed)
 {
     if(speed>0) {
@@ -64,17 +66,50 @@
 {
     if(d%2) {
         sup1_speed(0.9);
+        target_count1 +=3;
+        supply_seq1 = 1;
     } else {
         sup2_speed(0.9);
+        target_count2 +=3;
+        supply_seq2 = 1;
     }
     d++;
 }
+void count_check(){
+    if((target_count1 == count1)&&(supply_seq1 == 1)){
+        sup1_speed(-0.9);
+        target_count1 += 2;
+        supply_seq1 = 2;
+    }else if((target_count1 == count1) && (supply_seq1 == 2)){
+        sup1_speed(0);
+        supply_seq1 = 0;
+    }
+    if((target_count2 == count2)&&(supply_seq2 == 1)){
+        sup2_speed(-0.9);
+        target_count2 += 2;
+        supply_seq2 = 2;
+    }else if((target_count2 == count2) && (supply_seq2 == 2)){
+        sup2_speed(0);
+        supply_seq2 = 0;
+    }
+}
+void limit1_count()
+{
+    new_limit1 = limit1;
+    if ((new_limit1==0) && (old_limit1==1)) count1++;
+    old_limit1 = new_limit1;
+}
+void limit2_count()
+{
+    new_limit2 = limit2;
+    if ((new_limit2==0) && (old_limit2==1)) count2++;
+    old_limit2 = new_limit2;
+}
 void zero()
 {
     air = 0;
     i = 0;
     out = 1;
-    //supply();
 }
 void rev()
 {
@@ -84,6 +119,8 @@
 }
 int main()
 {
+    limit1.mode(PullUp);
+    limit2.mode(PullUp);
     led = 1;
     double tilt = 0,lo = 0,ro = 0;
     int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
@@ -107,48 +144,37 @@
         }
         //nara シリアル受信
         if(slave.readable()) {
-        narasup = slave.getc();
+            narasup = slave.getc();
         }
-        if(narasup == 1) {
-        sup1_speed(0.9);
+        /*if(narasup%2) {
+            sup1_speed(0.9);
             sup2_brake();
-        } else if (narasup == 2) {
-        sup1_speed(-0.9);
+        } else if (narasup>>1%2 ) {
+            sup1_speed(-0.9);
             sup2_brake();
-        } else if (narasup == 4) {
-        sup2_speed(0.9);
+        } else if (narasup>>2%2) {
+            sup2_speed(0.9);
             sup1_brake();
-        } else if (narasup ==8) {
-        sup2_speed(-0.9);
+        } else if (narasup>>3 %2) {
+            sup2_speed(-0.9);
             sup1_brake();
         } else {
             sup1_brake();
             sup2_brake();
-        }
+        }*/
         if(read>4) {
-        read = 0;
-    }
-    led = read;
-    if((read>>1)%2 && i == 0) {
-        air = 1;
-        ai.attach(&rev,shoottime);
+            read = 0;
+        }
+        led = read;
+        if((read>>1)%2 && i == 0) {
+            air = 1;
+            ai.attach(&rev,shoottime);
             i = 1;
-    }
-        /*補給制御*/
-        /*if(limit1 == 0) {
-            su1 = 1;
-        }
-        if(limit2 == 0){
-            su2 = 1;
+            supply();
         }
-        if(limit1 == 1 && su1 == 1) {
-            su1 = 0;
-            sup1_speed(0);
-        }
-        if(limit2 == 1 && su2 == 1) {
-            su2 = 0;
-            sup2_speed(0);
-        }*/
+        limit1_count();
+        limit2_count();
+        count_check();
         /*射角制御*/
         Tp.setSetPoint(ttilt/2.0);
         tilt = double(sensort.getPulses());
@@ -156,21 +182,21 @@
         Tp.setProcessValue(tilt);
         /*足の出力が小さい場合はゼロとする*/
         if (abs(lo) < 0.1) {
-        lo = 0;
-    }
-    if (abs(ro) < 0.1) {
-        ro = 0;
+            lo = 0;
+        }
+        if (abs(ro) < 0.1) {
+            ro = 0;
+        }
+        /*スレーブにreadを送信*/
+        slave.putc(read);
+        /*各モーターに出力*/
+        L = (ro+1.0)/2.0;
+        Ll = (ro+1.0)/2.0;
+        R = (lo+1.0)/2.0;
+        Rl = (lo+1.0)/2.0;
+        ot.speed(Tp.compute());
+        pc.printf("%d\n\r",count1);
+        //pc.printf("%d\n\r",sensort.getPulses());
+        //pc.printf("%f \n\r",Tp.compute());
     }
-    /*スレーブにreadを送信*/
-    slave.putc(read);
-    /*各モーターに出力*/
-    L = (ro+1.0)/2.0;
-    Ll = (ro+1.0)/2.0;
-    R = 1.0-(lo+1.0)/2.0;
-    Rl = 1.0-(lo+1.0)/2.0;
-    ot.speed(Tp.compute());
-    //pc.printf("%f\n\r",tilt);
-    //pc.printf("%d\n\r",sensort.getPulses());
-    //pc.printf("%f \n\r",Tp.compute());
 }
-}