2626#include " ctbot.h"
2727#include " scheduler.h"
2828
29- #include < PID_v1 .h>
29+ #include < pid_v1 .h>
3030#include < cmath>
3131
3232
3333namespace ctbot {
3434
3535std::list<SpeedControl*> SpeedControl::controller_list_;
3636
37- SpeedControl::SpeedControl (Encoder& wheel_enc, Motor& motor) : direction_ { true }, setpoint_ { 0 .f }, kp_ { 40 }, ki_ { 30 }, kd_ { 0 },
38- p_pid_controller_ { new Pid (input_, output_, setpoint_, kp_, ki_, kd_, true ) }, wheel_encoder_ { wheel_enc }, motor_ { motor } {
39- if (! p_pid_controller_) {
37+ SpeedControl::SpeedControl (Encoder& wheel_enc, Motor& motor)
38+ : direction_ { true }, setpoint_ {}, input_ {}, output_ {}, kp_ { 40 .f }, ki_ { 30 .f }, kd_ { 0 .f },
39+ p_pid_controller_ { new Pid (input_, output_, setpoint_, kp_, ki_, kd_, true ) }, wheel_encoder_ { wheel_enc }, motor_ { motor } {
40+ if (!p_pid_controller_) {
4041 return ;
4142 }
4243
@@ -57,7 +58,7 @@ SpeedControl::~SpeedControl() {
5758}
5859
5960void SpeedControl::run () {
60- if (! p_pid_controller_) {
61+ if (!p_pid_controller_) {
6162 return ;
6263 }
6364
@@ -76,7 +77,7 @@ void SpeedControl::run() {
7677}
7778
7879void SpeedControl::set_parameters (const float kp, const float ki, const float kd) {
79- if (! p_pid_controller_) {
80+ if (!p_pid_controller_) {
8081 return ;
8182 }
8283
@@ -87,8 +88,6 @@ void SpeedControl::set_parameters(const float kp, const float ki, const float kd
8788}
8889
8990void SpeedControl::controller (void *) {
90- // std::cout << "SpeedControl::controller(): running speed controller at " << Timer::get_ms() << " ms\n";
91-
9291 for (auto p_ctrl : controller_list_) {
9392 p_ctrl->run ();
9493 }
0 commit comments