Diff: main.cpp

Revision:

11:427bb7a43d7a

Parent:

7:920cbfb28112

Child:

13:400d640bb447

--- a/main.cpp	Thu Sep 10 01:43:12 2015 +0000
+++ b/main.cpp	Thu Sep 17 01:07:52 2015 +0000
@@ -3,27 +3,45 @@
 #include "Motor.h"
 #include "PID.h"
 #include "QEI.h"
+#include "Servo.h"
 #define RATE 0.05
-BusOut air(p13,p14);
-DigitalOut out(p12);
-Serial conn(p28,p27);
+BusOut air(p5,p6);
+DigitalOut out(p7);
+Serial conn(NC,p27);
 Serial pc(USBTX,USBRX);
-Motor L(p21,p15,p16);
-Motor R(p22,p17,p18);
+Serial slave(p9,NC);
+Servo L(p24);
+Servo R(p25);
 PID Tp(50,40000,0,0.001);
 BusOut led(LED1,LED2,LED3,LED4);
-Motor ot(p23,p19,p20);
+Motor ot(p23,p13,p14);
 QEI sensort(p29,p30,NC,624);
+Motor sup1(p21,p15,p17);
+Motor sup2(p22,p19,p20);
+BusIn limit1(p10);
+BusIn limit2(p11);
 Timeout ai;
 char read;
-int Rs = 0,Ls = 0;
+int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
 int i = 0;
-void zero(){ 
+void supply()
+{
+    if(d%2){
+        sup1.speed(1);
+    }else{
+        sup2.speed(1);
+    }
+
+}
+void zero()
+{
     air = 0;
     i = 0;
     out = 1;
+    supply();
 }
-void rev(){
+void rev()
+{
     air = 2;
     ai.attach(&zero,1.0);
     out = 0;
@@ -43,30 +61,50 @@
             tmpttilt = conn.getc();
             tro = conn.getc();
             tlo = conn.getc();
-            if(tmpread^tmpttilt^tro^tlo == conn.getc()){
+            if(tmpread^tmpttilt^tro^tlo == conn.getc()) {
                 ttilt = tmpttilt;
                 read = tmpread;
                 ro = (tro-127)/127.0*0.9;
                 lo = (tlo-127)/127.0*0.9;
             }
         }
-        if((read>>2)%2 && i == 0){
+        if((read>>2)%2 && i == 0) {
             air = 1;
             ai.attach(&rev,1.0);
             i = 1;
         }
+        /*補給制御*/
+        if(limit1 == 0) {
+            su1 = 1;
+        }
+        if(limit2 == 0){
+            su2 = 1;
+        }
+        if(limit1 == 1 && su1 == 1) {
+            su1 = 0;
+            sup1.speed(0);
+        }
+        if(limit2 == 1 && su2 == 1) {
+            su2 = 0;
+            sup2.speed(0);
+        }
+        /*射角制御*/
         Tp.setSetPoint(ttilt);
         tilt = double(sensort.getPulses());
         tilt = tilt*61/5128.0;
         Tp.setProcessValue(tilt);
-        if (abs(lo) < 0.1){
+        /*足の出力が小さい場合はゼロとする*/
+        if (abs(lo) < 0.1) {
             lo = 0;
         }
-        if (abs(ro) < 0.1){
+        if (abs(ro) < 0.1) {
             ro = 0;
         }
-        L.speed(lo);
-        R.speed(ro);
+        /*スレーブにreadを送信*/
+        slave.putc(read);
+        /*各モーターに出力*/
+        L = (lo+1.0)/2.0;
+        R = (ro+1.0)/2.0;;
         ot.speed(Tp.compute());
         //pc.printf("%f %d\n\r",Tp.compute(),read);
         //pc.printf("%d-%d\r\n",tlo,tro);