航空研究会
a
Dependencies: mbed SRF02 HMC5983 SDFileSystem
Diff: main.cpp
- Revision:
- 0:cd96dd54b3d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Dec 19 06:06:41 2018 +0000
@@ -0,0 +1,497 @@
+#include "mbed.h"
+#include "HMC5983.h"
+#include "SRF02.h"
+#include "MacroC.h"
+#include "math.h"
+#include "SDFileSystem.h"
+
+#define BAUD_SERIAL 9600
+#define BAUD_GPS 9600
+
+#define MOVE_NEUTRAL 0
+#define MOVE_STOP 1
+#define MOVE_FORWARD 2
+#define MOVE_LEFT 3
+#define MOVE_RIGHT 4
+#define MOVE_BACK 5
+#define GOAL_FORWARD 6 //ゴール付近 ゆっくり
+#define GOAL_LEFT 7
+#define GOAL_RIGHT 8
+#define MAX_FORWARD 9 //はやい 姿勢修正用
+#define MAX_BACK 10
+
+#define DANGER_RAD M_PI/6//風向きベクトルからのずれの範囲指定(パラシュートがあるかもしれない範囲)
+
+#define V_FORWARD 0.7 //前進速度 0.0~1.0で指定
+#define V_TURN 0.3 //旋回速度 0.0~1.0で指定
+#define V_BACK 0.4 //後退速度 0.0~1.0で指定
+
+#define VG_FORWARD 0.4 //ゴール付近
+#define VG_TURN 0.3
+
+#define VM_FORWARD 1.0 //姿勢
+#define VM_BACK 1.0
+
+
+#define MOVE_TIME 0.83 //MoveCansat用パラメータ、角度→回転時間
+
+#define ROTATERAD 20 //回転開始角度(度)
+#define ALLOWRAD 10 //回転停止角度(度)
+#define CALIB_X -50 //コンパスセンサキャリブレーション
+#define CALIB_Y -200 //
+#define LOGNAME "Datalog" //拡張子を除いたログファイル名
+#define LOWPASS 0.1 //コンパスのローパスフィルタの係数 0.0~1.0で大きいほどフィルタが強い
+#define TIM_COMPASS 0.05 //コンパスのタイマ割り込みの間隔(sec)
+#define TIM_WATCHREF 2.0 //WatchdogRefreshのタイマ割り込みの間隔(sec)
+#define TIM_PARACUT 125.0 //タイマーでのパラシュートカットまでの秒数
+#define TIM_REVERSE 10.0 //回転し続けたときに復帰動作を取るまでの秒数
+
+#define LandingTime 90 //パラシュート切り離しから展開機構までの時間
+
+#define FORWARD_TIME 5 //一回の方向判定から直線に走る時間
+
+#define CameraCalib_x 0 //機体カメラの中央からずれてる分のX方向補正
+#define CameraCalib_y 0 //機体カメラの中央からずれてる分のY方向補正
+
+/*--------初期化（各Object宣言)--------*/
+Serial pc(SERIAL_TX, SERIAL_RX); //パソコンとのシリアル通信用
+Serial Twelite(pinTWETx,pinTWERx); //気球側からのデータ受信用
+Serial Raspi(pinRasPiTx,pinRasPiRx); //RaspiZero通信用
+IWDG_HandleTypeDef hiwdg; //WatchDog
+
+//HMC5983 compass(pinCmpsSDA, pinCmpsSCL); //コンパスセンサー
+SRF02 sensor(pinCmpsSDA, pinCmpsSCL,0xE0); //長距離超音波
+SDFileSystem sd(pinSPI_MOSI, pinSPI_MISO, pinSPI_SCK, pinSDcardCS, "sd");
+
+DigitalIn fallSwitch(pinFallSwitch); //投下検知スイッチ(デジタル入力) (mbedstn
+PwmOut cutServo(pinCutServo); //パラシュート切り離し用サーボ(PWM) (mbedstn
+PwmOut expandServo(pinExpandServo); //ターゲット展開用サーボ(PWM)
+DigitalOut led1(pinLED1); //LED1(緑)
+DigitalOut led2(pinLED2); //LED2(橙)
+
+DigitalOut letRasPi(pinCmpsSDA); //ラズパイに制御渡す用
+
+PwmOut driveR_FIN(pinDriveR_FIN); //モータードライバー右-FIN foward
+PwmOut driveR_RIN(pinDriveR_RIN); //モータードライバー右-RIN reverse
+PwmOut driveL_FIN(pinDriveL_FIN);
+PwmOut driveL_RIN(pinDriveL_RIN);
+
+Ticker t_compass; //コンパスのタイマー割込み
+Ticker t_WatchRefresh; //Watchdog Refreshのタイマ割り込み
+Timeout t_ParaCut; //カットサーボのたいま
+Timer timer; //時間計測
+Timer timer2; //かぶるとき用
+
+/*--------初期化終わり--------*/
+
+
+/*--------グローバル変数の宣言--------*/
+double g_NowRad = 0.0; //割り込み用コンパス値;
+//設定ファイルの内容
+double g_calibX;
+double g_calibY;
+double g_goalLat;
+double g_goalLong;
+double g_goalArea;
+
+double SkyRad;
+double NowRad;
+/*------グローバル変数の宣言終わり------*/
+
+/*--------関数のプロトタイプ宣言--------*/
+void get_median(double[5],double*);
+void measure_distance(double,double,double*);
+int Check_distance(double);
+void MoveCansat(int code); //Cansat移動
+int TurnCansat(double defrad); //Cansat旋回用 whileの()の中に入れて使う
+int CheckRadian(double Radian); //角度のずれが許容値以下かどうか、以上なら正か負かをそれぞれ0,1,2の整（RadianCondition）で返す
+
+void ServoCut(void); //タイマー割込み
+
+double Read_Compass(void);
+//void Receive_Data(void);
+double GetDefRad(double NowRad, double SkyRad);
+void FixReverse(void);
+
+void NewFileCreate(char *fname); //新しくファイルを生成
+
+int main() {
+ pc.printf("Hello world\r\n");
+ /*--------変数宣言--------*/
+ FILE *fp;
+ char filename[30];
+
+ //長距離超音波用
+ int SensorTest = 0;
+ int NextTest = 0;
+ int preCount = 0;
+ int CutCount = 0;
+
+ //初期化
+
+ led1 = 1;
+ led2 = 1;
+ letRasPi = 0;
+
+ NewFileCreate(filename);
+ //sprintf(filename, "/sd/%s%02d.txt", LOGNAME, 50);
+
+ cutServo.period(0.020); //値：サーボのPWMの1波長の長さ20 msec (20~22 msec)
+ cutServo.pulsewidth(0.0011); //0.0016 信号が立ち上がっている時間 パラシュートサーボの初期角度
+
+ //コンパスセンサ初期化
+ //compass.init();
+ //compass.setConfigurationA(TMP_SENSOR_ENABLE | AVG8_SAMPLES | OUTPUT_RATE_15); //センサーの設定 データシートを読もう
+ //compass.setScale(8.1); //コンパスセンサのライブラリ参照 精度(レンジ）を決めてる
+
+ fallSwitch.mode(PullUp);
+
+ //モータードライバー初期化（OFF)
+ MoveCansat(MOVE_NEUTRAL);
+
+ //割り込み初期化
+ //WatchdogRefreshのタイマー割込み
+ /*t_WatchRefresh.attach(&WatchdogRefresh,TIM_WATCHREF);
+
+ NVIC_SetPriority(TIM2_IRQn,0); //タイマー割込みの優先度設定 最高に
+ */
+ wait(0.5);
+ led1 = 0; led2 = 0;
+ pc.printf("complete initializing\r\n");
+
+
+ double DefRad;
+ /*--------初期化終わり--------*/
+
+ /*------------プログラムスタート------------*/
+
+ printf("Hello World!\n");
+
+ //落下開始検知
+
+ pc.printf("Ready\r\n"); //printfでは\rを入れると見やすいゾ
+ led1 = 1;
+ while(fallSwitch == 1){ //イヤホンプラグが挿してあるなら
+ wait_ms(500); //10ms待ってチェックを繰り返す
+ pc.printf("fallswitch==1\r\n"); //
+ }
+ if(fallSwitch !=0) pc.printf("start.\r\n");
+ led1 = 0; //led1点灯
+
+ fp = fopen(filename, "a");
+ if(fp == NULL) pc.printf("Failed to open. \r\n");
+ fprintf(fp,"Fall Start.\r\n");
+
+ /*------パラシュート早めに切り離す------*/
+ wait(5);
+ pc.printf("attach_start");
+
+ //t_ParaCut.attach(&ServoCut,60.0); //超音波動作しなかった場合、タイマーで切り離す用
+
+ timer.reset();
+ timer.start();
+
+ wait(60);
+
+ cutServo.pulsewidth(0.0015); //0.0010 パラシュート分離
+ //pc.printf("Parachute was cutted. \r\n");
+ fprintf(fp, "Parachute was cutted by timer. \r\n");
+
+ fclose(fp);
+
+ timer.stop();
+ //t_ParaCut.detach();
+
+ //pc.printf("Finish cut servo\r\n");
+ /*--------パラシュート切断完了--------*/
+ /*--------着地のちターゲット機構の展開--------*/
+ wait(LandingTime);
+ pc.printf("TENKAI!!\r\n");
+
+ fp = fopen(filename, "a");
+ if(fp == NULL) pc.printf("Failed to open. \r\n");
+ fprintf(fp, "Target marker was expanded. \r\n");
+ fprintf(fp,"Mbed's program finished.\r\n");
+ fclose(fp);
+
+ expandServo.pulsewidth(0.00145);
+
+
+ /*-------Raspberry Piに制御をわたす----------*/
+ letRasPi = 1;
+ wait(10);
+ letRasPi = 0;
+
+
+
+
+ /*-------ターゲット展開終了---------*/
+
+ /*テキトーに動作するプログラム*/
+ /* while(1){
+ MoveCansat(MOVE_FORWARD);
+ printf("MOVE_FORWARD\r\n");
+ wait(1);
+
+ double deg;
+ deg=compass.getHeadingXYDeg(CALIB_X,CALIB_Y);
+ DefRad=deg+10;
+ printf("%lf\r\n",DefRad);
+ TurnCansat(DefRad);
+ printf("Turn_Cansat\r\n");
+ wait(1);
+ }*/
+ /*気球側からもらったコンパスの進行方向角度でカンサットを動かす*/
+ /* while(1){
+ MoveCansat(MOVE_STOP);
+ fp = fopen(filename, "a");
+ if(fp != NULL) {
+ fprintf(fp, "Start compass reading mode. \r\n");
+ fclose(fp);
+ }
+ Twelite.attach(Receive_Data,Serial::RxIrq); //空撮カメラから目的の方向を送ってもらう
+ if(Receive_Data==NULL){
+ fprintf(fp,"Compass fail.");
+ }
+ else fprintf(fp,"%lf\r\n",SkyRad);
+
+ //NVIC_SetPriority(UART1_IRQn, 0);
+
+ NowRad=Read_Compass(); //機体側Compassを読む
+ pc.printf("Compassgain=%lf\r\n",NowRad);
+
+
+ SkyRad=2;
+ DefRad=GetDefRad(NowRad,SkyRad);
+ timer.reset();
+ timer.start();
+ while(TurnCansat(DefRad)){
+ led1 = !led1;
+ NowRad=Read_Compass();
+ pc.printf("NowRad=%lf\r\n",NowRad);
+ DefRad = GetDefRad(NowRad,SkyRad);
+ if(timer.read() > TIM_REVERSE){
+ FixReverse();
+ timer.reset();
+ timer2.reset();
+ }
+ }
+ timer.stop();
+ MoveCansat(MOVE_FORWARD);
+ wait(FORWARD_TIME); //再度Compassの判定をかけるまでの時間
+
+ if(SkyRad<3)break; //確認用
+ }
+ */
+ /*何らかの条件で最終Modeに移行*/
+
+ /*RaspberryPiZeroとの終末誘導*/
+ /*fp = fopen(filename, "a");
+ if(fp == NULL) pc.printf("Failed to open. \r\n");
+ fprintf(fp, "Start last mode. \r\n");
+
+ MoveCansat(MOVE_STOP);
+
+ Receive_Data;
+
+
+ fclose(fp);
+
+
+ pc.printf("END\r\n");*/
+ }
+
+//コードを引数にしてCanSatを前進、回転させる。delayと併用することが多い
+void MoveCansat(int code)
+{
+ switch(code){
+ case 0: //MOVE_NEUTRAL
+ driveR_FIN = 0; //ライブラリ参照 F=forward R=reverse
+ driveR_RIN = 0;
+ driveL_FIN = 0;
+ driveL_RIN = 0;
+ break;
+ case 1: //MOVE_STOP
+ driveR_FIN = 1;
+ driveR_RIN = 1;
+ driveL_FIN = 1;
+ driveL_RIN = 1;
+ break;
+ case 2: //MOVE_FORWARD
+ driveR_FIN = V_FORWARD;
+ driveR_RIN = 0;
+ driveL_FIN = V_FORWARD;
+ driveL_RIN = 0;
+ break;
+ case 3: //MOVE_LEFT
+ driveR_FIN = V_TURN;
+ driveR_RIN = 0;
+ driveL_FIN = 0;
+ driveL_RIN = V_TURN;
+ break;
+ case 4: //MOVE_RIGHT
+ driveR_FIN = 0;
+ driveR_RIN = V_TURN;
+ driveL_FIN = V_TURN;
+ driveL_RIN = 0;
+ break;
+ case 5: //MOVE_BACK
+ driveR_FIN = 0;
+ driveR_RIN = V_BACK;
+ driveL_FIN = 0;
+ driveL_RIN = V_BACK;
+ break;
+ case 6: //GOAL_FORWARD
+ driveR_FIN = VG_FORWARD;
+ driveR_RIN = 0;
+ driveL_FIN = VG_FORWARD;
+ driveL_RIN = 0;
+ break;
+ case 7: //GOAL_LEFT
+ driveR_FIN = VG_TURN;
+ driveR_RIN = 0;
+ driveL_FIN = 0;
+ driveL_RIN = VG_TURN;
+ break;
+ case 8: //GOAL_RIGHT
+ driveR_FIN = 0;
+ driveR_RIN = VG_TURN;
+ driveL_FIN = VG_TURN;
+ driveL_RIN = 0;
+ break;
+ case 9: //MAX_FORWARD
+ driveR_FIN = VM_FORWARD;
+ driveR_RIN = 0;
+ driveL_FIN = VM_FORWARD;
+ driveL_RIN = 0;
+ break;
+ case 10: //MAX_BACK
+ driveR_FIN = 0;
+ driveR_RIN = VM_BACK;
+ driveL_FIN = 0;
+ driveL_RIN = VM_BACK;
+ break;
+ }
+ return;
+}
+
+//Cansat旋回関数 whileの()内に入れて使う 旋回継続は1,終了時には0を返す
+int TurnCansat(double defrad){
+ if(CheckRadian(defrad) == 1){//turn right
+ MoveCansat(MOVE_RIGHT);
+ if(defrad <= ALLOWRAD*M_PI/180.0 && defrad >= -5*M_PI/180.0) return 0; //旋回終了
+ else return 1; //旋回継続
+ }
+ else if(CheckRadian(defrad) == 2){//turn left
+ MoveCansat(MOVE_LEFT);
+ if(defrad >= -1.0*ALLOWRAD*M_PI/180.0 && defrad <= 5*M_PI/180.0) return 0;
+ else return 1;
+ }
+ else return 0; //前進時
+}
+
+int CheckRadian(double Radian){
+ int RadianCondition;
+
+ if(fabs(Radian) < ROTATERAD*M_PI/180.0) RadianCondition = 0;
+ else if(Radian <= -1.0*ROTATERAD*M_PI/180.0) RadianCondition = 1; //turn right
+ else if(Radian >= ROTATERAD*M_PI/180.0) RadianCondition = 2; //turn left
+ return RadianCondition;
+}
+
+int Check_distance(double a){
+ int DistanceCondition;
+
+ if(a>100)DistanceCondition=0;
+ else if(fabs(a)<=100) DistanceCondition=1;
+ else DistanceCondition=2;
+
+
+ return DistanceCondition;
+ }
+
+void ServoCut(void){ //パラタイマー切断用
+ cutServo.pulsewidth(0.00145);
+ pc.printf("para was cutted by timer.\r\n");
+}
+/*
+void Receive_Data(void){
+ char temp[5];
+ int i;
+ for(i=0;i<4;i++){
+ temp[i]=Twelite.getc();
+ }
+ return atof(temp);
+ }
+*/
+/*
+double Read_Compass(void){
+ int h,i,j,k,n;
+ double CompassRad[n];
+ n=5;
+
+ for(h=0;h<n-1;h++){ //5回Compass読む
+ CompassRad[h]=compass.getHeadingXY(CALIB_X,CALIB_Y);
+ }
+
+ for(i=0;i<n-1;i++){ //Compassの値の中央値を取る
+ j=i;
+ for(k=i;k<n;k++){
+ if(CompassRad[k]<CompassRad[j])j=k;
+ if(i<j){
+ double v=CompassRad[i];
+ CompassRad[i]=CompassRad[j];
+ CompassRad[j]=v;
+ }
+ }
+
+ }
+ return CompassRad[2];
+}
+*/
+double GetDefRad(double NowRad, double SkyRad){ //目標角度(空撮から)-現在の角度を算出
+
+ double DefRad; //Radでの計算・・・今回は両方ともがCompass由来なので北からの角度に直す必要なし(たぶん)
+
+ DefRad=SkyRad-NowRad;
+
+ if(DefRad >= M_PI) DefRad = DefRad - 2.0*M_PI;
+ if(DefRad <= -1.0*M_PI) DefRad = DefRad + 2.0*M_PI;
+
+ return DefRad;
+}
+
+void FixReverse(void){
+ pc.printf("FixReverse Mode. \r\n");
+
+ MoveCansat(MOVE_STOP);
+ wait(0.5);
+ MoveCansat(MAX_BACK);
+ wait(0.7);
+ MoveCansat(MAX_FORWARD);
+ wait(2.5);
+}
+
+void NewFileCreate(char fname[]){
+ uint16_t i_FileCheck;
+ FILE *fileP;
+
+ for (i_FileCheck = 1; i_FileCheck < 99; i_FileCheck++) {
+ sprintf(fname, "/sd/%s%02d.txt", LOGNAME, i_FileCheck);
+ pc.printf("%s\r\n",fname);
+
+ fileP = fopen(fname, "r");
+ if(fileP == NULL){ //参照した番号のファイルがなければ生成
+ fileP = fopen(fname,"w");
+
+ if(fileP != NULL) {
+ pc.printf("%s was created\r\n",fname);
+
+ fclose(fileP);
+ break;
+ }
+ else pc.printf("Faired to create a new file.\r\n");
+ }
+ else fclose(fileP);
+ }
+}
\ No newline at end of file