F3RC1班
/
f3rc_auto
自動機
Diff: main.cpp
- Revision:
- 0:1f542f8756d6
- Child:
- 1:651bd0514eb9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Aug 22 02:39:13 2017 +0000 @@ -0,0 +1,258 @@ +#include "mbed.h" +#include "EC.h" + +PwmOut servo(PB_7); +DigitalIn sw(PA_15); +SPISlave spi(PB_15,PB_14,PB_13,PB_12); + +Ec Ec1(PB_6,PC_7,NC,1048,0.05); + Ticker ticker; + Ticker ticker2; + DigitalIn button(USER_BUTTON); + Serial pc(USBTX,USBRX); + +SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //left +SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //right + + + +int n=1; + + +void calOmega() //角速度計算関数 +{ + motor1.CalOmega(); + motor2.CalOmega(); + Ec1.CalOmega(); +} + +void st() +{ + int kai=0; + while(kai<50000){ + motor1.stop(); + motor2.stop(); + kai=kai + 1; + } +} + +/*void print() +{ + // pc.printf("n=%d",n); + // pc.printf("count1=%d",Ec1.getCount()); + // pc.printf("gyro=%d",spi.read()); + pc.printf("count1=%f ",motor1.getOmega()); + pc.printf("count2=%f ",motor2.getOmega()); + pc.printf("duty1=%f ",motor1.duty); + pc.printf("duty2=%f\r\n",motor2.duty); +}*/ + +void str(int a) // +{ +int kai=0; + + while(n==1){ + int dis=0.301*Ec1.getCount(); + if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 + // if(spi.receive()){ + pc.printf("count1=%f ",motor1.getOmega()); + pc.printf("count2=%f ",motor2.getOmega()); + pc.printf("duty1=%f ",motor1.duty); + pc.printf("duty2=%f\r\n",motor2.duty); + kai=0; + // } + } + if(dis < a){ //距離 + motor1.Sc(5); + motor2.Sc(5); + }else{ + n=2; + } + kai++; + //pc.printf("kai=%d\r\n",kai); + +} +st(); +} +void back(int a) +{ + int kai=0; + while(n==2){ + int dis=0.301*Ec1.getCount(); + kai++; + if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 + // if(spi.receive()){ + pc.printf("count1=%f ",motor1.getOmega()); + pc.printf("count2=%f ",motor2.getOmega()); + pc.printf("duty1=%f ",motor1.duty); + pc.printf("duty2=%f\r\n",motor2.duty); + kai=0; + // } + } + if(dis > a){ + motor1.Sc(-5); + motor2.Sc(-5); + }else{ + motor1.Sc(0); + motor2.Sc(0); + n=1; + } + } + st(); +} + +void turnR(int b,int c) +{ + int kai=0; + int ang=0; + while(n==2){ + + kai++; + if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 + // if(spi.receive()){ + // pc.printf("count1=%f ",motor1.getOmega()); + pc.printf("count2=%f ",motor2.getOmega()); + // pc.printf("duty1=%f ",motor1.duty); + pc.printf("duty2=%f\r\n",motor2.duty); + // pc.printf("ang=%f\r\n",ang); + kai=0; + // } + } + /* if(spi.receive()){ + pc.printf("count1=%d",Ec1.getCount()); + pc.printf("gyro=%d\r\n",spi.read()); + }*/ + if(spi.receive()){ + ang=spi.read(); +// pc.printf("ang=%d\r\n",ang); + } + if((ang < b) || (ang > c)){ //角 + motor1.Sc(5); + motor2.Sc(-5); + + }else{ + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + n=1; + } +} +st(); +} + +void turnL(int b,int c) +{ + int kai=0; + int ang=0; + while(n==2){ + + kai++; + if(kai>=500){ //ループ500回ごとに角速度・出力duty比を表示 + // if(spi.receive()){ + pc.printf("count1=%f ",motor1.getOmega()); + // pc.printf("count2=%f ",motor2.getOmega()); + pc.printf("duty1=%f\r\n ",motor1.duty); + // pc.printf("duty2=%f\r\n",motor2.duty); + // pc.printf("ang=%f\r\n",ang); + kai=0; + // } + } +/* if(spi.receive()){ + pc.printf("n=%d",n); + pc.printf("count1=%d",Ec1.getCount()); + pc.printf("gyro=%d\r\n",spi.read()); + }*/ + if(spi.receive()){ + ang=spi.read(); +// pc.printf("ang=%d\r\n",ang); + } + if((ang < b) || (ang > c)){ //角度 + motor1.Sc(-5); + motor2.Sc(5); + }else{ + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + n=1; + } + + } + st(); +} + + +int main() { + + servo.period_ms(20); + spi.format(16,3); + spi.frequency(1000000); + + + ticker.attach(&calOmega,0.05); + // ticker2.attach(&print,0.5); + + motor1.setPDparam(0.55,0.5); //PDパラメータを設定 + motor2.setPDparam(0.6,0.5); //PDパラメータを設定 + + motor1.setDOconstant(13.1); + motor2.setDOconstant(15.8); + + + sw.mode(PullUp); + + + + if(sw==1){ //スタートゾーン1 + + servo.pulsewidth_us(900); + str(540); + turnL(2650,2750); + str(1481); + turnL(1750,1850); + str(540); + turnR(2650,2750); + str(1481); + servo.pulsewidth_us(1800); + wait(0.5); + back(0); + n=2; + turnR(3550,3600); + str(540); + turnR(850,950); + str(1481); + turnR(1750,1850); + str(570); + servo.pulsewidth_us(900); + back(450); + servo.pulsewidth_us(1800); + str(570); + + //押し出し + + }else{ //スタートゾーン2 + servo.pulsewidth_us(900); + str(540); + servo.pulsewidth_us(1800); + wait(0.5); + turnL(850,950); + str(1481); + turnL(0,50); + str(540); + turnR(850,950); + str(1481); + turnR(1750,1850); + str(570); + servo.pulsewidth_us(900); + back(450); + servo.pulsewidth_us(1800); + str(570); + + + + + + } + + +} //・距離とエンコーダ出力の関係 ・ジャイロの値 +