Diff: main.cpp

Revision:

9:98ef1eee7ace

Parent:

8:9d8999740604

--- a/main.cpp	Mon Sep 14 04:51:25 2015 +0000
+++ b/main.cpp	Tue Sep 15 10:58:01 2015 +0000
@@ -9,14 +9,20 @@
 DigitalOut out(PC_12);
 Serial conn(PA_15,PB_7);
 Serial pc(USBTX,USBRX);
+Serial nara(PA_2,PA_3);
 PID Tp(50,40000,0,0.001);
 BusOut led(LED1,LED2,LED3,LED4);
 Motor ot(PB_13,PB_3,PA_10);
+Motor pak(PB_14,PB_4,PA_11);
+Motor sup1(PB_15,PB_10,PB_4);
+Motor sup2(PA_11,PA_6,PA_7);
 QEI sensort(PC_3,PC_0,NC,624);
 Servo L(PC_9);
 Servo R(PC_8); 
+BusIn limits(PB_8,PB_9);
 Timeout ai;
-char read;
+Timeout yanagi;
+char read ,yayaya = 0;
 int Rs = 0,Ls = 0;
 int i = 0;
 void zero(){ 
@@ -29,6 +35,9 @@
     ai.attach(&zero,1.0);
     out = 0;
 }
+void pakri(){
+    yayaya = 2;
+}
 int main()
 {
     ot.setfrequency(60000);
@@ -52,11 +61,24 @@
                 lo = (tlo-127)/127.0*0.9;
             }
         }
+        if(read%2){ //yanagi shoot.
+            yanagi.attach(&pakri,3.0);
+            yayaya = 1;
+        }   
         if((read>>2)%2 && i == 0){
             air = 1;
             ai.attach(&rev,1.0);
             i = 1;
         }
+        /* yanagi controll */
+        if (yayaya == 1 && limits != 2) {
+            pak.speed(1);
+        }else if (yayaya == 2 && limits !=1) {
+            pak.speed(-1);
+        }else {
+            pak.speed(0);
+        }    
+        /* 射角調整 */
         Tp.setSetPoint(ttilt);
         tilt = double(sensort.getPulses());
         tilt = tilt*61/5128.0;