F3RC1班
/
f3rc_auto_2
自動機 時間制限ver.
Diff: main.cpp
- Revision:
- 1:4b622e363fa1
- Parent:
- 0:2694d5a2f90a
diff -r 2694d5a2f90a -r 4b622e363fa1 main.cpp --- a/main.cpp Fri Sep 01 23:50:06 2017 +0000 +++ b/main.cpp Sat Sep 02 06:17:48 2017 +0000 @@ -6,49 +6,50 @@ 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); - +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 dis=0; +int ang=0; + int n=1; void calOmega() //角速度計算関数 { - motor1.CalOmega(); - motor2.CalOmega(); - Ec1.CalOmega(); + motor1.CalOmega(); + motor2.CalOmega(); + Ec1.CalOmega(); } void st() { int kai=0; - while(kai<50000){ - motor1.stop(); - motor2.stop(); - kai=kai + 1; - } + while(kai<50000) { + motor1.stop(); + motor2.stop(); + kai=kai + 1; + } } -int Dis(){ - dis=0.301*Ec1.getCount(); +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("gyro=%d",spi.read()); ////pc.printf("count1=%f ",motor1.getOmega()); ////pc.printf("count2=%f ",motor2.getOmega()); ////pc.printf("duty1=%f ",motor1.duty); @@ -57,201 +58,202 @@ 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()); + 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(); + // } + } + 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); + 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(); + // } + } + + 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; +// 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; + 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; +// 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; + if(b > 0) { //角 + motor1.Sc(0); + motor2.Sc(5); + wait(b); + + Ec1.reset(); + motor1.Sc(0); + motor2.Sc(0); + wait(0.5); + + + } else { + motor1.Sc(-5); + motor2.Sc(0); + wait(-b); + + 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() { +int main() +{ servo.period_ms(20); spi.format(16,3); spi.frequency(1000000); - + ticker.attach(&calOmega,0.05); - // ticker2.attach(&print,0.5); + // 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); - - + motor2.setDOconstant(15.8); + + sw.mode(PullUp); - + + + + if(sw==1) { //スタートゾーン1 - - 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); - - - - - - } - - + 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); + + + + + + } + + } //・距離とエンコーダ出力の関係 ・ジャイロの値