main.cpp

00001 //*****************************************
00002 //  部内ロボコン（A班）用コントロールプログラム
00003 //*****************************************
00004 
00005 #include "mbed.h"
00006 #include "SoftPWM.h"
00007 #include "main.h"
00008 
00009 //デジタル読み取りピン（タクトスイッチ）の指定
00010 DigitalIn Abutton(dp10);
00011 DigitalIn Bbutton(dp14);
00012 DigitalIn Xbutton(dp28);
00013 
00014 //デジタル出力ピン（モーター（リフト部分））の指定
00015 DigitalOut motor_Rift1(dp1);
00016 DigitalOut motor_Rift2(dp2);
00017 
00018 //デジタル出力ピン（モーター（コップ止め））の指定
00019 DigitalOut motor_Stopper1(dp6);
00020 DigitalOut motor_Stopper2(dp9);
00021 
00022 //デジタル出力ピン（空気圧シリンダー）の指定
00023 DigitalOut valve(dp11);
00024 
00025 //アナログ読み取りピン（ジョイスティック）の指定
00026 AnalogIn JOYstick_LR(dp13);
00027 AnalogIn JOYstick_UD(dp4);
00028 
00029 //PWM出力ピン（モーター）の指定
00030 SoftPWM motor_LEFT1(dp17);
00031 SoftPWM motor_LEFT2(dp18);
00032 SoftPWM motor_RIGHT1(dp25);
00033 SoftPWM motor_RIGHT2(dp26);
00034 
00035 //シリアル入出力ピン（デバック用）の指定
00036 Serial Debug(dp16, dp15);
00037 
00038 int Cylinder_toggle = 0; 
00039 int Cylinder_Open_Flag = 0;
00040 int Cylinder_Close_Flag = 0;
00041 int Cylinder_Open_Standby = 1;
00042 int Cylinder_Close_Standby = 0;
00043 float move_FB = 0.5;
00044 float move_LR = 0.5;
00045 
00046 int main(void){
00047     
00048     //パルス周期の設定
00049     motor_LEFT1.period_ms(1);
00050     motor_LEFT2.period_ms(1);
00051     motor_RIGHT1.period_ms(1);
00052     motor_RIGHT2.period_ms(1);
00053     
00054     //初期設定
00055     motor_Rift1 = 1;
00056     motor_Rift2 = 1;
00057     motor_Stopper1 = 1;
00058     motor_Stopper2 = 1;
00059     valve = 0;
00060     motor_LEFT1  = 0.5;
00061     motor_LEFT2  = 0.5;
00062     motor_RIGHT1 = 0.5;
00063     motor_RIGHT2 = 0.5;
00064     
00065     //出力レートの指定
00066     //Debug.baud(9600);
00067 
00068     //デバッグ用無限ループ
00069     //Debug_roop();
00070     
00071     //Debug.printf("LEFT1   LEFT2   RIGHT1  RIGHT2  \n");
00072     
00073     while(1){
00074         
00075         move_LR = 1-JOYstick_LR.read();
00076         move_FB = 1-JOYstick_UD.read();
00077         
00078         //車体の前後左右のコントロール
00079         Moving_control(move_LR, move_FB);
00080         //リフトの上下操作のコントロール
00081         Rifting_control();
00082         //シリンダーの開閉操作のコントロール
00083         //Cylinder_control();
00084         
00085         //フラグ管理
00086         if(Cylinder_Open_Flag >= 1){
00087             Cylinder_Open_Flag = Cylinder_Open_Flag + 1;
00088         }
00089         if(Cylinder_Open_Flag > Iteration){
00090             Cylinder_Open_Flag = 0;
00091             Cylinder_Open_Standby = 1;
00092             //Debug.printf("Cylinder Open Standby \n");
00093         }
00094         if(Cylinder_Close_Flag >= 1){
00095             Cylinder_Close_Flag = Cylinder_Close_Flag + 1;
00096         }
00097         if(Cylinder_Close_Flag > Iteration){
00098             Cylinder_Close_Flag = 0;
00099             Cylinder_Close_Standby = 1;
00100             //Debug.printf("Cylinder Close Standby \n");
00101         }
00102 
00103         wait(0.05);
00104         
00105     }
00106     
00107 }
00108 
00109 //void Debug_roop(){
00110 //    int l=0;
00111 //    
00112 //    //なにかPCでシリアル通信で送るまでは動作しない
00113 //    while(1){       
00114 //        wait(0.5);
00115 //        if(Debug.readable()){
00116 //            break;
00117 //        }
00118 //        l ^= 1;
00119 //    }
00120 //}
00121 
00122 //車体の前後左右移動のコントロール
00123 void Moving_control(float x, float y){
00124     
00125     /*デバッグ用
00126     Debug.printf("%f  ", LEFT1_pow);
00127     Debug.printf("%f  ", LEFT2_pow);
00128     Debug.printf("%f  ", RIGHT1_pow);
00129     Debug.printf("%f  \n", RIGHT2_pow);
00130     */
00131     
00132     float x1 = 0;
00133     float x_left  = 0;
00134     float x_right = 0;          
00135     float y1 = 0;
00136     float V_amount = 0;
00137     float LEFT1_pow = 0.5;
00138     float LEFT2_pow = 0.5;
00139     float RIGHT1_pow = 0.5;
00140     float RIGHT2_pow = 0.5;
00141     
00142     x1 = (x - 0.5) * 2.0;   //-1 < x1 < 1
00143     x1 = SizeDown(x1, 1.0);
00144     x_left  = x1 + 1.0;
00145     x_left  = SizeDown(x_left, 1.0); //0 < x_left < 1
00146     x_right = -x1 + 1.0;
00147     x_right = SizeDown(x_right, 1.0); //0 < x_right < 1
00148     y1 = (y - 0.5) * 2.0;   //-1 < y1 < 1
00149     y1 = SizeDown(y1, 1.0);
00150     V_amount = ( (x1*x1) + (y1*y1) ) / 1.41;
00151     V_amount = SizeDown(V_amount, 1.0);
00152     
00153     //移動用モータ（前方向移動時）の調整
00154     if(y1 >= 0.0 && y1 <= 1.0){
00155         LEFT1_pow  = ((x_left * V_amount)/2.0) + 0.5;
00156         if(LEFT1_pow >= 1.0){
00157             LEFT1_pow = 1.0;
00158         }
00159         else if(LEFT1_pow <= 0.0){
00160             LEFT1_pow = 0.0;
00161         }
00162         motor_LEFT1 = LEFT1_pow;
00163         LEFT2_pow  = ((x_left * (-V_amount))/2.0) + 0.5;
00164         if(LEFT2_pow >= 1.0){
00165             LEFT2_pow = 1.0;
00166         }
00167         else if(LEFT2_pow <= 0.0){
00168             LEFT2_pow = 0.0;
00169         }
00170         motor_LEFT2 = LEFT2_pow;
00171     
00172         RIGHT1_pow = ((x_right * (-V_amount))/2.0) + 0.5;
00173         if(RIGHT1_pow >= 1.0){
00174             RIGHT1_pow = 1.0;
00175         }
00176         else if(RIGHT1_pow <= 0.0){
00177             RIGHT1_pow = 0.0;
00178         }
00179         motor_RIGHT1 = RIGHT1_pow;
00180         RIGHT2_pow = ((x_right * V_amount)/2.0) + 0.5;
00181         if(RIGHT2_pow >= 1.0){
00182             RIGHT2_pow = 1.0;
00183         }
00184         else if(RIGHT2_pow <= 0.0){
00185             RIGHT2_pow = 0.0;
00186         }
00187         motor_RIGHT2 = RIGHT2_pow;
00188     }
00189     else{       //移動用モータ（後ろ方向移動時）の調整
00190         LEFT1_pow  = ((x_left * (-V_amount))/2.0) + 0.5;
00191         if(LEFT1_pow >= 1.0){
00192             LEFT1_pow = 1.0;
00193         }
00194         else if(LEFT1_pow <= 0.0){
00195             LEFT1_pow = 0.0;
00196         }
00197         motor_LEFT1 = LEFT1_pow;
00198         LEFT2_pow  = ((x_left * V_amount)/2.0) + 0.5;
00199         if(LEFT2_pow >= 1.0){
00200             LEFT2_pow = 1.0;
00201         }
00202         else if(LEFT2_pow <= 0.0){
00203             LEFT2_pow = 0.0;
00204         }
00205         motor_LEFT2 = LEFT2_pow;
00206     
00207         RIGHT1_pow = ((x_right * V_amount)/2.0) + 0.5;
00208         if(RIGHT1_pow >= 1.0){
00209             RIGHT1_pow = 1.0;
00210         }
00211         else if(RIGHT1_pow <= 0.0){
00212             RIGHT1_pow = 0.0;
00213         }
00214         motor_RIGHT1 = RIGHT1_pow;
00215         RIGHT2_pow = ((x_right * (-V_amount))/2.0) + 0.5;
00216         if(RIGHT2_pow >= 1.0){
00217             RIGHT2_pow = 1.0;
00218         }
00219         else if(RIGHT2_pow <= 0.0){
00220             RIGHT2_pow = 0.0;
00221         }
00222         motor_RIGHT2 = RIGHT2_pow;
00223     }    
00224     
00225     wait(0.01);
00226 }
00227 
00228 //車体後部リフトの上下操作のコントロール
00229 void Rifting_control(){
00230     
00231     //Aボタンが押されていれば、リフトを上に移動
00232     if(Abutton == 1 && Bbutton == 0){
00233         //Debug.printf("RiftUP  \n");
00234         motor_Rift1 = 0;
00235         motor_Rift2 = 1;
00236         wait(0.01);
00237     }
00238     //Bボタンが押されていれば、リフトを下に移動
00239     else if(Abutton == 0 && Bbutton == 1){
00240         //Debug.printf("RiftDOWN  \n");
00241         motor_Rift1 = 1;
00242         motor_Rift2 = 0;
00243         wait(0.01);
00244     }
00245     //それ以外は停止
00246     else{
00247         motor_Rift1 = 1;
00248         motor_Rift2 = 1;
00249         wait(0.01);
00250     }
00251 }
00252 
00253 //シリンダーの開閉操作をコントロール
00254 void Cylinder_control(){
00255     
00256     //フラグが0の状態でXボタンが押されれば、シリンダーを開放、コップ固定用出っ張りを開放
00257     if(Xbutton == 1 && Cylinder_toggle == 0 && Cylinder_Open_Standby == 1){
00258         //シリンダーを開放
00259         //Debug.printf("Cylinder Open  \n");
00260         valve = 1;
00261         Cylinder_toggle = 1;
00262         Cylinder_Open_Standby = 0;
00263         Cylinder_Close_Flag = 1;
00264         //0.5秒間待機
00265         wait(0.5);
00266         //ストッパー（ウォームギア）を0.15秒間正回転
00267         motor_Stopper1 = 1;
00268         motor_Stopper2 = 0;
00269         wait(0.15);
00270         //ストッパー（ウォームギア）を停止
00271         motor_Stopper1 = 1;
00272         motor_Stopper2 = 1;
00273         wait(0.05);
00274     }
00275     //フラグが1の状態でXボタンが押されれば、シリンダーを収納、コップ固定用出っ張りを収納
00276     //スタンバイフラグが1になるタイミングは、while文がIteration回だけ反復した時
00277     else if(Xbutton == 1 && Cylinder_toggle == 1 && Cylinder_Close_Standby == 1){
00278         //シリンダーを収納
00279         //Debug.printf("Cylinder Close  \n");
00280         valve = 0;
00281         Cylinder_toggle = 0;
00282         Cylinder_Close_Standby = 0;
00283         Cylinder_Open_Flag = 1;
00284         wait(0.01);
00285         //0.5秒間待機
00286         wait(0.5);
00287         //ストッパー（ウォームギア）を0.15秒間逆回転
00288         motor_Stopper1 = 0;
00289         motor_Stopper2 = 1;
00290         wait(0.15);
00291         //ストッパー（ウォームギア）を停止
00292         motor_Stopper1 = 1;
00293         motor_Stopper2 = 1;
00294         wait(0.05);
00295     }
00296 }
00297 
00298 //特定の範囲内にパラメータを抑え込む関数(widthは正数で定義)
00299 float SizeDown(float param, float width){
00300     if(param >= width){
00301         param = width;
00302     }
00303     else if(param <= -width){
00304         param = -width;
00305     }
00306     return param;
00307 }