F3RC1班
/
f3rc_auto
自動機
Diff: main.cpp
- Revision:
- 1:651bd0514eb9
- Parent:
- 0:1f542f8756d6
- Child:
- 2:c8c264a9a7f6
diff -r 1f542f8756d6 -r 651bd0514eb9 main.cpp --- a/main.cpp Tue Aug 22 02:39:13 2017 +0000 +++ b/main.cpp Wed Sep 06 02:46:11 2017 +0000 @@ -6,253 +6,344 @@ 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 +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); //左 +SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //右 + +Timer timer; - - +int kai=0; int n=1; +int ang=0; +int a=0; +int c=0; +int d=0; +int dis=0; +double V=0; +double v(); +int e=0; +int f=0; + -void calOmega() //角速度計算関数 +void calOmega() //角速度計算関数 +{ + motor1.CalOmega(); + motor2.CalOmega(); + Ec1.CalOmega(); +} + +void Ang() //Nucleoリセット時からの機体角度(0~3600) { - motor1.CalOmega(); - motor2.CalOmega(); - Ec1.CalOmega(); + if(spi.receive())ang=spi.read() - d; + + if(ang < 0)ang += 3600; + if(ang > 3600)ang-=3600; + } + +void print() +{ + if(kai>=500) { + //pc.printf("count1=%d\r\n",dis); + // pc.printf("count=%f ",motor1.getOmega()); + // pc.printf("count2=%f\r\n ",motor2.getOmega()); + //pc.printf("duty1=%f ",motor1.duty); + //pc.printf("duty2=%f\r\n",motor2.duty); + //pc.printf("d=%d",d); + //pc.printf("v=%f\r\n",v()); + pc.printf("ang=%d\r\n",ang); + kai=0; + } + kai++; +} + + 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(); + wait(0.5); } -void turnR(int b,int c) + + +void str(int a) { - int kai=0; - int ang=0; - while(n==2){ + + while(n==1) { + dis=0.301*Ec1.getCount(); + if(dis < a) { + V=15; + e=400; - 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(); + if(dis > a-e) { + V = (a - dis)*V/e; + if(V<2)V=2; + } + + motor1.Sc(V); + motor2.Sc(V); + + } else { + n=2; + } + } + st(); } -void turnL(int b,int c) + +void back(int a) { - int kai=0; - int ang=0; - while(n==2){ + e=400; + + motor1.setDOconstant(20.7); + motor2.setDOconstant(20.3); + - 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(); + while(n==2) { + V=15; + dis=0.301*Ec1.getCount(); + if(dis > a) { + if(dis < a+e ) { + V = (a - dis)*V/e; + if(V<2)V=2; + } + motor1.Sc(-V); + motor2.Sc(-V); + } else { + motor1.Sc(0); + motor2.Sc(0); + n=1; + } + } + st(); + motor1.setDOconstant(19.7); + motor2.setDOconstant(21.3); } -int main() { +void turn(int b) +{ + c=0; + f=700; + + while(n==2) { + V=5; + print(); + if(b >= 0) { + + + a = ang - b; //角度(目標値から±1800) + + if(a < -1800) { + a += 3600; + } else if(a > 1800) { + a -= 3600; + } + print(); + if(c==1 && a > 20) { + motor1.Sc(-v()); + motor2.stop(); + } else if(c==2 && a < -20) { + motor1.stop(); + motor2.Sc(-v()); + } else if(a < -f) { + motor1.Sc(V); + motor2.stop(); + } else if(a > f) { + motor1.stop(); + motor2.Sc(V); + } else if(a < -10) { + motor1.Sc(v()); + motor2.stop(); + c=1; + } else if(a > 10) { + motor1.stop(); + motor2.Sc(v()); + c=2; + } else { + Ec1.reset(); + motor1.stop(); + motor2.stop(); + n=1; + } + + } else { + a = ang + b; + + if(a < -1800) { + a += 3600; + } else if(a > 1800) { + a -= 3600; + } + + if(c==1 && a > 20) { + motor1.stop(); + motor2.Sc(v()); + } else if(c==2 && a < -20) { + motor1.Sc(v()); + motor2.stop(); + } else if(a < -f) { + motor1.stop(); + motor2.Sc(-V); + } else if(a > f) { + motor1.Sc(-V); + motor2.stop(); + } else if(a < -10) { + motor1.stop(); + motor2.Sc(-v()); + c=1; + } else if(a > 10) { + motor1.Sc(-v()); + motor2.stop(); + c=2; + } else { + Ec1.reset(); + motor1.stop(); + motor2.stop(); + n=1; + } + } + } + while(1){ + print(); + } +} + +double v(){ + if(a < 0)a=-a; + V=(a-10)*V/f; + if(V < 0.3)V=0.3; + return V; +} + + +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(&Ang,0.05); + + motor1.setPDparam(0.3,0.4); //PDパラメータを設定 + motor2.setPDparam(0.3,0.2); //PDパラメータを設定 + + motor1.setDOconstant(19.6); + motor2.setDOconstant(22.3); + + + sw.mode(PullUp); + + + + - 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); + wait(0.5); + d=ang; //初期角度 + n=2; + turn(2700); + + /* turn(3100); + str(950); + turn(2410); + str(1070); + turn(2700); + servo.pulsewidth_us(1800); + str(870); + servo.pulsewidth_us(900); + wait(1); + back(0); + n=2; + turn(-2430); + n=2; + back(-1080); + n=2; + turn(-2700); + n=2; + back(-970); + n=2; + turn(1800); + str(500); + servo.pulsewidth_us(1800); + wait(0.3); - - 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); - - - + motor1.setDOconstant(20.7); + motor2.setDOconstant(20.3); + while(n==2) { + dis=0.301*Ec1.getCount(); + if(dis > 370) { + motor1.Sc(-15); + motor2.Sc(-15); + } else { + motor1.Sc(0); + motor2.Sc(0); + n=1; + } + } + st(); + motor1.setDOconstant(19.7); + motor2.setDOconstant(21.3); + + servo.pulsewidth_us(900); + str(500); + + while(n==2) { + dis=0.301*Ec1.getCount(); + if(dis > 370) { + motor1.Sc(-15); + motor2.Sc(-15); + } else { + motor1.stop(); + motor2.stop(); + n=1; + } + }*/ + + } else { //スタートゾーン2 + servo.pulsewidth_us(1800); + wait(0.5); + d=ang - 1800; + str(450); + servo.pulsewidth_us(900); + wait(1); + turn(880); + str(820); + turn(610); + str(1080); + turn(1320); + str(930); + turn(1800); + servo.pulsewidth_us(1800); + n=2; + wait(0.3); - - } - - -} //・距離とエンコーダ出力の関係 ・ジャイロの値 + motor1.setDOconstant(20.7); + motor2.setDOconstant(20.3); + while(n==2) { + dis=0.301*Ec1.getCount(); + if(dis > -130) { + motor1.Sc(-15); + motor2.Sc(-15); + } else { + motor1.stop(); + motor2.stop(); + n=1; + } + } + } + +} +