移動用モータ、リフト上下用モータ、シリンダー開閉&でっぱり用モータの3種類の制御から成り立ったプログラム

Dependencies:   SoftPWM mbed

Files at this revision

API Documentation at this revision

Comitter:
Kansuni
Date:
Wed Jun 24 09:48:01 2015 +0000
Commit message:
?????????????????for ?????

Changed in this revision

SoftPWM.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 5486cc47e272 SoftPWM.lib
--- /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
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
diff -r 000000000000 -r 5486cc47e272 main.h
--- /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
diff -r 000000000000 -r 5486cc47e272 mbed.bld
--- /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