cansat-d_2018 / Mbed 2 deprecated cansat_main

ニクロム線切断、GPS取得、モーター、障害物回避、フォトインタラプタ、（距離推定）、加速度と地磁気のデータ取得

Dependencies:   mbed

Fork of cansat_main by cansat-d_2018

main.cpp@14:984454b9f2ab, 2018-11-09 (annotated)

Committer:
sayan2
Date:
Fri Nov 09 15:05:09 2018 +0000
Revision:
14:984454b9f2ab
Parent:
13:d2f0aef62909 
11/10 0:04 FET??????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
seangshim 0:a01fda36fde8 1 #include "mbed.h"
seangshim 0:a01fda36fde8 2 #include "gps.h"
seangshim 0:a01fda36fde8 3 #include "ultrasonic.h"
seangshim 0:a01fda36fde8 4 #include "motordriver.h"
seangshim 6:574d9a6253c7 5 #include "HMC5883L.h"
seangshim 8:d41f5d7d2aa5 6 #include "MPU6050.h"
seangshim 6:574d9a6253c7 7
seangshim 8:d41f5d7d2aa5 8
seangshim 8:d41f5d7d2aa5 9 Serial pc(USBTX, USBRX); //地磁気センサー,GPS
seangshim 9:fb19a93df88f 10 GPS gps(p28, p27); //GPS
seangshim 9:fb19a93df88f 11 HMC5883L compass(p9, p10); //地磁気センサー
seangshim 9:fb19a93df88f 12
394 10:280a25bcc8bb 13 MPU6050 mpu(p9, p10); //(SDA,SCL)のピンをアサインしてね☆ 加速度センサー
seangshim 9:fb19a93df88f 14
seangshim 0:a01fda36fde8 15
seangshim 1:10af8aaa5b40 16 DigitalOut FET(p21); //FET
seangshim 1:10af8aaa5b40 17
seangshim 1:10af8aaa5b40 18 InterruptIn button1(p15); //フォトインタラプタ
seangshim 2:37d831f82840 19 InterruptIn button2(p18);
seangshim 1:10af8aaa5b40 20 DigitalIn test(p15); //ここでピン15からの電圧の値、つまりフォトインタラプタが何か遮るものを検知すればハイの1を返して、
seangshim 1:10af8aaa5b40 21 //何もないつまりスリットの部分ではローの0を返す。それを変数testに代入している
seangshim 2:37d831f82840 22 DigitalIn test2(p18);
seangshim 1:10af8aaa5b40 23
seangshim 0:a01fda36fde8 24
seangshim 1:10af8aaa5b40 25 Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake モーター
seangshim 0:a01fda36fde8 26 Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
seangshim 0:a01fda36fde8 27
seangshim 0:a01fda36fde8 28 void dist(int distance)
seangshim 0:a01fda36fde8 29 {
seangshim 0:a01fda36fde8 30 //put code here to happen when the distance is changed
seangshim 0:a01fda36fde8 31 printf("Distance changed to %dmm\r\n", distance);
seangshim 0:a01fda36fde8 32 }
seangshim 0:a01fda36fde8 33
seangshim 1:10af8aaa5b40 34 ultrasonic mu(p11, p12, .1, 1, &dist); //Set the trigger pin to D8 and the echo pin to D9 超音波センサー
seangshim 0:a01fda36fde8 35 //have updates every .1 seconds and a timeout after 1
seangshim 0:a01fda36fde8 36 //second, and call dist when the distance changes
seangshim 0:a01fda36fde8 37
seangshim 0:a01fda36fde8 38
seangshim 0:a01fda36fde8 39
seangshim 1:10af8aaa5b40 40 LocalFileSystem local("local"); // Create the local filesystem under the name "local"　　 データ保存
seangshim 0:a01fda36fde8 41
seangshim 0:a01fda36fde8 42 int main() {
seangshim 0:a01fda36fde8 43
seangshim 1:10af8aaa5b40 44
seangshim 3:c1e0db4832b7 45 /* FET = 0; //FET、ニクロム線切断
sayan2 14:984454b9f2ab 46 wait(600);
seangshim 1:10af8aaa5b40 47 FET = 1;
sayan2 14:984454b9f2ab 48 wait(10);
sayan2 14:984454b9f2ab 49 FET = 0;
sayan2 14:984454b9f2ab 50 wait(15);
sayan2 14:984454b9f2ab 51 FET = 1;
sayan2 14:984454b9f2ab 52 wait(10);
sayan2 14:984454b9f2ab 53 FET = 0; */
seangshim 1:10af8aaa5b40 54
seangshim 0:a01fda36fde8 55 motor1.stop(0);
seangshim 0:a01fda36fde8 56 motor2.stop(0);
seangshim 0:a01fda36fde8 57
seangshim 12:2f0c841e6078 58 /* printf("GPS start\r\n");
seangshim 3:c1e0db4832b7 59 FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing
seangshim 1:10af8aaa5b40 60 fprintf(fp, "GPS Start\r\n");
seangshim 0:a01fda36fde8 61 int n;
seangshim 1:10af8aaa5b40 62 for(n=0;n<45;n++) //GPSの取得を45回行う(これで大体1分半)
seangshim 0:a01fda36fde8 63 {
seangshim 9:fb19a93df88f 64 printf("gps for\r\n");
seangshim 0:a01fda36fde8 65 if(gps.getgps()) //現在地取得
seangshim 0:a01fda36fde8 66 fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
seangshim 0:a01fda36fde8 67
seangshim 0:a01fda36fde8 68 else
seangshim 0:a01fda36fde8 69 fprintf(fp,"No data\r\n");//データ取得に失敗した場合
seangshim 0:a01fda36fde8 70
seangshim 0:a01fda36fde8 71 wait(1);
seangshim 1:10af8aaa5b40 72
seangshim 1:10af8aaa5b40 73 printf("%d\r\n",n); //今何回目かを出力（本番ではいらない）
seangshim 1:10af8aaa5b40 74
seangshim 0:a01fda36fde8 75 }
seangshim 0:a01fda36fde8 76 fprintf(fp,"GPS finish\r\n");
seangshim 12:2f0c841e6078 77 fclose(fp); //GPSの測定終了 */
seangshim 9:fb19a93df88f 78
seangshim 9:fb19a93df88f 79
seangshim 6:574d9a6253c7 80
seangshim 9:fb19a93df88f 81
394 10:280a25bcc8bb 82 compass.init(); //地磁気センサー動作
seangshim 9:fb19a93df88f 83
394 10:280a25bcc8bb 84 int i;
394 10:280a25bcc8bb 85 for(i=0;i<20;i++) //地磁気測定
394 10:280a25bcc8bb 86 {
394 10:280a25bcc8bb 87 pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
394 10:280a25bcc8bb 88 wait(0.5);
394 10:280a25bcc8bb 89 }
394 10:280a25bcc8bb 90 float mc1,mc2;
394 10:280a25bcc8bb 91 mc1=3.0;
394 10:280a25bcc8bb 92 mc2=3.0;
394 10:280a25bcc8bb 93 //地磁気センサのキャリブレーション
394 10:280a25bcc8bb 94 motor1.speed(mc1); //車体を時計回りに３秒回転
394 10:280a25bcc8bb 95 motor2.speed(-mc2);
394 10:280a25bcc8bb 96 wait(2);
394 10:280a25bcc8bb 97
394 10:280a25bcc8bb 98 motor1.stop(0);
394 10:280a25bcc8bb 99 motor2.stop(0);
394 10:280a25bcc8bb 100 wait(1);
394 10:280a25bcc8bb 101
394 10:280a25bcc8bb 102 motor1.speed(-mc1); //車体を反時計回りに３秒回転
394 10:280a25bcc8bb 103 motor2.speed(mc2);
394 10:280a25bcc8bb 104 wait(2);
394 10:280a25bcc8bb 105
394 10:280a25bcc8bb 106 motor1.stop(0);
394 10:280a25bcc8bb 107 motor2.stop(0);
394 10:280a25bcc8bb 108 wait(1);
394 10:280a25bcc8bb 109 printf("compass carriblation\r\n"); //キャリブレーション終了
394 10:280a25bcc8bb 110
394 10:280a25bcc8bb 111 float mcr1,mcr2,mcl1,mcl2;
394 10:280a25bcc8bb 112 double heading;
394 10:280a25bcc8bb 113 mcr1=heading*10*0.00026;
394 10:280a25bcc8bb 114 mcr2=heading*10*0.00026;
394 10:280a25bcc8bb 115 mcl1=(PI2-heading)*10*0.00026;
394 10:280a25bcc8bb 116 mcl2=(PI2-heading)*10*0.00026;
394 10:280a25bcc8bb 117 compass.init();
394 10:280a25bcc8bb 118 if(2.5<heading<=M_PI){
394 10:280a25bcc8bb 119 motor1.speed(mcr1);
394 10:280a25bcc8bb 120 motor2.speed(-mcr2);
394 10:280a25bcc8bb 121 wait(1);
394 10:280a25bcc8bb 122 }else if(M_PI<heading<357.5){
394 10:280a25bcc8bb 123 motor1.speed(-mcl1);
394 10:280a25bcc8bb 124 motor2.speed(mcl2);
394 10:280a25bcc8bb 125 wait(1);
394 10:280a25bcc8bb 126 }else{
394 10:280a25bcc8bb 127 wait(2);
394 10:280a25bcc8bb 128 }
394 10:280a25bcc8bb 129 printf("searchN\r\n"); //機体が北を向く
394 10:280a25bcc8bb 130
394 10:280a25bcc8bb 131
seangshim 0:a01fda36fde8 132
seangshim 0:a01fda36fde8 133 mu.startUpdates();//start mesuring the distance(超音波センサー)
seangshim 3:c1e0db4832b7 134 int distance;
seangshim 0:a01fda36fde8 135
seangshim 2:37d831f82840 136 int flag,flag2; //変数flagを整数で型づけする。これがスイッチで、1の間は瞬間は何もしないけど、
ShuntaAoji 13:d2f0aef62909 137 flag=0;
ShuntaAoji 13:d2f0aef62909 138 flag2=0;
ShuntaAoji 13:d2f0aef62909 139 //スリットの間隔であるπ/4とタイヤの半径70mmつまり一つのスリットを通過するごとに52.5mm加算していく必要があるから
ShuntaAoji 13:d2f0aef62909 140 //0になった瞬間はこれを総距離に加えるというスイッチの役割をする。
seangshim 2:37d831f82840 141 float rightrun; //変数runをフロートで型づけする
seangshim 2:37d831f82840 142 float leftrun2;
seangshim 4:8b52fd631b32 143 rightrun=0.0;
seangshim 4:8b52fd631b32 144 leftrun2=0.0;
seangshim 9:fb19a93df88f 145
seangshim 9:fb19a93df88f 146 float accel[3];//accelを3つの配列で定義。 加速度センサー
seangshim 1:10af8aaa5b40 147
seangshim 3:c1e0db4832b7 148 while(1)
seangshim 3:c1e0db4832b7 149 {
seangshim 3:c1e0db4832b7 150 distance=mu.getCurrentDistance();
seangshim 3:c1e0db4832b7 151 printf("%d\r\n",distance);
seangshim 8:d41f5d7d2aa5 152
seangshim 8:d41f5d7d2aa5 153 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
seangshim 8:d41f5d7d2aa5 154 float x = accel[0]/16384;//x軸方向の加速度
seangshim 8:d41f5d7d2aa5 155 float y = accel[1]/16384;//y軸方向の加速度
seangshim 8:d41f5d7d2aa5 156 float z = accel[2]/16384;//z軸方向の加速度
seangshim 8:d41f5d7d2aa5 157 printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで～～
seangshim 3:c1e0db4832b7 158
seangshim 3:c1e0db4832b7 159
seangshim 8:d41f5d7d2aa5 160 printf("%d\r\n", test.read()); //フォトインタラプタ
seangshim 3:c1e0db4832b7 161 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 162 if (test.read() == 1 and flag == 0){ //もしtestが1つまり何か障害物があって、かつflagが0つまりスイッチが切れているときは
seangshim 1:10af8aaa5b40 163 flag = 1; //この時はスイッチを1に切る。ただ障害物があるかつスイッチが1で切れているときはそのまま
seangshim 3:c1e0db4832b7 164 printf("test.read if\r\n");
seangshim 1:10af8aaa5b40 165 }
seangshim 1:10af8aaa5b40 166 else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは
seangshim 1:10af8aaa5b40 167 flag = 0; //まずこれでスイッチを0にして入れる。
seangshim 1:10af8aaa5b40 168 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる
seangshim 2:37d831f82840 169 rightrun += 52.5; //総距離runに52.5を加算する
seangshim 3:c1e0db4832b7 170 printf("test.read else\r\n");
seangshim 2:37d831f82840 171 }
seangshim 2:37d831f82840 172 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 173 flag2 = 1;
seangshim 3:c1e0db4832b7 174 printf("test2.read if\r\n");
seangshim 1:10af8aaa5b40 175 }
seangshim 2:37d831f82840 176 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 177 flag2 = 0;
seangshim 2:37d831f82840 178 leftrun2 += 52.5;
seangshim 3:c1e0db4832b7 179 printf("test2.read else\r\n");
seangshim 2:37d831f82840 180 }
ShuntaAoji 13:d2f0aef62909 181 printf("%f", leftrun2);
ShuntaAoji 13:d2f0aef62909 182 printf("\t%f\r\n", rightrun);
seangshim 3:c1e0db4832b7 183 if (rightrun > 250){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する
seangshim 1:10af8aaa5b40 184 break; //つまりゴールについたらこのループからぬける
seangshim 3:c1e0db4832b7 185 }
seangshim 0:a01fda36fde8 186
seangshim 1:10af8aaa5b40 187 motor1.speed(0.5); //通常走行
seangshim 0:a01fda36fde8 188 motor2.speed(0.5);
seangshim 1:10af8aaa5b40 189 //Do something else here
seangshim 1:10af8aaa5b40 190 // mu.checkDistance(); //call checkDistance() as much as possible, as this is where
seangshim 1:10af8aaa5b40 191 //the class checks if dist needs to be called.
seangshim 0:a01fda36fde8 192
seangshim 9:fb19a93df88f 193 wait(0.01);
seangshim 0:a01fda36fde8 194
seangshim 3:c1e0db4832b7 195 if(100 < distance && distance < 500) //障害物発見❕
seangshim 0:a01fda36fde8 196 {
seangshim 0:a01fda36fde8 197
seangshim 0:a01fda36fde8 198 printf("if success!\r\n");
seangshim 0:a01fda36fde8 199
seangshim 0:a01fda36fde8 200 float ms1,ms2,msj1,msj2;
seangshim 1:10af8aaa5b40 201 ms1=1.0; //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず1.0にしておく⇒waitの秒数を変えた方が良い感じ
seangshim 1:10af8aaa5b40 202 ms2=1.0;
seangshim 0:a01fda36fde8 203
seangshim 12:2f0c841e6078 204 msj1=1.0; //回転の時
seangshim 12:2f0c841e6078 205 msj2=1.0;
seangshim 0:a01fda36fde8 206
seangshim 0:a01fda36fde8 207 motor1.stop(0);
seangshim 0:a01fda36fde8 208 motor2.stop(0);
seangshim 1:10af8aaa5b40 209 wait(2);
seangshim 0:a01fda36fde8 210 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 211
seangshim 0:a01fda36fde8 212 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 213 motor2.speed(-msj2);
seangshim 12:2f0c841e6078 214 wait(0.77);
seangshim 0:a01fda36fde8 215 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 216
seangshim 3:c1e0db4832b7 217 motor1.stop(0);
seangshim 3:c1e0db4832b7 218 motor2.stop(0);
seangshim 3:c1e0db4832b7 219 wait(2);
seangshim 3:c1e0db4832b7 220 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 221
seangshim 0:a01fda36fde8 222 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 223 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 224 wait(2);
seangshim 0:a01fda36fde8 225 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 226
seangshim 3:c1e0db4832b7 227 motor1.stop(0);
seangshim 3:c1e0db4832b7 228 motor2.stop(0);
seangshim 3:c1e0db4832b7 229 wait(2);
seangshim 3:c1e0db4832b7 230 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 231
seangshim 0:a01fda36fde8 232 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 233 motor2.speed(msj2);
seangshim 12:2f0c841e6078 234 wait(0.77);
seangshim 0:a01fda36fde8 235 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 236
seangshim 3:c1e0db4832b7 237 motor1.stop(0);
seangshim 3:c1e0db4832b7 238 motor2.stop(0);
seangshim 3:c1e0db4832b7 239 wait(2);
seangshim 3:c1e0db4832b7 240 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 241
seangshim 0:a01fda36fde8 242 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 243 motor2.speed(ms2);
seangshim 12:2f0c841e6078 244 int t=0;
seangshim 12:2f0c841e6078 245 for(t=0;t<10;t++)
seangshim 12:2f0c841e6078 246 {
seangshim 12:2f0c841e6078 247 printf("%d\r\n", test.read());
seangshim 12:2f0c841e6078 248 printf("%d\r\n", test2.read());
seangshim 1:10af8aaa5b40 249 if (test.read() == 1 and flag == 0)
seangshim 1:10af8aaa5b40 250 {
seangshim 1:10af8aaa5b40 251 flag = 1;
seangshim 1:10af8aaa5b40 252 }
seangshim 1:10af8aaa5b40 253 else if (test.read() == 0 and flag == 1)
seangshim 1:10af8aaa5b40 254 {
seangshim 1:10af8aaa5b40 255 flag=0;
seangshim 2:37d831f82840 256 rightrun += 52.5;
seangshim 1:10af8aaa5b40 257 }
seangshim 2:37d831f82840 258 if (test2.read() == 1 and flag2 == 0){
seangshim 2:37d831f82840 259 flag2 = 1;
seangshim 2:37d831f82840 260 }
seangshim 2:37d831f82840 261 else if (test2.read() == 0 and flag2 == 1){
seangshim 2:37d831f82840 262 flag2 = 0;
seangshim 2:37d831f82840 263 leftrun2 += 52.5;
seangshim 2:37d831f82840 264 }
seangshim 2:37d831f82840 265 printf("%f", rightrun);
seangshim 12:2f0c841e6078 266 printf("\t%f\r\n", leftrun2);
seangshim 12:2f0c841e6078 267 }
seangshim 3:c1e0db4832b7 268 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 269
seangshim 3:c1e0db4832b7 270 motor1.stop(0);
seangshim 3:c1e0db4832b7 271 motor2.stop(0);
seangshim 3:c1e0db4832b7 272 wait(2);
seangshim 3:c1e0db4832b7 273 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 274
seangshim 0:a01fda36fde8 275 motor1.speed(-msj1); //機体を反時計回りに９０度回転
seangshim 0:a01fda36fde8 276 motor2.speed(msj2);
seangshim 12:2f0c841e6078 277 wait(0.77);
seangshim 0:a01fda36fde8 278 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 279
seangshim 3:c1e0db4832b7 280 motor1.stop(0);
seangshim 3:c1e0db4832b7 281 motor2.stop(0);
seangshim 3:c1e0db4832b7 282 wait(2);
seangshim 3:c1e0db4832b7 283 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 284
seangshim 0:a01fda36fde8 285 motor1.speed(ms1); //直進
seangshim 0:a01fda36fde8 286 motor2.speed(ms2);
seangshim 1:10af8aaa5b40 287 wait(2);
seangshim 0:a01fda36fde8 288 printf("mortor straight\r\n");
seangshim 3:c1e0db4832b7 289
seangshim 3:c1e0db4832b7 290 motor1.stop(0);
seangshim 3:c1e0db4832b7 291 motor2.stop(0);
seangshim 3:c1e0db4832b7 292 wait(2);
seangshim 3:c1e0db4832b7 293 printf("mortor stop\r\n");
seangshim 0:a01fda36fde8 294
seangshim 0:a01fda36fde8 295 motor1.speed(msj1); //機体を時計回りに９０度回転
seangshim 0:a01fda36fde8 296 motor2.speed(-msj2);
seangshim 12:2f0c841e6078 297 wait(0.77);
seangshim 0:a01fda36fde8 298 printf("mortor rotation\r\n");
seangshim 3:c1e0db4832b7 299
seangshim 3:c1e0db4832b7 300 motor1.stop(0);
seangshim 3:c1e0db4832b7 301 motor2.stop(0);
seangshim 3:c1e0db4832b7 302 wait(2);
seangshim 3:c1e0db4832b7 303 printf("mortor stop\r\n");
394 10:280a25bcc8bb 304
394 10:280a25bcc8bb 305
seangshim 0:a01fda36fde8 306 }
seangshim 0:a01fda36fde8 307 }
seangshim 4:8b52fd631b32 308 motor1.stop(0);
seangshim 4:8b52fd631b32 309 motor2.stop(0);
seangshim 4:8b52fd631b32 310
seangshim 0:a01fda36fde8 311 }
seangshim 1:10af8aaa5b40 312
seangshim 1:10af8aaa5b40 313
seangshim 1:10af8aaa5b40 314
seangshim 1:10af8aaa5b40 315 float culculate_distance_3(float a,float b) //距離推定プログラム、加速度の計算が送られてきたら,mainの中に入れる
seangshim 1:10af8aaa5b40 316 {
seangshim 1:10af8aaa5b40 317 float c;
seangshim 1:10af8aaa5b40 318 c=0.5*a+0.5*b;//今は平均。計測をもとに修正を加える
seangshim 1:10af8aaa5b40 319 return c;
seangshim 1:10af8aaa5b40 320 }
seangshim 1:10af8aaa5b40 321
seangshim 1:10af8aaa5b40 322
seangshim 1:10af8aaa5b40 323
seangshim 1:10af8aaa5b40 324
seangshim 1:10af8aaa5b40 325