広大 目黑
/
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.
main.cpp
- Committer:
- megu29
- Date:
- 2019-09-18
- Revision:
- 1:8a67adccebd9
- Parent:
- 0:d14e21c64226
- Child:
- 2:9dae549ae1b4
File content as of revision 1:8a67adccebd9:
#include "mbed.h"
#include "main.h"
#include "moterdrive.h"
#include "DualShockMod.h"
#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);
DigitalOut md1_cw1(PB_12);
DigitalOut md1_ccw1(PB_1);
DigitalOut md1_dis1(PA_11);
PwmOut md1_pwm2(PB_15);
DigitalOut md1_cw2(PA_12);
DigitalOut md1_ccw2(PB_14);
DigitalOut md1_dis2(PB_13);
PwmOut md2_pwm1(PC_7);
DigitalOut md2_cw1(PA_8);
DigitalOut md2_ccw1(PC_4);//ここ
DigitalOut md2_dis1(PB_10);
PwmOut md2_pwm2(PB_3);
DigitalOut md2_cw2(PB_4);
DigitalOut md2_ccw2(PA_10);
DigitalOut md2_dis2(PB_5);
DigitalIn Limit1(PC_9);
DigitalIn Limit2(PC_8);
DigitalIn Limit3(PC_6);
DigitalIn Limit4(PC_5);
DigitalIn photo1(PA_6);
DigitalIn photo2(PA_7);
DigitalIn photo3(PB_6);
//serial通信
Serial pc(SERIAL_TX, SERIAL_RX);
Serial tsuushin(PC_10,PC_11);
/*プロトタイプ宣言*/
void moter(int num, char dir, float power);
Timer timer;
int main(){
typedef enum{
WAIT,
HARI1,
HARI2,
HARI3,
HARI4,
WAIT2,
SYM1,
SYM2,
SYM3,
SYM4,
SYM5,
SYM6,
SYM7,
END
}SEQENCE;
SEQENCE seq = WAIT;
typedef enum{
STOP,
FFAST,
FSLOW,
RIGHT,
LEFT,
RROLL,
LROLL,
BACK,
UP,
DOWN
}MOVEDIR;
MOVEDIR move = STOP;
//エンコーダーの値
tsuushin.baud(115200);
uint8_t InitDS(Serial* f_serial);
void getPOSdata(void);
InitDS(&tsuushin);
tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始
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:
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;
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(1,STOP,0);
moter(2,STOP,0);
moter(3,STOP,0);
moter(4,STOP,0);
break;
case FFAST:
moter(1,cw,1);
moter(2,ccw,1);
moter(3,STOP,0);
moter(4,STOP,0);
break;
case FSLOW:
moter(1,cw,0.5);
moter(2,ccw,0.5);
moter(3,STOP,0);
moter(4,STOP,0);
break;
case BACK:
moter(1,ccw,1);
moter(2,cw,1);
moter(3,STOP,0);
moter(4,STOP,0);
break;
case RROLL:
moter(1,ccw,0.3);
moter(2,ccw,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(3,cw,0.3);
moter(4,STOP,0);
break;
case RIGHT:
moter(1,ccw,0.5);
moter(2,ccw,0.5);
moter(3,cw,1);
moter(4,STOP,0);
break;
case LEFT:
moter(1,cw,0.5);
moter(2,cw,0.5);
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);
}
}
void moter(int num, char dir, float power){
int output;
if(num == 1){
if(dir == cw){
md1_cw1 = 1;
md1_ccw1 = 0;
}
else if(dir == ccw){
md1_cw1 = 0;
md1_ccw1 = 1;
}
else if(dir == STOP){
md1_cw1 = 0;
md1_ccw1 = 0;
}
output = PERIOD * power * hosei1;
md1_pwm1.pulsewidth_us(output);
}
if(num == 2){
if(dir == cw){
md1_cw2 = 1;
md1_ccw2 = 0;
}
else if(dir == ccw){
md1_cw2 = 0;
md1_ccw2 = 1;
}
else if(dir == STOP){
md1_cw2 = 0;
md1_ccw2 = 0;
}
output = PERIOD * power * hosei2;
md1_pwm2.pulsewidth_us(output);
}
if(num == 3){
if(dir == cw){
md2_cw1 = 1;
md2_ccw1 = 0;
}
else if(dir == ccw){
md2_cw1 = 0;
md2_ccw1 = 1;
}
else if(dir == STOP){
md2_cw1 = 0;
md2_ccw1 = 0;
}
output = PERIOD * power * hosei3;
md2_pwm1.pulsewidth_us(output);
}
if(num == 4){
if(dir == cw){
md2_cw2 = 1;
md2_ccw2 = 0;
}
else if(dir == ccw){
md2_cw2 = 0;
md2_ccw2 = 1;
}
else if(dir == STOP){
md2_cw2 = 0;
md2_ccw2 = 0;
}
output = PERIOD * power ;
md2_pwm2.pulsewidth_us(output);
}
}