File content as of revision 0:5486cc47e272:

//*****************************************
//  部内ロボコン（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;
}