This repo is for driving a differential-drived robot.
Dependencies: mbed ros_lib_kinetic_1 Encoder
Diff: controller.cpp
- Revision:
- 0:b21301d612b9
diff -r 000000000000 -r b21301d612b9 controller.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller.cpp Wed Mar 10 03:23:54 2021 +0000 @@ -0,0 +1,71 @@ +#include "controller.h" +Ctrl::Ctrl() { + +} + + +void Ctrl::init(){ + lp_w = 0; + error_last = 0; + volt = 0; + w0 = 0; + w1 = 0; + w2 = 0; + theta_last = 0; + dt = 0.02; +} + +float Ctrl::PID() { + error = ref - lp_w; + volt += error*0.018 - error_last*0.01689; + error_last = error; + return volt; +} + +float Ctrl::get_omega(){ + w2 = (theta - theta_last)/dt; + theta_last = theta; + return w2; +} + +float Ctrl::low_pass_filter(){ + lp_w = (w0 + 2*w1 + w2)/4; + w1 = w2; + w0 = w1; + return lp_w; +} + +void Ctrl::omega_processor(){ + Ctrl::get_omega(); + Ctrl::low_pass_filter(); +} + +/* + +int main(void) { + + Ctrl t; + t.ref = 11; + t.lp_w=0; + + while(1) { + t.ref = 11; + gain = t.PID(); + + if .... + endif + + t.theta_last = t.theta; + t.theta = counter /4; + + t.w0 = t.w1; + t.w1 = t.w2; + t.w2 = t.get_omega(); + t.lp_w = t.low_pass_filter(); + + } + + return 0; +} + +*/ \ No newline at end of file