涼太郎 中村 / Mbed 2 deprecated f3rc2

めいん

Dependencies:   mbed

header.h@0:df2659fd8031, 2016-08-28 (annotated)

Committer:
choutin
Date:
Sun Aug 28 17:03:34 2016 +0000
Revision:
0:df2659fd8031
Child:
1:a1e592eca305
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
choutin 0:df2659fd8031 1 //headerではPINを直接扱わない応用関数を扱う
choutin 0:df2659fd8031 2
choutin 0:df2659fd8031 3 #include <math.h>
choutin 0:df2659fd8031 4 #include"mbed.h"
choutin 0:df2659fd8031 5 #include "locate.h"
choutin 0:df2659fd8031 6 #include"prime.h"
choutin 0:df2659fd8031 7
choutin 0:df2659fd8031 8 const int speed=100;//基本速度
choutin 0:df2659fd8031 9 const float allowdegree = 0.02;//角度許容誤差
choutin 0:df2659fd8031 10 const int speedscaler = 10;//角度と速度を変換
choutin 0:df2659fd8031 11 const int minspeed = 50;//最小速度
choutin 0:df2659fd8031 12 const int allowlength=30;
choutin 0:df2659fd8031 13
choutin 0:df2659fd8031 14
choutin 0:df2659fd8031 15
choutin 0:df2659fd8031 16
choutin 0:df2659fd8031 17
choutin 0:df2659fd8031 18 void moveto(int targetx, int targety){
choutin 0:df2659fd8031 19 int x, y;
choutin 0:df2659fd8031 20 float theta, phi;
choutin 0:df2659fd8031 21 int onoff = 1;
choutin 0:df2659fd8031 22
choutin 0:df2659fd8031 23 //よく使うかたまり
choutin 0:df2659fd8031 24 x = coordinateX();
choutin 0:df2659fd8031 25 y = coordinateY();
choutin 0:df2659fd8031 26 theta = coordinateTheta();//自己位置取得
choutin 0:df2659fd8031 27 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
choutin 0:df2659fd8031 28
choutin 0:df2659fd8031 29
choutin 0:df2659fd8031 30 //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
choutin 0:df2659fd8031 31 while (1){
choutin 0:df2659fd8031 32 x = coordinateX();
choutin 0:df2659fd8031 33 y = coordinateY();
choutin 0:df2659fd8031 34 theta = coordinateTheta();
choutin 0:df2659fd8031 35 phi = atan(double(targety - y) / double(targetx - x));
choutin 0:df2659fd8031 36
choutin 0:df2659fd8031 37 move((-1)*speed, speed);
choutin 0:df2659fd8031 38 if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
choutin 0:df2659fd8031 39 }
choutin 0:df2659fd8031 40
choutin 0:df2659fd8031 41 //角度を合わせながら直進
choutin 0:df2659fd8031 42 while (onoff){
choutin 0:df2659fd8031 43 x = coordinateX();
choutin 0:df2659fd8031 44 y = coordinateY();
choutin 0:df2659fd8031 45 theta = coordinateTheta();
choutin 0:df2659fd8031 46 phi = atan(double(targety - y) / double(targetx - x));
choutin 0:df2659fd8031 47
choutin 0:df2659fd8031 48 move(speed + 10 * (phi - theta), speed); //この10はradを回転数に影響させるスカラー。右輪のみで方向を調節するが、
choutin 0:df2659fd8031 49
choutin 0:df2659fd8031 50 if (targetx - allowlength < x&&x < targetx + allowlength){
choutin 0:df2659fd8031 51 //x座標が合ったら
choutin 0:df2659fd8031 52 if (targety - allowlength < y&&y < targety + allowlength){
choutin 0:df2659fd8031 53 onoff = 0;//whileから抜ける
choutin 0:df2659fd8031 54 }
choutin 0:df2659fd8031 55 }
choutin 0:df2659fd8031 56 }
choutin 0:df2659fd8031 57 //位置合わせ完了(最終的にはx,yどっちか先に合わせたい)
choutin 0:df2659fd8031 58 }
choutin 0:df2659fd8031 59
choutin 0:df2659fd8031 60
choutin 0:df2659fd8031 61
choutin 0:df2659fd8031 62 void back(){
choutin 0:df2659fd8031 63
choutin 0:df2659fd8031 64 int x1, y1, x2, y2,distance;
choutin 0:df2659fd8031 65
choutin 0:df2659fd8031 66 x1 = coordinateX();
choutin 0:df2659fd8031 67 y1= coordinateY();
choutin 0:df2659fd8031 68
choutin 0:df2659fd8031 69
choutin 0:df2659fd8031 70 move((-1)*speed, (-1) *speed);
choutin 0:df2659fd8031 71
choutin 0:df2659fd8031 72 while (1){
choutin 0:df2659fd8031 73 x2 = coordinateX();
choutin 0:df2659fd8031 74 y2 = coordinateY();
choutin 0:df2659fd8031 75 distance = (x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2);
choutin 0:df2659fd8031 76
choutin 0:df2659fd8031 77 if (distance > 22500){
choutin 0:df2659fd8031 78 move(0, 0);
choutin 0:df2659fd8031 79 break; }
choutin 0:df2659fd8031 80 }
choutin 0:df2659fd8031 81
choutin 0:df2659fd8031 82
choutin 0:df2659fd8031 83 }
choutin 0:df2659fd8031 84
choutin 0:df2659fd8031 85
choutin 0:df2659fd8031 86 void fixto(int targetx,int targety){//必ずy正方向からとることとする
choutin 0:df2659fd8031 87 int onoff=1,lr=0;
choutin 0:df2659fd8031 88 const int X=20;//左右の位置調整。回転数をどれだけ増減させるかをこの定数に入れる
choutin 0:df2659fd8031 89
choutin 0:df2659fd8031 90 while (onoff){
choutin 0:df2659fd8031 91 //(right,left)=(off,off),(on,off),(off,on),(on,on)
choutin 0:df2659fd8031 92 // 0 1 2 3
choutin 0:df2659fd8031 93 lr = sensor();
choutin 0:df2659fd8031 94
choutin 0:df2659fd8031 95 switch (lr){
choutin 0:df2659fd8031 96
choutin 0:df2659fd8031 97 case 0:
choutin 0:df2659fd8031 98 break;
choutin 0:df2659fd8031 99 case 1:
choutin 0:df2659fd8031 100 move(speed-X , speed+X);
choutin 0:df2659fd8031 101 break;
choutin 0:df2659fd8031 102 case 2:
choutin 0:df2659fd8031 103 move(speed+X , speed-X);
choutin 0:df2659fd8031 104 break;
choutin 0:df2659fd8031 105 case 3:
choutin 0:df2659fd8031 106 move(speed,speed);
choutin 0:df2659fd8031 107 break;
choutin 0:df2659fd8031 108 default:
choutin 0:df2659fd8031 109 break;
choutin 0:df2659fd8031 110 }
choutin 0:df2659fd8031 111
choutin 0:df2659fd8031 112 }
choutin 0:df2659fd8031 113 }
choutin 0:df2659fd8031 114
choutin 0:df2659fd8031 115
choutin 0:df2659fd8031 116
choutin 0:df2659fd8031 117
choutin 0:df2659fd8031 118
choutin 0:df2659fd8031 119 void lift(int level) {//段数を指定して回転させる。
choutin 0:df2659fd8031 120 const int K=33;//階数と回転角度を反映させる比例点定数
choutin 0:df2659fd8031 121
choutin 0:df2659fd8031 122 step(K*level);
choutin 0:df2659fd8031 123
choutin 0:df2659fd8031 124
choutin 0:df2659fd8031 125 }
choutin 0:df2659fd8031 126
choutin 0:df2659fd8031 127 void release(void){
choutin 0:df2659fd8031 128
choutin 0:df2659fd8031 129 step(100);
choutin 0:df2659fd8031 130
choutin 0:df2659fd8031 131 }
choutin 0:df2659fd8031 132
choutin 0:df2659fd8031 133 void extraordinary(){
choutin 0:df2659fd8031 134
choutin 0:df2659fd8031 135 int x, y,targetx,targety;
choutin 0:df2659fd8031 136 float theta, phi;
choutin 0:df2659fd8031 137
choutin 0:df2659fd8031 138 targetx=sensor()*1819;
choutin 0:df2659fd8031 139 targety=1750;
choutin 0:df2659fd8031 140 moveto(targetx,0);
choutin 0:df2659fd8031 141
choutin 0:df2659fd8031 142 //よく使うかたまり
choutin 0:df2659fd8031 143 x = coordinateX();
choutin 0:df2659fd8031 144 y = coordinateY();
choutin 0:df2659fd8031 145 theta = coordinateTheta();//自己位置取得
choutin 0:df2659fd8031 146 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
choutin 0:df2659fd8031 147 int goalx = 0, goaly = blue()*1200;//受け渡しゾーンの中心
choutin 0:df2659fd8031 148
choutin 0:df2659fd8031 149 //まず目的地方面に回転。今回は必ず時計回りで位置合わせするが今後最短経路に直す
choutin 0:df2659fd8031 150 while (1){
choutin 0:df2659fd8031 151 x = coordinateX();
choutin 0:df2659fd8031 152 y = coordinateY();
choutin 0:df2659fd8031 153 theta = coordinateTheta();
choutin 0:df2659fd8031 154 phi = atan(double(targety - y) / double(targetx - x));
choutin 0:df2659fd8031 155
choutin 0:df2659fd8031 156 move((-1)*speed, speed);
choutin 0:df2659fd8031 157 if (phi - allowdegree < theta&&theta< phi + allowdegree){ break; }//thetaにpiを足して逆ベクトルを判定
choutin 0:df2659fd8031 158 }
choutin 0:df2659fd8031 159
choutin 0:df2659fd8031 160 open_arm();
choutin 0:df2659fd8031 161 open_hand();
choutin 0:df2659fd8031 162
choutin 0:df2659fd8031 163 moveto(targetx,1750);
choutin 0:df2659fd8031 164
choutin 0:df2659fd8031 165 close_hand();
choutin 0:df2659fd8031 166 close_arm();
choutin 0:df2659fd8031 167 lift(1);
choutin 0:df2659fd8031 168
choutin 0:df2659fd8031 169 moveto(goalx,goaly);
choutin 0:df2659fd8031 170 }
choutin 0:df2659fd8031 171
choutin 0:df2659fd8031 172
choutin 0:df2659fd8031 173