![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
足回りプログラム
Dependencies: mbed SBDBT arrc_mbed
PIDcontrol.cpp
- Committer:
- kazumayamanaka
- Date:
- 2022-01-17
- Revision:
- 0:4f39632d7a42
File content as of revision 0:4f39632d7a42:
#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; }