走行試験用のプログラムです。
Dependencies: mbed TB6612FNG HMC6352 US015 getGPS
Diff: Function.h
- Revision:
- 0:0454d5b5905b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Function.h Sat Nov 06 01:27:45 2021 +0000 @@ -0,0 +1,162 @@ +#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; +} \ No newline at end of file