F3RC1班
/
f3rc_auto
自動機
main.cpp
- Committer:
- eri
- Date:
- 2017-09-06
- Revision:
- 2:c8c264a9a7f6
- Parent:
- 1:651bd0514eb9
- Child:
- 3:f5e875d8df30
File content as of revision 2:c8c264a9a7f6:
#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); //左 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() //角速度計算関数 { motor1.CalOmega(); motor2.CalOmega(); Ec1.CalOmega(); } void Ang() //Nucleoリセット時からの機体角度(0~3600) { 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() { motor1.stop(); motor2.stop(); wait(0.5); } void str(int a) { while(n==1) { dis=0.301*Ec1.getCount(); if(dis < a) { V=15; e=400; 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 back(int a) { e=400; motor1.setDOconstant(20.7); motor2.setDOconstant(20.3); 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); } void turn(int b) { c=0; f=700; while(n==2) { V=5; 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(&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); if(sw==1) { //スタートゾーン1 servo.pulsewidth_us(900); wait(0.5); d=ang; //初期角度 n=2; 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); 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; } } } }