F3RC9/11
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of F3RC by
main.cpp
- Committer:
- aoikoizumi
- Date:
- 2018-09-07
- Revision:
- 0:29c024d6882f
- Child:
- 1:1817e9243a6e
File content as of revision 0:29c024d6882f:
#include "mbed.h" #include "SpeedController.h" #include "EC.h" #include "R1370P.h" #include "enc_1multi.h" #define BASIC_SPEED 30 //モーターはこの角速度で駆動させる SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue SpeedControl motorL(PB_3,PB_5,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc Ticker motor_tick; //角速度計算用ticker Ticker ticker;//for enc Serial pc(USBTX, USBRX); // tx, rx //PC USB R1370P gyro(PC_6,PC_7); void calOmega() //角速度計算関数 { motorR.CalOmega(); motorL.CalOmega(); EC1.CalOmega(); } DigitalIn button(USER_BUTTON); PwmOut servo(PB_14);//servo PwmOut motor_f(PC_9); PwmOut motor_b(PB_9);//arm DigitalOut denjiben(PC_0);//dennjibenn double new_dist=0; double old_dist=0; double d_dist=0; double x=250; double y=150;//start point Timer t; int i=0; int kai=0;//printf kansuu double target_R=0,target_L=0; double angle; //変数宣言 void tgt(double r,double l) { target_R=BASIC_SPEED*r; target_L=BASIC_SPEED*l; } void susumu_y(double ay,double by,double goaly) { if(y<goaly-50&&ay>=0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ay*2/3,by*2/3); } else { tgt(ay,by); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(y<goaly&&y>=goaly-50&&ay>=0) { tgt(ay/3,by/3); } if(y>goaly+50&&ay<0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ay*2/3,by*2/3); } else { tgt(ay,by); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(y>goaly&&y<=goaly+50&&ay<0) { tgt(ay/3,by/3); } else { i++; t.reset(); pc.printf("owari\r\n"); } } void susumu_x(double ax,double bx,double goalx) { if(x<goalx-50&&ax>=0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ax/3,bx/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ax*2/3,bx*2/3); } else { tgt(ax,bx); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(x<goalx&&x>=goalx-50&&ax>=0) { tgt(ax/3,bx/3); } if(x>goalx+50&&ax<0) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(ax/3,bx/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(ax*2/3,bx*2/3); } else { tgt(ax,bx); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(x>goalx&&x<=goalx+50&&ax<0) { tgt(ax/3,bx/3); } else { i++; t.reset(); pc.printf("owari\r\n"); } } void susumu_ang(double a,double b,double goal) { if(x<angle-5&&a>b) {//usetsu t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(a/3,b/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(a*2/3,b*2/3); } else { tgt(a,b); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(angle<goal&&angle>=goal-5&&a>b) { tgt(a/3,b/3); } if(angle>goal+5&&a<b) {//sasetsu t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { pc.printf("t=%f",t.read()); tgt(a/3,b/3); } if(t.read_ms()>=100&&t.read_ms()<200) { pc.printf("t=%f",t.read()); tgt(a*2/3,b*2/3); } else { tgt(a,b); t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } } if(angle>goal&&angle<=goal+5&&a<b) { tgt(a/3,b/3); } else { i++; t.reset(); pc.printf("reset\r\n"); } } int main(void) { printf("start\r\n"); motor_tick.attach(&calOmega,0.05); motorR.setDOconstant(34.1); motorL.setDOconstant(30);//c motorR.setPDparam(0,0); motorR.setPDparam(0,0);//pd gyro.initialize(); //main関数の最初に一度だけ実行 EC1.setDiameter_mm(50);//sokuteirinnhannkei double getDistance_mm(); //int EC1.getCount()=0; EC1.reset(); void reset (); // servo.period_ms(60); motor_f.period_ms(30); motor_b.period_ms(30);//arm while(1) { // motorR.turnF(0.3); //motorL.turnF(0.3);//for debug motorR.Sc(target_R); motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う angle=gyro.getAngle(); //角度の値を受け取る EC1.getDistance_mm(); // EC1.CalOmega(); if(target_R==0) motorR.stop(); else motorR.Sc(target_R); if(target_L==0) motorL.stop(); else motorL.Sc(target_L); double d_x=d_dist*sin(angle); double d_y=d_dist*cos(angle); x=x+d_x; y=y+d_y; if(kai>=3) { new_dist=EC1.getDistance_mm(); d_dist=new_dist-old_dist; old_dist=new_dist; //double d_x=d_dist*sin(angle);; //double d_y=d_dist*cos(angle);; //x=x+d_x; //y=y+d_y; pc.printf("R=%f L=%f",target_R,target_L); // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega()); pc.printf("i=%d\r\n",i); //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); pc.printf("x=%f y=%f",x,y); //pc.printf("angle=%f\r\n",angle); //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 kai=0; } kai++; if(i==0) { t.start(); if(t<1) { motor_f=0.8; motor_b=0; } else { motor_f=0; motor_b=0; printf("finish"); t.reset(); i++; } }//motor test //kokomade kihonnsettei /* if(i==0) { printf("a\n\r"); target_R=0; target_L=0; i++; pc.printf("i=0\r\n"); pc.printf("R=%f L=%f",target_R,target_L); }*/ if(i==1) { susumu_y(1,1,472); /* if(y<472) { t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<200) { pc.printf("t=%f",t.read()); target_R=BASIC_SPEED/3; target_L=BASIC_SPEED/3; } else { target_R=BASIC_SPEED; target_L=BASIC_SPEED; t.stop(); pc.printf("R=%f L=%f",target_R,target_L); }//250150 250472 } else { i++; t.reset(); pc.printf("reset\r\n"); */ //} } if(i==2) { susumu_ang(1/3,1,45); /* t.start(); if(angle<45) { if(t.read_ms()<200) { target_R=BASIC_SPEED/6; target_L=BASIC_SPEED/2; } else { target_R=BASIC_SPEED/3; target_L=BASIC_SPEED; }//250472 341691 } else i++; t.stop(); t.reset(); */ }//kairyoutyuu if(i==3) { susumu_x(1,1,709); // if(x<709) { // target_R=BASIC_SPEED; // target_L=BASIC_SPEED; // //341691 7091059 // } else i++; } if(i==4) { susumu_ang(1,1/3,0); /* if(angle>0) { target_R=BASIC_SPEED; target_L=BASIC_SPEED/3; //7091059 8001278 } else i++; */ } if(i==5) { susumu_y(1,1,1700); // if(y<1700) { // target_R=BASIC_SPEED; // target_L=BASIC_SPEED; // //8001278 8001700 // } else i++; motorR.stop(); motorL.stop(); } if(i==6) { i++; }//gatiasari /* if(i==7) { if(angle>=-89) { target_R=BASIC_SPEED/4; target_L=BASIC_SPEED*(-4); } if(angle<=-91) { target_R=BASIC_SPEED/4; target_L=BASIC_SPEED*(-4 ); } if(angle>-91&angle<89){ motorR.stop(); motorL.stop(); wait(0.5); if(angle>-91&angle<89){ i++;} } } if(i==8) { if(x>700) { target_R=BASIC_SPEED; target_L=BASIC_SPEED; //8001700 7001700 } else i++; } if(i==9) { if(angle<0) { target_R=BASIC_SPEED/3; target_L=BASIC_SPEED; //7001700 4002000 } else i++; } if(i==10) { if(y<2200) { target_R=BASIC_SPEED; target_L=BASIC_SPEED; //4002000 4002200 } else i++; } if(i==11) { if(angle<90) { target_R=BASIC_SPEED/3; target_L=BASIC_SPEED; //4002200 7002500 } else i++; } if(i==12) { if(x<1000) { target_R=BASIC_SPEED; target_L=BASIC_SPEED; //7002500 10002500 } else i++; motorR.stop(); motorL.stop(); } if(i==13){ i++; } if(i==14) { if(x>700) { target_R=BASIC_SPEED*(-1); target_L=BASIC_SPEED*(-1); //10002500 7002500 } else i++; } if(i==15) { if(angle>0) { target_R=BASIC_SPEED/(-3); target_L=BASIC_SPEED/(-1); //7002500 4002200 } else i++; } if(i==16) { if(y>2000) { target_R=BASIC_SPEED*(-1); target_L=BASIC_SPEED*(-1); //4002200 4002000 } else i++; } if(i==17) { if(angle>-90) { target_R=BASIC_SPEED/(-3); target_L=BASIC_SPEED/(-1); //4002000 7001700 } else i++; } if(i==16) { if(x<1100) { target_R=BASIC_SPEED*(-1); target_L=BASIC_SPEED*(-1); //7001700 4002000 } else i++; } */ //kairyoutyuu kokomade }//while motorR.stop(); motorL.stop(); //LR(0,0); return 0; }//intmain