車両操縦用のプログラム 日本語Betaで作成してます

Dependencies:   mbed

main.cpp

Committer:
neoqased
Date:
2016-03-03
Revision:
0:bce22b52f3e2

File content as of revision 0:bce22b52f3e2:

#include "mbed.h"
//#include "USBHostMSD.h"
//#include "USBHostXpad.h"
//#include "SDFileSystem.h"

/******************************入出力設定******************************/
DigitalOut led1(LED1);      //mbed上LED出力4つ
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//モータのPWM出力
PwmOut pwm1(p21);       //一番上
PwmOut pwm2(p22);       //上から二番目
PwmOut pwm3(p23);       //上から三番目
PwmOut pwm4(p24);       //一番下

//モータドライバのDIRへの信号
DigitalOut dir1(p30);
DigitalOut dir2(p29);
DigitalOut dir3(p28);
DigitalOut dir4(p27);

//スイッチ類入力
DigitalIn startsw(p11);     //スタートスイッチ、押した後離してプログラム開始
DigitalIn dip1(p12);        //DIPスイッチ、dip1は一番左(DIPスイッチの1)
DigitalIn dip2(p13);        //2
DigitalIn dip3(p14);        //3
DigitalIn dip4(p15);        //4
DigitalIn forward(p16);     //FRスイッチのF
DigitalIn reverse(p17);     //FRスイッチのR
AnalogIn potentiometer(p19);        //ポテンショメータ
AnalogIn lrsw(p20);

//ブザー
DigitalOut buzzer(p18);      //電子ブザー用アナorデジ出力

//USBフラッシュメモリ
//USBHostMSD msc("msc");
//USBでXbox360ワイヤレスコントローラを使うには、USBHostXpadを使う

//SDカード
//SDFileSystem sd(p5, p6, p7, p8, "sd");

//予備のデジタル入出力:p16,p17,p19,p20

/******************************プロトタイプ宣言******************************/
int Init();     //初期化関数
int DipLed();       //DIPスイッチの状態によってledを光らせる関数
int GetDipValue();      //DIPスイッチの値を取得し,値を返す関数
int SW();        //スタートスイッチ用関数,押して離したらスタート
int Buzzer(int buzvar = 1);     //電子ブザーを鳴らす関数
int GraduallyStop();        //ゆるやかにモータを停止させるための関数
int GraduallyStop2();
int GraduallyStart(float pot, int lr, int fr);      //ゆるやかにモータを始動させる関数
int GraduallyStart2(float a, float b, float c, float d, float v);
int MotorsPwm(double a = 0, double b = 0, double c = 0, double d = 0, double v = 0);       //4つのモータに速度を与える関数(デフォルトで停止)
int MotorsPwm2(double pot = 0, int lr = 0, int fr = -1);        //4つのモータに速度を与える関数その2(デフォルトで停止)
int MotorsDir(int a);       //4つのモータの回転方向を決定する関数)
float RoundDemical1(float a);       //小数点以下第一桁で四捨五入
float RoundDemicaln(float a, int n);        //小数点以下第n桁で四捨五入
int SwitchState();      //2つのスイッチからの入力を返す

