header.h

00001 //headerではPINを直接扱わない応用関数を扱う
00002 
00003 #include <math.h>
00004 #include"mbed.h"
00005 #include "locate.h"
00006 #include"prime.h"
00007 
00008 const int speed=100;//基本速度
00009 const float allowdegree = 0.02;//角度許容誤差
00010 const int speedscaler = 10;//角度と速度を変換
00011 const int minspeed = 50;//最小速度
00012 const int allowlength=30;
00013 
00014 
00015 
00016 
00017 
00018 void moveto(int targetx, int targety){
00019     int x, y;
00020     float theta, phi;
00021     int onoff = 1;
00022 
00023     //よく使うかたまり
00024         x = coordinateX();
00025         y = coordinateY();
00026         theta = coordinateTheta();//自己位置取得
00027         phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
00028 
00029 
00030     //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
00031     while (1){
00032         x = coordinateX();
00033         y = coordinateY();
00034         theta = coordinateTheta();
00035         phi = atan(double(targety - y) / double(targetx - x));
00036 
00037         move((-1)*speed, speed);
00038         if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
00039     }
00040 
00041     //角度を合わせながら直進
00042     while (onoff){
00043         x = coordinateX();
00044         y = coordinateY();
00045         theta = coordinateTheta();
00046         phi = atan(double(targety - y) / double(targetx - x));
00047 
00048         move(speed + 10 * (phi - theta), speed); //この10はradを回転数に影響させるスカラー。右輪のみで方向を調節するが、
00049 
00050         if (targetx - allowlength < x&&x < targetx + allowlength){
00051             //x座標が合ったら
00052             if (targety - allowlength < y&&y < targety + allowlength){
00053                 onoff = 0;//whileから抜ける
00054             }
00055         }
00056     }
00057     //位置合わせ完了(最終的にはx,yどっちか先に合わせたい)
00058 }
00059 
00060 
00061 
00062 void back(){
00063 
00064     int x1, y1, x2, y2,distance;
00065 
00066     x1 = coordinateX();
00067     y1= coordinateY();
00068 
00069 
00070     move((-1)*speed, (-1) *speed);
00071 
00072     while (1){
00073         x2 = coordinateX();
00074         y2 = coordinateY();
00075         distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
00076         
00077         if (distance > 22500){ 
00078             move(0, 0);
00079             break; }
00080     }
00081 
00082 
00083 }
00084 
00085 
00086 void fixto(int targetx,int targety){//必ずy正方向からとることとする
00087     int onoff=1,lr=0;
00088 const int X=20;//左右の位置調整。回転数をどれだけ増減させるかをこの定数に入れる
00089 
00090     while (onoff){
00091             //(right,left)=(off,off),(on,off),(off,on),(on,on)
00092              //                 0        1         2       3    
00093         lr = sensor();
00094         
00095         switch (lr){
00096         
00097         case 0:
00098             break;
00099         case 1:
00100             move(speed-X , speed+X);
00101             break;
00102         case 2:
00103             move(speed+X , speed-X);
00104             break;
00105         case 3:
00106             move(speed,speed);
00107             break;
00108         default:
00109             break;
00110         }
00111 
00112     }
00113 }
00114 
00115 
00116 
00117 
00118 
00119 void lift(int level) {//段数を指定して回転させる。
00120     const int K=33;//階数と回転角度を反映させる比例点定数
00121 
00122     step(K*level); 
00123         
00124         
00125 }
00126 
00127 void release(void){
00128 
00129     step(100);
00130 
00131 }
00132 
00133