広大 目黑
/
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.
Revision 2:9dae549ae1b4, committed 2019-09-19
- Comitter:
- megu29
- Date:
- Thu Sep 19 14:31:17 2019 +0000
- Parent:
- 1:8a67adccebd9
- Commit message:
- azsxdc
Changed in this revision
| DualShockMod.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/DualShockMod.cpp Wed Sep 18 14:14:01 2019 +0000
+++ b/DualShockMod.cpp Thu Sep 19 14:31:17 2019 +0000
@@ -52,10 +52,10 @@
posX = ((dat[1]<<7) + dat[2]);
}
if(dat[0] & 0b00000001){ //Yが負であるビット
- posY = (int)((dat[3]<<7) + dat[4])*-1;
+ posY = (int)((dat[3]<<7) + dat[4]);
}
else{
- posY = (dat[3]<<7) + dat[4];
+ posY = (dat[3]<<7) + dat[4]*-1;
}
}
dat[0] = 0;
--- a/main.cpp Wed Sep 18 14:14:01 2019 +0000
+++ b/main.cpp Thu Sep 19 14:31:17 2019 +0000
@@ -3,12 +3,14 @@
#include "moterdrive.h"
#include "DualShockMod.h"
-#define PERIOD 50
+#define PERIOD 40
#define hosei1 1.070 //left front
#define hosei2 1.000 //right front
-#define hosei3 1.000 //back
+#define hosei3 0.960 //back
+#define LIFTPOWER_UP 0.2
+#define LIFTPOWER_DOWN 0.2
DigitalOut my_led(LED1);
PwmOut md1_pwm1(PB_2);
@@ -31,14 +33,18 @@
DigitalOut md2_ccw2(PA_10);
DigitalOut md2_dis2(PB_5);
-DigitalIn Limit1(PC_9);
-DigitalIn Limit2(PC_8);
+DigitalIn Limit1(PC_9); //back
+DigitalIn Limit2(PC_8); //right
DigitalIn Limit3(PC_6);
DigitalIn Limit4(PC_5);
-DigitalIn photo1(PA_6);
-DigitalIn photo2(PA_7);
-DigitalIn photo3(PB_6);
+DigitalIn photo1(PA_13);
+DigitalIn photo2(PA_14);
+DigitalIn photo3(PA_15);
+
+DigitalIn button1(USER_BUTTON);
+DigitalOut led1(D14);
+DigitalOut led2(D15);
//serial通信
Serial pc(SERIAL_TX, SERIAL_RX);
@@ -55,11 +61,14 @@
typedef enum{
WAIT,
+ INIT1,
+ INIT2,
+ WAIT2,
HARI1,
HARI2,
HARI3,
HARI4,
- WAIT2,
+ HARI_END,
SYM1,
SYM2,
SYM3,
@@ -67,7 +76,9 @@
SYM5,
SYM6,
SYM7,
- END
+ SYM8,
+ END,
+ DEBUG
}SEQENCE;
SEQENCE seq = WAIT;
@@ -75,11 +86,12 @@
STOP,
FFAST,
FSLOW,
+ BFAST,
+ BSLOW,
RIGHT,
LEFT,
RROLL,
LROLL,
- BACK,
UP,
DOWN
}MOVEDIR;
@@ -98,7 +110,7 @@
md2_pwm1.period_us(100);
md2_pwm2.period_us(100);
- int pho1, pho2, pho3, lim1, lim2, lim3, lim4 = {0};
+ int pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1,k = {0};
while(1){
@@ -109,7 +121,7 @@
lim2 = Limit2;
lim3 = Limit3;
lim4 = !Limit4;
-
+ btn1 = !button1;
// pc.printf("ENC = %d \n\r", qei.getPulses());
@@ -118,44 +130,90 @@
// posX = ;
// posY = ;
+ if(move == STOP) led1 = 1;
+ else led1 = 0;
+ if(lim4 == 1) seq = DEBUG;
switch(seq){
case WAIT:
- move = FFAST;
+ seq = INIT1;
+
+ break;
- if(posY <= -1000 ){
- move = FSLOW;
- if(posY <= -1080)
+ case INIT1:
+ move = DOWN;
+
+ if(pho1 == 1){
+ move = STOP;
+ seq = INIT2;
+ }
+
+ break;
+
+ case INIT2:
+ timer.start();
+ move = UP;
+ if(timer.read_ms() > 2500){
+ move = STOP;
+ timer.reset();
+ timer.stop();
+ seq = WAIT2;
+ }
+ break;
+
+ case WAIT2:
+ move = STOP;
+
+ if(btn1 == 1){
seq = HARI1;
- }
-
+ }
+ if(lim2 == 1){
+ seq = SYM1;
+ }
break;
case HARI1:
- move = BACK;
- if(posY <= -800)
-
+ move = FFAST;
+
+ if(posY > 1000 ){
+ move = FSLOW;
+ if(posY > 1080)
+ seq = HARI2;
+ }
break;
-
case HARI2:
move = DOWN;
+ if(pho1 == 1){
+ move = STOP;
+ seq = HARI3;
+ }
break;
case HARI3:
- move = BACK;
+ move = BFAST;
+
+ if(posY < 800){
+ seq = HARI4;
+ timer.reset();
+ }
break;
case HARI4:
+ timer.start();
move = UP;
- if(0){
- seq = WAIT2;}
+
+ if(timer.read_ms() > 2000){
+ move = STOP;
+ timer.reset();
+ seq = HARI_END;
+ }
break;
- case WAIT2:
+ case HARI_END:
move = STOP;
break;
@@ -163,63 +221,86 @@
case SYM1:
move = FFAST;
-
+ if(posY > 750 ){
+ move = FSLOW;
+ if(posY > 800)
+ seq = SYM2;
+ }
break;
case SYM2:
move = RIGHT;
- if(0){
+ if(posX > 850){
seq = SYM3;
-
}
break;
case SYM3:
- move = FFAST;
+ move = BSLOW;
- if(0){
+ if(lim1 == 1){
+ move = FFAST;
seq = SYM4;
-
+ k = posY;
}
break;
case SYM4:
- move = LEFT;
+ move = FFAST;
- if(0){
+ if( posY - k >360){
seq = SYM5;
-
- }
+ }
break;
case SYM5:
- move = LROLL;
+ move = LEFT;
- if(0){
+ if(posX < 300){
seq = SYM6;
-
- }
+ }
break;
case SYM6:
- move = DOWN;
+ move = LROLL;
- if(0){
+ timer.start();
+ if(timer.read_ms() > 5000){
+ move = STOP;
+ timer.reset();
+ timer.stop();
seq = SYM7;
-
}
break;
case SYM7:
- move = BACK;
+ move = DOWN;
- if(0){
- seq = END;
-
+ if(pho1 == 1){
+ move = STOP;
+ seq = SYM8;
}
break;
+ case SYM8:
+ move = BFAST;
+ timer.start();
+ if(timer.read_ms() > 3000){
+ move = STOP;
+ timer.reset();
+ timer.stop();
+
+ seq = END;
+ }
+ break;
+
+ case DEBUG:
+ if(lim1 == 1) move = UP;
+ else if (lim2 == 1) move = DOWN;
+ else move = STOP;
+ break;
+
}
@@ -247,37 +328,44 @@
moter(4,STOP,0);
break;
- case BACK:
+ case BFAST:
moter(1,ccw,1);
moter(2,cw,1);
moter(3,STOP,0);
moter(4,STOP,0);
break;
+
+ case BSLOW:
+ moter(1,ccw,0.5);
+ moter(2,cw,0.5);
+ moter(3,STOP,0);
+ moter(4,STOP,0);
+ break;
case RROLL:
- moter(1,ccw,0.3);
- moter(2,ccw,0.3);
+ moter(1,cw,0.3);
+ moter(2,cw,0.3);
moter(3,ccw,0.3);
moter(4,STOP,0);
break;
case LROLL:
- moter(1,cw,0.3);
- moter(2,cw,0.3);
+ moter(1,ccw,0.3);
+ moter(2,ccw,0.3);
moter(3,cw,0.3);
moter(4,STOP,0);
break;
case RIGHT:
- moter(1,ccw,0.5);
- moter(2,ccw,0.5);
+ moter(1,cw,0.5);
+ moter(2,cw,0.5);
moter(3,cw,1);
moter(4,STOP,0);
break;
case LEFT:
- moter(1,cw,0.5);
- moter(2,cw,0.5);
+ moter(1,ccw,0.5);
+ moter(2,ccw,0.5);
moter(3,ccw,1);
moter(4,STOP,0);
break;
@@ -286,28 +374,28 @@
moter(1,STOP,0);
moter(2,STOP,0);
moter(3,STOP,0);
- moter(4,cw,0.1);
+ moter(4,cw,LIFTPOWER_UP);
break;
case DOWN:
moter(1,STOP,0);
moter(2,STOP,0);
moter(3,STOP,0);
- moter(4,ccw,0.1);
+ moter(4,ccw,LIFTPOWER_DOWN);
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);
+ pc.printf("posX=%d,posY=%d ", posX, posY);
+ pc.printf("PHO = %d%d%d\tLIM = %d%d%d%d\t BTN=%d ", pho1, pho2, pho3, lim1, lim2, lim3, lim4, btn1);
+ pc.printf("seq = %d\n\r", seq);
}
}
void moter(int num, char dir, float power){
-
+ int pho1 = !photo1;
int output;
if(num == 1){
if(dir == cw){
@@ -322,7 +410,7 @@
md1_cw1 = 0;
md1_ccw1 = 0;
}
- output = PERIOD * power * hosei1;
+ output = PERIOD * power * (float)hosei1;
md1_pwm1.pulsewidth_us(output);
}
if(num == 2){
@@ -338,7 +426,7 @@
md1_cw2 = 0;
md1_ccw2 = 0;
}
- output = PERIOD * power * hosei2;
+ output = PERIOD * power * (float)hosei2;
md1_pwm2.pulsewidth_us(output);
}
if(num == 3){
@@ -354,7 +442,7 @@
md2_cw1 = 0;
md2_ccw1 = 0;
}
- output = PERIOD * power * hosei3;
+ output = PERIOD * power * (float)hosei3;
md2_pwm1.pulsewidth_us(output);
}
if(num == 4){
@@ -364,18 +452,20 @@
}
else if(dir == ccw){
md2_cw2 = 0;
- md2_ccw2 = 1;
+
+ if(pho1 == 1){
+ md2_ccw2 = 0;
+ }
+ else{
+ md2_ccw2 = 1;
+ }
}
else if(dir == STOP){
md2_cw2 = 0;
md2_ccw2 = 0;
}
- output = PERIOD * power ;
+ output = 100 * power ;
md2_pwm2.pulsewidth_us(output);
- }
-
-
-
-
+ }
}
\ No newline at end of file