Makoto Kobayashi
/
link_SAV
車両操縦用のプログラム 日本語Betaで作成してます
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; }