CansatB_2021 / Mbed 2 deprecated cansat_integrated

統合プログラム

Dependencies:   mbed Servo BMP180

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?

UserRevisionLine numberNew 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