f3rc
Dependencies: CruizCore_R1370P EC2 delta enc_1multi mbed
Fork of F3RC by
F3rc.h
- Committer:
- aoikoizumi
- Date:
- 2018-11-03
- Revision:
- 10:dc2a1e81c318
- Parent:
- 9:b172328cc975
File content as of revision 10:dc2a1e81c318:
#include "mbed.h" #include "SpeedController.h" #include "EC.h" #include "R1370P.h" #include "enc_1multi.h" #define BASIC_SPEED 15 //モーターはこの角速度で駆動させる SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue SpeedControl motorL(PB_5,PB_3,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,PullUp); DigitalIn reset_f(PC_1,PullUp); DigitalIn reset_a(PA_4,PullUp); PwmOut servo(PB_7);//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; double y; double asari_x=810; double asari_y=2674;//asariwo toru tekisetsuna zahyouwo kaeraremasu double goal_x=1020; double goal_y=1562; double start_x=185; double start_y=300; 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)//void de sika tsukawanaino ha mottainai kimosuru { target_R=BASIC_SPEED*r; target_L=BASIC_SPEED*l; } //printf wo dousa kakunin sidai kesou toha omotte imasu void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete { //pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly); if(y<goaly-100&&ay>=0) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); } else if(t.read_ms()>=150&&t.read_ms()<300) { 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); } } else if(y<goaly&&y>=goaly-100&&ay>=0) { tgt(ay/2,by/2); } else if(y>goaly+100&&ay<0) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { pc.printf("t=%f",t.read()); tgt(ay/3,by/3); } else if(t.read_ms()>=150&&t.read_ms()<300) { 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); } } else if(y>goaly&&y<=goaly+100&&ay<0) { tgt(ay/2,by/2); } else { i++; t.reset(); pc.printf("owari%d\r\n",i); } } void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa { if(x<goalxl-100) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { pc.printf("t=%f",t.read()); tgt(axl/3,bxl/3); } else if(t.read_ms()>=150&&t.read_ms()<300) { pc.printf("t=%f",t.read()); tgt(axl*2/3,bxl*2/3); } else { tgt(axl,bxl); t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } } else if(x<goalxl&&x>=goalxl-100) { tgt(axl/2,bxl/2); } else { t.reset(); i++; pc.printf("owari\r\n"); } } void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa { if(x>goalxr+100) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { pc.printf("t=%f",t.read()); tgt(axr/3,bxr/3); } else if(t.read_ms()>=150&&t.read_ms()<300) { pc.printf("t=%f",t.read()); tgt(axr*2/3,bxr*2/3); } else { tgt(axr,bxr); t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } } else if(x>goalxr&&x<=goalxr+100) { tgt(axr/2,bxr/2); } else { t.reset(); i++; pc.printf("owari\r\n"); } } void susumu_ang(double a,double b,double goal)//kakudo { if(goal-20>angle&&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); } else 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); } } else if(angle<goal&&angle>=goal-30&&a<b) { tgt(a/2,b/2); } else if(angle>goal+20&&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); } else 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); } } else if(angle>goal&&angle<=goal+20&&a>b) { tgt(a/2,b/2); } else { i++; t.reset(); pc.printf("reset\r\n"); } } /* int main(void) { gyro.initialize(); //main関数の最初に一度だけ実行 gyro.acc_offset(); //やってもやらなくてもいい printf("start\r\n"); motor_tick.attach(&calOmega,0.05); motorR.setDOconstant(30); motorL.setDOconstant(30);//c motorR.setPDparam(0.2,0.1); motorL.setPDparam(0.2,0.1); EC1.setDiameter_mm(48);//sokuteirinnhannkei double getDistance_mm(); //int EC1.getCount()=0; void reset (); EC1.reset(); //servo.period_ms(30); motor_f.period_ms(30); motor_b.period_ms(30);//arm x=start_x; y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu while(1) { //target_R=(-1)*BASIC_SPEED; //target_L=(-1)*BASIC_SPEED; // 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()*(-1); //角度の値を受け取る 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); new_dist=EC1.getDistance_mm(); d_dist=new_dist-old_dist; old_dist=new_dist; double d_x=d_dist*sin(angle*3.1415926535/180); double d_y=d_dist*cos(angle*3.1415926535/180); x=x+d_x; y=y+d_y; if(kai>=3) { //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) { servo.pulsewidth_us(950); if(reset_f.read()==0&&button.read()==0) { wait(0.05); if(reset_f.read()==0&&button.read()==0) { denjiben=1; i++; } } if(reset_a.read()==0&&button.read()==0) { wait(0.05); if(reset_a.read()==0&&button.read()==0) { denjiben=1; x=180; y=2674; i=12; } }//x,y } if(i==1) { servo.pulsewidth_us(950); susumu_y(1,1,857);//x,x+178 } if(i==2) { susumu_ang(0,1,90);//x+155,x+471.5//219.2,65.8//534.2,730.8//180,576.6 } if(i==3) { susumu_xl(1,1,745);//728.5,1078.5 } if(i==4) { susumu_ang(1,0,0);//850,1372 } if(i==5) { susumu_y(1,1,2016);//850,1700 } if(i==6) { // motorR.stop(); //motorL.stop(); tgt(0,0); wait(2); t.start(); if(t<1) { motor_f=0.82; motor_b=0; printf("motor"); } else { motor_f=0; motor_b=0; printf("finish"); t.stop(); t.reset(); i++; } }//gatiasari if(i==7) { wait(2); t.start(); if(t<2) { motor_f=0; motor_b=0.82; printf("motor"); } else { motor_f=0; motor_b=0; printf("finish"); t.stop(); t.reset(); i++; } if(angle>=-89) { target_R=BASIC_SPEED/2; target_L=BASIC_SPEED/(-2); } if(angle<=-91) { target_R=BASIC_SPEED/(-2); target_L=BASIC_SPEED/2; } if(angle>-91&angle<-89) { motorR.stop(); motorL.stop(); wait(2); if(angle>-91&angle<-89) { i++; } } }//kakudo tyousei if(i==8) { susumu_xr(1,1,555);//700,1700 } if(i==9) { susumu_ang(0,1,0);//390,2010 } if(i==10) { susumu_y(1,1,asari_y-155);//390,y-310 } if(i==11) { susumu_ang(0,1,90);//700,y } if(i==12) { motorR.stop(); motorL.stop(); servo.pulsewidth_us(2100); wait(1.5); servo.pulsewidth_us(2400); wait(2); i++; } if(i==13) { if(angle>=91) { target_R=BASIC_SPEED/2; target_L=BASIC_SPEED/(-2); } if(angle<=89) { target_R=BASIC_SPEED/(-2); target_L=BASIC_SPEED/2; } if(angle>89&angle<91) { motorR.stop(); motorL.stop(); wait(2); if(angle>89&angle<91) { i++; } } } if(i==14) { susumu_xl(1,1,asari_x);//x,y } if(i==15) { motorR.stop(); motorL.stop(); tgt(0,0); wait(2); wait(0.5); denjiben=0; wait(0.5); servo.pulsewidth_us(900); wait(2); i++; } if(i==16) { susumu_xr(-1,-1,555);//700,y } if(i==17) { susumu_ang(0,-1,0);//390,y-310 } if(i==18) { susumu_y(-1,-1,goal_y+155);//390,y+310 } if(i==19) { susumu_ang(0,-1,-90);//700,y } if(i==20) { susumu_xl(-1,-1,goal_x);//x,y } if(i==21) { motorR.stop(); motorL.stop(); wait(0.5); denjiben=1; wait(0.5); break; } if(i==30) { tgt(-1,-1); } */ // }//while //tgt(0,0); // return 0; //}//intmain