航空研究会 / Mbed 2 deprecated Okitiabr_skaia

a

Dependencies:   mbed SRF02 HMC5983 SDFileSystem

main.cpp@0:cd96dd54b3d5, 2018-12-19 (annotated)

Committer:
TUATBM
Date:
Wed Dec 19 06:06:41 2018 +0000
Revision:
0:cd96dd54b3d5
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TUATBM 0:cd96dd54b3d5 1 #include "mbed.h"
TUATBM 0:cd96dd54b3d5 2 #include "HMC5983.h"
TUATBM 0:cd96dd54b3d5 3 #include "SRF02.h"
TUATBM 0:cd96dd54b3d5 4 #include "MacroC.h"
TUATBM 0:cd96dd54b3d5 5 #include "math.h"
TUATBM 0:cd96dd54b3d5 6 #include "SDFileSystem.h"
TUATBM 0:cd96dd54b3d5 7
TUATBM 0:cd96dd54b3d5 8 #define BAUD_SERIAL 9600
TUATBM 0:cd96dd54b3d5 9 #define BAUD_GPS 9600
TUATBM 0:cd96dd54b3d5 10
TUATBM 0:cd96dd54b3d5 11 #define MOVE_NEUTRAL 0
TUATBM 0:cd96dd54b3d5 12 #define MOVE_STOP 1
TUATBM 0:cd96dd54b3d5 13 #define MOVE_FORWARD 2
TUATBM 0:cd96dd54b3d5 14 #define MOVE_LEFT 3
TUATBM 0:cd96dd54b3d5 15 #define MOVE_RIGHT 4
TUATBM 0:cd96dd54b3d5 16 #define MOVE_BACK 5
TUATBM 0:cd96dd54b3d5 17 #define GOAL_FORWARD 6 //ゴール付近　ゆっくり
TUATBM 0:cd96dd54b3d5 18 #define GOAL_LEFT 7
TUATBM 0:cd96dd54b3d5 19 #define GOAL_RIGHT 8
TUATBM 0:cd96dd54b3d5 20 #define MAX_FORWARD 9 //はやい　姿勢修正用
TUATBM 0:cd96dd54b3d5 21 #define MAX_BACK 10
TUATBM 0:cd96dd54b3d5 22
TUATBM 0:cd96dd54b3d5 23 #define DANGER_RAD M_PI/6//風向きベクトルからのずれの範囲指定(パラシュートがあるかもしれない範囲)
TUATBM 0:cd96dd54b3d5 24
TUATBM 0:cd96dd54b3d5 25 #define V_FORWARD 0.7 //前進速度 0.0~1.0で指定
TUATBM 0:cd96dd54b3d5 26 #define V_TURN 0.3 //旋回速度 0.0~1.0で指定
TUATBM 0:cd96dd54b3d5 27 #define V_BACK 0.4 //後退速度 0.0~1.0で指定
TUATBM 0:cd96dd54b3d5 28
TUATBM 0:cd96dd54b3d5 29 #define VG_FORWARD 0.4 //ゴール付近
TUATBM 0:cd96dd54b3d5 30 #define VG_TURN 0.3
TUATBM 0:cd96dd54b3d5 31
TUATBM 0:cd96dd54b3d5 32 #define VM_FORWARD 1.0 //姿勢
TUATBM 0:cd96dd54b3d5 33 #define VM_BACK 1.0
TUATBM 0:cd96dd54b3d5 34
TUATBM 0:cd96dd54b3d5 35
TUATBM 0:cd96dd54b3d5 36 #define MOVE_TIME 0.83 //MoveCansat用パラメータ、角度→回転時間
TUATBM 0:cd96dd54b3d5 37
TUATBM 0:cd96dd54b3d5 38 #define ROTATERAD 20 //回転開始角度(度)
TUATBM 0:cd96dd54b3d5 39 #define ALLOWRAD 10 //回転停止角度(度)
TUATBM 0:cd96dd54b3d5 40 #define CALIB_X -50 //コンパスセンサキャリブレーション
TUATBM 0:cd96dd54b3d5 41 #define CALIB_Y -200 //
TUATBM 0:cd96dd54b3d5 42 #define LOGNAME "Datalog" //拡張子を除いたログファイル名
TUATBM 0:cd96dd54b3d5 43 #define LOWPASS 0.1 //コンパスのローパスフィルタの係数 0.0~1.0で大きいほどフィルタが強い
TUATBM 0:cd96dd54b3d5 44 #define TIM_COMPASS 0.05 //コンパスのタイマ割り込みの間隔(sec)
TUATBM 0:cd96dd54b3d5 45 #define TIM_WATCHREF 2.0 //WatchdogRefreshのタイマ割り込みの間隔(sec)
TUATBM 0:cd96dd54b3d5 46 #define TIM_PARACUT 125.0 //タイマーでのパラシュートカットまでの秒数
TUATBM 0:cd96dd54b3d5 47 #define TIM_REVERSE 10.0 //回転し続けたときに復帰動作を取るまでの秒数
TUATBM 0:cd96dd54b3d5 48
TUATBM 0:cd96dd54b3d5 49 #define LandingTime 90 //パラシュート切り離しから展開機構までの時間
TUATBM 0:cd96dd54b3d5 50
TUATBM 0:cd96dd54b3d5 51 #define FORWARD_TIME 5 //一回の方向判定から直線に走る時間
TUATBM 0:cd96dd54b3d5 52
TUATBM 0:cd96dd54b3d5 53 #define CameraCalib_x 0 //機体カメラの中央からずれてる分のX方向補正
TUATBM 0:cd96dd54b3d5 54 #define CameraCalib_y 0 //機体カメラの中央からずれてる分のY方向補正
TUATBM 0:cd96dd54b3d5 55
TUATBM 0:cd96dd54b3d5 56 /*--------初期化（各Object宣言)--------*/
TUATBM 0:cd96dd54b3d5 57 Serial pc(SERIAL_TX, SERIAL_RX); //パソコンとのシリアル通信用
TUATBM 0:cd96dd54b3d5 58 Serial Twelite(pinTWETx,pinTWERx); //気球側からのデータ受信用
TUATBM 0:cd96dd54b3d5 59 Serial Raspi(pinRasPiTx,pinRasPiRx); //RaspiZero通信用
TUATBM 0:cd96dd54b3d5 60 IWDG_HandleTypeDef hiwdg; //WatchDog
TUATBM 0:cd96dd54b3d5 61
TUATBM 0:cd96dd54b3d5 62 //HMC5983 compass(pinCmpsSDA, pinCmpsSCL); //コンパスセンサー
TUATBM 0:cd96dd54b3d5 63 SRF02 sensor(pinCmpsSDA, pinCmpsSCL,0xE0); //長距離超音波
TUATBM 0:cd96dd54b3d5 64 SDFileSystem sd(pinSPI_MOSI, pinSPI_MISO, pinSPI_SCK, pinSDcardCS, "sd");
TUATBM 0:cd96dd54b3d5 65
TUATBM 0:cd96dd54b3d5 66 DigitalIn fallSwitch(pinFallSwitch); //投下検知スイッチ(デジタル入力) (mbedstn
TUATBM 0:cd96dd54b3d5 67 PwmOut cutServo(pinCutServo); //パラシュート切り離し用サーボ(PWM) (mbedstn
TUATBM 0:cd96dd54b3d5 68 PwmOut expandServo(pinExpandServo); //ターゲット展開用サーボ(PWM)
TUATBM 0:cd96dd54b3d5 69 DigitalOut led1(pinLED1); //LED1(緑)
TUATBM 0:cd96dd54b3d5 70 DigitalOut led2(pinLED2); //LED2(橙)
TUATBM 0:cd96dd54b3d5 71
TUATBM 0:cd96dd54b3d5 72 DigitalOut letRasPi(pinCmpsSDA); //ラズパイに制御渡す用
TUATBM 0:cd96dd54b3d5 73
TUATBM 0:cd96dd54b3d5 74 PwmOut driveR_FIN(pinDriveR_FIN); //モータードライバー右-FIN foward
TUATBM 0:cd96dd54b3d5 75 PwmOut driveR_RIN(pinDriveR_RIN); //モータードライバー右-RIN reverse
TUATBM 0:cd96dd54b3d5 76 PwmOut driveL_FIN(pinDriveL_FIN);
TUATBM 0:cd96dd54b3d5 77 PwmOut driveL_RIN(pinDriveL_RIN);
TUATBM 0:cd96dd54b3d5 78
TUATBM 0:cd96dd54b3d5 79 Ticker t_compass; //コンパスのタイマー割込み
TUATBM 0:cd96dd54b3d5 80 Ticker t_WatchRefresh; //Watchdog Refreshのタイマ割り込み
TUATBM 0:cd96dd54b3d5 81 Timeout t_ParaCut; //カットサーボのたいま
TUATBM 0:cd96dd54b3d5 82 Timer timer; //時間計測
TUATBM 0:cd96dd54b3d5 83 Timer timer2; //かぶるとき用
TUATBM 0:cd96dd54b3d5 84
TUATBM 0:cd96dd54b3d5 85 /*--------初期化終わり--------*/
TUATBM 0:cd96dd54b3d5 86
TUATBM 0:cd96dd54b3d5 87
TUATBM 0:cd96dd54b3d5 88 /*--------グローバル変数の宣言--------*/
TUATBM 0:cd96dd54b3d5 89 double g_NowRad = 0.0; //割り込み用コンパス値;
TUATBM 0:cd96dd54b3d5 90 //設定ファイルの内容
TUATBM 0:cd96dd54b3d5 91 double g_calibX;
TUATBM 0:cd96dd54b3d5 92 double g_calibY;
TUATBM 0:cd96dd54b3d5 93 double g_goalLat;
TUATBM 0:cd96dd54b3d5 94 double g_goalLong;
TUATBM 0:cd96dd54b3d5 95 double g_goalArea;
TUATBM 0:cd96dd54b3d5 96
TUATBM 0:cd96dd54b3d5 97 double SkyRad;
TUATBM 0:cd96dd54b3d5 98 double NowRad;
TUATBM 0:cd96dd54b3d5 99 /*------グローバル変数の宣言終わり------*/
TUATBM 0:cd96dd54b3d5 100
TUATBM 0:cd96dd54b3d5 101 /*--------関数のプロトタイプ宣言--------*/
TUATBM 0:cd96dd54b3d5 102 void get_median(double[5],double*);
TUATBM 0:cd96dd54b3d5 103 void measure_distance(double,double,double*);
TUATBM 0:cd96dd54b3d5 104 int Check_distance(double);
TUATBM 0:cd96dd54b3d5 105 void MoveCansat(int code); //Cansat移動
TUATBM 0:cd96dd54b3d5 106 int TurnCansat(double defrad); //Cansat旋回用 whileの()の中に入れて使う
TUATBM 0:cd96dd54b3d5 107 int CheckRadian(double Radian); //角度のずれが許容値以下かどうか、以上なら正か負かをそれぞれ0,1,2の整（RadianCondition）で返す
TUATBM 0:cd96dd54b3d5 108
TUATBM 0:cd96dd54b3d5 109 void ServoCut(void); //タイマー割込み
TUATBM 0:cd96dd54b3d5 110
TUATBM 0:cd96dd54b3d5 111 double Read_Compass(void);
TUATBM 0:cd96dd54b3d5 112 //void Receive_Data(void);
TUATBM 0:cd96dd54b3d5 113 double GetDefRad(double NowRad, double SkyRad);
TUATBM 0:cd96dd54b3d5 114 void FixReverse(void);
TUATBM 0:cd96dd54b3d5 115
TUATBM 0:cd96dd54b3d5 116 void NewFileCreate(char *fname); //新しくファイルを生成
TUATBM 0:cd96dd54b3d5 117
TUATBM 0:cd96dd54b3d5 118 int main() {
TUATBM 0:cd96dd54b3d5 119 pc.printf("Hello world\r\n");
TUATBM 0:cd96dd54b3d5 120 /*--------変数宣言--------*/
TUATBM 0:cd96dd54b3d5 121 FILE *fp;
TUATBM 0:cd96dd54b3d5 122 char filename[30];
TUATBM 0:cd96dd54b3d5 123
TUATBM 0:cd96dd54b3d5 124 //長距離超音波用
TUATBM 0:cd96dd54b3d5 125 int SensorTest = 0;
TUATBM 0:cd96dd54b3d5 126 int NextTest = 0;
TUATBM 0:cd96dd54b3d5 127 int preCount = 0;
TUATBM 0:cd96dd54b3d5 128 int CutCount = 0;
TUATBM 0:cd96dd54b3d5 129
TUATBM 0:cd96dd54b3d5 130 //初期化
TUATBM 0:cd96dd54b3d5 131
TUATBM 0:cd96dd54b3d5 132 led1 = 1;
TUATBM 0:cd96dd54b3d5 133 led2 = 1;
TUATBM 0:cd96dd54b3d5 134 letRasPi = 0;
TUATBM 0:cd96dd54b3d5 135
TUATBM 0:cd96dd54b3d5 136 NewFileCreate(filename);
TUATBM 0:cd96dd54b3d5 137 //sprintf(filename, "/sd/%s%02d.txt", LOGNAME, 50);
TUATBM 0:cd96dd54b3d5 138
TUATBM 0:cd96dd54b3d5 139 cutServo.period(0.020); //値：サーボのPWMの1波長の長さ20 msec (20~22 msec)
TUATBM 0:cd96dd54b3d5 140 cutServo.pulsewidth(0.0011); //0.0016　信号が立ち上がっている時間　パラシュートサーボの初期角度
TUATBM 0:cd96dd54b3d5 141
TUATBM 0:cd96dd54b3d5 142 //コンパスセンサ初期化
TUATBM 0:cd96dd54b3d5 143 //compass.init();
TUATBM 0:cd96dd54b3d5 144 //compass.setConfigurationA(TMP_SENSOR_ENABLE | AVG8_SAMPLES | OUTPUT_RATE_15); //センサーの設定　データシートを読もう
TUATBM 0:cd96dd54b3d5 145 //compass.setScale(8.1); //コンパスセンサのライブラリ参照　精度(レンジ）を決めてる
TUATBM 0:cd96dd54b3d5 146
TUATBM 0:cd96dd54b3d5 147 fallSwitch.mode(PullUp);
TUATBM 0:cd96dd54b3d5 148
TUATBM 0:cd96dd54b3d5 149 //モータードライバー初期化（OFF)
TUATBM 0:cd96dd54b3d5 150 MoveCansat(MOVE_NEUTRAL);
TUATBM 0:cd96dd54b3d5 151
TUATBM 0:cd96dd54b3d5 152 //割り込み初期化
TUATBM 0:cd96dd54b3d5 153 //WatchdogRefreshのタイマー割込み
TUATBM 0:cd96dd54b3d5 154 /*t_WatchRefresh.attach(&WatchdogRefresh,TIM_WATCHREF);
TUATBM 0:cd96dd54b3d5 155
TUATBM 0:cd96dd54b3d5 156 NVIC_SetPriority(TIM2_IRQn,0); //タイマー割込みの優先度設定 最高に
TUATBM 0:cd96dd54b3d5 157 */
TUATBM 0:cd96dd54b3d5 158 wait(0.5);
TUATBM 0:cd96dd54b3d5 159 led1 = 0; led2 = 0;
TUATBM 0:cd96dd54b3d5 160 pc.printf("complete initializing\r\n");
TUATBM 0:cd96dd54b3d5 161
TUATBM 0:cd96dd54b3d5 162
TUATBM 0:cd96dd54b3d5 163 double DefRad;
TUATBM 0:cd96dd54b3d5 164 /*--------初期化終わり--------*/
TUATBM 0:cd96dd54b3d5 165
TUATBM 0:cd96dd54b3d5 166 /*------------プログラムスタート------------*/
TUATBM 0:cd96dd54b3d5 167
TUATBM 0:cd96dd54b3d5 168 printf("Hello World!\n");
TUATBM 0:cd96dd54b3d5 169
TUATBM 0:cd96dd54b3d5 170 //落下開始検知
TUATBM 0:cd96dd54b3d5 171
TUATBM 0:cd96dd54b3d5 172 pc.printf("Ready\r\n"); //printfでは\rを入れると見やすいゾ
TUATBM 0:cd96dd54b3d5 173 led1 = 1;
TUATBM 0:cd96dd54b3d5 174 while(fallSwitch == 1){ //イヤホンプラグが挿してあるなら
TUATBM 0:cd96dd54b3d5 175 wait_ms(500); //10ms待ってチェックを繰り返す
TUATBM 0:cd96dd54b3d5 176 pc.printf("fallswitch==1\r\n"); //
TUATBM 0:cd96dd54b3d5 177 }
TUATBM 0:cd96dd54b3d5 178 if(fallSwitch !=0) pc.printf("start.\r\n");
TUATBM 0:cd96dd54b3d5 179 led1 = 0; //led1点灯
TUATBM 0:cd96dd54b3d5 180
TUATBM 0:cd96dd54b3d5 181 fp = fopen(filename, "a");
TUATBM 0:cd96dd54b3d5 182 if(fp == NULL) pc.printf("Failed to open. \r\n");
TUATBM 0:cd96dd54b3d5 183 fprintf(fp,"Fall Start.\r\n");
TUATBM 0:cd96dd54b3d5 184
TUATBM 0:cd96dd54b3d5 185 /*------パラシュート早めに切り離す------*/
TUATBM 0:cd96dd54b3d5 186 wait(5);
TUATBM 0:cd96dd54b3d5 187 pc.printf("attach_start");
TUATBM 0:cd96dd54b3d5 188
TUATBM 0:cd96dd54b3d5 189 //t_ParaCut.attach(&ServoCut,60.0); //超音波動作しなかった場合、タイマーで切り離す用
TUATBM 0:cd96dd54b3d5 190
TUATBM 0:cd96dd54b3d5 191 timer.reset();
TUATBM 0:cd96dd54b3d5 192 timer.start();
TUATBM 0:cd96dd54b3d5 193
TUATBM 0:cd96dd54b3d5 194 wait(60);
TUATBM 0:cd96dd54b3d5 195
TUATBM 0:cd96dd54b3d5 196 cutServo.pulsewidth(0.0015); //0.0010　パラシュート分離
TUATBM 0:cd96dd54b3d5 197 //pc.printf("Parachute was cutted. \r\n");
TUATBM 0:cd96dd54b3d5 198 fprintf(fp, "Parachute was cutted by timer. \r\n");
TUATBM 0:cd96dd54b3d5 199
TUATBM 0:cd96dd54b3d5 200 fclose(fp);
TUATBM 0:cd96dd54b3d5 201
TUATBM 0:cd96dd54b3d5 202 timer.stop();
TUATBM 0:cd96dd54b3d5 203 //t_ParaCut.detach();
TUATBM 0:cd96dd54b3d5 204
TUATBM 0:cd96dd54b3d5 205 //pc.printf("Finish cut servo\r\n");
TUATBM 0:cd96dd54b3d5 206 /*--------パラシュート切断完了--------*/
TUATBM 0:cd96dd54b3d5 207 /*--------着地のちターゲット機構の展開--------*/
TUATBM 0:cd96dd54b3d5 208 wait(LandingTime);
TUATBM 0:cd96dd54b3d5 209 pc.printf("TENKAI!!\r\n");
TUATBM 0:cd96dd54b3d5 210
TUATBM 0:cd96dd54b3d5 211 fp = fopen(filename, "a");
TUATBM 0:cd96dd54b3d5 212 if(fp == NULL) pc.printf("Failed to open. \r\n");
TUATBM 0:cd96dd54b3d5 213 fprintf(fp, "Target marker was expanded. \r\n");
TUATBM 0:cd96dd54b3d5 214 fprintf(fp,"Mbed's program finished.\r\n");
TUATBM 0:cd96dd54b3d5 215 fclose(fp);
TUATBM 0:cd96dd54b3d5 216
TUATBM 0:cd96dd54b3d5 217 expandServo.pulsewidth(0.00145);
TUATBM 0:cd96dd54b3d5 218
TUATBM 0:cd96dd54b3d5 219
TUATBM 0:cd96dd54b3d5 220 /*-------Raspberry Piに制御をわたす----------*/
TUATBM 0:cd96dd54b3d5 221 letRasPi = 1;
TUATBM 0:cd96dd54b3d5 222 wait(10);
TUATBM 0:cd96dd54b3d5 223 letRasPi = 0;
TUATBM 0:cd96dd54b3d5 224
TUATBM 0:cd96dd54b3d5 225
TUATBM 0:cd96dd54b3d5 226
TUATBM 0:cd96dd54b3d5 227
TUATBM 0:cd96dd54b3d5 228 /*-------ターゲット展開終了---------*/
TUATBM 0:cd96dd54b3d5 229
TUATBM 0:cd96dd54b3d5 230 /*テキトーに動作するプログラム*/
TUATBM 0:cd96dd54b3d5 231 /* while(1){
TUATBM 0:cd96dd54b3d5 232 MoveCansat(MOVE_FORWARD);
TUATBM 0:cd96dd54b3d5 233 printf("MOVE_FORWARD\r\n");
TUATBM 0:cd96dd54b3d5 234 wait(1);
TUATBM 0:cd96dd54b3d5 235
TUATBM 0:cd96dd54b3d5 236 double deg;
TUATBM 0:cd96dd54b3d5 237 deg=compass.getHeadingXYDeg(CALIB_X,CALIB_Y);
TUATBM 0:cd96dd54b3d5 238 DefRad=deg+10;
TUATBM 0:cd96dd54b3d5 239 printf("%lf\r\n",DefRad);
TUATBM 0:cd96dd54b3d5 240 TurnCansat(DefRad);
TUATBM 0:cd96dd54b3d5 241 printf("Turn_Cansat\r\n");
TUATBM 0:cd96dd54b3d5 242 wait(1);
TUATBM 0:cd96dd54b3d5 243 }*/
TUATBM 0:cd96dd54b3d5 244 /*気球側からもらったコンパスの進行方向角度でカンサットを動かす*/
TUATBM 0:cd96dd54b3d5 245 /* while(1){
TUATBM 0:cd96dd54b3d5 246 MoveCansat(MOVE_STOP);
TUATBM 0:cd96dd54b3d5 247 fp = fopen(filename, "a");
TUATBM 0:cd96dd54b3d5 248 if(fp != NULL) {
TUATBM 0:cd96dd54b3d5 249 fprintf(fp, "Start compass reading mode. \r\n");
TUATBM 0:cd96dd54b3d5 250 fclose(fp);
TUATBM 0:cd96dd54b3d5 251 }
TUATBM 0:cd96dd54b3d5 252 Twelite.attach(Receive_Data,Serial::RxIrq); //空撮カメラから目的の方向を送ってもらう
TUATBM 0:cd96dd54b3d5 253 if(Receive_Data==NULL){
TUATBM 0:cd96dd54b3d5 254 fprintf(fp,"Compass fail.");
TUATBM 0:cd96dd54b3d5 255 }
TUATBM 0:cd96dd54b3d5 256 else fprintf(fp,"%lf\r\n",SkyRad);
TUATBM 0:cd96dd54b3d5 257
TUATBM 0:cd96dd54b3d5 258 //NVIC_SetPriority(UART1_IRQn, 0);
TUATBM 0:cd96dd54b3d5 259
TUATBM 0:cd96dd54b3d5 260 NowRad=Read_Compass(); //機体側Compassを読む
TUATBM 0:cd96dd54b3d5 261 pc.printf("Compassgain=%lf\r\n",NowRad);
TUATBM 0:cd96dd54b3d5 262
TUATBM 0:cd96dd54b3d5 263
TUATBM 0:cd96dd54b3d5 264 SkyRad=2;
TUATBM 0:cd96dd54b3d5 265 DefRad=GetDefRad(NowRad,SkyRad);
TUATBM 0:cd96dd54b3d5 266 timer.reset();
TUATBM 0:cd96dd54b3d5 267 timer.start();
TUATBM 0:cd96dd54b3d5 268 while(TurnCansat(DefRad)){
TUATBM 0:cd96dd54b3d5 269 led1 = !led1;
TUATBM 0:cd96dd54b3d5 270 NowRad=Read_Compass();
TUATBM 0:cd96dd54b3d5 271 pc.printf("NowRad=%lf\r\n",NowRad);
TUATBM 0:cd96dd54b3d5 272 DefRad = GetDefRad(NowRad,SkyRad);
TUATBM 0:cd96dd54b3d5 273 if(timer.read() > TIM_REVERSE){
TUATBM 0:cd96dd54b3d5 274 FixReverse();
TUATBM 0:cd96dd54b3d5 275 timer.reset();
TUATBM 0:cd96dd54b3d5 276 timer2.reset();
TUATBM 0:cd96dd54b3d5 277 }
TUATBM 0:cd96dd54b3d5 278 }
TUATBM 0:cd96dd54b3d5 279 timer.stop();
TUATBM 0:cd96dd54b3d5 280 MoveCansat(MOVE_FORWARD);
TUATBM 0:cd96dd54b3d5 281 wait(FORWARD_TIME); //再度Compassの判定をかけるまでの時間
TUATBM 0:cd96dd54b3d5 282
TUATBM 0:cd96dd54b3d5 283 if(SkyRad<3)break; //確認用
TUATBM 0:cd96dd54b3d5 284 }
TUATBM 0:cd96dd54b3d5 285 */
TUATBM 0:cd96dd54b3d5 286 /*何らかの条件で最終Modeに移行*/
TUATBM 0:cd96dd54b3d5 287
TUATBM 0:cd96dd54b3d5 288 /*RaspberryPiZeroとの終末誘導*/
TUATBM 0:cd96dd54b3d5 289 /*fp = fopen(filename, "a");
TUATBM 0:cd96dd54b3d5 290 if(fp == NULL) pc.printf("Failed to open. \r\n");
TUATBM 0:cd96dd54b3d5 291 fprintf(fp, "Start last mode. \r\n");
TUATBM 0:cd96dd54b3d5 292
TUATBM 0:cd96dd54b3d5 293 MoveCansat(MOVE_STOP);
TUATBM 0:cd96dd54b3d5 294
TUATBM 0:cd96dd54b3d5 295 Receive_Data;
TUATBM 0:cd96dd54b3d5 296
TUATBM 0:cd96dd54b3d5 297
TUATBM 0:cd96dd54b3d5 298 fclose(fp);
TUATBM 0:cd96dd54b3d5 299
TUATBM 0:cd96dd54b3d5 300
TUATBM 0:cd96dd54b3d5 301 pc.printf("END\r\n");*/
TUATBM 0:cd96dd54b3d5 302 }
TUATBM 0:cd96dd54b3d5 303
TUATBM 0:cd96dd54b3d5 304 //コードを引数にしてCanSatを前進、回転させる。delayと併用することが多い
TUATBM 0:cd96dd54b3d5 305 void MoveCansat(int code)
TUATBM 0:cd96dd54b3d5 306 {
TUATBM 0:cd96dd54b3d5 307 switch(code){
TUATBM 0:cd96dd54b3d5 308 case 0: //MOVE_NEUTRAL
TUATBM 0:cd96dd54b3d5 309 driveR_FIN = 0; //ライブラリ参照　F=forward R=reverse
TUATBM 0:cd96dd54b3d5 310 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 311 driveL_FIN = 0;
TUATBM 0:cd96dd54b3d5 312 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 313 break;
TUATBM 0:cd96dd54b3d5 314 case 1: //MOVE_STOP
TUATBM 0:cd96dd54b3d5 315 driveR_FIN = 1;
TUATBM 0:cd96dd54b3d5 316 driveR_RIN = 1;
TUATBM 0:cd96dd54b3d5 317 driveL_FIN = 1;
TUATBM 0:cd96dd54b3d5 318 driveL_RIN = 1;
TUATBM 0:cd96dd54b3d5 319 break;
TUATBM 0:cd96dd54b3d5 320 case 2: //MOVE_FORWARD
TUATBM 0:cd96dd54b3d5 321 driveR_FIN = V_FORWARD;
TUATBM 0:cd96dd54b3d5 322 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 323 driveL_FIN = V_FORWARD;
TUATBM 0:cd96dd54b3d5 324 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 325 break;
TUATBM 0:cd96dd54b3d5 326 case 3: //MOVE_LEFT
TUATBM 0:cd96dd54b3d5 327 driveR_FIN = V_TURN;
TUATBM 0:cd96dd54b3d5 328 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 329 driveL_FIN = 0;
TUATBM 0:cd96dd54b3d5 330 driveL_RIN = V_TURN;
TUATBM 0:cd96dd54b3d5 331 break;
TUATBM 0:cd96dd54b3d5 332 case 4: //MOVE_RIGHT
TUATBM 0:cd96dd54b3d5 333 driveR_FIN = 0;
TUATBM 0:cd96dd54b3d5 334 driveR_RIN = V_TURN;
TUATBM 0:cd96dd54b3d5 335 driveL_FIN = V_TURN;
TUATBM 0:cd96dd54b3d5 336 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 337 break;
TUATBM 0:cd96dd54b3d5 338 case 5: //MOVE_BACK
TUATBM 0:cd96dd54b3d5 339 driveR_FIN = 0;
TUATBM 0:cd96dd54b3d5 340 driveR_RIN = V_BACK;
TUATBM 0:cd96dd54b3d5 341 driveL_FIN = 0;
TUATBM 0:cd96dd54b3d5 342 driveL_RIN = V_BACK;
TUATBM 0:cd96dd54b3d5 343 break;
TUATBM 0:cd96dd54b3d5 344 case 6: //GOAL_FORWARD
TUATBM 0:cd96dd54b3d5 345 driveR_FIN = VG_FORWARD;
TUATBM 0:cd96dd54b3d5 346 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 347 driveL_FIN = VG_FORWARD;
TUATBM 0:cd96dd54b3d5 348 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 349 break;
TUATBM 0:cd96dd54b3d5 350 case 7: //GOAL_LEFT
TUATBM 0:cd96dd54b3d5 351 driveR_FIN = VG_TURN;
TUATBM 0:cd96dd54b3d5 352 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 353 driveL_FIN = 0;
TUATBM 0:cd96dd54b3d5 354 driveL_RIN = VG_TURN;
TUATBM 0:cd96dd54b3d5 355 break;
TUATBM 0:cd96dd54b3d5 356 case 8: //GOAL_RIGHT
TUATBM 0:cd96dd54b3d5 357 driveR_FIN = 0;
TUATBM 0:cd96dd54b3d5 358 driveR_RIN = VG_TURN;
TUATBM 0:cd96dd54b3d5 359 driveL_FIN = VG_TURN;
TUATBM 0:cd96dd54b3d5 360 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 361 break;
TUATBM 0:cd96dd54b3d5 362 case 9: //MAX_FORWARD
TUATBM 0:cd96dd54b3d5 363 driveR_FIN = VM_FORWARD;
TUATBM 0:cd96dd54b3d5 364 driveR_RIN = 0;
TUATBM 0:cd96dd54b3d5 365 driveL_FIN = VM_FORWARD;
TUATBM 0:cd96dd54b3d5 366 driveL_RIN = 0;
TUATBM 0:cd96dd54b3d5 367 break;
TUATBM 0:cd96dd54b3d5 368 case 10: //MAX_BACK
TUATBM 0:cd96dd54b3d5 369 driveR_FIN = 0;
TUATBM 0:cd96dd54b3d5 370 driveR_RIN = VM_BACK;
TUATBM 0:cd96dd54b3d5 371 driveL_FIN = 0;
TUATBM 0:cd96dd54b3d5 372 driveL_RIN = VM_BACK;
TUATBM 0:cd96dd54b3d5 373 break;
TUATBM 0:cd96dd54b3d5 374 }
TUATBM 0:cd96dd54b3d5 375 return;
TUATBM 0:cd96dd54b3d5 376 }
TUATBM 0:cd96dd54b3d5 377
TUATBM 0:cd96dd54b3d5 378 //Cansat旋回関数 whileの()内に入れて使う 旋回継続は1,終了時には0を返す
TUATBM 0:cd96dd54b3d5 379 int TurnCansat(double defrad){
TUATBM 0:cd96dd54b3d5 380 if(CheckRadian(defrad) == 1){//turn right
TUATBM 0:cd96dd54b3d5 381 MoveCansat(MOVE_RIGHT);
TUATBM 0:cd96dd54b3d5 382 if(defrad <= ALLOWRAD*M_PI/180.0 && defrad >= -5*M_PI/180.0) return 0; //旋回終了
TUATBM 0:cd96dd54b3d5 383 else return 1; //旋回継続
TUATBM 0:cd96dd54b3d5 384 }
TUATBM 0:cd96dd54b3d5 385 else if(CheckRadian(defrad) == 2){//turn left
TUATBM 0:cd96dd54b3d5 386 MoveCansat(MOVE_LEFT);
TUATBM 0:cd96dd54b3d5 387 if(defrad >= -1.0*ALLOWRAD*M_PI/180.0 && defrad <= 5*M_PI/180.0) return 0;
TUATBM 0:cd96dd54b3d5 388 else return 1;
TUATBM 0:cd96dd54b3d5 389 }
TUATBM 0:cd96dd54b3d5 390 else return 0; //前進時
TUATBM 0:cd96dd54b3d5 391 }
TUATBM 0:cd96dd54b3d5 392
TUATBM 0:cd96dd54b3d5 393 int CheckRadian(double Radian){
TUATBM 0:cd96dd54b3d5 394 int RadianCondition;
TUATBM 0:cd96dd54b3d5 395
TUATBM 0:cd96dd54b3d5 396 if(fabs(Radian) < ROTATERAD*M_PI/180.0) RadianCondition = 0;
TUATBM 0:cd96dd54b3d5 397 else if(Radian <= -1.0*ROTATERAD*M_PI/180.0) RadianCondition = 1; //turn right
TUATBM 0:cd96dd54b3d5 398 else if(Radian >= ROTATERAD*M_PI/180.0) RadianCondition = 2; //turn left
TUATBM 0:cd96dd54b3d5 399 return RadianCondition;
TUATBM 0:cd96dd54b3d5 400 }
TUATBM 0:cd96dd54b3d5 401
TUATBM 0:cd96dd54b3d5 402 int Check_distance(double a){
TUATBM 0:cd96dd54b3d5 403 int DistanceCondition;
TUATBM 0:cd96dd54b3d5 404
TUATBM 0:cd96dd54b3d5 405 if(a>100)DistanceCondition=0;
TUATBM 0:cd96dd54b3d5 406 else if(fabs(a)<=100) DistanceCondition=1;
TUATBM 0:cd96dd54b3d5 407 else DistanceCondition=2;
TUATBM 0:cd96dd54b3d5 408
TUATBM 0:cd96dd54b3d5 409
TUATBM 0:cd96dd54b3d5 410 return DistanceCondition;
TUATBM 0:cd96dd54b3d5 411 }
TUATBM 0:cd96dd54b3d5 412
TUATBM 0:cd96dd54b3d5 413 void ServoCut(void){ //パラタイマー切断用
TUATBM 0:cd96dd54b3d5 414 cutServo.pulsewidth(0.00145);
TUATBM 0:cd96dd54b3d5 415 pc.printf("para was cutted by timer.\r\n");
TUATBM 0:cd96dd54b3d5 416 }
TUATBM 0:cd96dd54b3d5 417 /*
TUATBM 0:cd96dd54b3d5 418 void Receive_Data(void){
TUATBM 0:cd96dd54b3d5 419 char temp[5];
TUATBM 0:cd96dd54b3d5 420 int i;
TUATBM 0:cd96dd54b3d5 421 for(i=0;i<4;i++){
TUATBM 0:cd96dd54b3d5 422 temp[i]=Twelite.getc();
TUATBM 0:cd96dd54b3d5 423 }
TUATBM 0:cd96dd54b3d5 424 return atof(temp);
TUATBM 0:cd96dd54b3d5 425 }
TUATBM 0:cd96dd54b3d5 426 */
TUATBM 0:cd96dd54b3d5 427 /*
TUATBM 0:cd96dd54b3d5 428 double Read_Compass(void){
TUATBM 0:cd96dd54b3d5 429 int h,i,j,k,n;
TUATBM 0:cd96dd54b3d5 430 double CompassRad[n];
TUATBM 0:cd96dd54b3d5 431 n=5;
TUATBM 0:cd96dd54b3d5 432
TUATBM 0:cd96dd54b3d5 433 for(h=0;h<n-1;h++){ //5回Compass読む
TUATBM 0:cd96dd54b3d5 434 CompassRad[h]=compass.getHeadingXY(CALIB_X,CALIB_Y);
TUATBM 0:cd96dd54b3d5 435 }
TUATBM 0:cd96dd54b3d5 436
TUATBM 0:cd96dd54b3d5 437 for(i=0;i<n-1;i++){ //Compassの値の中央値を取る
TUATBM 0:cd96dd54b3d5 438 j=i;
TUATBM 0:cd96dd54b3d5 439 for(k=i;k<n;k++){
TUATBM 0:cd96dd54b3d5 440 if(CompassRad[k]<CompassRad[j])j=k;
TUATBM 0:cd96dd54b3d5 441 if(i<j){
TUATBM 0:cd96dd54b3d5 442 double v=CompassRad[i];
TUATBM 0:cd96dd54b3d5 443 CompassRad[i]=CompassRad[j];
TUATBM 0:cd96dd54b3d5 444 CompassRad[j]=v;
TUATBM 0:cd96dd54b3d5 445 }
TUATBM 0:cd96dd54b3d5 446 }
TUATBM 0:cd96dd54b3d5 447
TUATBM 0:cd96dd54b3d5 448 }
TUATBM 0:cd96dd54b3d5 449 return CompassRad[2];
TUATBM 0:cd96dd54b3d5 450 }
TUATBM 0:cd96dd54b3d5 451 */
TUATBM 0:cd96dd54b3d5 452 double GetDefRad(double NowRad, double SkyRad){ //目標角度(空撮から)-現在の角度を算出
TUATBM 0:cd96dd54b3d5 453
TUATBM 0:cd96dd54b3d5 454 double DefRad; //Radでの計算・・・今回は両方ともがCompass由来なので北からの角度に直す必要なし(たぶん)
TUATBM 0:cd96dd54b3d5 455
TUATBM 0:cd96dd54b3d5 456 DefRad=SkyRad-NowRad;
TUATBM 0:cd96dd54b3d5 457
TUATBM 0:cd96dd54b3d5 458 if(DefRad >= M_PI) DefRad = DefRad - 2.0*M_PI;
TUATBM 0:cd96dd54b3d5 459 if(DefRad <= -1.0*M_PI) DefRad = DefRad + 2.0*M_PI;
TUATBM 0:cd96dd54b3d5 460
TUATBM 0:cd96dd54b3d5 461 return DefRad;
TUATBM 0:cd96dd54b3d5 462 }
TUATBM 0:cd96dd54b3d5 463
TUATBM 0:cd96dd54b3d5 464 void FixReverse(void){
TUATBM 0:cd96dd54b3d5 465 pc.printf("FixReverse Mode. \r\n");
TUATBM 0:cd96dd54b3d5 466
TUATBM 0:cd96dd54b3d5 467 MoveCansat(MOVE_STOP);
TUATBM 0:cd96dd54b3d5 468 wait(0.5);
TUATBM 0:cd96dd54b3d5 469 MoveCansat(MAX_BACK);
TUATBM 0:cd96dd54b3d5 470 wait(0.7);
TUATBM 0:cd96dd54b3d5 471 MoveCansat(MAX_FORWARD);
TUATBM 0:cd96dd54b3d5 472 wait(2.5);
TUATBM 0:cd96dd54b3d5 473 }
TUATBM 0:cd96dd54b3d5 474
TUATBM 0:cd96dd54b3d5 475 void NewFileCreate(char fname[]){
TUATBM 0:cd96dd54b3d5 476 uint16_t i_FileCheck;
TUATBM 0:cd96dd54b3d5 477 FILE *fileP;
TUATBM 0:cd96dd54b3d5 478
TUATBM 0:cd96dd54b3d5 479 for (i_FileCheck = 1; i_FileCheck < 99; i_FileCheck++) {
TUATBM 0:cd96dd54b3d5 480 sprintf(fname, "/sd/%s%02d.txt", LOGNAME, i_FileCheck);
TUATBM 0:cd96dd54b3d5 481 pc.printf("%s\r\n",fname);
TUATBM 0:cd96dd54b3d5 482
TUATBM 0:cd96dd54b3d5 483 fileP = fopen(fname, "r");
TUATBM 0:cd96dd54b3d5 484 if(fileP == NULL){ //参照した番号のファイルがなければ生成
TUATBM 0:cd96dd54b3d5 485 fileP = fopen(fname,"w");
TUATBM 0:cd96dd54b3d5 486
TUATBM 0:cd96dd54b3d5 487 if(fileP != NULL) {
TUATBM 0:cd96dd54b3d5 488 pc.printf("%s was created\r\n",fname);
TUATBM 0:cd96dd54b3d5 489
TUATBM 0:cd96dd54b3d5 490 fclose(fileP);
TUATBM 0:cd96dd54b3d5 491 break;
TUATBM 0:cd96dd54b3d5 492 }
TUATBM 0:cd96dd54b3d5 493 else pc.printf("Faired to create a new file.\r\n");
TUATBM 0:cd96dd54b3d5 494 }
TUATBM 0:cd96dd54b3d5 495 else fclose(fileP);
TUATBM 0:cd96dd54b3d5 496 }
TUATBM 0:cd96dd54b3d5 497 }