淳熙 姜
移動用モータ、リフト上下用モータ、シリンダー開閉＆でっぱり用モータの３種類の制御から成り立ったプログラム
Diff: main.cpp
- Revision:
- 0:5486cc47e272
diff -r 000000000000 -r 5486cc47e272 main.cpp
--- /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