Diff: main.cpp

Revision:

14:3403ce49a37a

Parent:

11:f8fac9693cd5

--- a/main.cpp	Mon Apr 04 09:26:26 2016 +0000
+++ b/main.cpp	Tue Apr 05 17:25:09 2016 +0000
@@ -3,35 +3,55 @@
 #include "spi_nucleo.h"
 #include "transfer.h"
 #include <string.h>
-
+#include "arm_updown.h"
+#include "encoder.h"
+#include "speed_control.h"
+#include "math.h"
 int main(){
+    setup();
+    //set-Ticker
+    bole_checker.attach(bole_check,0.2);    
     spiInit();
     push.mode(PullUp);
     while(pushed_number==0){    //手動
         transfer();
         spi_mbed();     //コントローラーからの信号を読み取る
-        Cilinder=cilinder;
+        valve1=cilinder;
         if(RR==1){
-                    //右旋回の関数
+                motor_f(1,0.5);   //右旋回の関数
+                motor_r(1,0.5);
+                motor_l(1,0.5);
                 }
         else if(LR==1){
-                    //左旋回の関数
+                motor_f(-1,0.5);
+                motor_r(-1,0.5);
+                motor_l(-1,0.5);    //左旋回の関数
                 }
         else if(onoff==1){
-            if(updown==1) up=0.1;//上下機構上昇
-            else down=0.1;//上下機構下降
+            if(updown==1) bole_f=0.1;//上下機構上昇
+            else bole_b=0.1;//上下機構下降
                 }
         else{
+               motor_target(((-1)*speed_X)/30,((2/sqrt(3.0)*speed_X)-(speed_Y*2))/30,((2/sqrt(3.0)*speed_X)+(speed_Y*2))/30);
                     //読み取った値speed_X,speed_Yを速度制御にぶち込む
                 }
-        }
+            }
     while(pushed_number==1){    //自動
         transfer();
         getGyro();
             //自己位置
-            //エアシリンダー
+//測距関係により検知
+//エアシリンダー
+        if(UW*IR){   //幹検知
+            n++;
+            bole_checker.detach();
         }
-        
+        if(n==1){
+            expand_gf();
+        }else if(n==2){
+            expand_ff();
+        }else if(n>2){
+            expand_sf();
+        }
     }
-    
-
+}
\ No newline at end of file