修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
direction.h@2:d2cb6b50a8c4, 2021-10-27 (annotated)
- Committer:
- tsubasa_nakajima
- Date:
- Wed Oct 27 20:59:02 2021 +0000
- Revision:
- 2:d2cb6b50a8c4
- Parent:
- 1:bb89b58cfa0e
- Child:
- 3:a583276d9fef
still has bug
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 | 2:d2cb6b50a8c4 | 3 | #include "Servo.h" |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 4 | #include "math.h" |
minanao | 0:e7b7def631c2 | 5 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 6 | //停止 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 7 | void stop(){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 8 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 9 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 10 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 11 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 12 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 13 | Servo servo6(A5); |
minanao | 0:e7b7def631c2 | 14 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 15 | servo1 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 16 | servo2 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 17 | servo3 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 18 | servo4 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 19 | servo5 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 20 | servo6 = 0.5; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 21 | wait(1); |
minanao | 0:e7b7def631c2 | 22 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 23 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 24 | //前進 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 25 | void move_forward(int time = 20) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 26 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 27 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 28 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 29 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 30 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 31 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 32 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 33 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 34 | servo1 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 35 | servo2 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 36 | servo3 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 37 | servo4 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 38 | servo5 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 39 | servo6 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 40 | wait(time); |
minanao | 0:e7b7def631c2 | 41 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 42 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 43 | //後退 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 44 | void move_backward() |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 45 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 46 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 47 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 48 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 49 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 50 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 51 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 52 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 53 | servo1 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 54 | servo2 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 55 | servo3 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 56 | servo4 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 57 | servo5 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 58 | servo6 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 59 | wait(5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 60 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 61 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 62 | //右に曲がる |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 63 | void turn_right(int theta = 15) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 64 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 65 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 66 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 67 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 68 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 69 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 70 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 71 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 72 | servo1 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 73 | servo2 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 74 | servo3 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 75 | servo4 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 76 | servo5 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 77 | servo6 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 78 | wait(theta/15); |
minanao | 0:e7b7def631c2 | 79 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 80 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 81 | //左に曲がる |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 82 | void turn_left(int theta = 15) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 83 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 84 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 85 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 86 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 87 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 88 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 89 | Servo servo6(A5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 90 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 91 | servo1 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 92 | servo2 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 93 | servo3 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 94 | servo4 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 95 | servo5 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 96 | servo6 = 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 97 | wait(theta/15); |
minanao | 0:e7b7def631c2 | 98 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 99 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 100 | //倒れているときの処理 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 101 | void wakeup(int time) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 102 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 103 | Servo servo1(D7); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 104 | Servo servo2(A3); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 105 | Servo servo3(A1); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 106 | Servo servo4(D12); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 107 | Servo servo5(D10); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 108 | Servo servo6(A5); |
tsubasa_nakajima | 1:bb89b58cfa0e | 109 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 110 | int i; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 111 | for(i=1;i<=time;i++) |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 112 | { |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 113 | move_forward(5); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 114 | move_backward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 115 | turn_right(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 116 | turn_left(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 117 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 118 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 119 | class direction |
tsubasa_nakajima | 1:bb89b58cfa0e | 120 | { |
tsubasa_nakajima | 1:bb89b58cfa0e | 121 | private: |
tsubasa_nakajima | 1:bb89b58cfa0e | 122 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 123 | int s; |
tsubasa_nakajima | 1:bb89b58cfa0e | 124 | float x_0 ,y_0; //中間地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 125 | float x_01,y_01; //ゴール地点の座標(未定) |
tsubasa_nakajima | 1:bb89b58cfa0e | 126 | float x_1 ,y_1; //現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 127 | float x_2 ,y_2; //20秒前の現在地 |
tsubasa_nakajima | 1:bb89b58cfa0e | 128 | float theta; //CanSatから見た目的地の角度 |
tsubasa_nakajima | 1:bb89b58cfa0e | 129 | float d,d1,d2; //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm) |
tsubasa_nakajima | 1:bb89b58cfa0e | 130 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 131 | public: |
tsubasa_nakajima | 1:bb89b58cfa0e | 132 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 133 | //歩行 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 134 | void walk(){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 135 | GPS gps(D1, D0); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 136 | s = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 137 | x_0 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 138 | y_0 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 139 | x_01 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 140 | y_01 = 0; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 141 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 142 | while(s<1){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 143 | if(gps.getgps()){ |
tsubasa_nakajima | 1:bb89b58cfa0e | 144 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 145 | y_1 = gps.latitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 146 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 147 | s = s + 1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 148 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 149 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 150 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 151 | while(1){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 152 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 153 | x_2 = x_1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 154 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 155 | x_1 = gps.longitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 156 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 157 | d1 = distance(y_1,x_1,y_0,x_0); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 158 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 159 | theta = calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 160 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 161 | //中間地点に到達したらbreak |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 162 | if(d1 < 15){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 163 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 164 | break; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 165 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 166 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 167 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 168 | if(d < 3){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 169 | wakeup(2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 170 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 171 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 172 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 173 | //Θによって回転か直進か決定 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 174 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 175 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 176 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 177 | if(theta >=30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 178 | turn_right(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 179 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 180 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 181 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 182 | if(theta<=-30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 183 | turn_left(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 184 | stop(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 185 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 186 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 187 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 188 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 189 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 190 | while(s < 2){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 191 | if(gps.getgps()){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 192 | x_1 = gps.longitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 193 | y_1 = gps.latitude; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 194 | move_forward(); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 195 | s = s + 1; |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 196 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 197 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 198 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 199 | while(1){ |
minanao | 0:e7b7def631c2 | 200 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 201 | if(gps.getgps()){ //現在地取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 202 | x_2 = x_1; |
tsubasa_nakajima | 1:bb89b58cfa0e | 203 | y_2 = y_2; //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得 |
tsubasa_nakajima | 1:bb89b58cfa0e | 204 | x_1 = gps.longitude; |
tsubasa_nakajima | 1:bb89b58cfa0e | 205 | y_1 = gps.latitude; //現在地の緯度と経度を取得 |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 206 | d2 = distance(y_1,x_1,y_01,x_01); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 207 | d = distance(y_1,x_1,y_2,x_2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 208 | theta = calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2); |
tsubasa_nakajima | 1:bb89b58cfa0e | 209 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 210 | //ゴール地点に到達したらbreak |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 211 | if(d2 < 15){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 212 | stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 213 | break; |
tsubasa_nakajima | 1:bb89b58cfa0e | 214 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 215 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 216 | //移動距離が短ければ横転と判定し復帰 |
tsubasa_nakajima | 1:bb89b58cfa0e | 217 | if(d < 3){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 218 | wakeup(2); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 219 | stop(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 220 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 221 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 222 | //thetaによって回転か直進か決定 |
tsubasa_nakajima | 1:bb89b58cfa0e | 223 | if(-30 < theta && theta < 30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 224 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 225 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 226 | if(theta >=30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 227 | turn_right(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 228 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 229 | } |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 230 | if(theta<= -30){ |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 231 | turn_left(theta); |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 232 | move_forward(); |
tsubasa_nakajima | 1:bb89b58cfa0e | 233 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 234 | |
tsubasa_nakajima | 1:bb89b58cfa0e | 235 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 236 | |
minanao | 0:e7b7def631c2 | 237 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 238 | } |
tsubasa_nakajima | 1:bb89b58cfa0e | 239 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 240 | stop(); |
minanao | 0:e7b7def631c2 | 241 | } |
minanao | 0:e7b7def631c2 | 242 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 243 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 244 | }; |
minanao | 0:e7b7def631c2 | 245 | |
tsubasa_nakajima | 2:d2cb6b50a8c4 | 246 |