Makoto Kobayashi
車両操縦用のプログラム 日本語Betaで作成してます
Diff: main.cpp
- Revision:
- 0:bce22b52f3e2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 03 23:29:45 2016 +0000
@@ -0,0 +1,949 @@
+#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;
+}
+
+
\ No newline at end of file