CanSat-C 2021
本番用
Dependencies: ATP3012 mbed a HMC US015_2 getGPS
Revision 0:00e63cfc4661, committed 2021-12-13
- Comitter:
- miyajitakenari
- Date:
- Mon Dec 13 11:35:59 2021 +0000
- Commit message:
- honban
Changed in this revision
diff -r 000000000000 -r 00e63cfc4661 ATP3012.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ATP3012.lib Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/ATP3012/#61aadb168ef3
diff -r 000000000000 -r 00e63cfc4661 Function.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Function.h Mon Dec 13 11:35:59 2021 +0000
@@ -0,0 +1,179 @@
+#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 next_CP_x, next_CP_y;
+double CPs_x[100]; // = []; //CPリスト(x座標)
+double CPs_y[100]; // = []; // CPリスト(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 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_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
+ printf("theta=%f\r\n", theta);
+ if(theta >= 0) {
+ delta = angle - theta;
+ }
+ else {
+ theta = theta + 360;
+ delta = angle - theta;
+ }
+ if(delta<0){
+ delta+=360;
+ }
+ 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.2);
+ xbee.printf("mortor mode:4 speed:0.15\n\r");
+ wait(60);
+ Move('1', 0);
+ xbee.printf("mortor mode:1 speed:0\n\r");
+ 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];
+ 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);
+ }
+ else{
+ xbee.printf("preset_message speak\r\n");
+ talk.Synthe("hinannyuudoudesu,,tuitekitekudasai.");
+ }
+ }
+ else{
+ xbee.printf("\r\nNot Active\n");
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 00e63cfc4661 HMC6352.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC6352.lib Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/HMC/#0a44cb78fd9a
diff -r 000000000000 -r 00e63cfc4661 TB6612FNG.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TB6612FNG.lib Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/a/#096c2484805d
diff -r 000000000000 -r 00e63cfc4661 US015.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/US015.lib Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CanSat-C-2021/code/US015_2/#e842315ec717
diff -r 000000000000 -r 00e63cfc4661 getGPS.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/getGPS.lib Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/CanSat_C/code/getGPS/#2046f20df896
diff -r 000000000000 -r 00e63cfc4661 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Dec 13 11:35:59 2021 +0000
@@ -0,0 +1,119 @@
+/*ライブラリ*/
+#include "mbed.h"
+
+// 自作関数
+#include "Function.h"
+
+// フライトピン・ニクロム線関係
+DigitalIn flight_pin(A0);
+DigitalOut nichrome(D13);
+//
+#define cp_max 2 //CPの数を入力する
+
+int main() {
+ // 変数宣言
+ double GPS_x, GPS_y; // 現在地の座標
+ double direction; // 次CPへの向き
+ double CPs_x[2]={34.545588426585454, 34.5454484832847}; //CPリスト(x座標)
+ double CPs_y[2]={135.50282053551706, 135.50262437335775}; // CPリスト(y座標)
+ double next_CP_x, next_CP_y;
+
+ // 落下検知
+ // パラシュート分離
+
+ wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
+ while(flight_pin){}
+ xbee.printf("flight_pin nuketa\n");
+ wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
+ //nichrome=1;
+ xbee.printf("nichrome in\n");
+ 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) {
+ speak();
+ while(FrontGet()) {
+ xbee.printf("frontget\n\r");
+ 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.1);
+ wait(1);
+ Move('2', 0.2);
+ wait(2);
+ Move('2', 0.17);
+ wait(0.2);
+ Move('1', 0);
+ //ちょっと走るのおわり
+ //走りながらanglegetできたら、止まらない
+
+ direction = AngleGet();
+ xbee.printf("direction=%f\n\rrotation_start", direction);
+ //角度調節
+ while(1) {
+ if(direction < 20 || direction > 340) {
+ xbee.printf("direction finish\n\r");
+ Move('1', 0); //停止
+ Move('2', 0.5);
+ xbee.printf("now_angle=%f\r\n", direction);
+ break;
+ }
+ else {
+ Move('4', 0.15);//時計回りに回転
+ xbee.printf("now_angle=%lf\r\n", direction);
+ direction = AngleGet();
+ }
+ }
+
+ xbee.printf("speed flag=");
+ wait(3);
+ float as[2];//advance speed
+ if(xbee.readable()){
+ xbee.printf("advance speed=");
+ xbee.scanf("%f",&as[1]);
+ }else{
+ as[1]=0.5;
+ }
+ Move('2', as[1]);
+ xbee.printf("mortor mode:2 speed:%f",as[1]);
+ 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 < 25) { // CP到着判定 //試験で調整
+ xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
+ break;
+ }
+
+ }//while(1){}
+ }//for(){}
+ // 行動フロー終了
+ xbee.printf("End\r\n");
+ Move('2', 0.17);
+ wait(0.2);
+ Move('1', 0); //停止
+ xbee.printf("mortor mode:1 speed:0");
+ return 0;
+}
diff -r 000000000000 -r 00e63cfc4661 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Dec 13 11:35:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/7c328cabac7e \ No newline at end of file