Diff: main.cpp

Revision:

23:bfe8c3791f1f

Parent:

22:a834fd13fcfd

Child:

24:7e93db7d1c32

--- a/main.cpp	Mon Oct 19 08:23:12 2015 +0000
+++ b/main.cpp	Mon Oct 26 07:10:02 2015 +0000
@@ -7,6 +7,7 @@
 #include "DebounceIn.h"
 #define RATE 0.05
 #define shoottime 0.5
+#define DEADTIME 500
 BusOut air(p5,p6);
 DigitalOut out(p7);
 Serial conn(p9,p10);
@@ -14,6 +15,7 @@
 Serial slave(p28,p27);
 PwmOut Ll(LED1);
 PwmOut Rl(LED2);
+BusOut mled(LED3,LED4);
 Servo L(p25);
 Servo R(p26);
 PID Tp(50,4000000,0,0.001);
@@ -30,6 +32,8 @@
 DebounceIn limit1(p11);
 DebounceIn limit2(p12);
 Timeout ai;
+Timer timer1;
+Timer timer2;
 char read;
 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
 int i = 0;
@@ -62,35 +66,49 @@
     sup2d = 0;
     sup2p = 1;
 }
+void supply_1()
+{
+    sup1_speed(0.9);
+    target_count1 +=3;
+    supply_seq1 = 1;
+}
+void supply_2()
+{
+    sup2_speed(0.9);
+    target_count2 +=3;
+    supply_seq2 = 1;
+}
 void supply()
 {
-    if(d%2) {
-        sup1_speed(0.9);
-        target_count1 +=3;
-        supply_seq1 = 1;
-    } else {
-        sup2_speed(0.9);
-        target_count2 +=3;
-        supply_seq2 = 1;
+    if (supply_seq1 ==0 && supply_seq2 == 0) {
+        if(d%2) {
+            supply_1();
+        } else {
+            supply_2();
+        }
+        d++;
     }
-    d++;
 }
-void count_check(){
-    if((target_count1 == count1)&&(supply_seq1 == 1)){
+
+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);
+    } else if((target_count1 == count1) && (supply_seq1 == 2)) {
+        sup1_brake();
         supply_seq1 = 0;
+        mled = 0;
     }
-    if((target_count2 == count2)&&(supply_seq2 == 1)){
+    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);
+    } else if((target_count2 == count2) && (supply_seq2 == 2)) {
+        sup2_brake();
         supply_seq2 = 0;
+        mled = 0;
     }
 }
 void limit1_count()
@@ -122,51 +140,56 @@
     limit1.mode(PullUp);
     limit2.mode(PullUp);
     led = 1;
+    int dead1 = 0,dead2 = 0;
     double tilt = 0,lo = 0,ro = 0;
-    int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
-    char tro = 0,tlo = 0,narasup = 0;
+    int8_t ttilt = 60,tmpread = 0,tmpttilt = 0,ajst = 0;
+    char tro = 0,tlo = 0,narasup = 60;
     Tp.setInputLimits(-45,45);
     Tp.setOutputLimits(-0.9,0.9);
     Tp.setMode(1);
     Tp.setBias(0.0);
+    Tp.setProcessValue(sensort.getPulses()*49.0/1745.0);
+    Tp.setSetPoint(0);
+    ot.speed(0);
     L = 0.5;
     R = 0.5;
-    wait(3);
+    wait(5);
     led= 15;
     conn.baud(38400);
     while(1) {
         //ps3コントローラーシリアル受信
         if(conn.getc() == 255) {
             read = conn.getc();
-            ttilt = conn.getc()-60;
-            ro = (conn.getc()-127)/127.0*0.9;
-            lo = (conn.getc()-127)/127.0*0.9;
+            ttilt = conn.getc();
+            tro =conn.getc();
+            tlo = conn.getc();
+            if (tro == 255){
+                ro =ro;
+            }else{
+                ro = (tro-127)/127.0*0.9;
+            }
+            if(tlo ==255){
+                lo = lo;
+            }else {
+                lo = (tlo-127)/127.0*0.9;
+            }
         }
         //nara シリアル受信
         if(slave.readable()) {
             narasup = slave.getc();
+            if((narasup-60) == 1 && supply_seq1 == 0) {
+                mled = 1;
+                supply_1();
+            } else if((narasup-60) == 2 && supply_seq2 == 0) {
+                supply_2();
+                mled =2;
+            }
         }
-        /*if(narasup%2) {
-            sup1_speed(0.9);
-            sup2_brake();
-        } else if (narasup>>1%2 ) {
-            sup1_speed(-0.9);
-            sup2_brake();
-        } else if (narasup>>2%2) {
-            sup2_speed(0.9);
-            sup1_brake();
-        } else if (narasup>>3 %2) {
-            sup2_speed(-0.9);
-            sup1_brake();
-        } else {
-            sup1_brake();
-            sup2_brake();
-        }*/
-        if(read>4) {
+        if (read > 4){ //通信不良対策
             read = 0;
         }
         led = read;
-        if((read>>1)%2 && i == 0) {
+        if(read==2 && i == 0) {
             air = 1;
             ai.attach(&rev,shoottime);
             i = 1;
@@ -176,10 +199,12 @@
         limit2_count();
         count_check();
         /*射角制御*/
-        Tp.setSetPoint(ttilt/2.0);
+        Tp.setSetPoint((ttilt-60)/2.0);
         tilt = double(sensort.getPulses());
         tilt = tilt*49.0/1745.0;
         Tp.setProcessValue(tilt);
+        ro *= abs(ro)*0.5;
+        lo *= abs(lo)*0.5;
         /*足の出力が小さい場合はゼロとする*/
         if (abs(lo) < 0.1) {
             lo = 0;
@@ -187,15 +212,39 @@
         if (abs(ro) < 0.1) {
             ro = 0;
         }
+        if(timer1.read_ms() > DEADTIME) {
+            dead1 = 0;
+            timer1.stop();
+            timer1.reset();
+        }
+        if(timer2.read_ms() > DEADTIME) {
+            dead2 = 0;
+            timer2.stop();
+            timer2.reset();
+        }
+        if ((L >0.5 && (lo+1.0)/2.0<=0.5) || (L <0.5 && (lo+1.0)/2.0>=0.5) || dead1 ==1) {
+            L = 0.5;
+            Ll = 0.5;
+            dead1 =1;
+            timer1.start();
+        } else {
+            L = (lo+1.0)/2.0;
+            Ll = (lo+1.0)/2.0;
+        }
+        if ( (R >0.5 && (ro+1.0)/2.0<=0.5) || (R <0.5 && (ro+1.0)/2.0>=0.5)  || dead2 ==1) {
+            R = 0.5;
+            Rl = 0.5;
+            dead2 = 1;
+            timer2.start();
+        } else {
+            R = (ro+1.0)/2.0;
+            Rl = (ro+1.0)/2.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;
+        pc.printf("%d\n\r",narasup);
         ot.speed(Tp.compute());
-        pc.printf("%d\n\r",count1);
         //pc.printf("%d\n\r",sensort.getPulses());
         //pc.printf("%f \n\r",Tp.compute());
     }