淳熙 姜
/
mbed_Ahan_robocon
移動用モータ、リフト上下用モータ、シリンダー開閉&でっぱり用モータの3種類の制御から成り立ったプログラム
Revision 0:5486cc47e272, committed 2015-06-24
- Comitter:
- Kansuni
- Date:
- Wed Jun 24 09:48:01 2015 +0000
- Commit message:
- ?????????????????for ?????
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SoftPWM.lib Wed Jun 24 09:48:01 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/komaida424/code/SoftPWM/#7918ce37626c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 24 09:48:01 2015 +0000 @@ -0,0 +1,307 @@ +//***************************************** +// 部内ロボコン(A班)用コントロールプログラム +//***************************************** + +#include "mbed.h" +#include "SoftPWM.h" +#include "main.h" + +//デジタル読み取りピン(タクトスイッチ)の指定 +DigitalIn Abutton(dp10); +DigitalIn Bbutton(dp14); +DigitalIn Xbutton(dp28); + +//デジタル出力ピン(モーター(リフト部分))の指定 +DigitalOut motor_Rift1(dp1); +DigitalOut motor_Rift2(dp2); + +//デジタル出力ピン(モーター(コップ止め))の指定 +DigitalOut motor_Stopper1(dp6); +DigitalOut motor_Stopper2(dp9); + +//デジタル出力ピン(空気圧シリンダー)の指定 +DigitalOut valve(dp11); + +//アナログ読み取りピン(ジョイスティック)の指定 +AnalogIn JOYstick_LR(dp13); +AnalogIn JOYstick_UD(dp4); + +//PWM出力ピン(モーター)の指定 +SoftPWM motor_LEFT1(dp17); +SoftPWM motor_LEFT2(dp18); +SoftPWM motor_RIGHT1(dp25); +SoftPWM motor_RIGHT2(dp26); + +//シリアル入出力ピン(デバック用)の指定 +Serial Debug(dp16, dp15); + +int Cylinder_toggle = 0; +int Cylinder_Open_Flag = 0; +int Cylinder_Close_Flag = 0; +int Cylinder_Open_Standby = 1; +int Cylinder_Close_Standby = 0; +float move_FB = 0.5; +float move_LR = 0.5; + +int main(void){ + + //パルス周期の設定 + motor_LEFT1.period_ms(1); + motor_LEFT2.period_ms(1); + motor_RIGHT1.period_ms(1); + motor_RIGHT2.period_ms(1); + + //初期設定 + motor_Rift1 = 1; + motor_Rift2 = 1; + motor_Stopper1 = 1; + motor_Stopper2 = 1; + valve = 0; + motor_LEFT1 = 0.5; + motor_LEFT2 = 0.5; + motor_RIGHT1 = 0.5; + motor_RIGHT2 = 0.5; + + //出力レートの指定 + //Debug.baud(9600); + + //デバッグ用無限ループ + //Debug_roop(); + + //Debug.printf("LEFT1 LEFT2 RIGHT1 RIGHT2 \n"); + + while(1){ + + move_LR = 1-JOYstick_LR.read(); + move_FB = 1-JOYstick_UD.read(); + + //車体の前後左右のコントロール + Moving_control(move_LR, move_FB); + //リフトの上下操作のコントロール + Rifting_control(); + //シリンダーの開閉操作のコントロール + //Cylinder_control(); + + //フラグ管理 + if(Cylinder_Open_Flag >= 1){ + Cylinder_Open_Flag = Cylinder_Open_Flag + 1; + } + if(Cylinder_Open_Flag > Iteration){ + Cylinder_Open_Flag = 0; + Cylinder_Open_Standby = 1; + //Debug.printf("Cylinder Open Standby \n"); + } + if(Cylinder_Close_Flag >= 1){ + Cylinder_Close_Flag = Cylinder_Close_Flag + 1; + } + if(Cylinder_Close_Flag > Iteration){ + Cylinder_Close_Flag = 0; + Cylinder_Close_Standby = 1; + //Debug.printf("Cylinder Close Standby \n"); + } + + wait(0.05); + + } + +} + +//void Debug_roop(){ +// int l=0; +// +// //なにかPCでシリアル通信で送るまでは動作しない +// while(1){ +// wait(0.5); +// if(Debug.readable()){ +// break; +// } +// l ^= 1; +// } +//} + +//車体の前後左右移動のコントロール +void Moving_control(float x, float y){ + + /*デバッグ用 + Debug.printf("%f ", LEFT1_pow); + Debug.printf("%f ", LEFT2_pow); + Debug.printf("%f ", RIGHT1_pow); + Debug.printf("%f \n", RIGHT2_pow); + */ + + float x1 = 0; + float x_left = 0; + float x_right = 0; + float y1 = 0; + float V_amount = 0; + float LEFT1_pow = 0.5; + float LEFT2_pow = 0.5; + float RIGHT1_pow = 0.5; + float RIGHT2_pow = 0.5; + + x1 = (x - 0.5) * 2.0; //-1 < x1 < 1 + x1 = SizeDown(x1, 1.0); + x_left = x1 + 1.0; + x_left = SizeDown(x_left, 1.0); //0 < x_left < 1 + x_right = -x1 + 1.0; + x_right = SizeDown(x_right, 1.0); //0 < x_right < 1 + y1 = (y - 0.5) * 2.0; //-1 < y1 < 1 + y1 = SizeDown(y1, 1.0); + V_amount = ( (x1*x1) + (y1*y1) ) / 1.41; + V_amount = SizeDown(V_amount, 1.0); + + //移動用モータ(前方向移動時)の調整 + if(y1 >= 0.0 && y1 <= 1.0){ + LEFT1_pow = ((x_left * V_amount)/2.0) + 0.5; + if(LEFT1_pow >= 1.0){ + LEFT1_pow = 1.0; + } + else if(LEFT1_pow <= 0.0){ + LEFT1_pow = 0.0; + } + motor_LEFT1 = LEFT1_pow; + LEFT2_pow = ((x_left * (-V_amount))/2.0) + 0.5; + if(LEFT2_pow >= 1.0){ + LEFT2_pow = 1.0; + } + else if(LEFT2_pow <= 0.0){ + LEFT2_pow = 0.0; + } + motor_LEFT2 = LEFT2_pow; + + RIGHT1_pow = ((x_right * (-V_amount))/2.0) + 0.5; + if(RIGHT1_pow >= 1.0){ + RIGHT1_pow = 1.0; + } + else if(RIGHT1_pow <= 0.0){ + RIGHT1_pow = 0.0; + } + motor_RIGHT1 = RIGHT1_pow; + RIGHT2_pow = ((x_right * V_amount)/2.0) + 0.5; + if(RIGHT2_pow >= 1.0){ + RIGHT2_pow = 1.0; + } + else if(RIGHT2_pow <= 0.0){ + RIGHT2_pow = 0.0; + } + motor_RIGHT2 = RIGHT2_pow; + } + else{ //移動用モータ(後ろ方向移動時)の調整 + LEFT1_pow = ((x_left * (-V_amount))/2.0) + 0.5; + if(LEFT1_pow >= 1.0){ + LEFT1_pow = 1.0; + } + else if(LEFT1_pow <= 0.0){ + LEFT1_pow = 0.0; + } + motor_LEFT1 = LEFT1_pow; + LEFT2_pow = ((x_left * V_amount)/2.0) + 0.5; + if(LEFT2_pow >= 1.0){ + LEFT2_pow = 1.0; + } + else if(LEFT2_pow <= 0.0){ + LEFT2_pow = 0.0; + } + motor_LEFT2 = LEFT2_pow; + + RIGHT1_pow = ((x_right * V_amount)/2.0) + 0.5; + if(RIGHT1_pow >= 1.0){ + RIGHT1_pow = 1.0; + } + else if(RIGHT1_pow <= 0.0){ + RIGHT1_pow = 0.0; + } + motor_RIGHT1 = RIGHT1_pow; + RIGHT2_pow = ((x_right * (-V_amount))/2.0) + 0.5; + if(RIGHT2_pow >= 1.0){ + RIGHT2_pow = 1.0; + } + else if(RIGHT2_pow <= 0.0){ + RIGHT2_pow = 0.0; + } + motor_RIGHT2 = RIGHT2_pow; + } + + wait(0.01); +} + +//車体後部リフトの上下操作のコントロール +void Rifting_control(){ + + //Aボタンが押されていれば、リフトを上に移動 + if(Abutton == 1 && Bbutton == 0){ + //Debug.printf("RiftUP \n"); + motor_Rift1 = 0; + motor_Rift2 = 1; + wait(0.01); + } + //Bボタンが押されていれば、リフトを下に移動 + else if(Abutton == 0 && Bbutton == 1){ + //Debug.printf("RiftDOWN \n"); + motor_Rift1 = 1; + motor_Rift2 = 0; + wait(0.01); + } + //それ以外は停止 + else{ + motor_Rift1 = 1; + motor_Rift2 = 1; + wait(0.01); + } +} + +//シリンダーの開閉操作をコントロール +void Cylinder_control(){ + + //フラグが0の状態でXボタンが押されれば、シリンダーを開放、コップ固定用出っ張りを開放 + if(Xbutton == 1 && Cylinder_toggle == 0 && Cylinder_Open_Standby == 1){ + //シリンダーを開放 + //Debug.printf("Cylinder Open \n"); + valve = 1; + Cylinder_toggle = 1; + Cylinder_Open_Standby = 0; + Cylinder_Close_Flag = 1; + //0.5秒間待機 + wait(0.5); + //ストッパー(ウォームギア)を0.15秒間正回転 + motor_Stopper1 = 1; + motor_Stopper2 = 0; + wait(0.15); + //ストッパー(ウォームギア)を停止 + motor_Stopper1 = 1; + motor_Stopper2 = 1; + wait(0.05); + } + //フラグが1の状態でXボタンが押されれば、シリンダーを収納、コップ固定用出っ張りを収納 + //スタンバイフラグが1になるタイミングは、while文がIteration回だけ反復した時 + else if(Xbutton == 1 && Cylinder_toggle == 1 && Cylinder_Close_Standby == 1){ + //シリンダーを収納 + //Debug.printf("Cylinder Close \n"); + valve = 0; + Cylinder_toggle = 0; + Cylinder_Close_Standby = 0; + Cylinder_Open_Flag = 1; + wait(0.01); + //0.5秒間待機 + wait(0.5); + //ストッパー(ウォームギア)を0.15秒間逆回転 + motor_Stopper1 = 0; + motor_Stopper2 = 1; + wait(0.15); + //ストッパー(ウォームギア)を停止 + motor_Stopper1 = 1; + motor_Stopper2 = 1; + wait(0.05); + } +} + +//特定の範囲内にパラメータを抑え込む関数(widthは正数で定義) +float SizeDown(float param, float width){ + if(param >= width){ + param = width; + } + else if(param <= -width){ + param = -width; + } + return param; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.h Wed Jun 24 09:48:01 2015 +0000 @@ -0,0 +1,8 @@ +#define Iteration 10 + +void Debug_roop(); +void Moving_control(float , float ); +void Rifting_control(); +void Cylinder_control(); + +float SizeDown(float , float ); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Jun 24 09:48:01 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/7cff1c4259d7 \ No newline at end of file