CanSat-C 2021
気球試験1,2回目 3回目はtest2-xbee
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
Revision 0:e386cbba36d5, committed 2021-12-19
- Comitter:
- miyajitakenari
- Date:
- Sun Dec 19 09:31:36 2021 +0000
- Commit message:
- 1,2kaime
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ATP3012.lib Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/ATP3012/#61aadb168ef3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Function.h Sun Dec 19 09:31:36 2021 +0000
@@ -0,0 +1,181 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "us015.h"
+#include "HMC6352.h"
+#include "TB6612.h"
+#include "ATP3011.h"
+
+US015 hs(D2,D3);
+DigitalOut Ultra(D2);
+GPS gps(D1, D0);
+HMC6352 compass(D4, D5);
+ATP3011 talk(D4,D5); // I2C sda scl
+TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
+TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
+Serial xbee(A7, A2);
+
+double GPS_x, GPS_y; // 現在地の座標
+double theta;
+double delta;
+
+int FrontGet()
+{
+ Ultra = 1; //超音波on
+ hs.TrigerOut();
+ wait(1);
+ int distance;
+ distance = hs.GetDistance();
+ xbee.printf("distance=%d\r\n", distance); //距離出力
+ Ultra=0;//超音波off
+
+ if(distance < 1500) {
+ return 1;
+ }
+ else {
+ return 0;
+ }
+}
+
+
+void catchGPS()
+{
+ xbee.printf("GPS Start\r\n");
+ /* 1秒ごとに現在地を取得してターミナル出力 */
+ while(1) {
+ if(gps.getgps()) { //現在地取得
+ xbee.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+ GPS_x = gps.latitude;
+ GPS_y = gps.longitude;
+ break;
+ }
+ else {
+ xbee.printf("No data\r\n");//データ取得に失敗した場合
+ wait(1);
+ }
+ }
+ return;
+}
+
+
+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;
+ }
+}
+
+double AngleGet(double next_x, double next_y)
+{
+ double angle = 0;
+ compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ angle = compass.sample() / 10;
+
+ double theta;
+ double delta;
+
+ xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
+ theta = atan2(next_y - gps.longitude , next_x - gps.latitude) * 180 / 3.14159265359;
+ if(theta >= 0) {
+ delta = angle - theta;
+ }
+ else {
+
+ theta = theta + 360;
+ delta = angle - theta;
+ }
+ if(delta<0){
+ delta+=360;
+ }
+ xbee.printf("%f-%f=%f\r\n", angle, theta, delta);
+ return delta;
+}
+
+void Calibration()
+{
+ xbee.printf("calibration start\r\n");
+ compass.setCalibrationMode(0x43);
+ Move('4', 0.19);
+ wait(60);
+ Move('1', 0);
+ compass.setCalibrationMode(0x45);
+ xbee.printf("calibration end\r\n");
+ while(1) {
+ if(gps.getgps()) { //現在地取得
+ xbee.printf("lati=%lf\nlong=%lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+ GPS_x = gps.latitude;
+ GPS_y = gps.longitude;
+ break;
+ }
+ else {
+ xbee.printf("No data\r\n");//データ取得に失敗した場合
+ wait(1);
+ }
+ }
+
+ return;
+}
+
+ /*地上局から新情報を送るときはflag=がでてきたらスペースか.を入力
+ 3秒後ぐらいにmessage=が出てくるので、そしたら新情報を入力*/
+void speak()
+{
+ int timeout_ms=500;
+ char mess[100];
+ float as;
+ if(talk.IsActive(timeout_ms)==true){
+ xbee.printf("Active\n\rflag=");
+ wait(3);
+ if(xbee.readable()){
+ xbee.printf("\n\rmessage=");
+ int i=0;
+ do{
+ mess[i++]= xbee.getc();
+ }
+ while(mess[i-1]!= 0x0d && i<99);
+ talk.Synthe(mess);
+ xbee.printf("advance speed=");
+ xbee.scanf("%f",&as);
+ }
+ else{
+ xbee.printf("preset_message speak\r\n");
+ talk.Synthe("hinannyuudoudesu,,tuitekitekudasai.");
+ as=0.39;
+ }
+ }
+ else{
+ xbee.printf("\r\nNot Active\n");
+ as=0.39;
+ }
+ Move('2', as);
+ xbee.printf("mortor mode:2 speed:%f",as);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC6352.lib Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/HMC/#0a44cb78fd9a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TB6612FNG.lib Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/a/#096c2484805d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/US015.lib Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/US015_2/#e842315ec717
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.lib Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/CanSat_C/code/getGPS/#2046f20df896
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Dec 19 09:31:36 2021 +0000
@@ -0,0 +1,105 @@
+/*ライブラリ*/
+#include "mbed.h"
+
+// 自作関数
+#include "Function.h"
+
+// フライトピン・ニクロム線関係
+DigitalIn flight_pin(A0);
+DigitalOut nichrome(D13);
+//
+#define cp_max 4 //CPの数を入力する
+
+int main() {
+ // 変数宣言
+ double GPS_x, GPS_y; // 現在地の座標
+ double direction; // 次CPへの向き
+ double CPs_x[cp_max]={34.54497251645555,34.545054367534235,34.54502498510097,34.545342036897935}; //CPリスト(x座標)
+ double CPs_y[cp_max]={135.50877273927074,135.50837525002106,135.50798030878718,135.50778889521388}; // CPリスト(y座標)
+ double next_CP_x, next_CP_y;
+
+ // 落下検知
+ // パラシュート分離
+
+ wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+ while(flight_pin){}
+ xbee.printf("flight_pin nuketa\n\r");
+ wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
+ nichrome=1;// ここ変える
+ xbee.printf("nichrome in\n\r");
+ wait(10);
+ nichrome=0;
+ // 落下終了
+
+
+ // 行動フロー開始
+ Calibration();
+ xbee.printf("XBee Connected\r\n");
+ xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
+ for (int i = 0; i<=cp_max-1 ; i++) {//最後のcp=goalまで移動
+ next_CP_x = CPs_x[i];
+ next_CP_y = CPs_y[i];
+
+ xbee.printf("next_i=%d\r\n", i);
+
+ while (1) {
+ while(FrontGet()) {
+ xbee.printf("frontget\n\r");
+ Move('2', 0.1);
+ wait(0.5);
+ Move('1', 0); //停止
+ Move('4', 0.2); //時計回り回転
+ wait(0.5);
+ /*Move('2', 0.17);
+ wait(0.2);*/
+ Move('1', 0); //回転停止
+ xbee.printf("front_avoid_rotate\n\r");
+ }
+
+ //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
+ Move('2', 0.2);
+ wait(0.8);
+ Move('1', 0);
+ //ちょっと走るのおわり
+ //走りながらanglegetできたら、止まらない
+
+ catchGPS();
+ direction = AngleGet(next_CP_x,next_CP_y);
+ xbee.printf("\n\n\r----direction start-----\r\n");
+ xbee.printf("next_cp_x=%lf, next_cp_y=%lf\r\n",next_CP_x,next_CP_y);
+ //角度調節
+ while(1) {
+ if(direction < 5 || direction > 350) {
+ xbee.printf("\n-----direction finish-----\r\n");
+ Move('1', 0); //停止
+ Move('2', 0.39);
+ xbee.printf("now_direction=%f\r\n", direction);
+ break;
+ }
+ else {
+ Move('4', 0.19);//時計回りに回転
+ xbee.printf("now_direction=%lf\r\n", direction);
+ direction = AngleGet(next_CP_x,next_CP_y);
+ }
+ }
+ catchGPS();
+ xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
+
+ double lati = 111132.8715; //1度あたりの緯度の距離(m)
+ double longi = 91535.79099; //1度あたりの経度の距離(m)
+ GPS_x = gps.latitude;
+ GPS_y = gps.longitude;
+ if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 12.25) { // CP到着判定 //試験で調整
+ xbee.printf("!!!!!!!!now leach cp[%d]=x_%f,y_%f!!!!!!!!!!!\r\n",i,next_CP_x ,next_CP_y);
+ break;
+ }
+ speak();
+ }//while(1){}
+ }//for(){}
+ // 行動フロー終了
+ xbee.printf("End\r\n");
+ Move('2', 0.17);
+ wait(0.2);
+ Move('1', 0); //停止
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Dec 19 09:31:36 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/7c328cabac7e \ No newline at end of file