Diff: header.h

Revision:

0:df2659fd8031

Child:

1:a1e592eca305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/header.h	Sun Aug 28 17:03:34 2016 +0000
@@ -0,0 +1,173 @@
+//headerではPINを直接扱わない応用関数を扱う
+
+#include <math.h>
+#include"mbed.h"
+#include "locate.h"
+#include"prime.h"
+
+const int speed=100;//基本速度
+const float allowdegree = 0.02;//角度許容誤差
+const int speedscaler = 10;//角度と速度を変換
+const int minspeed = 50;//最小速度
+const int allowlength=30;
+
+
+
+
+
+void moveto(int targetx, int targety){
+    int x, y;
+    float theta, phi;
+    int onoff = 1;
+
+    //よく使うかたまり
+        x = coordinateX();
+        y = coordinateY();
+        theta = coordinateTheta();//自己位置取得
+        phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
+
+
+    //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
+    while (1){
+        x = coordinateX();
+        y = coordinateY();
+        theta = coordinateTheta();
+        phi = atan(double(targety - y) / double(targetx - x));
+
+        move((-1)*speed, speed);
+        if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
+    }
+
+    //角度を合わせながら直進
+    while (onoff){
+        x = coordinateX();
+        y = coordinateY();
+        theta = coordinateTheta();
+        phi = atan(double(targety - y) / double(targetx - x));
+
+        move(speed + 10 * (phi - theta), speed); //この10はradを回転数に影響させるスカラー。右輪のみで方向を調節するが、
+
+        if (targetx - allowlength < x&&x < targetx + allowlength){
+            //x座標が合ったら
+            if (targety - allowlength < y&&y < targety + allowlength){
+                onoff = 0;//whileから抜ける
+            }
+        }
+    }
+    //位置合わせ完了(最終的にはx,yどっちか先に合わせたい)
+}
+
+
+
+void back(){
+
+    int x1, y1, x2, y2,distance;
+
+    x1 = coordinateX();
+    y1= coordinateY();
+
+
+    move((-1)*speed, (-1) *speed);
+
+    while (1){
+        x2 = coordinateX();
+        y2 = coordinateY();
+        distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
+        
+        if (distance > 22500){ 
+            move(0, 0);
+            break; }
+    }
+
+
+}
+
+
+void fixto(int targetx,int targety){//必ずy正方向からとることとする
+    int onoff=1,lr=0;
+const int X=20;//左右の位置調整。回転数をどれだけ増減させるかをこの定数に入れる
+
+    while (onoff){
+            //(right,left)=(off,off),(on,off),(off,on),(on,on)
+             //                 0        1         2       3    
+        lr = sensor();
+        
+        switch (lr){
+        
+        case 0:
+            break;
+        case 1:
+            move(speed-X , speed+X);
+            break;
+        case 2:
+            move(speed+X , speed-X);
+            break;
+        case 3:
+            move(speed,speed);
+            break;
+        default:
+            break;
+        }
+
+    }
+}
+
+
+
+
+
+void lift(int level) {//段数を指定して回転させる。
+    const int K=33;//階数と回転角度を反映させる比例点定数
+
+    step(K*level); 
+        
+        
+}
+
+void release(void){
+
+    step(100);
+
+}
+
+void extraordinary(){
+
+    int x, y,targetx,targety;
+    float theta, phi;
+
+    targetx=sensor()*1819;
+    targety=1750;
+    moveto(targetx,0);
+    
+    //よく使うかたまり
+        x = coordinateX();
+        y = coordinateY();
+        theta = coordinateTheta();//自己位置取得
+        phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
+        int goalx = 0, goaly = blue()*1200;//受け渡しゾーンの中心
+
+    //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
+    while (1){
+        x = coordinateX();
+        y = coordinateY();
+        theta = coordinateTheta();
+        phi = atan(double(targety - y) / double(targetx - x));
+
+        move((-1)*speed, speed);
+        if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
+    }
+    
+    open_arm();
+    open_hand();
+        
+    moveto(targetx,1750);
+    
+    close_hand();
+    close_arm();
+    lift(1);
+    
+    moveto(goalx,goaly);
+}
+
+
+