Diff: main.cpp

Revision:

21:5ca943882097

Parent:

20:e05fdab66e68

Child:

22:a834fd13fcfd

--- a/main.cpp	Sun Oct 18 01:06:45 2015 +0000
+++ b/main.cpp	Sun Oct 18 01:17:08 2015 +0000
@@ -34,27 +34,37 @@
 int i = 0;
 void sup1_speed(float speed)
 {
-    if(speed>0){
+    if(speed>0) {
         sup1d = 1;
-    }else if(speed < 0){
+    } else if(speed < 0) {
         sup1d =2;
     }
     sup1p = fabs(speed);
 }
 void sup2_speed(float speed)
 {
-    if(speed>0){
+    if(speed>0) {
         sup2d = 1;
-    }else if(speed < 0){
+    } else if(speed < 0) {
         sup2d =2;
     }
     sup2p = fabs(speed);
 }
+void sup1_brake()
+{
+    sup1d = 0;
+    sup1p = 1;
+}
+void sup2_brake()
+{
+    sup2d = 0;
+    sup2p = 1;
+}
 void supply()
 {
-    if(d%2){
+    if(d%2) {
         sup1_speed(0.9);
-    }else{
+    } else {
         sup2_speed(0.9);
     }
     d++;
@@ -77,7 +87,7 @@
     led = 1;
     double tilt = 0,lo = 0,ro = 0;
     int8_t ttilt = 0,tmpread = 0,tmpttilt = 0,ajst = 0;
-    char tro = 0,tlo = 0;
+    char tro = 0,tlo = 0,narasup = 0;
     Tp.setInputLimits(-45,45);
     Tp.setOutputLimits(-0.9,0.9);
     Tp.setMode(1);
@@ -96,35 +106,34 @@
             lo = (conn.getc()-127)/127.0*0.9;
         }
         //nara シリアル受信
-        if(slave.avaiable(){
-            narasup = slave.getc();
+        if(slave.readable()) {
+        narasup = slave.getc();
         }
-        if(narasup == 1){
-            sup1_speed(0.9);
-            sup2_speed(0);
-        }else if (narasup == 2){
-            sup1_speed(-0.9);
-            sup2_speed(0);
-        }else if (narasup == 4){
-            sup2_speed(0.9);
-            sup1_speed(0);
-        }else if (narasup ==8){
-            sup2_speed(-0.9);
-            sup1_speed(0);
-        }else {
-            sup1_speed(0);
-            sup2_speed(0);
+        if(narasup == 1) {
+        sup1_speed(0.9);
+            sup2_brake();
+        } else if (narasup == 2) {
+        sup1_speed(-0.9);
+            sup2_brake();
+        } else if (narasup == 4) {
+        sup2_speed(0.9);
+            sup1_brake();
+        } else if (narasup ==8) {
+        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);
+        if(read>4) {
+        read = 0;
+    }
+    led = read;
+    if((read>>1)%2 && i == 0) {
+        air = 1;
+        ai.attach(&rev,shoottime);
             i = 1;
-        }
+    }
         /*補給制御*/
         /*if(limit1 == 0) {
             su1 = 1;
@@ -147,21 +156,21 @@
         Tp.setProcessValue(tilt);
         /*足の出力が小さい場合はゼロとする*/
         if (abs(lo) < 0.1) {
-            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 = 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());
+        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 = 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());
 }
+}