CanSat-C 2021 / Mbed 2 deprecated LSI_Compass

LSIと磁気センサーの統合用プログラムです。

Dependencies:   ATP3012 mbed TB6612FNG HMC6352 US015 getGPS

Function.h@2:82902be97576, 2021-10-27 (annotated)

Committer:
ushiroji
Date:
Wed Oct 27 12:33:10 2021 +0000
Revision:
2:82902be97576
Parent:
0:85414455b681 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:85414455b681 1 #include "mbed.h"
ushiroji 0:85414455b681 2
ushiroji 0:85414455b681 3 #include "getGPS.h"
ushiroji 0:85414455b681 4 #include "us015.h"
ushiroji 0:85414455b681 5 #include "HMC6352.h"
ushiroji 0:85414455b681 6 #include "Math.h"
ushiroji 0:85414455b681 7 #include "TB6612.h"
ushiroji 0:85414455b681 8
ushiroji 0:85414455b681 9 US015 hs(D2, D3);
ushiroji 0:85414455b681 10 DigitalOut Ultra(D2);
ushiroji 0:85414455b681 11 GPS gps(D1, D0);
ushiroji 0:85414455b681 12 HMC6352 compass(D4, D5);
ushiroji 0:85414455b681 13 TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
ushiroji 0:85414455b681 14 TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
ushiroji 0:85414455b681 15 Serial pc(USBTX, USBRX);
ushiroji 0:85414455b681 16
ushiroji 0:85414455b681 17 double GPS_x, GPS_y; // 現在地の座標
ushiroji 0:85414455b681 18 double next_CP_x, next_CP_y;
ushiroji 0:85414455b681 19 double CPs_x[100]; // = []; //CPリスト(x座標)
ushiroji 0:85414455b681 20 double CPs_y[100]; // = []; // CPリスト(y座標)
ushiroji 0:85414455b681 21 double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
ushiroji 0:85414455b681 22 double theta;
ushiroji 0:85414455b681 23 double delta;
ushiroji 0:85414455b681 24
ushiroji 0:85414455b681 25 int FrontGet()
ushiroji 0:85414455b681 26 {
ushiroji 0:85414455b681 27 Ultra = 1; //超音波on
ushiroji 0:85414455b681 28 hs.TrigerOut();
ushiroji 0:85414455b681 29 wait(1);
ushiroji 0:85414455b681 30 int distance;
ushiroji 0:85414455b681 31 distance = hs.GetDistance();
ushiroji 0:85414455b681 32 pc.printf("distance=%d\r\n", distance); //距離出力
ushiroji 0:85414455b681 33 Ultra=0;//超音波off
ushiroji 0:85414455b681 34
ushiroji 0:85414455b681 35 if(distance < 200) {
ushiroji 0:85414455b681 36 return 1;
ushiroji 0:85414455b681 37 }
ushiroji 0:85414455b681 38 else {
ushiroji 0:85414455b681 39 return 0;
ushiroji 0:85414455b681 40 }
ushiroji 0:85414455b681 41 }
ushiroji 0:85414455b681 42
ushiroji 0:85414455b681 43
ushiroji 0:85414455b681 44 int catchGPS()
ushiroji 0:85414455b681 45 {
ushiroji 0:85414455b681 46 pc.printf("GPS Start\r\n");
ushiroji 0:85414455b681 47 /* 1秒ごとに現在地を取得してターミナル出力 */
ushiroji 0:85414455b681 48 while(1) {
ushiroji 0:85414455b681 49 if(gps.getgps()) { //現在地取得
ushiroji 0:85414455b681 50 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 0:85414455b681 51 *pGPS_x = gps.latitude;
ushiroji 0:85414455b681 52 *pGPS_y = gps.longitude;
ushiroji 0:85414455b681 53 break;
ushiroji 0:85414455b681 54 }
ushiroji 0:85414455b681 55 else {
ushiroji 0:85414455b681 56 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 0:85414455b681 57 wait(1);
ushiroji 0:85414455b681 58 }
ushiroji 0:85414455b681 59 }
ushiroji 0:85414455b681 60 return 0;
ushiroji 0:85414455b681 61 }
ushiroji 0:85414455b681 62
ushiroji 0:85414455b681 63
ushiroji 0:85414455b681 64 void Move(char input_data, float motor_speed) {
ushiroji 0:85414455b681 65 switch (input_data) {
ushiroji 0:85414455b681 66 case '1': // 停止
ushiroji 0:85414455b681 67 motor_a = 0;
ushiroji 0:85414455b681 68 motor_b = 0;
ushiroji 0:85414455b681 69 break;
ushiroji 0:85414455b681 70 case '2': // 前進
ushiroji 0:85414455b681 71 motor_a = motor_speed;
ushiroji 0:85414455b681 72 motor_b = motor_speed;
ushiroji 0:85414455b681 73 break;
ushiroji 0:85414455b681 74 case '3': // 後退
ushiroji 0:85414455b681 75 motor_a = -motor_speed;
ushiroji 0:85414455b681 76 motor_b = -motor_speed;
ushiroji 0:85414455b681 77 break;
ushiroji 0:85414455b681 78 case '4': // 時計回りに回転
ushiroji 0:85414455b681 79 motor_a = motor_speed;
ushiroji 0:85414455b681 80 motor_b = -motor_speed;
ushiroji 0:85414455b681 81 break;
ushiroji 0:85414455b681 82 case '5': // 反時計回りに回転
ushiroji 0:85414455b681 83 motor_a = -motor_speed;
ushiroji 0:85414455b681 84 motor_b = motor_speed;
ushiroji 0:85414455b681 85 break;
ushiroji 0:85414455b681 86 case '6': // Aのみ正転
ushiroji 0:85414455b681 87 motor_a = motor_speed;
ushiroji 0:85414455b681 88 break;
ushiroji 0:85414455b681 89 case '7': // Bのみ正転
ushiroji 0:85414455b681 90 motor_b = motor_speed;
ushiroji 0:85414455b681 91 break;
ushiroji 0:85414455b681 92 case '8': // Aのみ逆転
ushiroji 0:85414455b681 93 motor_a = -motor_speed;
ushiroji 0:85414455b681 94 break;
ushiroji 0:85414455b681 95 case '9': // Bのみ逆転
ushiroji 0:85414455b681 96 motor_b = -motor_speed;
ushiroji 0:85414455b681 97 break;
ushiroji 0:85414455b681 98 }
ushiroji 0:85414455b681 99 pc.printf("input_data=%d\r\n", input_data);
ushiroji 0:85414455b681 100 }
ushiroji 0:85414455b681 101
ushiroji 0:85414455b681 102
ushiroji 0:85414455b681 103 void Rotate(double rot){
ushiroji 0:85414455b681 104 // double time = angle; // 試験で調整
ushiroji 0:85414455b681 105 int time = 3; // 試験用
ushiroji 0:85414455b681 106 if(rot > 0) {
ushiroji 0:85414455b681 107 Move('5', 1);
ushiroji 0:85414455b681 108 wait(time);
ushiroji 0:85414455b681 109 Move('1', 0);
ushiroji 0:85414455b681 110 }
ushiroji 0:85414455b681 111 else {
ushiroji 0:85414455b681 112 Move('6', 1);
ushiroji 0:85414455b681 113 wait(time);
ushiroji 0:85414455b681 114 Move('1', 0);
ushiroji 0:85414455b681 115 }
ushiroji 0:85414455b681 116 }
ushiroji 0:85414455b681 117
ushiroji 0:85414455b681 118 double AngleGet()
ushiroji 0:85414455b681 119 {
ushiroji 0:85414455b681 120 double angle;
ushiroji 0:85414455b681 121 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 0:85414455b681 122 angle = compass.sample() / 10;
ushiroji 0:85414455b681 123
ushiroji 0:85414455b681 124 double theta;
ushiroji 0:85414455b681 125 double delta;
ushiroji 0:85414455b681 126
ushiroji 0:85414455b681 127 pc.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
ushiroji 0:85414455b681 128 theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / M_PI;
ushiroji 0:85414455b681 129 printf("theta=%f\r\n", theta);
ushiroji 0:85414455b681 130 if(theta >= 0) {
ushiroji 0:85414455b681 131 delta = angle - theta;
ushiroji 0:85414455b681 132 }
ushiroji 0:85414455b681 133 else {
ushiroji 0:85414455b681 134 theta = theta + 360;
ushiroji 0:85414455b681 135 delta = angle - theta;
ushiroji 0:85414455b681 136 }
ushiroji 0:85414455b681 137 printf("%f-%f=%f\r\n", angle, theta, delta);
ushiroji 0:85414455b681 138 wait(2);
ushiroji 0:85414455b681 139 return delta;
ushiroji 0:85414455b681 140 }