LabRobocon
Added one task
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:fb4269aa5fb4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,855 @@
+#include "mbed.h"
+
+#include "gpio.h"
+
+#include "robot.h"
+#include "GP2Y0E03.h"
+#include "MCP3208.h"
+#include "L3GD20.h"
+
+#define ERROR (-1)
+#define SUCCESS 0
+
+//#define L3GD20_TEST
+//#define GP2Y0E03_TEST
+//#define MCP3208_TEST
+//#define LBR127_TEST
+
+/*---------RCサーボ制御用--------*/
+float servo_pulse = 0.001f; //RCサーボパルス幅[s]
+uint32_t servo_state = 0; //サーボピン状態
+/*-----------------------------*/
+
+/*-----------------ラインセンサ--------------------*/
+float line_pos[2] = {0.0f, 0.0f};
+uint32_t intersection = 0; //交差点検知
+/*------------------------------------------------*/
+
+/*---------状態変数----------*/
+float v_tr = 0.0f; //並進方向電圧, unit: V
+float theta = 0.0f; //姿勢角, unit: degree
+
+float omega_d = 0.0f; //目標角速度, unit: degree/s
+/*--------------------------*/
+
+/*-----------------------------*/
+float ts = 0.001f; //制御周期
+
+uint32_t sequence = 0; //タスク管理変数
+uint32_t error_seq = 0xFF; //エラーを起こしているタスク番号
+int32_t (*Task_Fptr[10])(void); //タスク管理用関数ポインタ
+/*-----------------------------*/
+
+/*-----制御器-----*/
+PID_Struct trace_pid; //ライントレース用
+PID_Struct gyro_pid; //ジャイロによるオドメトリ用
+
+PID_Struct manual_control; //手動モータ制御用
+
+PID_Struct *current_pid_ptr; //現在接続されている制御器
+/*----------------*/
+
+/*---------各タスク関数--------*/
+//ロボットの動作をシーケンシャルに記述する
+//各タスク関数内ではフィードバック制御則へのアクセス原則禁止
+int32_t Task0(void);
+int32_t Task1(void);
+int32_t Task2(void);
+int32_t Task3(void);
+int32_t Task4(void);
+int32_t Task5(void);
+int32_t Task6(void);
+int32_t Task7(void);
+int32_t Task8(void);
+int32_t Task9(void);
+/*---------------------------*/
+
+void Turn_90deg(int32_t);
+void Turn_180deg(int32_t);
+
+void Accelerate(float);
+void Decelerate();
+void Stop();
+
+int main()
+{
+ uint32_t i;
+
+ /*Peripheral initialization*/
+ //GPIO_Init();
+
+ SPI_Init();
+
+ /*-----各種デバイス初期化----*/
+ L3GD20_Init();
+ MCP3208_Init();
+ /*-------------------------*/
+
+ /*------フィードバックコントローラ-------*/
+ PID_Struct_Init(&trace_pid, 1.1, 20.0, 0.0);
+ //PID_Struct_Init(&gyro_pid, 0.015, 0.0, 0.0);
+ PID_Struct_Init(&gyro_pid, 0.02, 0.0, 0.0);
+ current_pid_ptr = &manual_control;
+ /*-----------------------------------*/
+
+ TIM_Init(); //タイマスタート
+ /*-------------------------*/
+
+ /*-----関数ポインタ設定------*/
+ Task_Fptr[0] = Task0;
+ Task_Fptr[1] = Task1;
+ Task_Fptr[2] = Task2;
+ Task_Fptr[3] = Task3;
+ Task_Fptr[4] = Task4;
+ Task_Fptr[5] = Task5;
+ Task_Fptr[6] = Task6;
+ Task_Fptr[7] = Task7;
+ Task_Fptr[8] = Task8;
+ Task_Fptr[9] = Task9;
+ /*-------------------------*/
+
+
+
+
+ while(1)
+ {
+ if(Task_Fptr[sequence]() == ERROR) //タスク呼び出し, エラーチェック
+ {
+ error_seq = sequence; //エラーを起こしているタスク番号を取得
+ }
+
+ sequence ++;
+ /*
+ if(sequence == 1)
+ {
+ sequence = 3;
+ }
+ */
+ }
+}
+
+int32_t Task0(void)
+{
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ servo_pulse = 0.0021f; //アームを折りたたむ
+ wait(0.5f);
+
+ /*-----最下点までリフトを移動-----*/
+ Put_Vm_Lift(-0.8f); //リフト下降
+ wait(2.0f);
+ Put_Vm_Lift(0.0f); //リフト停止
+ /*-----------------------------*/
+ /*----リフトのリポジショニング----*/
+ Put_Vm_Lift(1.0f); //リフト上昇(どん兵衛用)
+ wait(0.8f);
+ Put_Vm_Lift(0.0f); //リフト停止
+ /*-----------------------------*/
+
+ /*--------------------スタート待機-----------------------*/
+ while(ain_cup > 0.5f) //カップ検出センサが反応するまで待機
+ {
+ printf("cup = %f\n", (float)ain_cup);
+ }
+
+ servo_pulse = 0.001f; //アームを展開
+ wait(0.3f);
+ /*-----------------------------------------------------*/
+ /*
+ while(1)
+ {
+ HAL_Delay(10);
+ printf("pos = %f\n", line_pos[0]);
+ }
+ */
+ return 0;
+}
+
+int32_t Task1(void)
+{
+ uint32_t i;
+ float v = 0.0f;
+
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(2.0f); //加速
+
+ for(i = 0; i < 2; i ++) //1-2回目の交差点はスキップ
+ {
+ while(!intersection) //一回目の交差点検出はスキップ
+ {
+ printf("running...\n");
+ }
+ while(intersection) //一回目の交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+ }
+ /*-----------------------------------------------------*/
+
+ return 0;
+}
+
+int32_t Task2(void) //3本目の交差点を左折
+{
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ while(!intersection) //3回目の交差点検出まで直進
+ {
+ printf("running...\n");
+ }
+ while(intersection) //3回目の交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+
+ wait(0.4f);
+
+ Stop(); //停止
+ Turn_90deg(1); //左90度旋回
+
+ return 0;
+}
+
+int32_t Task3(void) //横道を直進し、どん兵衛を回収、180度回転
+{
+ uint32_t i;
+ float v = 0.0f;
+
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.0f); //加速
+ /*-----------------------------------------------------*/
+
+ #if 0
+ //本来はここでカップを検出するまで前進する
+ wait(1.3f);
+ #endif
+
+ #if 1
+ while(ain_cup > 0.6f) //カップ検出まで前進
+ {
+ printf("waiting...\n");
+ }
+
+ wait(0.05f);
+ Stop(); //停止
+
+ servo_pulse = 0.0021f; //カップ把持
+ wait(1.0f);
+
+ Put_Vm_Lift(1.5f); //リフト上昇
+ wait(0.8f);
+ Put_Vm_Lift(0.0f); //リフト停止
+ #endif
+
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ wait(0.5f);
+ Stop(); //停止
+
+ Turn_180deg(1); //左180度旋回
+
+ return 0;
+}
+
+int32_t Task4(void) //カップ回収後、メインストリートに戻る
+{
+ uint32_t i;
+ float v = 0.0f;
+
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ /*-----------------------------------------------------*/
+
+ while(!intersection) //交差点検出まで直進
+ {
+ printf("running...\n");
+ }
+ while(intersection) //交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+
+ wait(0.6f);
+
+ Stop();
+
+ Turn_90deg(1); //左90度旋回
+
+ return 0;
+}
+
+int32_t Task5(void) //カップ回収後のメインストリート走行+カップ棚置き
+{
+ uint32_t i;
+ float v = 0.0f;
+
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(2.0f); //加速
+ /*-----------------------------------------------------*/
+
+ while(ain_cup > 0.7f) //棚検出まで前進
+ {
+ printf("waiting...\n");
+ }
+
+ Stop();
+
+ servo_pulse = 0.001f; //カップ解放
+ wait(0.5f);
+
+ /*----------ジャイロを使って後退--------------*/
+ current_pid_ptr = &gyro_pid; //制御器変更
+ Accelerate(-1.5f); //加速
+
+ wait(0.6f);
+
+ Stop();
+ /*------------------------------------------*/
+
+ Turn_180deg(1); //左180度旋回
+
+ /*-----最下点までリフトを移動-----*/
+ Put_Vm_Lift(-0.8f); //リフト下降
+ wait(2.5f);
+ Put_Vm_Lift(0.0f); //リフト停止
+ /*-----------------------------*/
+ /*----リフトのリポジショニング----*/
+ Put_Vm_Lift(1.0f); //リフト上昇(UFO用)
+ wait(0.35f);
+ Put_Vm_Lift(0.0f); //リフト停止
+ /*-----------------------------*/
+
+ return 0;
+}
+
+int32_t Task6(void) //4本目の交差点を棚側から左折
+{
+
+ uint32_t i;
+ float v = 0;
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ /*-----------------------------------------------------*/
+
+ while(!intersection) //交差点検出まで直進
+ {
+ printf("running...\n");
+ }
+ while(intersection) //交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+
+ wait(0.6f);
+
+ Stop(); //停止
+ Turn_90deg(1); //左90度旋回
+
+ return 0;
+}
+
+int32_t Task7(void) //横道を直進し、UFOを回収、180度回転
+{
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.0f); //加速
+ /*-----------------------------------------------------*/
+
+
+ while(ain_cup > 0.6f) //カップ検出まで前進
+ {
+ printf("waiting...\n");
+ }
+
+ wait(0.05f);
+ Stop(); //停止
+
+ servo_pulse = 0.0021f; //カップ把持
+ wait(1.0f);
+
+ Put_Vm_Lift(1.5f); //リフト上昇
+ wait(0.8f);
+ Put_Vm_Lift(0.0f); //リフト停止
+
+
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ wait(0.5f);
+ Stop(); //停止
+
+ Turn_180deg(1); //左180度旋回
+}
+
+
+int32_t Task8(void) //棚左側にUFOを置くために、位置合わせ
+{
+ uint32_t i;
+ float v = 0;
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ /*-----------------------------------------------------*/
+
+ /*---------------1回目の交差点-----------------*/
+ while(!intersection) //交差点検出まで直進
+ {
+ printf("running...\n");
+ }
+ while(intersection) //交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+
+ wait(2.5f); //棚左側とカップの位置を合わせるためのパラメータ
+ /*--------------------------------------------*/
+
+ Stop();
+ Turn_90deg(-1); //右90度旋回
+
+ return 0;
+}
+
+int32_t Task9(void) //ジャイロを使って棚まで接近、UFOを置く
+{
+ uint32_t i;
+ float v = 0;
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*----------ジャイロを使って前進--------------*/
+ current_pid_ptr = &gyro_pid; //制御器変更
+ Accelerate(1.5f); //加速
+ /*------------------------------------------*/
+
+ while(ain_cup > 0.7f) //棚検出まで前進
+ {
+ printf("waiting...\n");
+ }
+
+ Stop();
+
+ servo_pulse = 0.001f; //カップ解放
+ wait(0.5f);
+
+ while(1)
+ {
+ printf("Tasks completed\n");
+ }
+
+
+ return 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#if 0
+
+int32_t Task8(void) //棚左側にUFOを置くために、位置合わせ
+{
+ uint32_t i;
+ float v = 0;
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ /*-------------------ライントレース--------------------*/
+ current_pid_ptr = &trace_pid; //制御器変更
+ Accelerate(2.0f); //加速
+
+ for(i = 0; i < 3; i ++) //1-3回目の交差点はスキップ
+ {
+ while(!intersection) //一回目の交差点検出はスキップ
+ {
+ printf("running...\n");
+ }
+ while(intersection) //一回目の交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+ }
+ /*-----------------------------------------------------*/
+
+ /*---------------4回目の交差点-----------------*/
+ while(!intersection) //交差点検出まで直進
+ {
+ printf("running...\n");
+ }
+ while(intersection) //交差点通過を待つ
+ {
+ printf("running...\n");
+ }
+
+ wait(1.5f);
+ /*--------------------------------------------*/
+
+ Stop();
+
+ return 0;
+}
+
+int32_t Task9(void)
+{
+ uint32_t i;
+ float v = 0;
+ /*
+ if()
+ {
+ return ERROR;
+ }
+ */
+
+ while(1)
+ {
+ printf("Tasks completed\n");
+ }
+
+
+ return 0;
+}
+
+#endif
+
+void Turn_90deg(int32_t dir)
+{
+ uint32_t i;
+ float v = 0.0f;
+ float sign = 1.0f;
+
+ if(dir == 1) //回転方向
+ {
+ sign = 1.0f;
+ }
+ else if(dir == -1)
+ {
+ sign = -1.0f;
+ }
+
+ theta = 0.0f; //角度初期化
+
+ /*--------台形加速(角度)---------*/
+ for(i = 0; i < 10; i ++)
+ {
+ v += 0.15f;
+ Put_Vm_L(-sign * v); //回転
+ Put_Vm_R(sign * v);
+ wait(0.05f);
+ }
+ /*-----------------------------*/
+
+ if(dir == 1)
+ {
+ while(theta < 80.0f)
+ {
+ printf("Turning...\n");
+ }
+ }
+ else if(dir == -1)
+ {
+ while(theta > -80.0f)
+ {
+ printf("Turning...\n");
+ }
+ }
+
+ Put_Vm_L(0.0f); //停止
+ Put_Vm_R(0.0f);
+ wait(0.5f);
+}
+
+void Turn_180deg(int32_t dir)
+{
+ uint32_t i;
+ float v = 0.0f;
+ float sign = 1.0f;
+
+ if(dir == 1) //回転方向
+ {
+ sign = 1.0f;
+ }
+ else if(dir == -1)
+ {
+ sign = -1.0f;
+ }
+
+ theta = 0.0f; //角度初期化
+
+ /*--------台形加速(角度)---------*/
+ for(i = 0; i < 10; i ++)
+ {
+ v += 0.15f;
+ Put_Vm_L(-sign * v); //回転
+ Put_Vm_R(sign * v);
+ wait(0.05f);
+ }
+ /*-----------------------------*/
+
+ while(theta < 170.0f)
+ {
+ printf("Turning...\n");
+ }
+ Put_Vm_L(0.0f); //停止
+ Put_Vm_R(0.0f);
+ wait(0.5f);
+}
+
+void Accelerate(float v_desired) //v_desired[V]まで緩やかに加速
+{
+ uint32_t i;
+ float v = 0.0f;
+ float v_step = v_desired / 10.0f; //単位速度
+
+ for(i = 0; i < 10; i ++)
+ {
+ v += v_step;
+ v_tr = v;
+ wait(0.05f);
+ }
+}
+
+void Decelerate() //緩やかに減速、停止
+{
+ uint32_t i;
+ float v = 0.0f;
+ float v_step = -v_tr / 10.0f;
+
+ for(i = 0; i < 10; i ++)
+ {
+ v += v_step;
+ v_tr = v;
+ wait(0.05f);
+ }
+
+ v_tr = 0.0f; //確実に停止
+ wait(0.1f);
+}
+
+void Stop() //急激に減速、停止
+{
+ current_pid_ptr = &manual_control; //制御OFF
+ Put_Vm_L(0.0f);
+ Put_Vm_R(0.0f);
+ wait(0.5f);
+}
+
+void Flip1_Callback() //RCサーボ制御用割り込み関数
+{
+ switch(servo_state)
+ {
+ case 0:
+ flipper1.attach(&Flip1_Callback, servo_pulse);
+ servo = 1;
+ servo_state ++;
+ break;
+ case 1:
+ flipper1.attach(&Flip1_Callback, 0.01f - servo_pulse);
+ servo = 0;
+ servo_state ++;
+ break;
+ default:
+ servo_state = 0;
+ break;
+ }
+}
+
+void Flip2_Callback() //フィードバック制御用割り込み関数
+{
+ int32_t i;
+ uint32_t adres_max = 0;
+ uint32_t adres_min = 65535;
+ uint32_t adres_mid = 0;
+ int32_t pos_int = 0;
+
+ float omega = 0.0f; //unit: deg/s
+ float vm_L, vm_R; //unit: V
+
+ /*-----------------センサ読み取り-----------------*/
+ //MCP3208_Read(0);
+ MCP3208_Read(1);
+ MCP3208_Read(2);
+ MCP3208_Read(3);
+ MCP3208_Read(4);
+ MCP3208_Read(5);
+
+ L3GD20_Read();
+ /*-----------------------------------------------*/
+
+ /*---------------最大最小値を求める----------------*/
+ for(i = 0; i < 5; i ++)
+ {
+ mcp3208.adres[i+1] = 4095 - mcp3208.adres[i+1]; //ADCの出力とラインの色に合わせて反転/非反転
+
+ if(adres_min > mcp3208.adres[i+1])
+ adres_min = mcp3208.adres[i+1];
+ if(adres_max < mcp3208.adres[i+1])
+ adres_max = mcp3208.adres[i+1];
+ }
+
+ adres_mid = (adres_max + adres_min) / 2;
+ /*-----------------------------------------------*/
+ if(adres_max - adres_min > 200)
+ {
+ line_pos[0] = 0.0f;
+ for(i = 0; i < 5; i ++)
+ {
+ pos_int += (i+1)*(mcp3208.adres[i+1] - adres_min);
+ }
+ line_pos[0] = ((float)pos_int);
+ line_pos[0] = line_pos[0] / (mcp3208.adres[1]+mcp3208.adres[2]+mcp3208.adres[3]+mcp3208.adres[4]+mcp3208.adres[5]-5*adres_min) - 3.0f;
+ }
+ else
+ {
+ line_pos[0] = 0.0f;
+ }
+
+ /*-------------------交差点検出-------------------*/
+ if((mcp3208.adres[1] > adres_mid) && (mcp3208.adres[5] > adres_mid))
+ {
+ intersection = 1;
+ }
+ else if((mcp3208.adres[1] < adres_mid) && (mcp3208.adres[5] < adres_mid))
+ {
+ intersection = 0;
+ }
+ /*-----------------------------------------------*/
+
+ /*---------------フィルタ----------------*/
+
+ /*--------------------------------------*/
+
+ /*-----------ジャイロ角速度取得-----------*/
+ omega = (float)l3gd20.z * 0.07f;
+ theta += omega * ts;
+ /*--------------------------------------*/
+
+ /*------------------制御則-----------------*/
+ if(current_pid_ptr == &trace_pid) //ライントレース
+ {
+ //vm_R = PD_Control(current_pid_ptr, omega, 10.0f * line_pos[0]);
+ vm_R = PD_Control(current_pid_ptr, -line_pos[0], 0.0f);
+ vm_L = -vm_R;
+
+ vm_L += v_tr;
+ vm_R += v_tr;
+ }
+ else if(current_pid_ptr == &gyro_pid)
+ {
+ vm_R = PD_Control(current_pid_ptr, omega, omega_d);
+ vm_L = -vm_R;
+
+ vm_L += v_tr;
+ vm_R += v_tr;
+ }
+ else
+ {
+ vm_L = 0.0f;
+ vm_R = 0.0f;
+ }
+
+ if(current_pid_ptr == &manual_control)
+ {
+ //手動でモータ電圧を管理している場合
+ }
+ else
+ {
+ Put_Vm_L(vm_L); //モータへ電圧印加
+ Put_Vm_R(vm_R);
+ }
+
+ /*----------------------------------------*/
+}
+
+void Flip3_Callback()
+{
+
+
+
+}
+
+void PinIntrrpt1_Callback()
+{
+
+}
+
+void PinIntrrpt2_Callback()
+{
+
+}
\ No newline at end of file