/******************************main文******************************/
int main() {
    Init();
    Buzzer(2);
    int flag = 0;       //動作モード切り替え用変数
    int mfrstate = 1;     //モータの状態を保存する変数(正転:0,逆転:1)        ←違うふうに使っている場合あり
    int mlrstate = 0;       //モータの状態を保存する変数(左:0,直進:1,右:2)
    float pot = 0;     //ポテンショメータからの入力を格納
    float lrvalue = 0;     //LRスイッチからの入力を格納
    int frvalue = 0;        //FRスイッチの状態を格納
    int lrstate = 0;        //進み方の状態を格納(0,1,2)
    int swstate = 0;       //スイッチの入力状態を格納
    int dir[4];
    
    while(1){
        flag = 0;
        mfrstate = 1;
        mlrstate = 1;
        pot = 0;
        lrvalue = 0;
        frvalue = 0;
        swstate = 0;
        for(int i = 0; i < 4; i++){
            dir[i] = 0;
        }
        
        Init();
        wait(1);        //1秒待つ
        SW();       //スイッチ押下でスタート
        wait(1);
        flag = GetDipValue();
        DipLed();
        
        switch(flag){
            case 0:     //DIPスイッチ0でなにもなし
                Buzzer(4);
                break;
            
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 1:     //テストモード1
                Buzzer(1);
                
                    dir1 = 0;
                    dir2 = 0;
                    dir3 = 0;
                    dir4 = 0;
                    pwm1 = 1;
                    pwm2 = 1;
                    pwm3 = 1;
                    pwm4 = 1;
                    
                    SW();       //スイッチの入力で停止
                    Buzzer(5);
                    GraduallyStop();
                
                pwm1 = 0;
                pwm2 = 0;
                pwm3 = 0;
                pwm4 = 0;
                break;
            
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 2:     //テストモード2(1の反対回転)
                Buzzer(2);
                    dir1 = 1;
                    dir2 = 1;
                    dir3 = 1;
                    dir4 = 1;
                    pwm1 = 1;
                    pwm2 = 1;
                    pwm3 = 1;
                    pwm4 = 1;
                    
                    SW();
                    Buzzer(5);
                    GraduallyStop();
                    
                pwm1 = 0;
                pwm2 = 0;
                pwm3 = 0;
                pwm4 = 0;
                break;
            
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 3:     //前後回転の切り替えと、ポテンショメータで回転速度を制御
                Buzzer(3);
                while(startsw == 0){
                    pot = potentiometer;
                    
                    if(((forward == 1 && mfrstate == 1) || (reverse == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
                        GraduallyStop();
                    }else if(forward == 1){     //正転
                        pwm1 = pot;
                        pwm2 = pot;
                        pwm3 = pot;
                        pwm4 = pot;
                        dir1 = 0;
                        dir2 = 0;
                        dir3 = 0;
                        dir4 = 0;
                        mfrstate = 0;
                    }else if(reverse == 1){        //逆転
                        pwm1 = pot;
                        pwm2 = pot;
                        pwm3 = pot;
                        pwm4 = pot;
                        dir1 = 1;
                        dir2 = 1;
                        dir3 = 1;
                        dir4 = 1;
                        mfrstate = 1;
                    }
                    
                    wait_ms(100);
                }
                GraduallyStop();
                pwm1 = 0;
                pwm2 = 0;
                pwm3 = 0;
                pwm4 = 0;
                mfrstate = 0;
                Buzzer(5);
                break;
            
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 4:     //左右移動の切り替え
                Buzzer(3);
                while(startsw == 0){
                    pot = potentiometer;
                    lrvalue = lrsw;
                    if(lrvalue < 0.33){                                 //LRスイッチ中央
                        MotorsPwm(0.75, 0.75, 0.75, 0.75, pot);
                    }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
                        MotorsPwm(1,0.5,1,0.5,pot);
                    }else{                                              //LRスイッチ左or右
                        MotorsPwm(0.5,1,0.5,1,pot);
                    }
                    wait_ms(100);
                }
                GraduallyStop();
                MotorsPwm();
                Buzzer(5);
                break;
            
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 5:     //正転・逆転と左右旋回
                Buzzer(3);
                while(startsw == 0){
                    pot = potentiometer;
                    lrvalue = lrsw;
                    
                    if(forward == 1){
                        frvalue = 0;        //正転
                    }else if(reverse == 1){
                        frvalue = 1;        //逆転
                    }
                    if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
                        GraduallyStop();
                    }
                    MotorsDir(frvalue);
                    mfrstate = frvalue;
                    
                    
                    
                    if(lrvalue < 0.33){                                 //LRスイッチ中央
                        MotorsPwm(0.5, 0.5, 0.5, 0.5, RoundDemical1(pot));
                    }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
                        MotorsPwm(1,0.33,1,0.33,RoundDemical1(pot));
                    }else{                                              //LRスイッチ左or右
                        MotorsPwm(0.33,1,0.33,1,RoundDemical1(pot));
                    }
                    
                    wait_ms(100);
                }
                GraduallyStop();
                MotorsPwm();
                Buzzer(5);
                break;
                
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 6:     //正転・逆転と左右旋回
                Buzzer(3);
                while(startsw == 0){
                    pot = potentiometer;
                    lrvalue = lrsw;
                    
                    if(forward == 1){
                        frvalue = 0;        //正転
                    }else if(reverse == 1){
                        frvalue = 1;        //逆転
                    }else{
                        frvalue = -1;       //停止
                    }
                    if(lrvalue < 0.33){                                 //LRスイッチ中央
                        lrstate = 0;
                    }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
                        lrstate = 1;
                    }else{                                              //LRスイッチ左or右
                        lrstate = 2;
                    }
                    
                    if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
                        GraduallyStop();
                        wait(0.5);
                        MotorsDir(frvalue);
                        GraduallyStart(pot, lrstate, frvalue);
                    }else if((frvalue == -1) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
                        GraduallyStop();
                    }
                    MotorsDir(frvalue);
                    MotorsPwm2(pot, lrstate, frvalue);
                    mfrstate = frvalue;
                    
                    wait_ms(100);
                }
                GraduallyStop();
                MotorsPwm();
                Buzzer(5);
                break;
                
            ////////////////////////////////////////////////////////////////////////////////////////////////
            case 7:     //1台向けモード
                Buzzer(1);
                while(startsw == 0){
                    lrvalue = lrsw;
                    if(forward == 1){
                        frvalue = 0;        //正転
                    }else if(reverse == 1){
                        frvalue = 1;        //逆転
                    }else{
                        frvalue = -1;       //停止
                    }
                    if(lrvalue < 0.33){                                 //LRスイッチ中央
                        lrstate = 0;
                    }else if(0.33 <= lrvalue && lrvalue < 0.66){        //LRスイッチ右or左
                        lrstate = 1;
                    }else{                                              //LRスイッチ左or右
                        lrstate = 2;
                    }
                    
                    /*if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (pwm1.read() != 0) || (pwm2.read() != 0)){      //この辺作業中
                        GraduallyStop();
                    }*/
                    
                    
                    if((mfrstate != frvalue) || (mlrstate != lrstate)){
                        GraduallyStop();
                        wait(0.5);
                    }
                    
                    if(lrstate == 0){
                        if(frvalue == 0){
                            dir1 = 0;
                            dir2 = 0;
                        }else if(frvalue == 1){
                            dir1 = 1;
                            dir2 = 1;
                        }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
                            GraduallyStop();
                        }
                    }else if(lrstate == 1){
                        dir1 = 0;
                        dir2 = 1;
                    }else if(lrstate == 2){
                        dir1 = 1;
                        dir2 = 0;
                    }
                    if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (frvalue >= 0)){
                        pot = potentiometer;
                        for(int i = 0; i <= 100; i++){
                            pwm1 = RoundDemical1(pot) * (double)i / 100;
                            pwm2 = RoundDemical1(pot) * (double)i / 100;
                            wait_ms(2);
                        }
                    }
                        
                    
                    /*if(lrstate == 0){       //lrスイッチが中央で
                        if(frvalue == 0){       //frスイッチが前で前進
                            if(dir1.read() != 0 || dir2.read() != 0){
                                //GraduallyStop();
                                dir1 = 0;
                                dir2 = 0;
                                for(int i = 0; i <= 100; i++){
                                    pwm1 = RoundDemical1(pot) * (double)i / 100;
                                    pwm2 = RoundDemical1(pot) * (double)i / 100;
                                    wait_ms(5);
                                }
                            }
                            dir1 = 0;
                            dir2 = 0;
                        }else if(frvalue == 1){
                            if(dir1.read() != 1 || dir2.read() != 1){
                                //GraduallyStop();
                                dir1 = 1;
                                dir2 = 1;
                                for(int i = 0; i <= 100; i++){
                                    pwm1 = RoundDemical1(pot) * (double)i / 100;
                                    pwm2 = RoundDemical1(pot) * (double)i / 100;
                                    wait_ms(5);
                                }
                            }
                            dir1 = 1;
                            dir2 = 1;
                        }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
                            GraduallyStop();
                        }
                    }else if(lrstate == 1){
                        if(dir1.read() != 0 || dir2.read() != 1){
                                //GraduallyStop();
                                dir1 = 0;
                                dir2 = 1;
                                for(int i = 0; i <= 100; i++){
                                    pwm1 = RoundDemical1(pot) * (double)i / 100;
                                    pwm2 = RoundDemical1(pot) * (double)i / 100;
                                    wait_ms(5);
                                }
                            }
                        dir1 = 0;
                        dir2 = 1;
                    }else if(lrstate == 2){
                        if(dir1.read() != 1 || dir2.read() != 0){
                                //GraduallyStop();
                                dir1 = 1;
                                dir2 = 0;
                                for(int i = 0; i <= 100; i++){
                                    pwm1 = RoundDemical1(pot) * (double)i / 100;
                                    pwm2 = RoundDemical1(pot) * (double)i / 100;
                                    wait_ms(5);
                                }
                            }
                        dir1 = 1;
                        dir2 = 0;
                    }*/
                    
                    pot = potentiometer;
                    if(frvalue >= 0){
                        pwm1 = RoundDemical1(pot);
                        pwm2 = RoundDemical1(pot);
                    }
                    mfrstate = frvalue;
                    mlrstate = lrstate;
                    
                    wait_ms(100);
                }
                
                Buzzer(5);
                break;
            
            case 8:     //case7 のブラッシュアップ版(1台モード)
                Buzzer(1);
                while(startsw == 0){
                    swstate = SwitchState();        //スイッチの入力を見る
                    if(1 <= swstate && swstate <= 3){       //1<= swstate<=3で前進
                        dir[0] = 0;
                        dir[1] = 0;
                    }else if(swstate == 4){     //どっちも中点で停止
                    }else if(swstate == 5){     //右に信地旋回
                        dir[0] = 0;
                        dir[1] = 1;
                    }else if(swstate == 6){     //左に信地旋回
                        dir[0] = 1;
                        dir[1] = 0;
                    }else if(swstate <= 9){     //7<=swstate<=9で後退
                        dir[0] = 1;
                        dir[1] = 1;
                    }
                    
                    if((dir1.read() != dir[0] || dir2.read() != dir[1] || swstate == 4) && (pwm1.read() != 0 || pwm2.read() != 0)){
                        GraduallyStop2();
                        mfrstate = 1;       //フラグ
                    }
                    dir1 = dir[0];      //dirに入力
                    dir2 = dir[1];
                    
                    pot = potentiometer;        //ポテンショメータの入力を見る
                    if(swstate % 3 == 1){       //ここ以下は走行するスピードを決定
                        if(swstate == 4){
                            pwm1 = 0;
                            pwm2 = 0;
                        }else{
                            if(mfrstate == 1){
                                GraduallyStart2(1,1,0,0,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = RoundDemical1(pot);
                            pwm2 = RoundDemical1(pot);
                        }
                    }else if(swstate % 3 == 2){     //右
                        if(swstate == 5){
                            if(mfrstate == 1){
                                GraduallyStart2(1,1,0,0,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = RoundDemical1(pot);
                            pwm2 = RoundDemical1(pot);
                        }else{
                            if(mfrstate == 1){
                                GraduallyStart2(1,0.5,0,0,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = RoundDemical1(pot);
                            pwm2 = 0.5 * RoundDemical1(pot);
                        }
                    }else{                              //左
                        if(swstate == 6){
                            if(mfrstate == 1){
                                GraduallyStart2(1,1,0,0,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = RoundDemical1(pot);
                            pwm2 = RoundDemical1(pot);
                        }else{
                            if(mfrstate == 1){
                                GraduallyStart2(0.5,1,0,0,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = 0.5 * RoundDemical1(pot);
                            pwm2 = RoundDemical1(pot);
                        }
                    }
                    wait_ms(100);
                }
                Buzzer(5);
                break;
            
            /////////////////////////////////////////////////////////////////////////////////////
            case 9:     //2台モード
                Buzzer(1);
                while(startsw == 0){
                    swstate = SwitchState();
                    if(1 <= swstate && swstate <= 3){       //1<= swstate<=3で前進
                        for(int i = 0; i < 4; i++){
                            dir[i] = 0;
                        }
                    }else if(swstate <= 4){     //中点で停止
                    }else if(swstate <= 5){     //中点,右
                        dir[0] = 1;     //右に姿勢変化
                        dir[1] = 0;
                        dir[2] = 0;
                        dir[3] = 1;
                    }else if(swstate <= 6){     //中点,左
                        dir[0] = 0;     //左に姿勢変化
                        dir[1] = 1;
                        dir[2] = 1;
                        dir[3] = 0;
                    }else if(swstate <= 9){     //7<=swstate<=9で後退
                        for(int i = 0; i < 4; i++){
                            dir[i] = 1;
                        }
                    }
                    
                    if((dir1.read() != dir[0] || dir2.read() != dir[1] || dir3.read() != dir[2] || dir4.read() != dir[3] || (swstate == 4))
                    && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
                        GraduallyStop2();
                        mfrstate = 1;
                    }
                    dir1 = dir[0];
                    dir2 = dir[1];
                    dir3 = dir[2];
                    dir4 = dir[3];
                    
                    pot = potentiometer;
                    
                    if(4 <= swstate && swstate <= 6){       //ここ変える←変えた
                        if(swstate == 4){       //中点+中点で停止
                            pwm1 = 0;
                            pwm2 = 0;
                            pwm3 = 0;
                            pwm4 = 0;
                        }else{                  //中点+左右でそれぞれ信地旋回
                            if(mfrstate == 1){
                                GraduallyStart2(1,1,1,1,RoundDemical1(pot));
                                mfrstate = 0;
                            }
                            pwm1 = RoundDemical1(pot);
                            pwm2 = RoundDemical1(pot);
                            pwm3 = RoundDemical1(pot);
                            pwm4 = RoundDemical1(pot);
                        }
                    }else if(swstate % 3 == 1){
                        if(mfrstate == 1){
                            GraduallyStart2(1,1,1,1,RoundDemical1(pot));
                            mfrstate = 0;
                        }
                        pwm1 = RoundDemical1(pot);
                        pwm2 = RoundDemical1(pot);
                        pwm3 = RoundDemical1(pot);
                        pwm4 = RoundDemical1(pot);
                    }else if(swstate % 3 == 2){     //右
                        if(mfrstate == 1){
                            GraduallyStart2(1,0.5,1,0.5,RoundDemical1(pot));
                            mfrstate = 0;
                        }
                        pwm1 = RoundDemical1(pot);
                        pwm2 = 0.5 * RoundDemical1(pot);
                        pwm3 = RoundDemical1(pot);
                        pwm4 = 0.5 * RoundDemical1(pot);
                    }else{                          //左
                        if(mfrstate == 1){
                            GraduallyStart2(0.5,1,0.5,1,RoundDemical1(pot));
                            mfrstate = 0;
                        }
                        pwm1 = 0.5 * RoundDemical1(pot);
                        pwm2 = RoundDemical1(pot);
                        pwm3 = 0.5 * RoundDemical1(pot);
                        pwm4 = RoundDemical1(pot);
                    }
                    wait_ms(100);
                }
                Buzzer(5);
                break;
            
            default:        //上記以外で何もなし
                Buzzer(-3);
                break;
            
        }
    }
    return 0;
}


/******************************ここから関数******************************/
int Init(){     //初期化関数
    pwm1 = 0;
    pwm2 = 0;
    pwm3 = 0;
    pwm4 = 0;
    dir1 = 0;
    dir2 = 0;
    dir3 = 0;
    dir4 = 0;
    buzzer = 0;
    pwm1.period_us(25);     //Pololu 18v15のpwm周波数40kHzに合わせる
    pwm2.period_us(25);
    pwm3.period_us(25);
    pwm4.period_us(25);
    DipLed();
    return 0;
}

int DipLed(){       //DIPスイッチの状態によってledを光らせる関数
    if(dip1 == 1) led1 = 1; else led1 = 0;     //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない
    if(dip2 == 1) led2 = 1; else led2 = 0;
    if(dip3 == 1) led3 = 1; else led3 = 0;
    if(dip4 == 1) led4 = 1; else led4 = 0;
    return 0;
}

int GetDipValue(){      //DIPスイッチの値を取得し,値を返す関数
    int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得
    if(Dip1 == 0){
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 0; else return 1;      //DIPスイッチが0000の場合0を返す.0001なら1
            }else{
                if(Dip4 == 0) return 2; else return 3;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 4; else return 5;
            }else{
                if(Dip4 == 0) return 6; else return 7;
            }
        }
    }else{
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 8; else return 9;
            }else{
                if(Dip4 == 0) return 10; else return 11;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 12; else return 13;
            }else{
                if(Dip4 == 0) return 14; else return 15;
            }
        }
    }
}

int SW(){        //スタートスイッチ用関数,押して離したらスタート
    int i = 0, j = 0;
    while(i < 3){       //チャタリング除去,15msにわたってスタートスイッチが押されていればbreak
        if(startsw == 1) i++;
        else i = 0;
        DipLed();
        wait_ms(5);
    }
    while(j < 3){       //上に同じ,スタートスイッチが離されたことを検知
        if(startsw == 0) j++;
        else j = 0;
        DipLed();
        wait_ms(5);
    }
    return 0;
}

int Buzzer(int buzvar){     //電子ブザーを鳴らす関数
    switch (buzvar){
        /**************エラーを知らせるbeep**************/
        case -3:        //error * - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -2:        //error * - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -1:        //error * - - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        /**************エラーここまで**************/
        case 0:         //サウンドなし
            buzzer = 0;
            break;
        /**************状態を知らせるためのbeep**************/
        case 1:         // *(短)
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 2:         // * *
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.05);
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 3:         // -(長)
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 4:         // - -
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 5:         // ---
            buzzer = 1; wait(0.9); buzzer = 0;
            break;
        case 6:         // * * *  * * *  * * *  *
            for(int i = 0; i < 3; i++){
                for(int j = 0; j < 3; j++){
                    buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
                }
                wait(0.2);
            }
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 7:         // **-* ** -* ** *** **** "finish"
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 8:         // *-*** -*- --* "オワリ"
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 9:        //ケータイの着信音的な
            for(int i = 0; i < 20; i++){
                buzzer = 1; wait(0.03);
                buzzer = 0; wait(0.03);
            }
            break;
        case 10:        //9の短いバージョン
            for(int i = 0; i < 20; i++){
                buzzer = 1; wait(0.02);
                buzzer = 0; wait(0.02);
            }
            break;
        /**************状態を知らせるためのbeepここまで**************/
        default:        //no sound
            buzzer = 0;
            break;
    }
    return 0;
}

int GraduallyStop(){        //ゆるやかにモータを停止させるための関数
    double pwmn1 = pwm1.read();
    double pwmn2 = pwm2.read();
    double pwmn3 = pwm3.read();
    double pwmn4 = pwm4.read();
    const int loop = 100;
    
    for(int i = 0; i < loop; i++){
        pwm1 = pwm1.read() - pwmn1 / loop;
        pwm2 = pwm2.read() - pwmn2 / loop;
        pwm3 = pwm3.read() - pwmn3 / loop;
        pwm4 = pwm4.read() - pwmn4 / loop;
        wait_ms(10);
    }
    
    pwm1 = 0;
    pwm2 = 0;
    pwm3 = 0;
    pwm4 = 0;
    
    return 0;
}

int GraduallyStop2(){
    double pwmn1 = pwm1.read();
    double pwmn2 = pwm2.read();
    double pwmn3 = pwm3.read();
    double pwmn4 = pwm4.read();
    const int loop = 100;
    
    for(int i = 0; i < loop; i++){
        pwm1 = pwm1.read() - pwmn1 / loop;
        pwm2 = pwm2.read() - pwmn2 / loop;
        pwm3 = pwm3.read() - pwmn3 / loop;
        pwm4 = pwm4.read() - pwmn4 / loop;
        if(i > 40){
            wait_ms(5);
            if(i > 60){
                wait_ms(5);
            }
        }
        wait_ms(5);
    }
    
    pwm1 = 0;
    pwm2 = 0;
    pwm3 = 0;
    pwm4 = 0;
    
    return 0;
}
    
    

int GraduallyStart(float pot, int lr, int fr){      //ゆるやかにモータを始動させる関数
    const int loop = 100;
    for(int i = 0; i < loop; i++){
        MotorsPwm2(pot * (double)i/(double)loop, lr, fr);
        wait_ms(10);
    }
    MotorsPwm2(pot, lr);
    return 0;
}

int GraduallyStart2(float a, float b, float c, float d, float v){       //vはポテンショメータ
    float threshold = 0.3;
    if(v <= threshold){
        pwm1 = a * v;
        pwm2 = b * v;
        pwm3 = c * v;
        pwm4 = d * v;
    }else{
        for(float i = 0; i < v; i += 0.1){
            pwm1 = a * i;
            pwm2 = b * i;
            pwm3 = c * i;
            pwm4 = d * i;
            wait(0.05);
        }
    }
    return 0;
}
    

int MotorsPwm(double a, double b, double c, double d, double v){       //4つのモータに速度を与える関数(デフォルトで停止)
    pwm1 = a * v;
    pwm2 = b * v;
    pwm3 = c * v;
    pwm4 = d * v;
    return 0;
}

int MotorsPwm2(double pot, int lr, int fr){        //4つのモータに速度を与える関数その2(デフォルトで停止)
    float H = 1.0;
    float M = 0.75;
    float L = 0.5;
    if(fr >= 0){
        switch (lr){
            case 0:     //前進
                MotorsPwm(M, M, M, M, RoundDemical1(pot));
                break;
            case 1:     //右or左旋回
                MotorsPwm(H, L, H, L, RoundDemical1(pot));
                break;
            case 2:     //左or右旋回(1と逆)
                MotorsPwm(L, H, L, H, RoundDemical1(pot));
                break;
            default:
                GraduallyStop();
                break;
        }
    }else{
        MotorsPwm();
    }
    return 0;
}

int MotorsDir(int a){       //4つのモータの回転方向を決定する関数)
    if(a >= 0){
        dir1 = a;
        dir2 = a;
        dir3 = a;
        dir4 = a;
    }
    
    return 0;
}

float RoundDemical1(float a){       //小数点以下第一桁で四捨五入
    float b;
    a *= 10;
    b = (float)(int)(a+0.5);
    b /= 10;
    return b;
}
/*
float RoundDemicaln(float a, int n){        //小数点以下第n桁で四捨五入
    float b, c;
    b = pow(a, n);
    c = (float)(int)(b+0.5);
    b = pow(c, 1.0/(double)n);
    return b;
}
*/

int SwitchState(){      //2つのスイッチからの入力を返す
    /*********************************************
    fr・lrで
    前・中点:1  前・右:2   前・左:3
    中点・中点:4 中点・右:5  中点・左:6
    後・中点:7  後・右:8   後・左:9
    を返す
    *********************************************/
    int state = 0;
    float lr = lrsw;
    
    if(forward == 1){        //frスイッチ前
        state = 1;
    }else if(reverse == 1){     //frスイッチ後ろ
        state = 7;
    }else{                  //frスイッチ中点
        state = 4;
    }
    if(lr < 0.33){      //lrスイッチ中点
        state += 0;
    }else if(lr < 0.67){        //lrスイッチ左
        state += 2;
    }else{              //lrスイッチ右
        state += 1;
    }
    
    return state;
}