Makoto Kobayashi / Mbed 2 deprecated link_SAV

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

Dependencies:   mbed

main.cpp@0:bce22b52f3e2, 2016-03-03 (annotated)

Committer:
neoqased
Date:
Thu Mar 03 23:29:45 2016 +0000
Revision:
0:bce22b52f3e2
small articulated vehicle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
neoqased 0:bce22b52f3e2 1 #include "mbed.h"
neoqased 0:bce22b52f3e2 2 //#include "USBHostMSD.h"
neoqased 0:bce22b52f3e2 3 //#include "USBHostXpad.h"
neoqased 0:bce22b52f3e2 4 //#include "SDFileSystem.h"
neoqased 0:bce22b52f3e2 5
neoqased 0:bce22b52f3e2 6 /******************************入出力設定******************************/
neoqased 0:bce22b52f3e2 7 DigitalOut led1(LED1); //mbed上LED出力4つ
neoqased 0:bce22b52f3e2 8 DigitalOut led2(LED2);
neoqased 0:bce22b52f3e2 9 DigitalOut led3(LED3);
neoqased 0:bce22b52f3e2 10 DigitalOut led4(LED4);
neoqased 0:bce22b52f3e2 11
neoqased 0:bce22b52f3e2 12 //モータのPWM出力
neoqased 0:bce22b52f3e2 13 PwmOut pwm1(p21); //一番上
neoqased 0:bce22b52f3e2 14 PwmOut pwm2(p22); //上から二番目
neoqased 0:bce22b52f3e2 15 PwmOut pwm3(p23); //上から三番目
neoqased 0:bce22b52f3e2 16 PwmOut pwm4(p24); //一番下
neoqased 0:bce22b52f3e2 17
neoqased 0:bce22b52f3e2 18 //モータドライバのDIRへの信号
neoqased 0:bce22b52f3e2 19 DigitalOut dir1(p30);
neoqased 0:bce22b52f3e2 20 DigitalOut dir2(p29);
neoqased 0:bce22b52f3e2 21 DigitalOut dir3(p28);
neoqased 0:bce22b52f3e2 22 DigitalOut dir4(p27);
neoqased 0:bce22b52f3e2 23
neoqased 0:bce22b52f3e2 24 //スイッチ類入力
neoqased 0:bce22b52f3e2 25 DigitalIn startsw(p11); //スタートスイッチ、押した後離してプログラム開始
neoqased 0:bce22b52f3e2 26 DigitalIn dip1(p12); //DIPスイッチ、dip1は一番左(DIPスイッチの1)
neoqased 0:bce22b52f3e2 27 DigitalIn dip2(p13); //2
neoqased 0:bce22b52f3e2 28 DigitalIn dip3(p14); //3
neoqased 0:bce22b52f3e2 29 DigitalIn dip4(p15); //4
neoqased 0:bce22b52f3e2 30 DigitalIn forward(p16); //FRスイッチのF
neoqased 0:bce22b52f3e2 31 DigitalIn reverse(p17); //FRスイッチのR
neoqased 0:bce22b52f3e2 32 AnalogIn potentiometer(p19); //ポテンショメータ
neoqased 0:bce22b52f3e2 33 AnalogIn lrsw(p20);
neoqased 0:bce22b52f3e2 34
neoqased 0:bce22b52f3e2 35 //ブザー
neoqased 0:bce22b52f3e2 36 DigitalOut buzzer(p18); //電子ブザー用アナorデジ出力
neoqased 0:bce22b52f3e2 37
neoqased 0:bce22b52f3e2 38 //USBフラッシュメモリ
neoqased 0:bce22b52f3e2 39 //USBHostMSD msc("msc");
neoqased 0:bce22b52f3e2 40 //USBでXbox360ワイヤレスコントローラを使うには、USBHostXpadを使う
neoqased 0:bce22b52f3e2 41
neoqased 0:bce22b52f3e2 42 //SDカード
neoqased 0:bce22b52f3e2 43 //SDFileSystem sd(p5, p6, p7, p8, "sd");
neoqased 0:bce22b52f3e2 44
neoqased 0:bce22b52f3e2 45 //予備のデジタル入出力:p16,p17,p19,p20
neoqased 0:bce22b52f3e2 46
neoqased 0:bce22b52f3e2 47 /******************************プロトタイプ宣言******************************/
neoqased 0:bce22b52f3e2 48 int Init(); //初期化関数
neoqased 0:bce22b52f3e2 49 int DipLed(); //DIPスイッチの状態によってledを光らせる関数
neoqased 0:bce22b52f3e2 50 int GetDipValue(); //DIPスイッチの値を取得し，値を返す関数
neoqased 0:bce22b52f3e2 51 int SW(); //スタートスイッチ用関数，押して離したらスタート
neoqased 0:bce22b52f3e2 52 int Buzzer(int buzvar = 1); //電子ブザーを鳴らす関数
neoqased 0:bce22b52f3e2 53 int GraduallyStop(); //ゆるやかにモータを停止させるための関数
neoqased 0:bce22b52f3e2 54 int GraduallyStop2();
neoqased 0:bce22b52f3e2 55 int GraduallyStart(float pot, int lr, int fr); //ゆるやかにモータを始動させる関数
neoqased 0:bce22b52f3e2 56 int GraduallyStart2(float a, float b, float c, float d, float v);
neoqased 0:bce22b52f3e2 57 int MotorsPwm(double a = 0, double b = 0, double c = 0, double d = 0, double v = 0); //4つのモータに速度を与える関数(デフォルトで停止)
neoqased 0:bce22b52f3e2 58 int MotorsPwm2(double pot = 0, int lr = 0, int fr = -1); //4つのモータに速度を与える関数その2(デフォルトで停止)
neoqased 0:bce22b52f3e2 59 int MotorsDir(int a); //4つのモータの回転方向を決定する関数)
neoqased 0:bce22b52f3e2 60 float RoundDemical1(float a); //小数点以下第一桁で四捨五入
neoqased 0:bce22b52f3e2 61 float RoundDemicaln(float a, int n); //小数点以下第n桁で四捨五入
neoqased 0:bce22b52f3e2 62 int SwitchState(); //2つのスイッチからの入力を返す
neoqased 0:bce22b52f3e2 63
neoqased 0:bce22b52f3e2 64 /******************************main文******************************/
neoqased 0:bce22b52f3e2 65 int main() {
neoqased 0:bce22b52f3e2 66 Init();
neoqased 0:bce22b52f3e2 67 Buzzer(2);
neoqased 0:bce22b52f3e2 68 int flag = 0; //動作モード切り替え用変数
neoqased 0:bce22b52f3e2 69 int mfrstate = 1; //モータの状態を保存する変数(正転:0,逆転:1) ←違うふうに使っている場合あり
neoqased 0:bce22b52f3e2 70 int mlrstate = 0; //モータの状態を保存する変数(左:0,直進:1,右:2)
neoqased 0:bce22b52f3e2 71 float pot = 0; //ポテンショメータからの入力を格納
neoqased 0:bce22b52f3e2 72 float lrvalue = 0; //LRスイッチからの入力を格納
neoqased 0:bce22b52f3e2 73 int frvalue = 0; //FRスイッチの状態を格納
neoqased 0:bce22b52f3e2 74 int lrstate = 0; //進み方の状態を格納(0,1,2)
neoqased 0:bce22b52f3e2 75 int swstate = 0; //スイッチの入力状態を格納
neoqased 0:bce22b52f3e2 76 int dir[4];
neoqased 0:bce22b52f3e2 77
neoqased 0:bce22b52f3e2 78 while(1){
neoqased 0:bce22b52f3e2 79 flag = 0;
neoqased 0:bce22b52f3e2 80 mfrstate = 1;
neoqased 0:bce22b52f3e2 81 mlrstate = 1;
neoqased 0:bce22b52f3e2 82 pot = 0;
neoqased 0:bce22b52f3e2 83 lrvalue = 0;
neoqased 0:bce22b52f3e2 84 frvalue = 0;
neoqased 0:bce22b52f3e2 85 swstate = 0;
neoqased 0:bce22b52f3e2 86 for(int i = 0; i < 4; i++){
neoqased 0:bce22b52f3e2 87 dir[i] = 0;
neoqased 0:bce22b52f3e2 88 }
neoqased 0:bce22b52f3e2 89
neoqased 0:bce22b52f3e2 90 Init();
neoqased 0:bce22b52f3e2 91 wait(1); //1秒待つ
neoqased 0:bce22b52f3e2 92 SW(); //スイッチ押下でスタート
neoqased 0:bce22b52f3e2 93 wait(1);
neoqased 0:bce22b52f3e2 94 flag = GetDipValue();
neoqased 0:bce22b52f3e2 95 DipLed();
neoqased 0:bce22b52f3e2 96
neoqased 0:bce22b52f3e2 97 switch(flag){
neoqased 0:bce22b52f3e2 98 case 0: //DIPスイッチ0でなにもなし
neoqased 0:bce22b52f3e2 99 Buzzer(4);
neoqased 0:bce22b52f3e2 100 break;
neoqased 0:bce22b52f3e2 101
neoqased 0:bce22b52f3e2 102 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 103 case 1: //テストモード1
neoqased 0:bce22b52f3e2 104 Buzzer(1);
neoqased 0:bce22b52f3e2 105
neoqased 0:bce22b52f3e2 106 dir1 = 0;
neoqased 0:bce22b52f3e2 107 dir2 = 0;
neoqased 0:bce22b52f3e2 108 dir3 = 0;
neoqased 0:bce22b52f3e2 109 dir4 = 0;
neoqased 0:bce22b52f3e2 110 pwm1 = 1;
neoqased 0:bce22b52f3e2 111 pwm2 = 1;
neoqased 0:bce22b52f3e2 112 pwm3 = 1;
neoqased 0:bce22b52f3e2 113 pwm4 = 1;
neoqased 0:bce22b52f3e2 114
neoqased 0:bce22b52f3e2 115 SW(); //スイッチの入力で停止
neoqased 0:bce22b52f3e2 116 Buzzer(5);
neoqased 0:bce22b52f3e2 117 GraduallyStop();
neoqased 0:bce22b52f3e2 118
neoqased 0:bce22b52f3e2 119 pwm1 = 0;
neoqased 0:bce22b52f3e2 120 pwm2 = 0;
neoqased 0:bce22b52f3e2 121 pwm3 = 0;
neoqased 0:bce22b52f3e2 122 pwm4 = 0;
neoqased 0:bce22b52f3e2 123 break;
neoqased 0:bce22b52f3e2 124
neoqased 0:bce22b52f3e2 125 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 126 case 2: //テストモード2(1の反対回転)
neoqased 0:bce22b52f3e2 127 Buzzer(2);
neoqased 0:bce22b52f3e2 128 dir1 = 1;
neoqased 0:bce22b52f3e2 129 dir2 = 1;
neoqased 0:bce22b52f3e2 130 dir3 = 1;
neoqased 0:bce22b52f3e2 131 dir4 = 1;
neoqased 0:bce22b52f3e2 132 pwm1 = 1;
neoqased 0:bce22b52f3e2 133 pwm2 = 1;
neoqased 0:bce22b52f3e2 134 pwm3 = 1;
neoqased 0:bce22b52f3e2 135 pwm4 = 1;
neoqased 0:bce22b52f3e2 136
neoqased 0:bce22b52f3e2 137 SW();
neoqased 0:bce22b52f3e2 138 Buzzer(5);
neoqased 0:bce22b52f3e2 139 GraduallyStop();
neoqased 0:bce22b52f3e2 140
neoqased 0:bce22b52f3e2 141 pwm1 = 0;
neoqased 0:bce22b52f3e2 142 pwm2 = 0;
neoqased 0:bce22b52f3e2 143 pwm3 = 0;
neoqased 0:bce22b52f3e2 144 pwm4 = 0;
neoqased 0:bce22b52f3e2 145 break;
neoqased 0:bce22b52f3e2 146
neoqased 0:bce22b52f3e2 147 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 148 case 3: //前後回転の切り替えと、ポテンショメータで回転速度を制御
neoqased 0:bce22b52f3e2 149 Buzzer(3);
neoqased 0:bce22b52f3e2 150 while(startsw == 0){
neoqased 0:bce22b52f3e2 151 pot = potentiometer;
neoqased 0:bce22b52f3e2 152
neoqased 0:bce22b52f3e2 153 if(((forward == 1 && mfrstate == 1) || (reverse == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
neoqased 0:bce22b52f3e2 154 GraduallyStop();
neoqased 0:bce22b52f3e2 155 }else if(forward == 1){ //正転
neoqased 0:bce22b52f3e2 156 pwm1 = pot;
neoqased 0:bce22b52f3e2 157 pwm2 = pot;
neoqased 0:bce22b52f3e2 158 pwm3 = pot;
neoqased 0:bce22b52f3e2 159 pwm4 = pot;
neoqased 0:bce22b52f3e2 160 dir1 = 0;
neoqased 0:bce22b52f3e2 161 dir2 = 0;
neoqased 0:bce22b52f3e2 162 dir3 = 0;
neoqased 0:bce22b52f3e2 163 dir4 = 0;
neoqased 0:bce22b52f3e2 164 mfrstate = 0;
neoqased 0:bce22b52f3e2 165 }else if(reverse == 1){ //逆転
neoqased 0:bce22b52f3e2 166 pwm1 = pot;
neoqased 0:bce22b52f3e2 167 pwm2 = pot;
neoqased 0:bce22b52f3e2 168 pwm3 = pot;
neoqased 0:bce22b52f3e2 169 pwm4 = pot;
neoqased 0:bce22b52f3e2 170 dir1 = 1;
neoqased 0:bce22b52f3e2 171 dir2 = 1;
neoqased 0:bce22b52f3e2 172 dir3 = 1;
neoqased 0:bce22b52f3e2 173 dir4 = 1;
neoqased 0:bce22b52f3e2 174 mfrstate = 1;
neoqased 0:bce22b52f3e2 175 }
neoqased 0:bce22b52f3e2 176
neoqased 0:bce22b52f3e2 177 wait_ms(100);
neoqased 0:bce22b52f3e2 178 }
neoqased 0:bce22b52f3e2 179 GraduallyStop();
neoqased 0:bce22b52f3e2 180 pwm1 = 0;
neoqased 0:bce22b52f3e2 181 pwm2 = 0;
neoqased 0:bce22b52f3e2 182 pwm3 = 0;
neoqased 0:bce22b52f3e2 183 pwm4 = 0;
neoqased 0:bce22b52f3e2 184 mfrstate = 0;
neoqased 0:bce22b52f3e2 185 Buzzer(5);
neoqased 0:bce22b52f3e2 186 break;
neoqased 0:bce22b52f3e2 187
neoqased 0:bce22b52f3e2 188 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 189 case 4: //左右移動の切り替え
neoqased 0:bce22b52f3e2 190 Buzzer(3);
neoqased 0:bce22b52f3e2 191 while(startsw == 0){
neoqased 0:bce22b52f3e2 192 pot = potentiometer;
neoqased 0:bce22b52f3e2 193 lrvalue = lrsw;
neoqased 0:bce22b52f3e2 194 if(lrvalue < 0.33){ //LRスイッチ中央
neoqased 0:bce22b52f3e2 195 MotorsPwm(0.75, 0.75, 0.75, 0.75, pot);
neoqased 0:bce22b52f3e2 196 }else if(0.33 <= lrvalue && lrvalue < 0.66){ //LRスイッチ右or左
neoqased 0:bce22b52f3e2 197 MotorsPwm(1,0.5,1,0.5,pot);
neoqased 0:bce22b52f3e2 198 }else{ //LRスイッチ左or右
neoqased 0:bce22b52f3e2 199 MotorsPwm(0.5,1,0.5,1,pot);
neoqased 0:bce22b52f3e2 200 }
neoqased 0:bce22b52f3e2 201 wait_ms(100);
neoqased 0:bce22b52f3e2 202 }
neoqased 0:bce22b52f3e2 203 GraduallyStop();
neoqased 0:bce22b52f3e2 204 MotorsPwm();
neoqased 0:bce22b52f3e2 205 Buzzer(5);
neoqased 0:bce22b52f3e2 206 break;
neoqased 0:bce22b52f3e2 207
neoqased 0:bce22b52f3e2 208 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 209 case 5: //正転・逆転と左右旋回
neoqased 0:bce22b52f3e2 210 Buzzer(3);
neoqased 0:bce22b52f3e2 211 while(startsw == 0){
neoqased 0:bce22b52f3e2 212 pot = potentiometer;
neoqased 0:bce22b52f3e2 213 lrvalue = lrsw;
neoqased 0:bce22b52f3e2 214
neoqased 0:bce22b52f3e2 215 if(forward == 1){
neoqased 0:bce22b52f3e2 216 frvalue = 0; //正転
neoqased 0:bce22b52f3e2 217 }else if(reverse == 1){
neoqased 0:bce22b52f3e2 218 frvalue = 1; //逆転
neoqased 0:bce22b52f3e2 219 }
neoqased 0:bce22b52f3e2 220 if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
neoqased 0:bce22b52f3e2 221 GraduallyStop();
neoqased 0:bce22b52f3e2 222 }
neoqased 0:bce22b52f3e2 223 MotorsDir(frvalue);
neoqased 0:bce22b52f3e2 224 mfrstate = frvalue;
neoqased 0:bce22b52f3e2 225
neoqased 0:bce22b52f3e2 226
neoqased 0:bce22b52f3e2 227
neoqased 0:bce22b52f3e2 228 if(lrvalue < 0.33){ //LRスイッチ中央
neoqased 0:bce22b52f3e2 229 MotorsPwm(0.5, 0.5, 0.5, 0.5, RoundDemical1(pot));
neoqased 0:bce22b52f3e2 230 }else if(0.33 <= lrvalue && lrvalue < 0.66){ //LRスイッチ右or左
neoqased 0:bce22b52f3e2 231 MotorsPwm(1,0.33,1,0.33,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 232 }else{ //LRスイッチ左or右
neoqased 0:bce22b52f3e2 233 MotorsPwm(0.33,1,0.33,1,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 234 }
neoqased 0:bce22b52f3e2 235
neoqased 0:bce22b52f3e2 236 wait_ms(100);
neoqased 0:bce22b52f3e2 237 }
neoqased 0:bce22b52f3e2 238 GraduallyStop();
neoqased 0:bce22b52f3e2 239 MotorsPwm();
neoqased 0:bce22b52f3e2 240 Buzzer(5);
neoqased 0:bce22b52f3e2 241 break;
neoqased 0:bce22b52f3e2 242
neoqased 0:bce22b52f3e2 243 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 244 case 6: //正転・逆転と左右旋回
neoqased 0:bce22b52f3e2 245 Buzzer(3);
neoqased 0:bce22b52f3e2 246 while(startsw == 0){
neoqased 0:bce22b52f3e2 247 pot = potentiometer;
neoqased 0:bce22b52f3e2 248 lrvalue = lrsw;
neoqased 0:bce22b52f3e2 249
neoqased 0:bce22b52f3e2 250 if(forward == 1){
neoqased 0:bce22b52f3e2 251 frvalue = 0; //正転
neoqased 0:bce22b52f3e2 252 }else if(reverse == 1){
neoqased 0:bce22b52f3e2 253 frvalue = 1; //逆転
neoqased 0:bce22b52f3e2 254 }else{
neoqased 0:bce22b52f3e2 255 frvalue = -1; //停止
neoqased 0:bce22b52f3e2 256 }
neoqased 0:bce22b52f3e2 257 if(lrvalue < 0.33){ //LRスイッチ中央
neoqased 0:bce22b52f3e2 258 lrstate = 0;
neoqased 0:bce22b52f3e2 259 }else if(0.33 <= lrvalue && lrvalue < 0.66){ //LRスイッチ右or左
neoqased 0:bce22b52f3e2 260 lrstate = 1;
neoqased 0:bce22b52f3e2 261 }else{ //LRスイッチ左or右
neoqased 0:bce22b52f3e2 262 lrstate = 2;
neoqased 0:bce22b52f3e2 263 }
neoqased 0:bce22b52f3e2 264
neoqased 0:bce22b52f3e2 265 if(((frvalue == 0 && mfrstate == 1) || (frvalue == 1 && mfrstate == 0)) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
neoqased 0:bce22b52f3e2 266 GraduallyStop();
neoqased 0:bce22b52f3e2 267 wait(0.5);
neoqased 0:bce22b52f3e2 268 MotorsDir(frvalue);
neoqased 0:bce22b52f3e2 269 GraduallyStart(pot, lrstate, frvalue);
neoqased 0:bce22b52f3e2 270 }else if((frvalue == -1) && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
neoqased 0:bce22b52f3e2 271 GraduallyStop();
neoqased 0:bce22b52f3e2 272 }
neoqased 0:bce22b52f3e2 273 MotorsDir(frvalue);
neoqased 0:bce22b52f3e2 274 MotorsPwm2(pot, lrstate, frvalue);
neoqased 0:bce22b52f3e2 275 mfrstate = frvalue;
neoqased 0:bce22b52f3e2 276
neoqased 0:bce22b52f3e2 277 wait_ms(100);
neoqased 0:bce22b52f3e2 278 }
neoqased 0:bce22b52f3e2 279 GraduallyStop();
neoqased 0:bce22b52f3e2 280 MotorsPwm();
neoqased 0:bce22b52f3e2 281 Buzzer(5);
neoqased 0:bce22b52f3e2 282 break;
neoqased 0:bce22b52f3e2 283
neoqased 0:bce22b52f3e2 284 ////////////////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 285 case 7: //1台向けモード
neoqased 0:bce22b52f3e2 286 Buzzer(1);
neoqased 0:bce22b52f3e2 287 while(startsw == 0){
neoqased 0:bce22b52f3e2 288 lrvalue = lrsw;
neoqased 0:bce22b52f3e2 289 if(forward == 1){
neoqased 0:bce22b52f3e2 290 frvalue = 0; //正転
neoqased 0:bce22b52f3e2 291 }else if(reverse == 1){
neoqased 0:bce22b52f3e2 292 frvalue = 1; //逆転
neoqased 0:bce22b52f3e2 293 }else{
neoqased 0:bce22b52f3e2 294 frvalue = -1; //停止
neoqased 0:bce22b52f3e2 295 }
neoqased 0:bce22b52f3e2 296 if(lrvalue < 0.33){ //LRスイッチ中央
neoqased 0:bce22b52f3e2 297 lrstate = 0;
neoqased 0:bce22b52f3e2 298 }else if(0.33 <= lrvalue && lrvalue < 0.66){ //LRスイッチ右or左
neoqased 0:bce22b52f3e2 299 lrstate = 1;
neoqased 0:bce22b52f3e2 300 }else{ //LRスイッチ左or右
neoqased 0:bce22b52f3e2 301 lrstate = 2;
neoqased 0:bce22b52f3e2 302 }
neoqased 0:bce22b52f3e2 303
neoqased 0:bce22b52f3e2 304 /*if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (pwm1.read() != 0) || (pwm2.read() != 0)){ //この辺作業中
neoqased 0:bce22b52f3e2 305 GraduallyStop();
neoqased 0:bce22b52f3e2 306 }*/
neoqased 0:bce22b52f3e2 307
neoqased 0:bce22b52f3e2 308
neoqased 0:bce22b52f3e2 309 if((mfrstate != frvalue) || (mlrstate != lrstate)){
neoqased 0:bce22b52f3e2 310 GraduallyStop();
neoqased 0:bce22b52f3e2 311 wait(0.5);
neoqased 0:bce22b52f3e2 312 }
neoqased 0:bce22b52f3e2 313
neoqased 0:bce22b52f3e2 314 if(lrstate == 0){
neoqased 0:bce22b52f3e2 315 if(frvalue == 0){
neoqased 0:bce22b52f3e2 316 dir1 = 0;
neoqased 0:bce22b52f3e2 317 dir2 = 0;
neoqased 0:bce22b52f3e2 318 }else if(frvalue == 1){
neoqased 0:bce22b52f3e2 319 dir1 = 1;
neoqased 0:bce22b52f3e2 320 dir2 = 1;
neoqased 0:bce22b52f3e2 321 }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
neoqased 0:bce22b52f3e2 322 GraduallyStop();
neoqased 0:bce22b52f3e2 323 }
neoqased 0:bce22b52f3e2 324 }else if(lrstate == 1){
neoqased 0:bce22b52f3e2 325 dir1 = 0;
neoqased 0:bce22b52f3e2 326 dir2 = 1;
neoqased 0:bce22b52f3e2 327 }else if(lrstate == 2){
neoqased 0:bce22b52f3e2 328 dir1 = 1;
neoqased 0:bce22b52f3e2 329 dir2 = 0;
neoqased 0:bce22b52f3e2 330 }
neoqased 0:bce22b52f3e2 331 if(((mfrstate != frvalue) || (mlrstate != lrstate)) && (frvalue >= 0)){
neoqased 0:bce22b52f3e2 332 pot = potentiometer;
neoqased 0:bce22b52f3e2 333 for(int i = 0; i <= 100; i++){
neoqased 0:bce22b52f3e2 334 pwm1 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 335 pwm2 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 336 wait_ms(2);
neoqased 0:bce22b52f3e2 337 }
neoqased 0:bce22b52f3e2 338 }
neoqased 0:bce22b52f3e2 339
neoqased 0:bce22b52f3e2 340
neoqased 0:bce22b52f3e2 341 /*if(lrstate == 0){ //lrスイッチが中央で
neoqased 0:bce22b52f3e2 342 if(frvalue == 0){ //frスイッチが前で前進
neoqased 0:bce22b52f3e2 343 if(dir1.read() != 0 || dir2.read() != 0){
neoqased 0:bce22b52f3e2 344 //GraduallyStop();
neoqased 0:bce22b52f3e2 345 dir1 = 0;
neoqased 0:bce22b52f3e2 346 dir2 = 0;
neoqased 0:bce22b52f3e2 347 for(int i = 0; i <= 100; i++){
neoqased 0:bce22b52f3e2 348 pwm1 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 349 pwm2 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 350 wait_ms(5);
neoqased 0:bce22b52f3e2 351 }
neoqased 0:bce22b52f3e2 352 }
neoqased 0:bce22b52f3e2 353 dir1 = 0;
neoqased 0:bce22b52f3e2 354 dir2 = 0;
neoqased 0:bce22b52f3e2 355 }else if(frvalue == 1){
neoqased 0:bce22b52f3e2 356 if(dir1.read() != 1 || dir2.read() != 1){
neoqased 0:bce22b52f3e2 357 //GraduallyStop();
neoqased 0:bce22b52f3e2 358 dir1 = 1;
neoqased 0:bce22b52f3e2 359 dir2 = 1;
neoqased 0:bce22b52f3e2 360 for(int i = 0; i <= 100; i++){
neoqased 0:bce22b52f3e2 361 pwm1 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 362 pwm2 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 363 wait_ms(5);
neoqased 0:bce22b52f3e2 364 }
neoqased 0:bce22b52f3e2 365 }
neoqased 0:bce22b52f3e2 366 dir1 = 1;
neoqased 0:bce22b52f3e2 367 dir2 = 1;
neoqased 0:bce22b52f3e2 368 }else if((pwm1.read() != 0) || (pwm2.read() != 0)){
neoqased 0:bce22b52f3e2 369 GraduallyStop();
neoqased 0:bce22b52f3e2 370 }
neoqased 0:bce22b52f3e2 371 }else if(lrstate == 1){
neoqased 0:bce22b52f3e2 372 if(dir1.read() != 0 || dir2.read() != 1){
neoqased 0:bce22b52f3e2 373 //GraduallyStop();
neoqased 0:bce22b52f3e2 374 dir1 = 0;
neoqased 0:bce22b52f3e2 375 dir2 = 1;
neoqased 0:bce22b52f3e2 376 for(int i = 0; i <= 100; i++){
neoqased 0:bce22b52f3e2 377 pwm1 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 378 pwm2 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 379 wait_ms(5);
neoqased 0:bce22b52f3e2 380 }
neoqased 0:bce22b52f3e2 381 }
neoqased 0:bce22b52f3e2 382 dir1 = 0;
neoqased 0:bce22b52f3e2 383 dir2 = 1;
neoqased 0:bce22b52f3e2 384 }else if(lrstate == 2){
neoqased 0:bce22b52f3e2 385 if(dir1.read() != 1 || dir2.read() != 0){
neoqased 0:bce22b52f3e2 386 //GraduallyStop();
neoqased 0:bce22b52f3e2 387 dir1 = 1;
neoqased 0:bce22b52f3e2 388 dir2 = 0;
neoqased 0:bce22b52f3e2 389 for(int i = 0; i <= 100; i++){
neoqased 0:bce22b52f3e2 390 pwm1 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 391 pwm2 = RoundDemical1(pot) * (double)i / 100;
neoqased 0:bce22b52f3e2 392 wait_ms(5);
neoqased 0:bce22b52f3e2 393 }
neoqased 0:bce22b52f3e2 394 }
neoqased 0:bce22b52f3e2 395 dir1 = 1;
neoqased 0:bce22b52f3e2 396 dir2 = 0;
neoqased 0:bce22b52f3e2 397 }*/
neoqased 0:bce22b52f3e2 398
neoqased 0:bce22b52f3e2 399 pot = potentiometer;
neoqased 0:bce22b52f3e2 400 if(frvalue >= 0){
neoqased 0:bce22b52f3e2 401 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 402 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 403 }
neoqased 0:bce22b52f3e2 404 mfrstate = frvalue;
neoqased 0:bce22b52f3e2 405 mlrstate = lrstate;
neoqased 0:bce22b52f3e2 406
neoqased 0:bce22b52f3e2 407 wait_ms(100);
neoqased 0:bce22b52f3e2 408 }
neoqased 0:bce22b52f3e2 409
neoqased 0:bce22b52f3e2 410 Buzzer(5);
neoqased 0:bce22b52f3e2 411 break;
neoqased 0:bce22b52f3e2 412
neoqased 0:bce22b52f3e2 413 case 8: //case7 のブラッシュアップ版(1台モード)
neoqased 0:bce22b52f3e2 414 Buzzer(1);
neoqased 0:bce22b52f3e2 415 while(startsw == 0){
neoqased 0:bce22b52f3e2 416 swstate = SwitchState(); //スイッチの入力を見る
neoqased 0:bce22b52f3e2 417 if(1 <= swstate && swstate <= 3){ //1<= swstate<=3で前進
neoqased 0:bce22b52f3e2 418 dir[0] = 0;
neoqased 0:bce22b52f3e2 419 dir[1] = 0;
neoqased 0:bce22b52f3e2 420 }else if(swstate == 4){ //どっちも中点で停止
neoqased 0:bce22b52f3e2 421 }else if(swstate == 5){ //右に信地旋回
neoqased 0:bce22b52f3e2 422 dir[0] = 0;
neoqased 0:bce22b52f3e2 423 dir[1] = 1;
neoqased 0:bce22b52f3e2 424 }else if(swstate == 6){ //左に信地旋回
neoqased 0:bce22b52f3e2 425 dir[0] = 1;
neoqased 0:bce22b52f3e2 426 dir[1] = 0;
neoqased 0:bce22b52f3e2 427 }else if(swstate <= 9){ //7<=swstate<=9で後退
neoqased 0:bce22b52f3e2 428 dir[0] = 1;
neoqased 0:bce22b52f3e2 429 dir[1] = 1;
neoqased 0:bce22b52f3e2 430 }
neoqased 0:bce22b52f3e2 431
neoqased 0:bce22b52f3e2 432 if((dir1.read() != dir[0] || dir2.read() != dir[1] || swstate == 4) && (pwm1.read() != 0 || pwm2.read() != 0)){
neoqased 0:bce22b52f3e2 433 GraduallyStop2();
neoqased 0:bce22b52f3e2 434 mfrstate = 1; //フラグ
neoqased 0:bce22b52f3e2 435 }
neoqased 0:bce22b52f3e2 436 dir1 = dir[0]; //dirに入力
neoqased 0:bce22b52f3e2 437 dir2 = dir[1];
neoqased 0:bce22b52f3e2 438
neoqased 0:bce22b52f3e2 439 pot = potentiometer; //ポテンショメータの入力を見る
neoqased 0:bce22b52f3e2 440 if(swstate % 3 == 1){ //ここ以下は走行するスピードを決定
neoqased 0:bce22b52f3e2 441 if(swstate == 4){
neoqased 0:bce22b52f3e2 442 pwm1 = 0;
neoqased 0:bce22b52f3e2 443 pwm2 = 0;
neoqased 0:bce22b52f3e2 444 }else{
neoqased 0:bce22b52f3e2 445 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 446 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 447 mfrstate = 0;
neoqased 0:bce22b52f3e2 448 }
neoqased 0:bce22b52f3e2 449 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 450 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 451 }
neoqased 0:bce22b52f3e2 452 }else if(swstate % 3 == 2){ //右
neoqased 0:bce22b52f3e2 453 if(swstate == 5){
neoqased 0:bce22b52f3e2 454 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 455 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 456 mfrstate = 0;
neoqased 0:bce22b52f3e2 457 }
neoqased 0:bce22b52f3e2 458 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 459 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 460 }else{
neoqased 0:bce22b52f3e2 461 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 462 GraduallyStart2(1,0.5,0,0,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 463 mfrstate = 0;
neoqased 0:bce22b52f3e2 464 }
neoqased 0:bce22b52f3e2 465 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 466 pwm2 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 467 }
neoqased 0:bce22b52f3e2 468 }else{ //左
neoqased 0:bce22b52f3e2 469 if(swstate == 6){
neoqased 0:bce22b52f3e2 470 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 471 GraduallyStart2(1,1,0,0,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 472 mfrstate = 0;
neoqased 0:bce22b52f3e2 473 }
neoqased 0:bce22b52f3e2 474 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 475 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 476 }else{
neoqased 0:bce22b52f3e2 477 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 478 GraduallyStart2(0.5,1,0,0,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 479 mfrstate = 0;
neoqased 0:bce22b52f3e2 480 }
neoqased 0:bce22b52f3e2 481 pwm1 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 482 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 483 }
neoqased 0:bce22b52f3e2 484 }
neoqased 0:bce22b52f3e2 485 wait_ms(100);
neoqased 0:bce22b52f3e2 486 }
neoqased 0:bce22b52f3e2 487 Buzzer(5);
neoqased 0:bce22b52f3e2 488 break;
neoqased 0:bce22b52f3e2 489
neoqased 0:bce22b52f3e2 490 /////////////////////////////////////////////////////////////////////////////////////
neoqased 0:bce22b52f3e2 491 case 9: //2台モード
neoqased 0:bce22b52f3e2 492 Buzzer(1);
neoqased 0:bce22b52f3e2 493 while(startsw == 0){
neoqased 0:bce22b52f3e2 494 swstate = SwitchState();
neoqased 0:bce22b52f3e2 495 if(1 <= swstate && swstate <= 3){ //1<= swstate<=3で前進
neoqased 0:bce22b52f3e2 496 for(int i = 0; i < 4; i++){
neoqased 0:bce22b52f3e2 497 dir[i] = 0;
neoqased 0:bce22b52f3e2 498 }
neoqased 0:bce22b52f3e2 499 }else if(swstate <= 4){ //中点で停止
neoqased 0:bce22b52f3e2 500 }else if(swstate <= 5){ //中点，右
neoqased 0:bce22b52f3e2 501 dir[0] = 1; //右に姿勢変化
neoqased 0:bce22b52f3e2 502 dir[1] = 0;
neoqased 0:bce22b52f3e2 503 dir[2] = 0;
neoqased 0:bce22b52f3e2 504 dir[3] = 1;
neoqased 0:bce22b52f3e2 505 }else if(swstate <= 6){ //中点，左
neoqased 0:bce22b52f3e2 506 dir[0] = 0; //左に姿勢変化
neoqased 0:bce22b52f3e2 507 dir[1] = 1;
neoqased 0:bce22b52f3e2 508 dir[2] = 1;
neoqased 0:bce22b52f3e2 509 dir[3] = 0;
neoqased 0:bce22b52f3e2 510 }else if(swstate <= 9){ //7<=swstate<=9で後退
neoqased 0:bce22b52f3e2 511 for(int i = 0; i < 4; i++){
neoqased 0:bce22b52f3e2 512 dir[i] = 1;
neoqased 0:bce22b52f3e2 513 }
neoqased 0:bce22b52f3e2 514 }
neoqased 0:bce22b52f3e2 515
neoqased 0:bce22b52f3e2 516 if((dir1.read() != dir[0] || dir2.read() != dir[1] || dir3.read() != dir[2] || dir4.read() != dir[3] || (swstate == 4))
neoqased 0:bce22b52f3e2 517 && (pwm1.read() != 0 || pwm2.read() != 0 || pwm3.read() != 0 || pwm4.read() != 0)){
neoqased 0:bce22b52f3e2 518 GraduallyStop2();
neoqased 0:bce22b52f3e2 519 mfrstate = 1;
neoqased 0:bce22b52f3e2 520 }
neoqased 0:bce22b52f3e2 521 dir1 = dir[0];
neoqased 0:bce22b52f3e2 522 dir2 = dir[1];
neoqased 0:bce22b52f3e2 523 dir3 = dir[2];
neoqased 0:bce22b52f3e2 524 dir4 = dir[3];
neoqased 0:bce22b52f3e2 525
neoqased 0:bce22b52f3e2 526 pot = potentiometer;
neoqased 0:bce22b52f3e2 527
neoqased 0:bce22b52f3e2 528 if(4 <= swstate && swstate <= 6){ //ここ変える←変えた
neoqased 0:bce22b52f3e2 529 if(swstate == 4){ //中点+中点で停止
neoqased 0:bce22b52f3e2 530 pwm1 = 0;
neoqased 0:bce22b52f3e2 531 pwm2 = 0;
neoqased 0:bce22b52f3e2 532 pwm3 = 0;
neoqased 0:bce22b52f3e2 533 pwm4 = 0;
neoqased 0:bce22b52f3e2 534 }else{ //中点+左右でそれぞれ信地旋回
neoqased 0:bce22b52f3e2 535 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 536 GraduallyStart2(1,1,1,1,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 537 mfrstate = 0;
neoqased 0:bce22b52f3e2 538 }
neoqased 0:bce22b52f3e2 539 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 540 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 541 pwm3 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 542 pwm4 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 543 }
neoqased 0:bce22b52f3e2 544 }else if(swstate % 3 == 1){
neoqased 0:bce22b52f3e2 545 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 546 GraduallyStart2(1,1,1,1,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 547 mfrstate = 0;
neoqased 0:bce22b52f3e2 548 }
neoqased 0:bce22b52f3e2 549 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 550 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 551 pwm3 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 552 pwm4 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 553 }else if(swstate % 3 == 2){ //右
neoqased 0:bce22b52f3e2 554 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 555 GraduallyStart2(1,0.5,1,0.5,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 556 mfrstate = 0;
neoqased 0:bce22b52f3e2 557 }
neoqased 0:bce22b52f3e2 558 pwm1 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 559 pwm2 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 560 pwm3 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 561 pwm4 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 562 }else{ //左
neoqased 0:bce22b52f3e2 563 if(mfrstate == 1){
neoqased 0:bce22b52f3e2 564 GraduallyStart2(0.5,1,0.5,1,RoundDemical1(pot));
neoqased 0:bce22b52f3e2 565 mfrstate = 0;
neoqased 0:bce22b52f3e2 566 }
neoqased 0:bce22b52f3e2 567 pwm1 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 568 pwm2 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 569 pwm3 = 0.5 * RoundDemical1(pot);
neoqased 0:bce22b52f3e2 570 pwm4 = RoundDemical1(pot);
neoqased 0:bce22b52f3e2 571 }
neoqased 0:bce22b52f3e2 572 wait_ms(100);
neoqased 0:bce22b52f3e2 573 }
neoqased 0:bce22b52f3e2 574 Buzzer(5);
neoqased 0:bce22b52f3e2 575 break;
neoqased 0:bce22b52f3e2 576
neoqased 0:bce22b52f3e2 577 default: //上記以外で何もなし
neoqased 0:bce22b52f3e2 578 Buzzer(-3);
neoqased 0:bce22b52f3e2 579 break;
neoqased 0:bce22b52f3e2 580
neoqased 0:bce22b52f3e2 581 }
neoqased 0:bce22b52f3e2 582 }
neoqased 0:bce22b52f3e2 583 return 0;
neoqased 0:bce22b52f3e2 584 }
neoqased 0:bce22b52f3e2 585
neoqased 0:bce22b52f3e2 586
neoqased 0:bce22b52f3e2 587 /******************************ここから関数******************************/
neoqased 0:bce22b52f3e2 588 int Init(){ //初期化関数
neoqased 0:bce22b52f3e2 589 pwm1 = 0;
neoqased 0:bce22b52f3e2 590 pwm2 = 0;
neoqased 0:bce22b52f3e2 591 pwm3 = 0;
neoqased 0:bce22b52f3e2 592 pwm4 = 0;
neoqased 0:bce22b52f3e2 593 dir1 = 0;
neoqased 0:bce22b52f3e2 594 dir2 = 0;
neoqased 0:bce22b52f3e2 595 dir3 = 0;
neoqased 0:bce22b52f3e2 596 dir4 = 0;
neoqased 0:bce22b52f3e2 597 buzzer = 0;
neoqased 0:bce22b52f3e2 598 pwm1.period_us(25); //Pololu 18v15のpwm周波数40kHzに合わせる
neoqased 0:bce22b52f3e2 599 pwm2.period_us(25);
neoqased 0:bce22b52f3e2 600 pwm3.period_us(25);
neoqased 0:bce22b52f3e2 601 pwm4.period_us(25);
neoqased 0:bce22b52f3e2 602 DipLed();
neoqased 0:bce22b52f3e2 603 return 0;
neoqased 0:bce22b52f3e2 604 }
neoqased 0:bce22b52f3e2 605
neoqased 0:bce22b52f3e2 606 int DipLed(){ //DIPスイッチの状態によってledを光らせる関数
neoqased 0:bce22b52f3e2 607 if(dip1 == 1) led1 = 1; else led1 = 0; //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない
neoqased 0:bce22b52f3e2 608 if(dip2 == 1) led2 = 1; else led2 = 0;
neoqased 0:bce22b52f3e2 609 if(dip3 == 1) led3 = 1; else led3 = 0;
neoqased 0:bce22b52f3e2 610 if(dip4 == 1) led4 = 1; else led4 = 0;
neoqased 0:bce22b52f3e2 611 return 0;
neoqased 0:bce22b52f3e2 612 }
neoqased 0:bce22b52f3e2 613
neoqased 0:bce22b52f3e2 614 int GetDipValue(){ //DIPスイッチの値を取得し，値を返す関数
neoqased 0:bce22b52f3e2 615 int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得
neoqased 0:bce22b52f3e2 616 if(Dip1 == 0){
neoqased 0:bce22b52f3e2 617 if(Dip2 == 0){
neoqased 0:bce22b52f3e2 618 if(Dip3 == 0){
neoqased 0:bce22b52f3e2 619 if(Dip4 == 0) return 0; else return 1; //DIPスイッチが0000の場合0を返す．0001なら1
neoqased 0:bce22b52f3e2 620 }else{
neoqased 0:bce22b52f3e2 621 if(Dip4 == 0) return 2; else return 3;
neoqased 0:bce22b52f3e2 622 }
neoqased 0:bce22b52f3e2 623 }else{
neoqased 0:bce22b52f3e2 624 if(Dip3 == 0){
neoqased 0:bce22b52f3e2 625 if(Dip4 == 0) return 4; else return 5;
neoqased 0:bce22b52f3e2 626 }else{
neoqased 0:bce22b52f3e2 627 if(Dip4 == 0) return 6; else return 7;
neoqased 0:bce22b52f3e2 628 }
neoqased 0:bce22b52f3e2 629 }
neoqased 0:bce22b52f3e2 630 }else{
neoqased 0:bce22b52f3e2 631 if(Dip2 == 0){
neoqased 0:bce22b52f3e2 632 if(Dip3 == 0){
neoqased 0:bce22b52f3e2 633 if(Dip4 == 0) return 8; else return 9;
neoqased 0:bce22b52f3e2 634 }else{
neoqased 0:bce22b52f3e2 635 if(Dip4 == 0) return 10; else return 11;
neoqased 0:bce22b52f3e2 636 }
neoqased 0:bce22b52f3e2 637 }else{
neoqased 0:bce22b52f3e2 638 if(Dip3 == 0){
neoqased 0:bce22b52f3e2 639 if(Dip4 == 0) return 12; else return 13;
neoqased 0:bce22b52f3e2 640 }else{
neoqased 0:bce22b52f3e2 641 if(Dip4 == 0) return 14; else return 15;
neoqased 0:bce22b52f3e2 642 }
neoqased 0:bce22b52f3e2 643 }
neoqased 0:bce22b52f3e2 644 }
neoqased 0:bce22b52f3e2 645 }
neoqased 0:bce22b52f3e2 646
neoqased 0:bce22b52f3e2 647 int SW(){ //スタートスイッチ用関数，押して離したらスタート
neoqased 0:bce22b52f3e2 648 int i = 0, j = 0;
neoqased 0:bce22b52f3e2 649 while(i < 3){ //チャタリング除去，15msにわたってスタートスイッチが押されていればbreak
neoqased 0:bce22b52f3e2 650 if(startsw == 1) i++;
neoqased 0:bce22b52f3e2 651 else i = 0;
neoqased 0:bce22b52f3e2 652 DipLed();
neoqased 0:bce22b52f3e2 653 wait_ms(5);
neoqased 0:bce22b52f3e2 654 }
neoqased 0:bce22b52f3e2 655 while(j < 3){ //上に同じ，スタートスイッチが離されたことを検知
neoqased 0:bce22b52f3e2 656 if(startsw == 0) j++;
neoqased 0:bce22b52f3e2 657 else j = 0;
neoqased 0:bce22b52f3e2 658 DipLed();
neoqased 0:bce22b52f3e2 659 wait_ms(5);
neoqased 0:bce22b52f3e2 660 }
neoqased 0:bce22b52f3e2 661 return 0;
neoqased 0:bce22b52f3e2 662 }
neoqased 0:bce22b52f3e2 663
neoqased 0:bce22b52f3e2 664 int Buzzer(int buzvar){ //電子ブザーを鳴らす関数
neoqased 0:bce22b52f3e2 665 switch (buzvar){
neoqased 0:bce22b52f3e2 666 /**************エラーを知らせるbeep**************/
neoqased 0:bce22b52f3e2 667 case -3: //error * - -
neoqased 0:bce22b52f3e2 668 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 669 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 670 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 671 break;
neoqased 0:bce22b52f3e2 672 case -2: //error * - - -
neoqased 0:bce22b52f3e2 673 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 674 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 675 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 676 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 677 break;
neoqased 0:bce22b52f3e2 678 case -1: //error * - - - -
neoqased 0:bce22b52f3e2 679 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 680 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 681 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 682 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 683 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 684 break;
neoqased 0:bce22b52f3e2 685 /**************エラーここまで**************/
neoqased 0:bce22b52f3e2 686 case 0: //サウンドなし
neoqased 0:bce22b52f3e2 687 buzzer = 0;
neoqased 0:bce22b52f3e2 688 break;
neoqased 0:bce22b52f3e2 689 /**************状態を知らせるためのbeep**************/
neoqased 0:bce22b52f3e2 690 case 1: // *(短)
neoqased 0:bce22b52f3e2 691 buzzer = 1; wait(0.1); buzzer = 0;
neoqased 0:bce22b52f3e2 692 break;
neoqased 0:bce22b52f3e2 693 case 2: // * *
neoqased 0:bce22b52f3e2 694 buzzer = 1; wait(0.1); buzzer = 0; wait(0.05);
neoqased 0:bce22b52f3e2 695 buzzer = 1; wait(0.1); buzzer = 0;
neoqased 0:bce22b52f3e2 696 break;
neoqased 0:bce22b52f3e2 697 case 3: // -(長)
neoqased 0:bce22b52f3e2 698 buzzer = 1; wait(0.3); buzzer = 0;
neoqased 0:bce22b52f3e2 699 break;
neoqased 0:bce22b52f3e2 700 case 4: // - -
neoqased 0:bce22b52f3e2 701 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
neoqased 0:bce22b52f3e2 702 buzzer = 1; wait(0.3); buzzer = 0;
neoqased 0:bce22b52f3e2 703 break;
neoqased 0:bce22b52f3e2 704 case 5: // ---
neoqased 0:bce22b52f3e2 705 buzzer = 1; wait(0.9); buzzer = 0;
neoqased 0:bce22b52f3e2 706 break;
neoqased 0:bce22b52f3e2 707 case 6: // * * * * * * * * * *
neoqased 0:bce22b52f3e2 708 for(int i = 0; i < 3; i++){
neoqased 0:bce22b52f3e2 709 for(int j = 0; j < 3; j++){
neoqased 0:bce22b52f3e2 710 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 711 }
neoqased 0:bce22b52f3e2 712 wait(0.2);
neoqased 0:bce22b52f3e2 713 }
neoqased 0:bce22b52f3e2 714 buzzer = 1; wait(0.1); buzzer = 0;
neoqased 0:bce22b52f3e2 715 break;
neoqased 0:bce22b52f3e2 716 case 7: // **-* ** -* ** *** **** "finish"
neoqased 0:bce22b52f3e2 717 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 718 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 719 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 720 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 721 wait(0.2);
neoqased 0:bce22b52f3e2 722 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 723 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 724 wait(0.2);
neoqased 0:bce22b52f3e2 725 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 726 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 727 wait(0.2);
neoqased 0:bce22b52f3e2 728 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 729 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 730 wait(0.2);
neoqased 0:bce22b52f3e2 731 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 732 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 733 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 734 wait(0.2);
neoqased 0:bce22b52f3e2 735 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 736 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 737 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 738 buzzer = 1; wait(0.1); buzzer = 0;
neoqased 0:bce22b52f3e2 739 break;
neoqased 0:bce22b52f3e2 740 case 8: // *-*** -*- --* "オワリ"
neoqased 0:bce22b52f3e2 741 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 742 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 743 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 744 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 745 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 746 wait(0.2);
neoqased 0:bce22b52f3e2 747 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 748 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 749 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 750 wait(0.2);
neoqased 0:bce22b52f3e2 751 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 752 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
neoqased 0:bce22b52f3e2 753 buzzer = 1; wait(0.1); buzzer = 0;
neoqased 0:bce22b52f3e2 754 break;
neoqased 0:bce22b52f3e2 755 case 9: //ケータイの着信音的な
neoqased 0:bce22b52f3e2 756 for(int i = 0; i < 20; i++){
neoqased 0:bce22b52f3e2 757 buzzer = 1; wait(0.03);
neoqased 0:bce22b52f3e2 758 buzzer = 0; wait(0.03);
neoqased 0:bce22b52f3e2 759 }
neoqased 0:bce22b52f3e2 760 break;
neoqased 0:bce22b52f3e2 761 case 10: //9の短いバージョン
neoqased 0:bce22b52f3e2 762 for(int i = 0; i < 20; i++){
neoqased 0:bce22b52f3e2 763 buzzer = 1; wait(0.02);
neoqased 0:bce22b52f3e2 764 buzzer = 0; wait(0.02);
neoqased 0:bce22b52f3e2 765 }
neoqased 0:bce22b52f3e2 766 break;
neoqased 0:bce22b52f3e2 767 /**************状態を知らせるためのbeepここまで**************/
neoqased 0:bce22b52f3e2 768 default: //no sound
neoqased 0:bce22b52f3e2 769 buzzer = 0;
neoqased 0:bce22b52f3e2 770 break;
neoqased 0:bce22b52f3e2 771 }
neoqased 0:bce22b52f3e2 772 return 0;
neoqased 0:bce22b52f3e2 773 }
neoqased 0:bce22b52f3e2 774
neoqased 0:bce22b52f3e2 775 int GraduallyStop(){ //ゆるやかにモータを停止させるための関数
neoqased 0:bce22b52f3e2 776 double pwmn1 = pwm1.read();
neoqased 0:bce22b52f3e2 777 double pwmn2 = pwm2.read();
neoqased 0:bce22b52f3e2 778 double pwmn3 = pwm3.read();
neoqased 0:bce22b52f3e2 779 double pwmn4 = pwm4.read();
neoqased 0:bce22b52f3e2 780 const int loop = 100;
neoqased 0:bce22b52f3e2 781
neoqased 0:bce22b52f3e2 782 for(int i = 0; i < loop; i++){
neoqased 0:bce22b52f3e2 783 pwm1 = pwm1.read() - pwmn1 / loop;
neoqased 0:bce22b52f3e2 784 pwm2 = pwm2.read() - pwmn2 / loop;
neoqased 0:bce22b52f3e2 785 pwm3 = pwm3.read() - pwmn3 / loop;
neoqased 0:bce22b52f3e2 786 pwm4 = pwm4.read() - pwmn4 / loop;
neoqased 0:bce22b52f3e2 787 wait_ms(10);
neoqased 0:bce22b52f3e2 788 }
neoqased 0:bce22b52f3e2 789
neoqased 0:bce22b52f3e2 790 pwm1 = 0;
neoqased 0:bce22b52f3e2 791 pwm2 = 0;
neoqased 0:bce22b52f3e2 792 pwm3 = 0;
neoqased 0:bce22b52f3e2 793 pwm4 = 0;
neoqased 0:bce22b52f3e2 794
neoqased 0:bce22b52f3e2 795 return 0;
neoqased 0:bce22b52f3e2 796 }
neoqased 0:bce22b52f3e2 797
neoqased 0:bce22b52f3e2 798 int GraduallyStop2(){
neoqased 0:bce22b52f3e2 799 double pwmn1 = pwm1.read();
neoqased 0:bce22b52f3e2 800 double pwmn2 = pwm2.read();
neoqased 0:bce22b52f3e2 801 double pwmn3 = pwm3.read();
neoqased 0:bce22b52f3e2 802 double pwmn4 = pwm4.read();
neoqased 0:bce22b52f3e2 803 const int loop = 100;
neoqased 0:bce22b52f3e2 804
neoqased 0:bce22b52f3e2 805 for(int i = 0; i < loop; i++){
neoqased 0:bce22b52f3e2 806 pwm1 = pwm1.read() - pwmn1 / loop;
neoqased 0:bce22b52f3e2 807 pwm2 = pwm2.read() - pwmn2 / loop;
neoqased 0:bce22b52f3e2 808 pwm3 = pwm3.read() - pwmn3 / loop;
neoqased 0:bce22b52f3e2 809 pwm4 = pwm4.read() - pwmn4 / loop;
neoqased 0:bce22b52f3e2 810 if(i > 40){
neoqased 0:bce22b52f3e2 811 wait_ms(5);
neoqased 0:bce22b52f3e2 812 if(i > 60){
neoqased 0:bce22b52f3e2 813 wait_ms(5);
neoqased 0:bce22b52f3e2 814 }
neoqased 0:bce22b52f3e2 815 }
neoqased 0:bce22b52f3e2 816 wait_ms(5);
neoqased 0:bce22b52f3e2 817 }
neoqased 0:bce22b52f3e2 818
neoqased 0:bce22b52f3e2 819 pwm1 = 0;
neoqased 0:bce22b52f3e2 820 pwm2 = 0;
neoqased 0:bce22b52f3e2 821 pwm3 = 0;
neoqased 0:bce22b52f3e2 822 pwm4 = 0;
neoqased 0:bce22b52f3e2 823
neoqased 0:bce22b52f3e2 824 return 0;
neoqased 0:bce22b52f3e2 825 }
neoqased 0:bce22b52f3e2 826
neoqased 0:bce22b52f3e2 827
neoqased 0:bce22b52f3e2 828
neoqased 0:bce22b52f3e2 829 int GraduallyStart(float pot, int lr, int fr){ //ゆるやかにモータを始動させる関数
neoqased 0:bce22b52f3e2 830 const int loop = 100;
neoqased 0:bce22b52f3e2 831 for(int i = 0; i < loop; i++){
neoqased 0:bce22b52f3e2 832 MotorsPwm2(pot * (double)i/(double)loop, lr, fr);
neoqased 0:bce22b52f3e2 833 wait_ms(10);
neoqased 0:bce22b52f3e2 834 }
neoqased 0:bce22b52f3e2 835 MotorsPwm2(pot, lr);
neoqased 0:bce22b52f3e2 836 return 0;
neoqased 0:bce22b52f3e2 837 }
neoqased 0:bce22b52f3e2 838
neoqased 0:bce22b52f3e2 839 int GraduallyStart2(float a, float b, float c, float d, float v){ //vはポテンショメータ
neoqased 0:bce22b52f3e2 840 float threshold = 0.3;
neoqased 0:bce22b52f3e2 841 if(v <= threshold){
neoqased 0:bce22b52f3e2 842 pwm1 = a * v;
neoqased 0:bce22b52f3e2 843 pwm2 = b * v;
neoqased 0:bce22b52f3e2 844 pwm3 = c * v;
neoqased 0:bce22b52f3e2 845 pwm4 = d * v;
neoqased 0:bce22b52f3e2 846 }else{
neoqased 0:bce22b52f3e2 847 for(float i = 0; i < v; i += 0.1){
neoqased 0:bce22b52f3e2 848 pwm1 = a * i;
neoqased 0:bce22b52f3e2 849 pwm2 = b * i;
neoqased 0:bce22b52f3e2 850 pwm3 = c * i;
neoqased 0:bce22b52f3e2 851 pwm4 = d * i;
neoqased 0:bce22b52f3e2 852 wait(0.05);
neoqased 0:bce22b52f3e2 853 }
neoqased 0:bce22b52f3e2 854 }
neoqased 0:bce22b52f3e2 855 return 0;
neoqased 0:bce22b52f3e2 856 }
neoqased 0:bce22b52f3e2 857
neoqased 0:bce22b52f3e2 858
neoqased 0:bce22b52f3e2 859 int MotorsPwm(double a, double b, double c, double d, double v){ //4つのモータに速度を与える関数(デフォルトで停止)
neoqased 0:bce22b52f3e2 860 pwm1 = a * v;
neoqased 0:bce22b52f3e2 861 pwm2 = b * v;
neoqased 0:bce22b52f3e2 862 pwm3 = c * v;
neoqased 0:bce22b52f3e2 863 pwm4 = d * v;
neoqased 0:bce22b52f3e2 864 return 0;
neoqased 0:bce22b52f3e2 865 }
neoqased 0:bce22b52f3e2 866
neoqased 0:bce22b52f3e2 867 int MotorsPwm2(double pot, int lr, int fr){ //4つのモータに速度を与える関数その2(デフォルトで停止)
neoqased 0:bce22b52f3e2 868 float H = 1.0;
neoqased 0:bce22b52f3e2 869 float M = 0.75;
neoqased 0:bce22b52f3e2 870 float L = 0.5;
neoqased 0:bce22b52f3e2 871 if(fr >= 0){
neoqased 0:bce22b52f3e2 872 switch (lr){
neoqased 0:bce22b52f3e2 873 case 0: //前進
neoqased 0:bce22b52f3e2 874 MotorsPwm(M, M, M, M, RoundDemical1(pot));
neoqased 0:bce22b52f3e2 875 break;
neoqased 0:bce22b52f3e2 876 case 1: //右or左旋回
neoqased 0:bce22b52f3e2 877 MotorsPwm(H, L, H, L, RoundDemical1(pot));
neoqased 0:bce22b52f3e2 878 break;
neoqased 0:bce22b52f3e2 879 case 2: //左or右旋回(1と逆)
neoqased 0:bce22b52f3e2 880 MotorsPwm(L, H, L, H, RoundDemical1(pot));
neoqased 0:bce22b52f3e2 881 break;
neoqased 0:bce22b52f3e2 882 default:
neoqased 0:bce22b52f3e2 883 GraduallyStop();
neoqased 0:bce22b52f3e2 884 break;
neoqased 0:bce22b52f3e2 885 }
neoqased 0:bce22b52f3e2 886 }else{
neoqased 0:bce22b52f3e2 887 MotorsPwm();
neoqased 0:bce22b52f3e2 888 }
neoqased 0:bce22b52f3e2 889 return 0;
neoqased 0:bce22b52f3e2 890 }
neoqased 0:bce22b52f3e2 891
neoqased 0:bce22b52f3e2 892 int MotorsDir(int a){ //4つのモータの回転方向を決定する関数)
neoqased 0:bce22b52f3e2 893 if(a >= 0){
neoqased 0:bce22b52f3e2 894 dir1 = a;
neoqased 0:bce22b52f3e2 895 dir2 = a;
neoqased 0:bce22b52f3e2 896 dir3 = a;
neoqased 0:bce22b52f3e2 897 dir4 = a;
neoqased 0:bce22b52f3e2 898 }
neoqased 0:bce22b52f3e2 899
neoqased 0:bce22b52f3e2 900 return 0;
neoqased 0:bce22b52f3e2 901 }
neoqased 0:bce22b52f3e2 902
neoqased 0:bce22b52f3e2 903 float RoundDemical1(float a){ //小数点以下第一桁で四捨五入
neoqased 0:bce22b52f3e2 904 float b;
neoqased 0:bce22b52f3e2 905 a *= 10;
neoqased 0:bce22b52f3e2 906 b = (float)(int)(a+0.5);
neoqased 0:bce22b52f3e2 907 b /= 10;
neoqased 0:bce22b52f3e2 908 return b;
neoqased 0:bce22b52f3e2 909 }
neoqased 0:bce22b52f3e2 910 /*
neoqased 0:bce22b52f3e2 911 float RoundDemicaln(float a, int n){ //小数点以下第n桁で四捨五入
neoqased 0:bce22b52f3e2 912 float b, c;
neoqased 0:bce22b52f3e2 913 b = pow(a, n);
neoqased 0:bce22b52f3e2 914 c = (float)(int)(b+0.5);
neoqased 0:bce22b52f3e2 915 b = pow(c, 1.0/(double)n);
neoqased 0:bce22b52f3e2 916 return b;
neoqased 0:bce22b52f3e2 917 }
neoqased 0:bce22b52f3e2 918 */
neoqased 0:bce22b52f3e2 919
neoqased 0:bce22b52f3e2 920 int SwitchState(){ //2つのスイッチからの入力を返す
neoqased 0:bce22b52f3e2 921 /*********************************************
neoqased 0:bce22b52f3e2 922 fr・lrで
neoqased 0:bce22b52f3e2 923 前・中点：1 前・右：2 前・左：3
neoqased 0:bce22b52f3e2 924 中点・中点：4 中点・右：5 中点・左：6
neoqased 0:bce22b52f3e2 925 後・中点：7 後・右：8 後・左：9
neoqased 0:bce22b52f3e2 926 を返す
neoqased 0:bce22b52f3e2 927 *********************************************/
neoqased 0:bce22b52f3e2 928 int state = 0;
neoqased 0:bce22b52f3e2 929 float lr = lrsw;
neoqased 0:bce22b52f3e2 930
neoqased 0:bce22b52f3e2 931 if(forward == 1){ //frスイッチ前
neoqased 0:bce22b52f3e2 932 state = 1;
neoqased 0:bce22b52f3e2 933 }else if(reverse == 1){ //frスイッチ後ろ
neoqased 0:bce22b52f3e2 934 state = 7;
neoqased 0:bce22b52f3e2 935 }else{ //frスイッチ中点
neoqased 0:bce22b52f3e2 936 state = 4;
neoqased 0:bce22b52f3e2 937 }
neoqased 0:bce22b52f3e2 938 if(lr < 0.33){ //lrスイッチ中点
neoqased 0:bce22b52f3e2 939 state += 0;
neoqased 0:bce22b52f3e2 940 }else if(lr < 0.67){ //lrスイッチ左
neoqased 0:bce22b52f3e2 941 state += 2;
neoqased 0:bce22b52f3e2 942 }else{ //lrスイッチ右
neoqased 0:bce22b52f3e2 943 state += 1;
neoqased 0:bce22b52f3e2 944 }
neoqased 0:bce22b52f3e2 945
neoqased 0:bce22b52f3e2 946 return state;
neoqased 0:bce22b52f3e2 947 }
neoqased 0:bce22b52f3e2 948
neoqased 0:bce22b52f3e2 949