From c7d00affc1f4efbafc9896e5a9089a1a9bf3a893 Mon Sep 17 00:00:00 2001
From: Pascal Engeler <engelerp@phys.ethz.ch>
Date: Fri, 9 Jun 2023 23:30:52 +0200
Subject: [PATCH] Added pid controller library
---
.../pid_controller/.pid_controller.cpp.swp | Bin 0 -> 12288 bytes
.../drivers/pid_controller/pid_controller.cpp | 83 ++++++++++++++++++
.../drivers/pid_controller/pid_controller.hpp | 33 +++++++
3 files changed, 116 insertions(+)
create mode 100644 firmware/drivers/pid_controller/.pid_controller.cpp.swp
create mode 100644 firmware/drivers/pid_controller/pid_controller.cpp
create mode 100644 firmware/drivers/pid_controller/pid_controller.hpp
diff --git a/firmware/drivers/pid_controller/.pid_controller.cpp.swp b/firmware/drivers/pid_controller/.pid_controller.cpp.swp
new file mode 100644
index 0000000000000000000000000000000000000000..23cc8998492688172b9c42322ffacccc4822b9fe
GIT binary patch
literal 12288
zcmeI2&2Jk;7{;fS5MM16Dscmba!G>i-53=bCn<<lNS0gVLI8;>vevufdXn|-c6RJm
zib|3CCm;cWBNZ2r-~uWYCn^CKE=U~uL4`PQ-~#G}E6?oCe%OiK!VQ>}ep!2W-sgSi
z*_qiynXdg>^Ef?Js}mgi2zhJmf_LfArQIL=KuF}ouH&m4vyXPtF0LMT+!w-dv)UG6
z%@4PSzL^SIf<<9$iLM(Cx>d;r5fhFa3RZQ)K#I_}dRz?7ID%Q8;3FntE8<>9-}IT7
z_*!$LXm)|-u5rLP00;Jxr|XNhvf{}DPtg4jziM1D4j2cF1I7X4fN{V$U>q<G7zh4G
z4#;R1IgkGC)_s0OpLb21^;dH;4j2cF1I7X4fN{V$U>q<G7zd04#sTAiao}I*fa4MJ
z+g*gbd^d{6|NpbU|9^Rqknh14;B#;hxL^VN_y8g2!HeMfUP7*ctKd8EE%*k!0iFjn
z@CdlIhmb$O@8B|c2W)`XfemilPskPU33v(A!5;AIeb59Sfs5cnumwEO1rAsO2LJ_+
zgFo*j<Y#ab`~*G*Z-cX71cu;Qupc}G_JOPS5b_nc488;xzyNH54R8=p@ECX$+=5>>
z!3}U7d<xzN?}2y023P~i*Au`T#sTAialkn6Uw7afp*RG?j<$ftOhzFOWM`Rp;jrs7
z+8*1=)@Yn!vXix%o2S_27RxO{##EXe!O&*5h;B4ZBOJ<TcpHhd%`^qI32($)R1r(B
z(ZEL;ij}lmhj;83BBqIL{(UtSvP~|tu0vO8X)EC}8HL>2R+KU^i<r|7q!cxu8=pDO
zgosZo?Xe3p)W_OoDaI}qNiqvgF0L0DmXqO=MU<~k?U8Ogk*&y+dwsI561BV;^|!oC
zrseIVkd)fuvlMu)z;{xxlNzE~3Q?{Q6$;0s`^?f(z0pt)(Pty(&*?EepL%9lF$+98
za{M8qGE`GZ?Iy*B(Zo{S7zQN6cff-zzC!KVk`|e4<(w_ITFg_qzB7nu+FB(O4)^1U
zJcaM}`a=G&(f_Ataci-33+9$@)l0YP>scqUuht#<j$}G}lC-S8N-=6fk+z>pJ})Fc
z>B{&p?#$EYXdbDO>rmPlyIhR^R5K4rhEpnCEh6l^)M4bQI@^_I<!Dl-o^ojj9z?b}
z((3r+r8q3B=xG|z6X76_*?EGe89S&j8$$nRdaL|9O+j@%d2|>>>YVyfbff6fl;O<-
zF0;zAuu3{0C)QIXU!mGCUCZS64t$AEt0_X-6R#Cq(y^0wvVJA8YH71;%P>2gHeoLv
zJbJ)5-EhR{nLc;>RQ4HdV!O5&P#)7h^F7J~yywu3+oj5rB(3!fMXKf<y-D)90+}tl
zre>?+oT}OlS+B=k&H@?JRzzDI%iA5P!rtU<rbVHdNe+d8Tv}yJQD&Pq>CtI@)C<O{
zU9%6Wii#!S*xC`bU`-3m9)Aa+Do{ECzYCP-f%5n3&yEnw@>q`t4CBO++>Nn&mj|vt
h^cY?7(^aw7kD|k+%Z`646kPTP<%_Dz#iE#o`~{zrT{Zv!
literal 0
HcmV?d00001
diff --git a/firmware/drivers/pid_controller/pid_controller.cpp b/firmware/drivers/pid_controller/pid_controller.cpp
new file mode 100644
index 0000000..3ed79ba
--- /dev/null
+++ b/firmware/drivers/pid_controller/pid_controller.cpp
@@ -0,0 +1,83 @@
+#include <pid_controller.hpp>
+#include <algorithm>
+#include <ltc6992.hpp>
+
+//define statics
+double Pid_controller::setpoint_=0.; //target temperature in °C
+double Pid_controller::output_=0.05; //output to LTC6992
+std::vector<double> Pid_controller::pid_vec_ = {0.,0.,0.}; //current values of P I D
+std::vector<double> Pid_controller::kpid_vec_ = {0., 0., 0.}; //coefficients Kp Ki Kd
+double Pid_controller::I_reset_ = 0.; //reset value of I
+double Pid_controller::I_reset_errsq_ = 0.; //value of error*error above which the I-term is held in reset
+
+
+Pid_controller::
+
+
+void Pid_controller::init(double setpoint, std::vector<double> kpid, double I_reset, double I_reset_errsq){
+ setpoint_ = setpoint;
+ kpid_ = kpid;
+ I_reset_ = I_reset;
+ I_reset_errsq_ = I_reset_errsq;
+}
+
+void Pid_controller::update_pid(double temperature){
+ //calculate error from setpoint
+ double error = setpoint_ - temperature;
+ //update P,I,D
+ pid_vec_[2] = error - pid_vec_[0];
+ (error*error < I_reset_errsq_) ? pid_vec_[1] += error : pid_vec_[1] = I_reset_;
+ pid_vec_[0] = error;
+ //calculate output
+ output_ = kpid_vec_[0]*pid_vec_[0] + kpid_vec_[1]*pid_vec_[1] + kpid_vec_[2]*pid_vec_[2];
+ //clamp output to rails
+ output_ = std::max(output_, 0.1);
+ output_ = std::min(output_, 0.9);
+ //send value to controller
+ Ltc6992::set_level(output_);
+}
+
+void Pid_controller::set_Kp(double Kp){
+ kpid_vec_[0] = Kp;
+}
+
+void Pid_controller::set_Ki(double Ki){
+ kpid_vec_[1] = Ki;
+}
+
+void Pid_controller::set_Kd(double Kd){
+ kpid_vec_[2] = Kd;
+}
+
+void Pid_controller::set_I_reset(double I_reset){
+ I_reset_ = I_reset;
+ pid_vec_[1] = I_reset;
+}
+
+void Pid_controller::set_I_reset_errsq(double I_reset_errsq){
+ I_reset_errsq_ = I_reset_errsq;
+}
+
+void Pid_controller::set_setpoint(double temperature){
+ setpoint_ = temperature;
+}
+
+std::vector<double> Pid_controller::get_kpid() const{
+ return kpid_vec_;
+}
+
+std::vector<double> Pid_controller::get_pid() const{
+ return pid_vec_;
+}
+
+double Pid_controller::get_output() const{
+ return output_;
+}
+
+double Pid_controller::get_I_reset() const{
+ return I_reset_;
+}
+
+double Pid_controller::get_setpoint() const{
+ return setpoint_;
+}
diff --git a/firmware/drivers/pid_controller/pid_controller.hpp b/firmware/drivers/pid_controller/pid_controller.hpp
new file mode 100644
index 0000000..95a89c5
--- /dev/null
+++ b/firmware/drivers/pid_controller/pid_controller.hpp
@@ -0,0 +1,33 @@
+#ifndef PID_CONTROLLER_HPP_INCLUDED
+#define PID_CONTROLLER_HPP_INCLUDED
+#include <ltc6992.hpp>
+#include <vector>
+
+class Pid_controller{
+public:
+ static void init(double setpoint, std::vector<double> kpid, double I_reset, double I_reset_errsq);
+ static void update_pid(double temperature);
+
+ static void set_Kp(double Kp);
+ static void set_Ki(double Ki);
+ static void set_Kd(double Kd);
+ static void set_I_reset(double I_reset);
+ static void set_I_reset_errsq(double I_reset_errsq);
+ static void set_setpoint(double temperature);
+
+ static std::vector<double> get_kpid() const;
+ static std::vector<double> get_pid() const;
+ static double get_output() const;
+ static double get_I_reset() const;
+ static double get_setpoint() const;
+
+private:
+ static double setpoint_; //target temperature in °C
+ static double output_; //output to LTC6992
+ static std::vector<double> pid_vec_; //current values of P I D
+ static std::vector<double> kpid_vec_; //coefficients Kp Ki Kd
+ static double I_reset_; //reset value of I
+ static double I_reset_errsq_; //value of error*error above which the I-term is held in reset
+};
+
+#endif
--
GitLab