F3RC1班
/
f3rc_auto_2
自動機 時間制限ver.
Diff: main.cpp
- Revision:
- 0:2694d5a2f90a
- Child:
- 1:4b622e363fa1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 01 23:50:06 2017 +0000 @@ -0,0 +1,257 @@ +#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 dis=0; +int ang=0; + +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; + } +} + +int Dis(){ + dis=0.301*Ec1.getCount(); + return dis; + } + + + +/*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){ + 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("gyro=%d",spi.read()); + 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){ + 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(-4.9); + motor2.Sc(-5.2); + }else{ + motor1.Sc(0); + motor2.Sc(0); + n=1; + } + } + st(); +} + +void turnR(double b) +{ + // int kai=0; + //int ang=0; + + + if(b > 0){ //角 + motor1.Sc(5); + motor2.Sc(0); + wait(b); + + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + wait(0.5); + + }else{ + motor1.Sc(0); + motor2.Sc(-5); + wait(-b); + + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + wait(0.5); + +} +n=1; +} + +void turnL(double b) +{ + // int kai=0; + //int ang=0; + + + if(b > 0){ //角 + motor1.Sc(0); + motor2.Sc(5); + wait(0.815); + + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + wait(0.5); + + + }else{ + motor1.Sc(-5); + motor2.Sc(0); + wait(0.81); + + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + wait(0.5); + +} +n=1; +} + +/*void turn(){ + motor1.Sc(-5); + motor2.Sc(5); + wait(1.5); + + + }*/ + + +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.3,0.4); //PDパラメータを設定 + motor2.setPDparam(0.35,0.4); //PDパラメータを設定 + + motor1.setDOconstant(14.4); //duty比と角速度の変換係数を46.3に設定←設定必要かも + motor2.setDOconstant(15.8); + + + sw.mode(PullUp); + + + + if(sw==1){ //スタートゾーン1 + + servo.pulsewidth_us(900); + str(395); + turnL(0.8); + str(1151); + turnL(0.8); + str(310); + turnR(0.74); + servo.pulsewidth_us(1800); + str(1326); + servo.pulsewidth_us(900); + wait(0.5); + back(0); + turnR(-0.84); + str(340); + turnR(0.8); + str(1131); + turnR(0.8); + str(375); + servo.pulsewidth_us(1800); + back(245); + servo.pulsewidth_us(900); + str(365); + + + //押し出し + + }else{ //スタートゾーン2 + servo.pulsewidth_us(1800); + str(400); + servo.pulsewidth_us(900); + wait(0.5); // + turnL(0.8); + str(1131); + turnL(0.81); + str(270); + turnR(0.77); + str(1131); + turnR(0.77); + str(380); + servo.pulsewidth_us(1800); + back(260); + servo.pulsewidth_us(900); + str(390); + + + + + + } + + +} //・距離とエンコーダ出力の関係 ・ジャイロの値 +