足回りプログラム
Dependencies: mbed SBDBT arrc_mbed
main.cpp
- Committer:
- kazumayamanaka
- Date:
- 2022-01-17
- Revision:
- 0:4f39632d7a42
File content as of revision 0:4f39632d7a42:
//------INCLUDE------- #include "mbed.h" #include "rotary_inc.hpp" #include "PIDcontrol.hpp" #include "sbdbt.hpp" //------CLASS_DEFINE------ Serial pc(USBTX,USBRX); RotaryInc Ror1(PA_6,PA_7,0); RotaryInc Ror2(PC_10,PC_11,0); RotaryInc Ror3(PA_8,PA_9,0); RotaryInc Ror4(PA_14,PA_15,0); PIDcontrol pid_1; PIDcontrol pid_2; PIDcontrol pid_3; PIDcontrol pid_4; sbdbt sb(PA_0,PA_1); //------VARIABLE------- Timer one; double val_pulse[4]; double val_spd[4]; double val_target[4]; double timer; const double PI=3.1415926535897; double theta,angle=45; double Xvalue,Yvalue,Xvelocity,Yvelocity; //------FUNCTION------- void get_pulse(void); void angular(void); int main(){ one.start(); while(1){ timer = one.read_us(); get_pulse(); angular(); //pid_a.motor_control(targetの値,pulseの値,PinName pin_a,PinName pin_b); pid_1.motor_control(val_target[0],val_pulse[0],PB_14,PB_13); pid_2.motor_control(val_target[1],val_pulse[1],PC_9,PC_8); pid_3.motor_control(val_target[2],val_pulse[2],PB_5,PB_4); pid_4.motor_control(val_target[3],val_pulse[3],PA_11,PB_1); val_spd[0]=pid_1.get_spd(); val_spd[1]=pid_2.get_spd(); val_spd[2]=pid_3.get_spd(); val_spd[3]=pid_4.get_spd(); pc.printf("%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",val_target[0],val_target[1],val_target[2],val_target[3],val_spd[0],val_spd[1],val_spd[2],val_spd[3]); while(one.read_us() - timer <= 50 * 1000); //0.05s周期 } } //-------FUNCTION_DEFINE------ void get_pulse(){ val_pulse[0]=Ror1.get(); val_pulse[1]=Ror2.get(); val_pulse[2]=Ror3.get(); val_pulse[3]=Ror4.get(); } void angular(){ theta=(angle/180.0)*PI; if(sb.rsx()!=0||sb.rsx()!=0){ Xvalue=(sb.rsx()-64)*500/64.0; Yvalue=(sb.rsy()-64)*500/64.0; Xvelocity=Xvalue*cos(theta)-Yvalue*sin(theta); Yvelocity=Xvalue*sin(theta)+Yvalue*cos(theta); val_target[0]=-Xvelocity; val_target[1]=-Yvelocity; val_target[2]=Xvelocity; val_target[3]=Yvelocity; } if(sb.l2()!=0){ Yvalue=sb.l2()*500; for(int i=0;i<=3;i++){ val_target[i]=-Yvalue; } } if(sb.r2()!=0){ Yvalue=sb.r2()*500; for(int i=0;i<=3;i++){ val_target[i]=Yvalue; } } }