広大 目黑 / Mbed 2 deprecated src2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@2:9dae549ae1b4, 2019-09-19 (annotated)

Committer:
megu29
Date:
Thu Sep 19 14:31:17 2019 +0000
Revision:
2:9dae549ae1b4
Parent:
1:8a67adccebd9 
azsxdc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megu29 0:d14e21c64226 1 #include "mbed.h"
megu29 0:d14e21c64226 2 #include "main.h"
megu29 0:d14e21c64226 3 #include "moterdrive.h"
megu29 0:d14e21c64226 4 #include "DualShockMod.h"
megu29 0:d14e21c64226 5
megu29 2:9dae549ae1b4 6 #define PERIOD 40
megu29 1:8a67adccebd9 7
megu29 1:8a67adccebd9 8 #define hosei1 1.070 //left front
megu29 1:8a67adccebd9 9 #define hosei2 1.000 //right front
megu29 2:9dae549ae1b4 10 #define hosei3 0.960 //back
megu29 1:8a67adccebd9 11
megu29 2:9dae549ae1b4 12 #define LIFTPOWER_UP 0.2
megu29 2:9dae549ae1b4 13 #define LIFTPOWER_DOWN 0.2
megu29 0:d14e21c64226 14
megu29 0:d14e21c64226 15 DigitalOut my_led(LED1);
megu29 0:d14e21c64226 16 PwmOut md1_pwm1(PB_2);
megu29 0:d14e21c64226 17 DigitalOut md1_cw1(PB_12);
megu29 0:d14e21c64226 18 DigitalOut md1_ccw1(PB_1);
megu29 0:d14e21c64226 19 DigitalOut md1_dis1(PA_11);
megu29 0:d14e21c64226 20
megu29 0:d14e21c64226 21 PwmOut md1_pwm2(PB_15);
megu29 0:d14e21c64226 22 DigitalOut md1_cw2(PA_12);
megu29 0:d14e21c64226 23 DigitalOut md1_ccw2(PB_14);
megu29 0:d14e21c64226 24 DigitalOut md1_dis2(PB_13);
megu29 0:d14e21c64226 25
megu29 0:d14e21c64226 26 PwmOut md2_pwm1(PC_7);
megu29 0:d14e21c64226 27 DigitalOut md2_cw1(PA_8);
megu29 1:8a67adccebd9 28 DigitalOut md2_ccw1(PC_4);//ここ
megu29 0:d14e21c64226 29 DigitalOut md2_dis1(PB_10);
megu29 0:d14e21c64226 30
megu29 0:d14e21c64226 31 PwmOut md2_pwm2(PB_3);
megu29 0:d14e21c64226 32 DigitalOut md2_cw2(PB_4);
megu29 0:d14e21c64226 33 DigitalOut md2_ccw2(PA_10);
megu29 0:d14e21c64226 34 DigitalOut md2_dis2(PB_5);
megu29 0:d14e21c64226 35
megu29 2:9dae549ae1b4 36 DigitalIn Limit1(PC_9); //back
megu29 2:9dae549ae1b4 37 DigitalIn Limit2(PC_8); //right
megu29 0:d14e21c64226 38 DigitalIn Limit3(PC_6);
megu29 0:d14e21c64226 39 DigitalIn Limit4(PC_5);
megu29 0:d14e21c64226 40
megu29 2:9dae549ae1b4 41 DigitalIn photo1(PA_13);
megu29 2:9dae549ae1b4 42 DigitalIn photo2(PA_14);
megu29 2:9dae549ae1b4 43 DigitalIn photo3(PA_15);
megu29 2:9dae549ae1b4 44
megu29 2:9dae549ae1b4 45 DigitalIn button1(USER_BUTTON);
megu29 2:9dae549ae1b4 46 DigitalOut led1(D14);
megu29 2:9dae549ae1b4 47 DigitalOut led2(D15);
megu29 0:d14e21c64226 48
megu29 0:d14e21c64226 49 //serial通信
megu29 0:d14e21c64226 50 Serial pc(SERIAL_TX, SERIAL_RX);
megu29 0:d14e21c64226 51 Serial tsuushin(PC_10,PC_11);
megu29 0:d14e21c64226 52
megu29 0:d14e21c64226 53
megu29 0:d14e21c64226 54 /*プロトタイプ宣言*/
megu29 0:d14e21c64226 55 void moter(int num, char dir, float power);
megu29 0:d14e21c64226 56
megu29 0:d14e21c64226 57
megu29 0:d14e21c64226 58 Timer timer;
megu29 0:d14e21c64226 59
megu29 0:d14e21c64226 60 int main(){
megu29 0:d14e21c64226 61
megu29 0:d14e21c64226 62 typedef enum{
megu29 0:d14e21c64226 63 WAIT,
megu29 2:9dae549ae1b4 64 INIT1,
megu29 2:9dae549ae1b4 65 INIT2,
megu29 2:9dae549ae1b4 66 WAIT2,
megu29 0:d14e21c64226 67 HARI1,
megu29 0:d14e21c64226 68 HARI2,
megu29 0:d14e21c64226 69 HARI3,
megu29 1:8a67adccebd9 70 HARI4,
megu29 2:9dae549ae1b4 71 HARI_END,
megu29 0:d14e21c64226 72 SYM1,
megu29 1:8a67adccebd9 73 SYM2,
megu29 1:8a67adccebd9 74 SYM3,
megu29 1:8a67adccebd9 75 SYM4,
megu29 1:8a67adccebd9 76 SYM5,
megu29 1:8a67adccebd9 77 SYM6,
megu29 1:8a67adccebd9 78 SYM7,
megu29 2:9dae549ae1b4 79 SYM8,
megu29 2:9dae549ae1b4 80 END,
megu29 2:9dae549ae1b4 81 DEBUG
megu29 0:d14e21c64226 82 }SEQENCE;
megu29 0:d14e21c64226 83 SEQENCE seq = WAIT;
megu29 0:d14e21c64226 84
megu29 0:d14e21c64226 85 typedef enum{
megu29 0:d14e21c64226 86 STOP,
megu29 0:d14e21c64226 87 FFAST,
megu29 0:d14e21c64226 88 FSLOW,
megu29 2:9dae549ae1b4 89 BFAST,
megu29 2:9dae549ae1b4 90 BSLOW,
megu29 0:d14e21c64226 91 RIGHT,
megu29 0:d14e21c64226 92 LEFT,
megu29 0:d14e21c64226 93 RROLL,
megu29 0:d14e21c64226 94 LROLL,
megu29 1:8a67adccebd9 95 UP,
megu29 1:8a67adccebd9 96 DOWN
megu29 0:d14e21c64226 97 }MOVEDIR;
megu29 0:d14e21c64226 98 MOVEDIR move = STOP;
megu29 0:d14e21c64226 99
megu29 0:d14e21c64226 100 //エンコーダーの値
megu29 1:8a67adccebd9 101 tsuushin.baud(115200);
megu29 0:d14e21c64226 102 uint8_t InitDS(Serial* f_serial);
megu29 1:8a67adccebd9 103 void getPOSdata(void);
megu29 0:d14e21c64226 104 InitDS(&tsuushin);
megu29 0:d14e21c64226 105 tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始
megu29 0:d14e21c64226 106
megu29 0:d14e21c64226 107
megu29 1:8a67adccebd9 108 md1_pwm1.period_us(100);
megu29 1:8a67adccebd9 109 md1_pwm2.period_us(100);
megu29 1:8a67adccebd9 110 md2_pwm1.period_us(100);
megu29 1:8a67adccebd9 111 md2_pwm2.period_us(100);
megu29 1:8a67adccebd9 112
megu29 2:9dae549ae1b4 113 int pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1,k = {0};
megu29 1:8a67adccebd9 114
megu29 0:d14e21c64226 115 while(1){
megu29 1:8a67adccebd9 116
megu29 1:8a67adccebd9 117 pho1 = !photo1;
megu29 1:8a67adccebd9 118 pho2 = !photo2;
megu29 1:8a67adccebd9 119 pho3 = !photo3;
megu29 1:8a67adccebd9 120 lim1 = Limit1;
megu29 1:8a67adccebd9 121 lim2 = Limit2;
megu29 1:8a67adccebd9 122 lim3 = Limit3;
megu29 1:8a67adccebd9 123 lim4 = !Limit4;
megu29 2:9dae549ae1b4 124 btn1 = !button1;
megu29 1:8a67adccebd9 125
megu29 1:8a67adccebd9 126
megu29 1:8a67adccebd9 127 // pc.printf("ENC = %d \n\r", qei.getPulses());
megu29 1:8a67adccebd9 128
megu29 0:d14e21c64226 129 //代入部
megu29 0:d14e21c64226 130 // posX = ;
megu29 0:d14e21c64226 131 // posY = ;
megu29 0:d14e21c64226 132
megu29 2:9dae549ae1b4 133 if(move == STOP) led1 = 1;
megu29 2:9dae549ae1b4 134 else led1 = 0;
megu29 1:8a67adccebd9 135
megu29 2:9dae549ae1b4 136 if(lim4 == 1) seq = DEBUG;
megu29 1:8a67adccebd9 137
megu29 0:d14e21c64226 138 switch(seq){
megu29 0:d14e21c64226 139 case WAIT:
megu29 2:9dae549ae1b4 140 seq = INIT1;
megu29 2:9dae549ae1b4 141
megu29 2:9dae549ae1b4 142 break;
megu29 1:8a67adccebd9 143
megu29 2:9dae549ae1b4 144 case INIT1:
megu29 2:9dae549ae1b4 145 move = DOWN;
megu29 2:9dae549ae1b4 146
megu29 2:9dae549ae1b4 147 if(pho1 == 1){
megu29 2:9dae549ae1b4 148 move = STOP;
megu29 2:9dae549ae1b4 149 seq = INIT2;
megu29 2:9dae549ae1b4 150 }
megu29 2:9dae549ae1b4 151
megu29 2:9dae549ae1b4 152 break;
megu29 2:9dae549ae1b4 153
megu29 2:9dae549ae1b4 154 case INIT2:
megu29 2:9dae549ae1b4 155 timer.start();
megu29 2:9dae549ae1b4 156 move = UP;
megu29 2:9dae549ae1b4 157 if(timer.read_ms() > 2500){
megu29 2:9dae549ae1b4 158 move = STOP;
megu29 2:9dae549ae1b4 159 timer.reset();
megu29 2:9dae549ae1b4 160 timer.stop();
megu29 2:9dae549ae1b4 161 seq = WAIT2;
megu29 2:9dae549ae1b4 162 }
megu29 2:9dae549ae1b4 163 break;
megu29 2:9dae549ae1b4 164
megu29 2:9dae549ae1b4 165 case WAIT2:
megu29 2:9dae549ae1b4 166 move = STOP;
megu29 2:9dae549ae1b4 167
megu29 2:9dae549ae1b4 168 if(btn1 == 1){
megu29 1:8a67adccebd9 169 seq = HARI1;
megu29 2:9dae549ae1b4 170 }
megu29 2:9dae549ae1b4 171 if(lim2 == 1){
megu29 2:9dae549ae1b4 172 seq = SYM1;
megu29 2:9dae549ae1b4 173 }
megu29 1:8a67adccebd9 174 break;
megu29 1:8a67adccebd9 175
megu29 0:d14e21c64226 176 case HARI1:
megu29 2:9dae549ae1b4 177 move = FFAST;
megu29 2:9dae549ae1b4 178
megu29 2:9dae549ae1b4 179 if(posY > 1000 ){
megu29 2:9dae549ae1b4 180 move = FSLOW;
megu29 2:9dae549ae1b4 181 if(posY > 1080)
megu29 2:9dae549ae1b4 182 seq = HARI2;
megu29 2:9dae549ae1b4 183 }
megu29 1:8a67adccebd9 184 break;
megu29 1:8a67adccebd9 185
megu29 1:8a67adccebd9 186 case HARI2:
megu29 1:8a67adccebd9 187 move = DOWN;
megu29 1:8a67adccebd9 188
megu29 2:9dae549ae1b4 189 if(pho1 == 1){
megu29 2:9dae549ae1b4 190 move = STOP;
megu29 2:9dae549ae1b4 191 seq = HARI3;
megu29 2:9dae549ae1b4 192 }
megu29 1:8a67adccebd9 193 break;
megu29 1:8a67adccebd9 194
megu29 1:8a67adccebd9 195 case HARI3:
megu29 2:9dae549ae1b4 196 move = BFAST;
megu29 2:9dae549ae1b4 197
megu29 2:9dae549ae1b4 198 if(posY < 800){
megu29 2:9dae549ae1b4 199 seq = HARI4;
megu29 2:9dae549ae1b4 200 timer.reset();
megu29 2:9dae549ae1b4 201 }
megu29 1:8a67adccebd9 202 break;
megu29 1:8a67adccebd9 203
megu29 1:8a67adccebd9 204 case HARI4:
megu29 2:9dae549ae1b4 205 timer.start();
megu29 1:8a67adccebd9 206 move = UP;
megu29 2:9dae549ae1b4 207
megu29 2:9dae549ae1b4 208 if(timer.read_ms() > 2000){
megu29 2:9dae549ae1b4 209 move = STOP;
megu29 2:9dae549ae1b4 210 timer.reset();
megu29 2:9dae549ae1b4 211 seq = HARI_END;
megu29 2:9dae549ae1b4 212 }
megu29 1:8a67adccebd9 213
megu29 1:8a67adccebd9 214 break;
megu29 1:8a67adccebd9 215
megu29 2:9dae549ae1b4 216 case HARI_END:
megu29 1:8a67adccebd9 217 move = STOP;
megu29 1:8a67adccebd9 218
megu29 1:8a67adccebd9 219 break;
megu29 1:8a67adccebd9 220
megu29 1:8a67adccebd9 221 case SYM1:
megu29 0:d14e21c64226 222 move = FFAST;
megu29 0:d14e21c64226 223
megu29 2:9dae549ae1b4 224 if(posY > 750 ){
megu29 2:9dae549ae1b4 225 move = FSLOW;
megu29 2:9dae549ae1b4 226 if(posY > 800)
megu29 2:9dae549ae1b4 227 seq = SYM2;
megu29 2:9dae549ae1b4 228 }
megu29 1:8a67adccebd9 229 break;
megu29 1:8a67adccebd9 230
megu29 1:8a67adccebd9 231 case SYM2:
megu29 1:8a67adccebd9 232 move = RIGHT;
megu29 1:8a67adccebd9 233
megu29 2:9dae549ae1b4 234 if(posX > 850){
megu29 1:8a67adccebd9 235 seq = SYM3;
megu29 1:8a67adccebd9 236 }
megu29 1:8a67adccebd9 237 break;
megu29 1:8a67adccebd9 238
megu29 1:8a67adccebd9 239 case SYM3:
megu29 2:9dae549ae1b4 240 move = BSLOW;
megu29 1:8a67adccebd9 241
megu29 2:9dae549ae1b4 242 if(lim1 == 1){
megu29 2:9dae549ae1b4 243 move = FFAST;
megu29 1:8a67adccebd9 244 seq = SYM4;
megu29 2:9dae549ae1b4 245 k = posY;
megu29 1:8a67adccebd9 246 }
megu29 1:8a67adccebd9 247 break;
megu29 0:d14e21c64226 248
megu29 1:8a67adccebd9 249 case SYM4:
megu29 2:9dae549ae1b4 250 move = FFAST;
megu29 1:8a67adccebd9 251
megu29 2:9dae549ae1b4 252 if( posY - k >360){
megu29 1:8a67adccebd9 253 seq = SYM5;
megu29 2:9dae549ae1b4 254 }
megu29 1:8a67adccebd9 255 break;
megu29 1:8a67adccebd9 256
megu29 1:8a67adccebd9 257 case SYM5:
megu29 2:9dae549ae1b4 258 move = LEFT;
megu29 1:8a67adccebd9 259
megu29 2:9dae549ae1b4 260 if(posX < 300){
megu29 1:8a67adccebd9 261 seq = SYM6;
megu29 2:9dae549ae1b4 262 }
megu29 1:8a67adccebd9 263 break;
megu29 1:8a67adccebd9 264
megu29 1:8a67adccebd9 265 case SYM6:
megu29 2:9dae549ae1b4 266 move = LROLL;
megu29 1:8a67adccebd9 267
megu29 2:9dae549ae1b4 268 timer.start();
megu29 2:9dae549ae1b4 269 if(timer.read_ms() > 5000){
megu29 2:9dae549ae1b4 270 move = STOP;
megu29 2:9dae549ae1b4 271 timer.reset();
megu29 2:9dae549ae1b4 272 timer.stop();
megu29 1:8a67adccebd9 273 seq = SYM7;
megu29 1:8a67adccebd9 274 }
megu29 1:8a67adccebd9 275 break;
megu29 1:8a67adccebd9 276
megu29 1:8a67adccebd9 277 case SYM7:
megu29 2:9dae549ae1b4 278 move = DOWN;
megu29 1:8a67adccebd9 279
megu29 2:9dae549ae1b4 280 if(pho1 == 1){
megu29 2:9dae549ae1b4 281 move = STOP;
megu29 2:9dae549ae1b4 282 seq = SYM8;
megu29 1:8a67adccebd9 283 }
megu29 1:8a67adccebd9 284 break;
megu29 1:8a67adccebd9 285
megu29 2:9dae549ae1b4 286 case SYM8:
megu29 2:9dae549ae1b4 287 move = BFAST;
megu29 2:9dae549ae1b4 288 timer.start();
megu29 2:9dae549ae1b4 289 if(timer.read_ms() > 3000){
megu29 2:9dae549ae1b4 290 move = STOP;
megu29 2:9dae549ae1b4 291 timer.reset();
megu29 2:9dae549ae1b4 292 timer.stop();
megu29 2:9dae549ae1b4 293
megu29 2:9dae549ae1b4 294 seq = END;
megu29 2:9dae549ae1b4 295 }
megu29 2:9dae549ae1b4 296 break;
megu29 2:9dae549ae1b4 297
megu29 2:9dae549ae1b4 298 case DEBUG:
megu29 2:9dae549ae1b4 299 if(lim1 == 1) move = UP;
megu29 2:9dae549ae1b4 300 else if (lim2 == 1) move = DOWN;
megu29 2:9dae549ae1b4 301 else move = STOP;
megu29 2:9dae549ae1b4 302 break;
megu29 2:9dae549ae1b4 303
megu29 0:d14e21c64226 304 }
megu29 0:d14e21c64226 305
megu29 0:d14e21c64226 306
megu29 1:8a67adccebd9 307
megu29 1:8a67adccebd9 308
megu29 0:d14e21c64226 309 switch(move){
megu29 0:d14e21c64226 310 case STOP:
megu29 0:d14e21c64226 311 moter(1,STOP,0);
megu29 0:d14e21c64226 312 moter(2,STOP,0);
megu29 1:8a67adccebd9 313 moter(3,STOP,0);
megu29 1:8a67adccebd9 314 moter(4,STOP,0);
megu29 1:8a67adccebd9 315 break;
megu29 1:8a67adccebd9 316
megu29 0:d14e21c64226 317 case FFAST:
megu29 0:d14e21c64226 318 moter(1,cw,1);
megu29 0:d14e21c64226 319 moter(2,ccw,1);
megu29 1:8a67adccebd9 320 moter(3,STOP,0);
megu29 1:8a67adccebd9 321 moter(4,STOP,0);
megu29 1:8a67adccebd9 322 break;
megu29 1:8a67adccebd9 323
megu29 0:d14e21c64226 324 case FSLOW:
megu29 0:d14e21c64226 325 moter(1,cw,0.5);
megu29 0:d14e21c64226 326 moter(2,ccw,0.5);
megu29 1:8a67adccebd9 327 moter(3,STOP,0);
megu29 1:8a67adccebd9 328 moter(4,STOP,0);
megu29 1:8a67adccebd9 329 break;
megu29 1:8a67adccebd9 330
megu29 2:9dae549ae1b4 331 case BFAST:
megu29 0:d14e21c64226 332 moter(1,ccw,1);
megu29 0:d14e21c64226 333 moter(2,cw,1);
megu29 1:8a67adccebd9 334 moter(3,STOP,0);
megu29 1:8a67adccebd9 335 moter(4,STOP,0);
megu29 1:8a67adccebd9 336 break;
megu29 2:9dae549ae1b4 337
megu29 2:9dae549ae1b4 338 case BSLOW:
megu29 2:9dae549ae1b4 339 moter(1,ccw,0.5);
megu29 2:9dae549ae1b4 340 moter(2,cw,0.5);
megu29 2:9dae549ae1b4 341 moter(3,STOP,0);
megu29 2:9dae549ae1b4 342 moter(4,STOP,0);
megu29 2:9dae549ae1b4 343 break;
megu29 1:8a67adccebd9 344
megu29 0:d14e21c64226 345 case RROLL:
megu29 2:9dae549ae1b4 346 moter(1,cw,0.3);
megu29 2:9dae549ae1b4 347 moter(2,cw,0.3);
megu29 1:8a67adccebd9 348 moter(3,ccw,0.3);
megu29 1:8a67adccebd9 349 moter(4,STOP,0);
megu29 1:8a67adccebd9 350 break;
megu29 1:8a67adccebd9 351
megu29 0:d14e21c64226 352 case LROLL:
megu29 2:9dae549ae1b4 353 moter(1,ccw,0.3);
megu29 2:9dae549ae1b4 354 moter(2,ccw,0.3);
megu29 1:8a67adccebd9 355 moter(3,cw,0.3);
megu29 1:8a67adccebd9 356 moter(4,STOP,0);
megu29 1:8a67adccebd9 357 break;
megu29 1:8a67adccebd9 358
megu29 0:d14e21c64226 359 case RIGHT:
megu29 2:9dae549ae1b4 360 moter(1,cw,0.5);
megu29 2:9dae549ae1b4 361 moter(2,cw,0.5);
megu29 1:8a67adccebd9 362 moter(3,cw,1);
megu29 1:8a67adccebd9 363 moter(4,STOP,0);
megu29 1:8a67adccebd9 364 break;
megu29 1:8a67adccebd9 365
megu29 0:d14e21c64226 366 case LEFT:
megu29 2:9dae549ae1b4 367 moter(1,ccw,0.5);
megu29 2:9dae549ae1b4 368 moter(2,ccw,0.5);
megu29 1:8a67adccebd9 369 moter(3,ccw,1);
megu29 1:8a67adccebd9 370 moter(4,STOP,0);
megu29 1:8a67adccebd9 371 break;
megu29 1:8a67adccebd9 372
megu29 1:8a67adccebd9 373 case UP:
megu29 1:8a67adccebd9 374 moter(1,STOP,0);
megu29 1:8a67adccebd9 375 moter(2,STOP,0);
megu29 1:8a67adccebd9 376 moter(3,STOP,0);
megu29 2:9dae549ae1b4 377 moter(4,cw,LIFTPOWER_UP);
megu29 1:8a67adccebd9 378 break;
megu29 1:8a67adccebd9 379
megu29 1:8a67adccebd9 380 case DOWN:
megu29 1:8a67adccebd9 381 moter(1,STOP,0);
megu29 1:8a67adccebd9 382 moter(2,STOP,0);
megu29 1:8a67adccebd9 383 moter(3,STOP,0);
megu29 2:9dae549ae1b4 384 moter(4,ccw,LIFTPOWER_DOWN);
megu29 1:8a67adccebd9 385 break;
megu29 0:d14e21c64226 386
megu29 0:d14e21c64226 387 }
megu29 0:d14e21c64226 388
megu29 2:9dae549ae1b4 389 pc.printf("posX=%d,posY=%d ", posX, posY);
megu29 2:9dae549ae1b4 390 pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\t BTN=%d ", pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1);
megu29 2:9dae549ae1b4 391 pc.printf("seq = %d\n\r", seq);
megu29 0:d14e21c64226 392 }
megu29 0:d14e21c64226 393 }
megu29 0:d14e21c64226 394
megu29 0:d14e21c64226 395
megu29 0:d14e21c64226 396
megu29 0:d14e21c64226 397 void moter(int num, char dir, float power){
megu29 2:9dae549ae1b4 398 int pho1 = !photo1;
megu29 0:d14e21c64226 399 int output;
megu29 1:8a67adccebd9 400 if(num == 1){
megu29 0:d14e21c64226 401 if(dir == cw){
megu29 0:d14e21c64226 402 md1_cw1 = 1;
megu29 0:d14e21c64226 403 md1_ccw1 = 0;
megu29 0:d14e21c64226 404 }
megu29 0:d14e21c64226 405 else if(dir == ccw){
megu29 0:d14e21c64226 406 md1_cw1 = 0;
megu29 0:d14e21c64226 407 md1_ccw1 = 1;
megu29 0:d14e21c64226 408 }
megu29 0:d14e21c64226 409 else if(dir == STOP){
megu29 0:d14e21c64226 410 md1_cw1 = 0;
megu29 0:d14e21c64226 411 md1_ccw1 = 0;
megu29 0:d14e21c64226 412 }
megu29 2:9dae549ae1b4 413 output = PERIOD * power * (float)hosei1;
megu29 1:8a67adccebd9 414 md1_pwm1.pulsewidth_us(output);
megu29 0:d14e21c64226 415 }
megu29 1:8a67adccebd9 416 if(num == 2){
megu29 0:d14e21c64226 417 if(dir == cw){
megu29 0:d14e21c64226 418 md1_cw2 = 1;
megu29 0:d14e21c64226 419 md1_ccw2 = 0;
megu29 0:d14e21c64226 420 }
megu29 0:d14e21c64226 421 else if(dir == ccw){
megu29 0:d14e21c64226 422 md1_cw2 = 0;
megu29 0:d14e21c64226 423 md1_ccw2 = 1;
megu29 0:d14e21c64226 424 }
megu29 0:d14e21c64226 425 else if(dir == STOP){
megu29 0:d14e21c64226 426 md1_cw2 = 0;
megu29 0:d14e21c64226 427 md1_ccw2 = 0;
megu29 0:d14e21c64226 428 }
megu29 2:9dae549ae1b4 429 output = PERIOD * power * (float)hosei2;
megu29 1:8a67adccebd9 430 md1_pwm2.pulsewidth_us(output);
megu29 0:d14e21c64226 431 }
megu29 1:8a67adccebd9 432 if(num == 3){
megu29 0:d14e21c64226 433 if(dir == cw){
megu29 0:d14e21c64226 434 md2_cw1 = 1;
megu29 0:d14e21c64226 435 md2_ccw1 = 0;
megu29 0:d14e21c64226 436 }
megu29 0:d14e21c64226 437 else if(dir == ccw){
megu29 0:d14e21c64226 438 md2_cw1 = 0;
megu29 0:d14e21c64226 439 md2_ccw1 = 1;
megu29 0:d14e21c64226 440 }
megu29 0:d14e21c64226 441 else if(dir == STOP){
megu29 0:d14e21c64226 442 md2_cw1 = 0;
megu29 0:d14e21c64226 443 md2_ccw1 = 0;
megu29 0:d14e21c64226 444 }
megu29 2:9dae549ae1b4 445 output = PERIOD * power * (float)hosei3;
megu29 1:8a67adccebd9 446 md2_pwm1.pulsewidth_us(output);
megu29 0:d14e21c64226 447 }
megu29 1:8a67adccebd9 448 if(num == 4){
megu29 0:d14e21c64226 449 if(dir == cw){
megu29 0:d14e21c64226 450 md2_cw2 = 1;
megu29 0:d14e21c64226 451 md2_ccw2 = 0;
megu29 0:d14e21c64226 452 }
megu29 0:d14e21c64226 453 else if(dir == ccw){
megu29 0:d14e21c64226 454 md2_cw2 = 0;
megu29 2:9dae549ae1b4 455
megu29 2:9dae549ae1b4 456 if(pho1 == 1){
megu29 2:9dae549ae1b4 457 md2_ccw2 = 0;
megu29 2:9dae549ae1b4 458 }
megu29 2:9dae549ae1b4 459 else{
megu29 2:9dae549ae1b4 460 md2_ccw2 = 1;
megu29 2:9dae549ae1b4 461 }
megu29 0:d14e21c64226 462 }
megu29 0:d14e21c64226 463 else if(dir == STOP){
megu29 0:d14e21c64226 464 md2_cw2 = 0;
megu29 0:d14e21c64226 465 md2_ccw2 = 0;
megu29 0:d14e21c64226 466 }
megu29 2:9dae549ae1b4 467 output = 100 * power ;
megu29 1:8a67adccebd9 468 md2_pwm2.pulsewidth_us(output);
megu29 2:9dae549ae1b4 469 }
megu29 0:d14e21c64226 470 }
megu29 0:d14e21c64226 471