広大 目黑 / 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@1:8a67adccebd9, 2019-09-18 (annotated)

Committer:
megu29
Date:
Wed Sep 18 14:14:01 2019 +0000
Revision:
1:8a67adccebd9
Parent:
0:d14e21c64226
Child:
2:9dae549ae1b4
gfds

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 1:8a67adccebd9 6 #define PERIOD 50
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 1:8a67adccebd9 10 #define hosei3 1.000 //back
megu29 1:8a67adccebd9 11
megu29 0:d14e21c64226 12
megu29 0:d14e21c64226 13 DigitalOut my_led(LED1);
megu29 0:d14e21c64226 14 PwmOut md1_pwm1(PB_2);
megu29 0:d14e21c64226 15 DigitalOut md1_cw1(PB_12);
megu29 0:d14e21c64226 16 DigitalOut md1_ccw1(PB_1);
megu29 0:d14e21c64226 17 DigitalOut md1_dis1(PA_11);
megu29 0:d14e21c64226 18
megu29 0:d14e21c64226 19 PwmOut md1_pwm2(PB_15);
megu29 0:d14e21c64226 20 DigitalOut md1_cw2(PA_12);
megu29 0:d14e21c64226 21 DigitalOut md1_ccw2(PB_14);
megu29 0:d14e21c64226 22 DigitalOut md1_dis2(PB_13);
megu29 0:d14e21c64226 23
megu29 0:d14e21c64226 24 PwmOut md2_pwm1(PC_7);
megu29 0:d14e21c64226 25 DigitalOut md2_cw1(PA_8);
megu29 1:8a67adccebd9 26 DigitalOut md2_ccw1(PC_4);//ここ
megu29 0:d14e21c64226 27 DigitalOut md2_dis1(PB_10);
megu29 0:d14e21c64226 28
megu29 0:d14e21c64226 29 PwmOut md2_pwm2(PB_3);
megu29 0:d14e21c64226 30 DigitalOut md2_cw2(PB_4);
megu29 0:d14e21c64226 31 DigitalOut md2_ccw2(PA_10);
megu29 0:d14e21c64226 32 DigitalOut md2_dis2(PB_5);
megu29 0:d14e21c64226 33
megu29 0:d14e21c64226 34 DigitalIn Limit1(PC_9);
megu29 0:d14e21c64226 35 DigitalIn Limit2(PC_8);
megu29 0:d14e21c64226 36 DigitalIn Limit3(PC_6);
megu29 0:d14e21c64226 37 DigitalIn Limit4(PC_5);
megu29 0:d14e21c64226 38
megu29 0:d14e21c64226 39 DigitalIn photo1(PA_6);
megu29 0:d14e21c64226 40 DigitalIn photo2(PA_7);
megu29 0:d14e21c64226 41 DigitalIn photo3(PB_6);
megu29 0:d14e21c64226 42
megu29 0:d14e21c64226 43 //serial通信
megu29 0:d14e21c64226 44 Serial pc(SERIAL_TX, SERIAL_RX);
megu29 0:d14e21c64226 45 Serial tsuushin(PC_10,PC_11);
megu29 0:d14e21c64226 46
megu29 0:d14e21c64226 47
megu29 0:d14e21c64226 48 /*プロトタイプ宣言*/
megu29 0:d14e21c64226 49 void moter(int num, char dir, float power);
megu29 0:d14e21c64226 50
megu29 0:d14e21c64226 51
megu29 0:d14e21c64226 52 Timer timer;
megu29 0:d14e21c64226 53
megu29 0:d14e21c64226 54 int main(){
megu29 0:d14e21c64226 55
megu29 0:d14e21c64226 56 typedef enum{
megu29 0:d14e21c64226 57 WAIT,
megu29 0:d14e21c64226 58 HARI1,
megu29 0:d14e21c64226 59 HARI2,
megu29 0:d14e21c64226 60 HARI3,
megu29 1:8a67adccebd9 61 HARI4,
megu29 1:8a67adccebd9 62 WAIT2,
megu29 0:d14e21c64226 63 SYM1,
megu29 1:8a67adccebd9 64 SYM2,
megu29 1:8a67adccebd9 65 SYM3,
megu29 1:8a67adccebd9 66 SYM4,
megu29 1:8a67adccebd9 67 SYM5,
megu29 1:8a67adccebd9 68 SYM6,
megu29 1:8a67adccebd9 69 SYM7,
megu29 0:d14e21c64226 70 END
megu29 0:d14e21c64226 71 }SEQENCE;
megu29 0:d14e21c64226 72 SEQENCE seq = WAIT;
megu29 0:d14e21c64226 73
megu29 0:d14e21c64226 74 typedef enum{
megu29 0:d14e21c64226 75 STOP,
megu29 0:d14e21c64226 76 FFAST,
megu29 0:d14e21c64226 77 FSLOW,
megu29 0:d14e21c64226 78 RIGHT,
megu29 0:d14e21c64226 79 LEFT,
megu29 0:d14e21c64226 80 RROLL,
megu29 0:d14e21c64226 81 LROLL,
megu29 1:8a67adccebd9 82 BACK,
megu29 1:8a67adccebd9 83 UP,
megu29 1:8a67adccebd9 84 DOWN
megu29 0:d14e21c64226 85 }MOVEDIR;
megu29 0:d14e21c64226 86 MOVEDIR move = STOP;
megu29 0:d14e21c64226 87
megu29 0:d14e21c64226 88 //エンコーダーの値
megu29 1:8a67adccebd9 89 tsuushin.baud(115200);
megu29 0:d14e21c64226 90 uint8_t InitDS(Serial* f_serial);
megu29 1:8a67adccebd9 91 void getPOSdata(void);
megu29 0:d14e21c64226 92 InitDS(&tsuushin);
megu29 0:d14e21c64226 93 tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始
megu29 0:d14e21c64226 94
megu29 0:d14e21c64226 95
megu29 1:8a67adccebd9 96 md1_pwm1.period_us(100);
megu29 1:8a67adccebd9 97 md1_pwm2.period_us(100);
megu29 1:8a67adccebd9 98 md2_pwm1.period_us(100);
megu29 1:8a67adccebd9 99 md2_pwm2.period_us(100);
megu29 1:8a67adccebd9 100
megu29 1:8a67adccebd9 101 int pho1, pho2, pho3, lim1, lim2, lim3, lim4 = {0};
megu29 1:8a67adccebd9 102
megu29 0:d14e21c64226 103 while(1){
megu29 1:8a67adccebd9 104
megu29 1:8a67adccebd9 105 pho1 = !photo1;
megu29 1:8a67adccebd9 106 pho2 = !photo2;
megu29 1:8a67adccebd9 107 pho3 = !photo3;
megu29 1:8a67adccebd9 108 lim1 = Limit1;
megu29 1:8a67adccebd9 109 lim2 = Limit2;
megu29 1:8a67adccebd9 110 lim3 = Limit3;
megu29 1:8a67adccebd9 111 lim4 = !Limit4;
megu29 1:8a67adccebd9 112
megu29 1:8a67adccebd9 113
megu29 1:8a67adccebd9 114
megu29 1:8a67adccebd9 115 // pc.printf("ENC = %d \n\r", qei.getPulses());
megu29 1:8a67adccebd9 116
megu29 0:d14e21c64226 117 //代入部
megu29 0:d14e21c64226 118 // posX = ;
megu29 0:d14e21c64226 119 // posY = ;
megu29 0:d14e21c64226 120
megu29 1:8a67adccebd9 121
megu29 1:8a67adccebd9 122
megu29 0:d14e21c64226 123 switch(seq){
megu29 0:d14e21c64226 124 case WAIT:
megu29 1:8a67adccebd9 125 move = FFAST;
megu29 1:8a67adccebd9 126
megu29 1:8a67adccebd9 127 if(posY <= -1000 ){
megu29 1:8a67adccebd9 128 move = FSLOW;
megu29 1:8a67adccebd9 129 if(posY <= -1080)
megu29 1:8a67adccebd9 130 seq = HARI1;
megu29 1:8a67adccebd9 131 }
megu29 1:8a67adccebd9 132
megu29 1:8a67adccebd9 133 break;
megu29 1:8a67adccebd9 134
megu29 0:d14e21c64226 135 case HARI1:
megu29 1:8a67adccebd9 136 move = BACK;
megu29 1:8a67adccebd9 137 if(posY <= -800)
megu29 1:8a67adccebd9 138
megu29 1:8a67adccebd9 139 break;
megu29 1:8a67adccebd9 140
megu29 1:8a67adccebd9 141
megu29 1:8a67adccebd9 142 case HARI2:
megu29 1:8a67adccebd9 143 move = DOWN;
megu29 1:8a67adccebd9 144
megu29 1:8a67adccebd9 145 break;
megu29 1:8a67adccebd9 146
megu29 1:8a67adccebd9 147 case HARI3:
megu29 1:8a67adccebd9 148 move = BACK;
megu29 1:8a67adccebd9 149 break;
megu29 1:8a67adccebd9 150
megu29 1:8a67adccebd9 151 case HARI4:
megu29 1:8a67adccebd9 152 move = UP;
megu29 1:8a67adccebd9 153 if(0){
megu29 1:8a67adccebd9 154 seq = WAIT2;}
megu29 1:8a67adccebd9 155
megu29 1:8a67adccebd9 156 break;
megu29 1:8a67adccebd9 157
megu29 1:8a67adccebd9 158 case WAIT2:
megu29 1:8a67adccebd9 159 move = STOP;
megu29 1:8a67adccebd9 160
megu29 1:8a67adccebd9 161 break;
megu29 1:8a67adccebd9 162
megu29 1:8a67adccebd9 163 case SYM1:
megu29 0:d14e21c64226 164 move = FFAST;
megu29 0:d14e21c64226 165
megu29 1:8a67adccebd9 166
megu29 1:8a67adccebd9 167 break;
megu29 1:8a67adccebd9 168
megu29 1:8a67adccebd9 169 case SYM2:
megu29 1:8a67adccebd9 170 move = RIGHT;
megu29 1:8a67adccebd9 171
megu29 1:8a67adccebd9 172 if(0){
megu29 1:8a67adccebd9 173 seq = SYM3;
megu29 1:8a67adccebd9 174
megu29 1:8a67adccebd9 175 }
megu29 1:8a67adccebd9 176 break;
megu29 1:8a67adccebd9 177
megu29 1:8a67adccebd9 178 case SYM3:
megu29 1:8a67adccebd9 179 move = FFAST;
megu29 1:8a67adccebd9 180
megu29 1:8a67adccebd9 181 if(0){
megu29 1:8a67adccebd9 182 seq = SYM4;
megu29 1:8a67adccebd9 183
megu29 1:8a67adccebd9 184 }
megu29 1:8a67adccebd9 185 break;
megu29 0:d14e21c64226 186
megu29 1:8a67adccebd9 187 case SYM4:
megu29 1:8a67adccebd9 188 move = LEFT;
megu29 1:8a67adccebd9 189
megu29 1:8a67adccebd9 190 if(0){
megu29 1:8a67adccebd9 191 seq = SYM5;
megu29 1:8a67adccebd9 192
megu29 1:8a67adccebd9 193 }
megu29 1:8a67adccebd9 194 break;
megu29 1:8a67adccebd9 195
megu29 1:8a67adccebd9 196 case SYM5:
megu29 1:8a67adccebd9 197 move = LROLL;
megu29 1:8a67adccebd9 198
megu29 1:8a67adccebd9 199 if(0){
megu29 1:8a67adccebd9 200 seq = SYM6;
megu29 1:8a67adccebd9 201
megu29 1:8a67adccebd9 202 }
megu29 1:8a67adccebd9 203 break;
megu29 1:8a67adccebd9 204
megu29 1:8a67adccebd9 205 case SYM6:
megu29 1:8a67adccebd9 206 move = DOWN;
megu29 1:8a67adccebd9 207
megu29 1:8a67adccebd9 208 if(0){
megu29 1:8a67adccebd9 209 seq = SYM7;
megu29 1:8a67adccebd9 210
megu29 1:8a67adccebd9 211 }
megu29 1:8a67adccebd9 212 break;
megu29 1:8a67adccebd9 213
megu29 1:8a67adccebd9 214 case SYM7:
megu29 1:8a67adccebd9 215 move = BACK;
megu29 1:8a67adccebd9 216
megu29 1:8a67adccebd9 217 if(0){
megu29 1:8a67adccebd9 218 seq = END;
megu29 1:8a67adccebd9 219
megu29 1:8a67adccebd9 220 }
megu29 1:8a67adccebd9 221 break;
megu29 1:8a67adccebd9 222
megu29 0:d14e21c64226 223 }
megu29 0:d14e21c64226 224
megu29 0:d14e21c64226 225
megu29 1:8a67adccebd9 226
megu29 1:8a67adccebd9 227
megu29 0:d14e21c64226 228 switch(move){
megu29 0:d14e21c64226 229 case STOP:
megu29 0:d14e21c64226 230 moter(1,STOP,0);
megu29 0:d14e21c64226 231 moter(2,STOP,0);
megu29 1:8a67adccebd9 232 moter(3,STOP,0);
megu29 1:8a67adccebd9 233 moter(4,STOP,0);
megu29 1:8a67adccebd9 234 break;
megu29 1:8a67adccebd9 235
megu29 0:d14e21c64226 236 case FFAST:
megu29 0:d14e21c64226 237 moter(1,cw,1);
megu29 0:d14e21c64226 238 moter(2,ccw,1);
megu29 1:8a67adccebd9 239 moter(3,STOP,0);
megu29 1:8a67adccebd9 240 moter(4,STOP,0);
megu29 1:8a67adccebd9 241 break;
megu29 1:8a67adccebd9 242
megu29 0:d14e21c64226 243 case FSLOW:
megu29 0:d14e21c64226 244 moter(1,cw,0.5);
megu29 0:d14e21c64226 245 moter(2,ccw,0.5);
megu29 1:8a67adccebd9 246 moter(3,STOP,0);
megu29 1:8a67adccebd9 247 moter(4,STOP,0);
megu29 1:8a67adccebd9 248 break;
megu29 1:8a67adccebd9 249
megu29 0:d14e21c64226 250 case BACK:
megu29 0:d14e21c64226 251 moter(1,ccw,1);
megu29 0:d14e21c64226 252 moter(2,cw,1);
megu29 1:8a67adccebd9 253 moter(3,STOP,0);
megu29 1:8a67adccebd9 254 moter(4,STOP,0);
megu29 1:8a67adccebd9 255 break;
megu29 1:8a67adccebd9 256
megu29 0:d14e21c64226 257 case RROLL:
megu29 0:d14e21c64226 258 moter(1,ccw,0.3);
megu29 0:d14e21c64226 259 moter(2,ccw,0.3);
megu29 1:8a67adccebd9 260 moter(3,ccw,0.3);
megu29 1:8a67adccebd9 261 moter(4,STOP,0);
megu29 1:8a67adccebd9 262 break;
megu29 1:8a67adccebd9 263
megu29 0:d14e21c64226 264 case LROLL:
megu29 0:d14e21c64226 265 moter(1,cw,0.3);
megu29 0:d14e21c64226 266 moter(2,cw,0.3);
megu29 1:8a67adccebd9 267 moter(3,cw,0.3);
megu29 1:8a67adccebd9 268 moter(4,STOP,0);
megu29 1:8a67adccebd9 269 break;
megu29 1:8a67adccebd9 270
megu29 0:d14e21c64226 271 case RIGHT:
megu29 0:d14e21c64226 272 moter(1,ccw,0.5);
megu29 0:d14e21c64226 273 moter(2,ccw,0.5);
megu29 1:8a67adccebd9 274 moter(3,cw,1);
megu29 1:8a67adccebd9 275 moter(4,STOP,0);
megu29 1:8a67adccebd9 276 break;
megu29 1:8a67adccebd9 277
megu29 0:d14e21c64226 278 case LEFT:
megu29 0:d14e21c64226 279 moter(1,cw,0.5);
megu29 0:d14e21c64226 280 moter(2,cw,0.5);
megu29 1:8a67adccebd9 281 moter(3,ccw,1);
megu29 1:8a67adccebd9 282 moter(4,STOP,0);
megu29 1:8a67adccebd9 283 break;
megu29 1:8a67adccebd9 284
megu29 1:8a67adccebd9 285 case UP:
megu29 1:8a67adccebd9 286 moter(1,STOP,0);
megu29 1:8a67adccebd9 287 moter(2,STOP,0);
megu29 1:8a67adccebd9 288 moter(3,STOP,0);
megu29 1:8a67adccebd9 289 moter(4,cw,0.1);
megu29 1:8a67adccebd9 290 break;
megu29 1:8a67adccebd9 291
megu29 1:8a67adccebd9 292 case DOWN:
megu29 1:8a67adccebd9 293 moter(1,STOP,0);
megu29 1:8a67adccebd9 294 moter(2,STOP,0);
megu29 1:8a67adccebd9 295 moter(3,STOP,0);
megu29 1:8a67adccebd9 296 moter(4,ccw,0.1);
megu29 1:8a67adccebd9 297 break;
megu29 0:d14e21c64226 298
megu29 0:d14e21c64226 299 }
megu29 0:d14e21c64226 300
megu29 1:8a67adccebd9 301 pc.printf("posX=%d,posY=%d\n\r", posX, posY);
megu29 1:8a67adccebd9 302 pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\n\r", pho1, pho2, pho3, lim1, lim2, lim3, lim4);
megu29 1:8a67adccebd9 303 pc.printf("seq = %d", seq);
megu29 0:d14e21c64226 304 }
megu29 0:d14e21c64226 305 }
megu29 0:d14e21c64226 306
megu29 0:d14e21c64226 307
megu29 0:d14e21c64226 308
megu29 0:d14e21c64226 309 void moter(int num, char dir, float power){
megu29 0:d14e21c64226 310
megu29 0:d14e21c64226 311 int output;
megu29 1:8a67adccebd9 312 if(num == 1){
megu29 0:d14e21c64226 313 if(dir == cw){
megu29 0:d14e21c64226 314 md1_cw1 = 1;
megu29 0:d14e21c64226 315 md1_ccw1 = 0;
megu29 0:d14e21c64226 316 }
megu29 0:d14e21c64226 317 else if(dir == ccw){
megu29 0:d14e21c64226 318 md1_cw1 = 0;
megu29 0:d14e21c64226 319 md1_ccw1 = 1;
megu29 0:d14e21c64226 320 }
megu29 0:d14e21c64226 321 else if(dir == STOP){
megu29 0:d14e21c64226 322 md1_cw1 = 0;
megu29 0:d14e21c64226 323 md1_ccw1 = 0;
megu29 0:d14e21c64226 324 }
megu29 1:8a67adccebd9 325 output = PERIOD * power * hosei1;
megu29 1:8a67adccebd9 326 md1_pwm1.pulsewidth_us(output);
megu29 0:d14e21c64226 327 }
megu29 1:8a67adccebd9 328 if(num == 2){
megu29 0:d14e21c64226 329 if(dir == cw){
megu29 0:d14e21c64226 330 md1_cw2 = 1;
megu29 0:d14e21c64226 331 md1_ccw2 = 0;
megu29 0:d14e21c64226 332 }
megu29 0:d14e21c64226 333 else if(dir == ccw){
megu29 0:d14e21c64226 334 md1_cw2 = 0;
megu29 0:d14e21c64226 335 md1_ccw2 = 1;
megu29 0:d14e21c64226 336 }
megu29 0:d14e21c64226 337 else if(dir == STOP){
megu29 0:d14e21c64226 338 md1_cw2 = 0;
megu29 0:d14e21c64226 339 md1_ccw2 = 0;
megu29 0:d14e21c64226 340 }
megu29 1:8a67adccebd9 341 output = PERIOD * power * hosei2;
megu29 1:8a67adccebd9 342 md1_pwm2.pulsewidth_us(output);
megu29 0:d14e21c64226 343 }
megu29 1:8a67adccebd9 344 if(num == 3){
megu29 0:d14e21c64226 345 if(dir == cw){
megu29 0:d14e21c64226 346 md2_cw1 = 1;
megu29 0:d14e21c64226 347 md2_ccw1 = 0;
megu29 0:d14e21c64226 348 }
megu29 0:d14e21c64226 349 else if(dir == ccw){
megu29 0:d14e21c64226 350 md2_cw1 = 0;
megu29 0:d14e21c64226 351 md2_ccw1 = 1;
megu29 0:d14e21c64226 352 }
megu29 0:d14e21c64226 353 else if(dir == STOP){
megu29 0:d14e21c64226 354 md2_cw1 = 0;
megu29 0:d14e21c64226 355 md2_ccw1 = 0;
megu29 0:d14e21c64226 356 }
megu29 1:8a67adccebd9 357 output = PERIOD * power * hosei3;
megu29 1:8a67adccebd9 358 md2_pwm1.pulsewidth_us(output);
megu29 0:d14e21c64226 359 }
megu29 1:8a67adccebd9 360 if(num == 4){
megu29 0:d14e21c64226 361 if(dir == cw){
megu29 0:d14e21c64226 362 md2_cw2 = 1;
megu29 0:d14e21c64226 363 md2_ccw2 = 0;
megu29 0:d14e21c64226 364 }
megu29 0:d14e21c64226 365 else if(dir == ccw){
megu29 0:d14e21c64226 366 md2_cw2 = 0;
megu29 0:d14e21c64226 367 md2_ccw2 = 1;
megu29 0:d14e21c64226 368 }
megu29 0:d14e21c64226 369 else if(dir == STOP){
megu29 0:d14e21c64226 370 md2_cw2 = 0;
megu29 0:d14e21c64226 371 md2_ccw2 = 0;
megu29 0:d14e21c64226 372 }
megu29 0:d14e21c64226 373 output = PERIOD * power ;
megu29 1:8a67adccebd9 374 md2_pwm2.pulsewidth_us(output);
megu29 0:d14e21c64226 375 }
megu29 0:d14e21c64226 376
megu29 0:d14e21c64226 377
megu29 0:d14e21c64226 378
megu29 0:d14e21c64226 379
megu29 0:d14e21c64226 380 }
megu29 0:d14e21c64226 381