走行試験用のプログラムです。
Dependencies: mbed TB6612FNG HMC6352 US015 getGPS
Function.h
- Committer:
- ushiroji
- Date:
- 2021-11-06
- Revision:
- 0:0454d5b5905b
File content as of revision 0:0454d5b5905b:
#include "mbed.h" #include "getGPS.h" #include "us015.h" #include "HMC6352.h" #include "Math.h" #include "TB6612.h" US015 hs(D2, D3); DigitalOut Ultra(D2); GPS gps(D1, D0); HMC6352 compass(D4, D5); TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2) TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2) Serial pc(USBTX, USBRX); double GPS_x, GPS_y; // 現在地の座標 double next_CP_x, next_CP_y; double CPs_x[100]; // = []; //CPリスト(x座標) double CPs_y[100]; // = []; // CPリスト(y座標) double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y; double theta; double delta; int FrontGet() { Ultra = 1; //超音波on hs.TrigerOut(); wait(1); int distance; distance = hs.GetDistance(); pc.printf("distance=%d\r\n", distance); //距離出力 Ultra=0;//超音波off if(distance < 200) { return 1; } else { return 0; } } int catchGPS() { pc.printf("GPS Start\r\n"); /* 1秒ごとに現在地を取得してターミナル出力 */ while(1) { if(gps.getgps()) { //現在地取得 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力 *pGPS_x = gps.latitude; *pGPS_y = gps.longitude; break; } else { pc.printf("No data\r\n");//データ取得に失敗した場合 wait(1); } } return 0; } void Move(char input_data, float motor_speed) { switch (input_data) { case '1': // 停止 motor_a = 0; motor_b = 0; break; case '2': // 前進 motor_a = motor_speed; motor_b = motor_speed; break; case '3': // 後退 motor_a = -motor_speed; motor_b = -motor_speed; break; case '4': // 時計回りに回転 motor_a = motor_speed; motor_b = -motor_speed; break; case '5': // 反時計回りに回転 motor_a = -motor_speed; motor_b = motor_speed; break; case '6': // Aのみ正転 motor_a = motor_speed; break; case '7': // Bのみ正転 motor_b = motor_speed; break; case '8': // Aのみ逆転 motor_a = -motor_speed; break; case '9': // Bのみ逆転 motor_b = -motor_speed; break; } pc.printf("input_data=%d\r\n", input_data); } void Rotate(double rot){ // double time = angle; // 試験で調整 int time = 3; // 試験用 if(rot > 0) { Move('5', 1); wait(time); Move('1', 0); } else { Move('6', 1); wait(time); Move('1', 0); } } double AngleGet() { double angle = 0; compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); angle = compass.sample() / 10; double theta; double delta; pc.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude); theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / M_PI; printf("theta=%f\r\n", theta); if(theta >= 0) { delta = angle - theta; } else { theta = theta + 360; delta = angle - theta; } printf("%f-%f=%f\r\n", angle, theta, delta); wait(2); return delta; } int Calibration() { pc.printf("calibration start\r\n"); compass.setCalibrationMode(0x43); wait(60); compass.setCalibrationMode(0x45); pc.printf("calibration end\r\n"); while(1) { if(gps.getgps()) { //現在地取得 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力 *pGPS_x = gps.latitude; *pGPS_y = gps.longitude; break; } else { pc.printf("No data\r\n");//データ取得に失敗した場合 wait(1); } } return 0; }