miki sumito / Mbed 2 deprecated four_wheel_omni

四輪オムニの足回りを動かすプログラム

Dependencies:   mbed

PID/PID.cpp@0:39c2bb18192b, 2022-10-08 (annotated)

Committer:
m_smt
Date:
Sat Oct 08 11:13:31 2022 +0000
Revision:
0:39c2bb18192b
move four wheel omni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m_smt 0:39c2bb18192b 1 #include "PID.hpp"
m_smt 0:39c2bb18192b 2 PID::PID(double kp,double ki,double kd,double dt) : Kp(kp),Ki(ki),Kd(kd),dt(dt)
m_smt 0:39c2bb18192b 3 {
m_smt 0:39c2bb18192b 4 pwm = 0;
m_smt 0:39c2bb18192b 5 e[0] = 0;
m_smt 0:39c2bb18192b 6 e[1] = 0;
m_smt 0:39c2bb18192b 7 integral = 0;
m_smt 0:39c2bb18192b 8 }
m_smt 0:39c2bb18192b 9
m_smt 0:39c2bb18192b 10 double PID::set(double nowvalue,double tar)
m_smt 0:39c2bb18192b 11 {
m_smt 0:39c2bb18192b 12 e[1] = tar - nowvalue;
m_smt 0:39c2bb18192b 13 integral += (e[0] + e[1]) * dt / 2;
m_smt 0:39c2bb18192b 14 getP = e[1];
m_smt 0:39c2bb18192b 15 getI = integral;
m_smt 0:39c2bb18192b 16 getD = (e[1] - e[0]) / dt;
m_smt 0:39c2bb18192b 17 pwm += (getP * Kp) + (getI * Ki) + (getD * Kd);
m_smt 0:39c2bb18192b 18 e[0] = e[1];
m_smt 0:39c2bb18192b 19 return pwm;
m_smt 0:39c2bb18192b 20 }
m_smt 0:39c2bb18192b 21
m_smt 0:39c2bb18192b 22 void PID::safety(double max,double min)
m_smt 0:39c2bb18192b 23 {
m_smt 0:39c2bb18192b 24 pwm >= max ? pwm = max : pwm = pwm;
m_smt 0:39c2bb18192b 25 pwm <= min ? pwm = min : pwm = pwm;
m_smt 0:39c2bb18192b 26 }
m_smt 0:39c2bb18192b 27
m_smt 0:39c2bb18192b 28 void PID::reset()
m_smt 0:39c2bb18192b 29 {
m_smt 0:39c2bb18192b 30 pwm = 0;
m_smt 0:39c2bb18192b 31 }
m_smt 0:39c2bb18192b 32
m_smt 0:39c2bb18192b 33 PID::~PID(){};