広大 目黑
/
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.
Diff: main.cpp
- Revision:
- 1:8a67adccebd9
- Parent:
- 0:d14e21c64226
- Child:
- 2:9dae549ae1b4
diff -r d14e21c64226 -r 8a67adccebd9 main.cpp
--- a/main.cpp Fri Sep 13 11:18:21 2019 +0000
+++ b/main.cpp Wed Sep 18 14:14:01 2019 +0000
@@ -3,7 +3,12 @@
#include "moterdrive.h"
#include "DualShockMod.h"
-#define PERIOD 100
+#define PERIOD 50
+
+#define hosei1 1.070 //left front
+#define hosei2 1.000 //right front
+#define hosei3 1.000 //back
+
DigitalOut my_led(LED1);
PwmOut md1_pwm1(PB_2);
@@ -18,7 +23,7 @@
PwmOut md2_pwm1(PC_7);
DigitalOut md2_cw1(PA_8);
-DigitalOut md2_ccw1(PA_3);
+DigitalOut md2_ccw1(PC_4);//ここ
DigitalOut md2_dis1(PB_10);
PwmOut md2_pwm2(PB_3);
@@ -47,15 +52,21 @@
Timer timer;
int main(){
- double posX = 0;
- double posY = 0;
typedef enum{
WAIT,
HARI1,
HARI2,
HARI3,
+ HARI4,
+ WAIT2,
SYM1,
+ SYM2,
+ SYM3,
+ SYM4,
+ SYM5,
+ SYM6,
+ SYM7,
END
}SEQENCE;
SEQENCE seq = WAIT;
@@ -68,87 +79,228 @@
LEFT,
RROLL,
LROLL,
- BACK
+ BACK,
+ UP,
+ DOWN
}MOVEDIR;
MOVEDIR move = STOP;
//エンコーダーの値
- pc.printf("posX=%d,posY=%d\n\r",posX,posY);
+ tsuushin.baud(115200);
uint8_t InitDS(Serial* f_serial);
- void getPOSdata(void);
- tsuushin.baud(115200);
+ void getPOSdata(void);
InitDS(&tsuushin);
tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始
- timer.start();
+ md1_pwm1.period_us(100);
+ md1_pwm2.period_us(100);
+ md2_pwm1.period_us(100);
+ md2_pwm2.period_us(100);
+
+ int pho1, pho2, pho3, lim1, lim2, lim3, lim4 = {0};
+
while(1){
+
+ pho1 = !photo1;
+ pho2 = !photo2;
+ pho3 = !photo3;
+ lim1 = Limit1;
+ lim2 = Limit2;
+ lim3 = Limit3;
+ lim4 = !Limit4;
+
+
+
+// pc.printf("ENC = %d \n\r", qei.getPulses());
+
//代入部
// posX = ;
// posY = ;
+
+
switch(seq){
case WAIT:
- if(timer.read_ms() > 3000){
- seq = HARI1;
- timer.reset();
- }
- break;
+ move = FFAST;
+
+ if(posY <= -1000 ){
+ move = FSLOW;
+ if(posY <= -1080)
+ seq = HARI1;
+ }
+
+ break;
+
case HARI1:
+ move = BACK;
+ if(posY <= -800)
+
+ break;
+
+
+ case HARI2:
+ move = DOWN;
+
+ break;
+
+ case HARI3:
+ move = BACK;
+ break;
+
+ case HARI4:
+ move = UP;
+ if(0){
+ seq = WAIT2;}
+
+ break;
+
+ case WAIT2:
+ move = STOP;
+
+ break;
+
+ case SYM1:
move = FFAST;
- break;
+
+ break;
+
+ case SYM2:
+ move = RIGHT;
+
+ if(0){
+ seq = SYM3;
+
+ }
+ break;
+
+ case SYM3:
+ move = FFAST;
+
+ if(0){
+ seq = SYM4;
+
+ }
+ break;
+ case SYM4:
+ move = LEFT;
+
+ if(0){
+ seq = SYM5;
+
+ }
+ break;
+
+ case SYM5:
+ move = LROLL;
+
+ if(0){
+ seq = SYM6;
+
+ }
+ break;
+
+ case SYM6:
+ move = DOWN;
+
+ if(0){
+ seq = SYM7;
+
+ }
+ break;
+
+ case SYM7:
+ move = BACK;
+
+ if(0){
+ seq = END;
+
+ }
+ break;
+
}
+
+
switch(move){
case STOP:
- moter(0,STOP,0);
moter(1,STOP,0);
moter(2,STOP,0);
- break;
+ moter(3,STOP,0);
+ moter(4,STOP,0);
+ break;
+
case FFAST:
- moter(0,STOP,0);
moter(1,cw,1);
moter(2,ccw,1);
- break;
+ moter(3,STOP,0);
+ moter(4,STOP,0);
+ break;
+
case FSLOW:
- moter(0,STOP,0);
moter(1,cw,0.5);
moter(2,ccw,0.5);
- break;
+ moter(3,STOP,0);
+ moter(4,STOP,0);
+ break;
+
case BACK:
- moter(0,STOP,0);
moter(1,ccw,1);
moter(2,cw,1);
- break;
+ moter(3,STOP,0);
+ moter(4,STOP,0);
+ break;
+
case RROLL:
- moter(0,ccw,0.3);
moter(1,ccw,0.3);
moter(2,ccw,0.3);
- break;
+ moter(3,ccw,0.3);
+ moter(4,STOP,0);
+ break;
+
case LROLL:
- moter(0,cw,0.3);
moter(1,cw,0.3);
moter(2,cw,0.3);
- break;
+ moter(3,cw,0.3);
+ moter(4,STOP,0);
+ break;
+
case RIGHT:
- moter(0,cw,1);
moter(1,ccw,0.5);
moter(2,ccw,0.5);
- break;
+ moter(3,cw,1);
+ moter(4,STOP,0);
+ break;
+
case LEFT:
- moter(0,ccw,1);
moter(1,cw,0.5);
moter(2,cw,0.5);
- break;
+ moter(3,ccw,1);
+ moter(4,STOP,0);
+ break;
+
+ case UP:
+ moter(1,STOP,0);
+ moter(2,STOP,0);
+ moter(3,STOP,0);
+ moter(4,cw,0.1);
+ break;
+
+ case DOWN:
+ moter(1,STOP,0);
+ moter(2,STOP,0);
+ moter(3,STOP,0);
+ moter(4,ccw,0.1);
+ break;
}
-
-
-
+ pc.printf("posX=%d,posY=%d\n\r", posX, posY);
+ pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\n\r", pho1, pho2, pho3, lim1, lim2, lim3, lim4);
+ pc.printf("seq = %d", seq);
}
}
@@ -157,7 +309,7 @@
void moter(int num, char dir, float power){
int output;
- if(num == 0){
+ if(num == 1){
if(dir == cw){
md1_cw1 = 1;
md1_ccw1 = 0;
@@ -170,10 +322,10 @@
md1_cw1 = 0;
md1_ccw1 = 0;
}
- output = PERIOD * power ;
- md1_pwm1.pulsewidth_ms(output);
+ output = PERIOD * power * hosei1;
+ md1_pwm1.pulsewidth_us(output);
}
- if(num == 1){
+ if(num == 2){
if(dir == cw){
md1_cw2 = 1;
md1_ccw2 = 0;
@@ -186,10 +338,10 @@
md1_cw2 = 0;
md1_ccw2 = 0;
}
- output = PERIOD * power ;
- md1_pwm2.pulsewidth_ms(output);
+ output = PERIOD * power * hosei2;
+ md1_pwm2.pulsewidth_us(output);
}
- if(num == 2){
+ if(num == 3){
if(dir == cw){
md2_cw1 = 1;
md2_ccw1 = 0;
@@ -202,10 +354,10 @@
md2_cw1 = 0;
md2_ccw1 = 0;
}
- output = PERIOD * power ;
- md2_pwm1.pulsewidth_ms(output);
+ output = PERIOD * power * hosei3;
+ md2_pwm1.pulsewidth_us(output);
}
- if(num == 3){
+ if(num == 4){
if(dir == cw){
md2_cw2 = 1;
md2_ccw2 = 0;
@@ -219,7 +371,7 @@
md2_ccw2 = 0;
}
output = PERIOD * power ;
- md2_pwm2.pulsewidth_ms(output);
+ md2_pwm2.pulsewidth_us(output);
}