f3rc
Dependencies: CruizCore_R1370P EC2 delta enc_1multi mbed
Fork of F3RC by
Diff: F3rc.h
- Revision:
- 9:b172328cc975
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/F3rc.h Sat Nov 03 23:10:12 2018 +0000 @@ -0,0 +1,467 @@ + +#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 + +