Tk A
/
f3rc3
fdlsj
Fork of f3rc2 by
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?
User | Revision | Line number | New 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 |