統合プログラム
Dependencies: mbed Servo BMP180
direction.h@1:bb89b58cfa0e, 2021-10-27 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Wed Oct 27 19:11:01 2021 +0000
- Revision:
- 1:bb89b58cfa0e
- Parent:
- 0:e7b7def631c2
- Child:
- 2:d2cb6b50a8c4
almost complete
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
minanao | 0:e7b7def631c2 | 1 | #include "mbed.h" |
minanao | 0:e7b7def631c2 | 2 | #include "getGPS.h" |
tsubasa_nakajima | 1:bb89b58cfa0e | 3 | #include "Movement.h" |
minanao | 0:e7b7def631c2 | 4 | |
minanao | 0:e7b7def631c2 | 5 | GPS gps(D1, D0); |
minanao | 0:e7b7def631c2 | 6 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 7 | float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 8 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 9 | //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 10 | //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180) |
tsubasa_nakajima | 1:bb89b58cfa0e | 11 | //theta_0,theta_1は北が90、東が0 |
tsubasa_nakajima | 1:bb89b58cfa0e | 12 | //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り) |
tsubasa_nakajima | 1:bb89b58cfa0e | 13 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 14 | float theta_0,theta_1; |
minanao | 0:e7b7def631c2 | 15 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 16 | if(x_0 == x_1 && x_1 == x_2){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 17 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 18 | if(y_0 - y_1 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 19 | theta_0 = 90; |
tsubasa_nakajima | 1:bb89b58cfa0e | 20 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 21 | if(y_0 - y_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 22 | theta_0 = 270; |
tsubasa_nakajima | 1:bb89b58cfa0e | 23 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 24 | if(y_0 - y_1 == 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 25 | theta_0 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 26 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 27 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 28 | if(y_1 - y_2 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 29 | theta_1 = 90; |
tsubasa_nakajima | 1:bb89b58cfa0e | 30 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 31 | if(y_1 - y_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 32 | theta_1 = 270; |
tsubasa_nakajima | 1:bb89b58cfa0e | 33 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 34 | if(y_1 - y_2 == 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 35 | theta_1 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 36 | } |
minanao | 0:e7b7def631c2 | 37 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 38 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 39 | if(x_0 == x_1 && x_1 != x_2){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 40 | theta_1 = atan((y_1 - y_2)/(x_1- x_2)); |
tsubasa_nakajima | 1:bb89b58cfa0e | 41 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 42 | if(y_0 - y_1 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 43 | theta_0 = 90; |
tsubasa_nakajima | 1:bb89b58cfa0e | 44 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 45 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 46 | if(y_0 - y_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 47 | theta_0 = 270; |
tsubasa_nakajima | 1:bb89b58cfa0e | 48 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 49 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 50 | if(y_0 - y_1 == 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 51 | theta_0 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 52 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 53 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 54 | if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 55 | theta_1 = theta_1 - 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 56 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 57 | if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 58 | theta_1 = 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 59 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 60 | if(y_1 - y_2 < 0 && x_1 - x_2 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 61 | theta_1 = theta_1 + 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 62 | } |
minanao | 0:e7b7def631c2 | 63 | } |
minanao | 0:e7b7def631c2 | 64 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 65 | if(x_0 != x_1 && x_1 == x_2){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 66 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 67 | theta_0 = atan((y_0 - y_1)/(x_0 - x_1)); |
minanao | 0:e7b7def631c2 | 68 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 69 | if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 70 | theta_0 = theta_0 - 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 71 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 72 | if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 73 | theta_0 = 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 74 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 75 | if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 76 | theta_0 = theta_0 + 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 77 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 78 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 79 | if(y_1 - y_2 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 80 | theta_1 = 90; |
tsubasa_nakajima | 1:bb89b58cfa0e | 81 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 82 | if(y_1 - y_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 83 | theta_1 = 270; |
tsubasa_nakajima | 1:bb89b58cfa0e | 84 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 85 | if(y_1 - y_2 == 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 86 | theta_1 = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 87 | } |
minanao | 0:e7b7def631c2 | 88 | } |
minanao | 0:e7b7def631c2 | 89 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 90 | else{ |
tsubasa_nakajima | 1:bb89b58cfa0e | 91 | theta_0 = atan((y_0 - y_1)/(x_0 - x_1)); |
tsubasa_nakajima | 1:bb89b58cfa0e | 92 | theta_1 = atan((y_1 - y_2)/(x_1- x_2)); |
minanao | 0:e7b7def631c2 | 93 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 94 | if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 95 | theta_0 = theta_0 - 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 96 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 97 | if(y_0 - y_1 == 0 && x_0 - x_1 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 98 | theta_0 = 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 99 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 100 | if(y_0 - y_1 < 0 && x_0 - x_1 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 101 | theta_0 = theta_0 + 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 102 | } |
minanao | 0:e7b7def631c2 | 103 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 104 | if(y_1 - y_2 > 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 105 | theta_1 = theta_1 - 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 106 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 107 | if(y_1 - y_2 == 0 && x_1 - x_2 < 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 108 | theta_1 = 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 109 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 110 | if(y_1 - y_2 < 0 && x_1- x_2 > 0){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 111 | theta_1 = theta_1 + 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 112 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 113 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 114 | |
minanao | 0:e7b7def631c2 | 115 | } |
minanao | 0:e7b7def631c2 | 116 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 117 | float theta = theta_0 - theta_1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 118 | if(theta < -180){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 119 | theta = 360 - theta; |
tsubasa_nakajima | 1:bb89b58cfa0e | 120 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 121 | if(theta > 180){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 122 | theta = -360 + theta; |
tsubasa_nakajima | 1:bb89b58cfa0e | 123 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 124 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 125 | return theta; |
tsubasa_nakajima | 1:bb89b58cfa0e | 126 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 127 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 128 | // 球面三角法により、大円距離(メートル)を求める |
tsubasa_nakajima | 1:bb89b58cfa0e | 129 | double distance(double lat1, double lng1, double lat2, double lng2) { |
tsubasa_nakajima | 1:bb89b58cfa0e | 130 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 131 | // 円周率 |
tsubasa_nakajima | 1:bb89b58cfa0e | 132 | const double pi = 3.14159265359; |
tsubasa_nakajima | 1:bb89b58cfa0e | 133 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 134 | // 緯度経度をラジアンに変換 |
tsubasa_nakajima | 1:bb89b58cfa0e | 135 | double rlat1 = lat1 * pi / 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 136 | double rlng1 = lng1 * pi / 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 137 | double rlat2 = lat2 * pi / 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 138 | double rlng2 = lng2 * pi / 180; |
tsubasa_nakajima | 1:bb89b58cfa0e | 139 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 140 | // 2点の中心角(ラジアン)を求める |
tsubasa_nakajima | 1:bb89b58cfa0e | 141 | double a = |
tsubasa_nakajima | 1:bb89b58cfa0e | 142 | sin(rlat1) * sin(rlat2) + |
tsubasa_nakajima | 1:bb89b58cfa0e | 143 | cos(rlat1) * cos(rlat2) * |
tsubasa_nakajima | 1:bb89b58cfa0e | 144 | cos(rlng1 - rlng2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 145 | double rr = acos(a); |
tsubasa_nakajima | 1:bb89b58cfa0e | 146 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 147 | // 地球赤道半径(メートル) |
tsubasa_nakajima | 1:bb89b58cfa0e | 148 | const double earth_radius = 6378140; |
tsubasa_nakajima | 1:bb89b58cfa0e | 149 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 150 | // 2点間の距離(メートル) |
tsubasa_nakajima | 1:bb89b58cfa0e | 151 | double distance = earth_radius * rr; |
tsubasa_nakajima | 1:bb89b58cfa0e | 152 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 153 | return distance; |
tsubasa_nakajima | 1:bb89b58cfa0e | 154 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 155 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 156 | class direction |
tsubasa_nakajima | 1:bb89b58cfa0e | 157 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 158 | private: |
tsubasa_nakajima | 1:bb89b58cfa0e | 159 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 160 | int s; |
tsubasa_nakajima | 1:bb89b58cfa0e | 161 | float x_0 ,y_0; //中間地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 162 | float x_01,y_01; //ゴール地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 163 | float x_1 ,y_1; //現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 164 | float x_2 ,y_2; //20秒前の現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 165 | float theta; //CanSatから見た目的地の角度 |
tsubasa_nakajima | 1:bb89b58cfa0e | 166 | float d,d1,d2; //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm) |
tsubasa_nakajima | 1:bb89b58cfa0e | 167 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 168 | public: |
tsubasa_nakajima | 1:bb89b58cfa0e | 169 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 170 | //歩行 |
tsubasa_nakajima | 1:bb89b58cfa0e | 171 | int walk(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 172 | }; |
tsubasa_nakajima | 1:bb89b58cfa0e | 173 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 174 | int direction::walk(){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 175 | s = 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 176 | x_0 = NULL; |
tsubasa_nakajima | 1:bb89b58cfa0e | 177 | y_0 = NULL; |
tsubasa_nakajima | 1:bb89b58cfa0e | 178 | x_01 = NULL; |
tsubasa_nakajima | 1:bb89b58cfa0e | 179 | y_01 = NULL; |
tsubasa_nakajima | 1:bb89b58cfa0e | 180 | while(s<1){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 181 | if(gps.getgps()){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 182 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 183 | y_1 = gps.latitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 184 | s = s + 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 185 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 186 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 187 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 188 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 189 | while(1) { |
minanao | 0:e7b7def631c2 | 190 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 191 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 192 | x_2 = x_1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 193 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 194 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 195 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 196 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 197 | //中間地点に到達したらbreak |
tsubasa_nakajima | 1:bb89b58cfa0e | 198 | d1 = distance(y_1,x_1,y_0,x_0); |
tsubasa_nakajima | 1:bb89b58cfa0e | 199 | if(d1 < 15){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 200 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 201 | break; |
tsubasa_nakajima | 1:bb89b58cfa0e | 202 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 203 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 204 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 1:bb89b58cfa0e | 205 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 206 | if(d < 3){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 207 | Movement.wakeup(2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 208 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 209 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 210 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 211 | //Θによって回転か直進か決定 |
tsubasa_nakajima | 1:bb89b58cfa0e | 212 | theta = calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 213 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 214 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 215 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 216 | if(theta >=30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 217 | Movement.turn_right(int theta); |
tsubasa_nakajima | 1:bb89b58cfa0e | 218 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 219 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 220 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 221 | if(theta <= -30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 222 | Movement.turn_left(int theta); |
tsubasa_nakajima | 1:bb89b58cfa0e | 223 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 224 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 225 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 226 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 227 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 228 | |
minanao | 0:e7b7def631c2 | 229 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 230 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 231 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 232 | while(1) { |
tsubasa_nakajima | 1:bb89b58cfa0e | 233 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 234 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 235 | x_2 = x_1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 236 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 237 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 238 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 239 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 240 | //ゴール地点に到達したらbreak |
tsubasa_nakajima | 1:bb89b58cfa0e | 241 | d2 = distance(y_1,x_1,y_01,x_01); |
tsubasa_nakajima | 1:bb89b58cfa0e | 242 | if(d2 < 15){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 243 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 244 | break; |
tsubasa_nakajima | 1:bb89b58cfa0e | 245 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 246 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 247 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 1:bb89b58cfa0e | 248 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 249 | if(d < 3){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 250 | Movement.wakeup(2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 251 | Movement.stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 252 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 253 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 254 | //thetaによって回転か直進か決定 |
tsubasa_nakajima | 1:bb89b58cfa0e | 255 | theta = calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 256 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 257 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 258 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 259 | if(theta >=30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 260 | Movement.turn_right(int theta); |
tsubasa_nakajima | 1:bb89b58cfa0e | 261 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 262 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 263 | if(theta <= -30){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 264 | Movement.turn_left(int theta); |
tsubasa_nakajima | 1:bb89b58cfa0e | 265 | Movement.move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 266 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 267 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 268 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 269 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 270 | |
minanao | 0:e7b7def631c2 | 271 | } |
minanao | 0:e7b7def631c2 | 272 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 273 | Movement.stop(); |
minanao | 0:e7b7def631c2 | 274 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 275 | return 0; |
tsubasa_nakajima | 1:bb89b58cfa0e | 276 | } |