![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
足回りプログラム
Dependencies: mbed SBDBT arrc_mbed
Revision 0:4f39632d7a42, committed 2022-01-17
- Comitter:
- kazumayamanaka
- Date:
- Mon Jan 17 02:40:16 2022 +0000
- Commit message:
- this program is to motor control;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDcontrol.cpp Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,63 @@ +#include "PIDcontrol.hpp" + +//コンストラクタ +PIDcontrol::PIDcontrol(){ + Kp=0.0001; + Ki=0.0000014; + Kd=0.0000001; + spd=0.0; + pwm=0.0; + dt=0.05; + integral=0.0; + finish=0; + pal=0; +} + + +//spd計算 +void PIDcontrol::speed(){ + finish = pulse - pal; + spd = co*finish/dt; + pal = pulse; +} + + +//pwm計算 +void PIDcontrol::control(){ + error_n = target-spd; + integral += (error_n+error_b) / 2.0 * dt; + P = Kp * error_n; + I = Ki * integral; + D = Kd * (error_n-error_b) / dt; + pwm += (P+I+D); + error_b=error_n; +} + +//モーター制御 +void PIDcontrol::motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b){ + PwmOut v1p(pin_a); + PwmOut v1m(pin_b); + v1p.period_us(2048); + v1m.period_us(2048); + target=var_1; + pulse=var_2; + speed(); + control(); + if(pwm>0){ + v1p = pwm > 0.5 ? 0.5 : pwm; + v1m=0; + }else if(pwm<0){ + v1p = 0; + v1m = -pwm > 0.5 ? 0.5 : -pwm; + }else{ + v1p = 0; + v1m = 0; + } +} + +//spdをreturnする +double PIDcontrol::get_spd(){ + return spd; +} + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDcontrol.hpp Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,28 @@ +#ifndef PID_CONTROL_H +#define PID_CONTROL_H +#include "mbed.h" +#define co (108*3.1415926535897)/500 + +class PIDcontrol{ + public: + double spd;//速度 + double error_n,error_b;//現在の誤差、過去の誤差 + double P,I,D;//pid制御のそれぞれの値 + double pwm,v1p_set,v1m_set;//モータに入れる値 + double integral;//誤差の面積 + double finish,pulse,pal;//パルス関係 + double target;//目標値 + PIDcontrol();//コンストラクタ + void motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b);//pwmをreturnする関数 + double get_spd();//spdをreturnする関数 + private: + void speed();//spd計算 + void control();//PID制御 + double dt;//dt=0.05 + double Kp,Ki,Kd;//PID制御 +}; + +#endif + + + \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SBDBT.lib Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/m2130/code/SBDBT/#9fd7393a3023
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/arrc_mbed.lib Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TanakaRobo/code/arrc_mbed/#77c13e86ad12
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,88 @@ +//------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; + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jan 17 02:40:16 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file