Diff: arm_updown.h

Revision:

12:2450012ce888

Child:

13:7335d90b551a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/arm_updown.h	Mon Apr 04 09:25:57 2016 +0000
@@ -0,0 +1,108 @@
+// 関数はヘッダファイル内にまとめる
+// IRはGP2Y0E02Aを使用する
+// (pc5.pc6.pa8.pa9)以外のpinを使う
+
+#include "mbed.h"
+
+#define PI 3.14159265f  //円周率
+
+//bole target
+#define TARGET1 150.0f      //２段目の位置
+#define TARGET2 0.0f        //１段目の位置
+#define TARGET3 30.0f       //移動時の位置
+
+//blossom target
+#define TARGET4 470.0f      //桜の位置
+
+//hand power
+#define CLOSE 0.5           //回収pwm値
+#define OPEN 0.3            //解放pwm値
+
+//target-range
+#define LIMITR 410          //右赤外線の判断距離[mm]
+#define LIMITL 410          //左赤外線の判断距離[mm]
+#define UWMAX 400           //超音波の許容最大値[mm]
+#define UWMIN 375           //超音波の許容最小値[mm]
+
+//arm limit
+#define HMAX 10             //昇降完了判断上限[mm]
+#define HMIN -10            //昇降完了判断下限[mm]
+#define PUT 5               //幹を離す高さ[mm]
+
+//pwm-MAX
+#define MAX 0.8f            //出力の上限
+
+
+//pid-coefficient
+//arm
+float Kp_a=4.3f;
+float Kd_a=0.0f;
+float Ki_a=1.1f;
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int arm=0;      //昇降完了判断カウント
+/*  arm=1->１段目完了
+    arm=2->２段目完了
+    arm=3->３段目完了、幹完成
+    arm=4->雑木林に接地完了
+    arm=5->桜取得完了         */
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "pinMode.h"        //使用するピンの宣言及び変数の定義
+#include "decision.h"       //幹・桜を感知する関数の定義（タイマー割り込み使用）
+#include "function.h"       //pid,valve,interrupt,setup関数を定義
+#include "step.h"           //各作業段階の動作を定義
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+int main(){
+    
+    setup();
+    
+    //set-Ticker
+    bole_checker.attach(bole_check,0.2);
+    
+    while(1){
+        
+//測距関係により検知
+        if(UW*IR){   //幹検知
+            n++;
+            bole_checker.detach();
+        }
+        
+        if(n==1){
+            expand_gf();
+        }else if(n==2){
+            expand_ff();
+        }else if(n>2){
+            expand_sf();
+        }
+/*
+//自己位置による条件
+        if(){//雑木林
+            set_bole();
+        }
+        
+        if(){//花見場
+            set_bole();
+        }
+                
+        if(){//桜
+            blossom();
+        }
+*/
+
+        pc.printf("|%d|\t%d\t%f\t%f\t%f\n\r",target,count,dist_n,devi,pwm);
+        wait(0.1);
+    }
+
+}