diff options
| -rw-r--r-- | zumo/pid.cpp | 36 | ||||
| -rw-r--r-- | zumo/pid.h | 7 | ||||
| -rw-r--r-- | zumo/protocol.cpp | 8 | ||||
| -rw-r--r-- | zumo/zumo.ino | 17 |
4 files changed, 58 insertions, 10 deletions
diff --git a/zumo/pid.cpp b/zumo/pid.cpp new file mode 100644 index 0000000..594d136 --- /dev/null +++ b/zumo/pid.cpp @@ -0,0 +1,36 @@ +#include "pid.h" + +class PID { +private: + float A0, A1, A2; + float error[3] = {0}; + float dt = 0.010; + float output = 0; + +public: + PID(float P, float I, float D) { + A0 = P + I*dt + D/dt; + A1 = -P - 2*D/dt; + A2 = D/dt; + } + + // https://en.wikipedia.org/wiki/PID_controller#Pseudocode + float iter(float current, float target) { + error[2] = error[1]; + error[1] = error[0]; + error[0] = target - current; + output = output + A0 * error[0] + A1 * error[1] + A2 * error[2]; + return output; + } +}; + +PID speed_pid(0.1, 0.0, 0.0); // TODO: tune these (garbage values) +PID steer_pid(0.1, 0.0, 0.0); +PID speed_mod_pid(1, 1, 1); +void apply_pid(dui_state_t* target, dui_state_t* current) { + current->speed = speed_pid.iter(current->speed, target->speed); + current->steer = steer_pid.iter(current->steer, target->steer); + // current->speed_mod = speed_mod_pid.iter(current->speed_mod, target->speed_mod); + current->speed_mod = target->speed_mod; +} + diff --git a/zumo/pid.h b/zumo/pid.h new file mode 100644 index 0000000..2cdbc91 --- /dev/null +++ b/zumo/pid.h @@ -0,0 +1,7 @@ +#pragma once + +#include "protocol.h" + +/** @brief edit `current` to be closer to `target` using PID controllers */ +void apply_pid(dui_state_t* target, dui_state_t* current); + diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index a0c72f0..fea8a00 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -2,6 +2,9 @@ #include "protocol.h" +#define DUI_SPEED_MOD 96.0f +#define DUI_MOTOR_DIFF 0.6f + #define DUI_CMD_NULL 0x00 #define DUI_CMD_SIGN_START 0x01 #define DUI_CMD_SIGN_END 0x0f @@ -22,9 +25,8 @@ void handle_cmd(unsigned char cmd, dui_state_t *state) { } void apply_state(dui_state_t *state) { - const float MAX_MOTOR_DIFF = 0.6f; // 0 to 1 - float motor_l = 0.5f * state->speed * (+1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod; - float motor_r = 0.5f * state->speed * (-1.f * state->steer * MAX_MOTOR_DIFF - MAX_MOTOR_DIFF + 2) * state->speed_mod; + float motor_l = 0.5f * state->speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD; + float motor_r = 0.5f * state->speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD; Zumo32U4Motors::setLeftSpeed((int16_t) motor_l); Zumo32U4Motors::setRightSpeed((int16_t) motor_r); diff --git a/zumo/zumo.ino b/zumo/zumo.ino index ed63f6e..f59bd36 100644 --- a/zumo/zumo.ino +++ b/zumo/zumo.ino @@ -3,29 +3,32 @@ #include <Wire.h> #include "protocol.h" +#include "pid.h" dui_state_t g_dui_target_state = { .steer = 1.0f, .speed = 1.0f, .current_sign = DUI_SIGN_NONE, - .speed_mod =96.0, + .speed_mod = 1.f, }; dui_state_t g_dui_current_state = { - .steer = 0, - .speed = 0, + .steer = 0.f, + .speed = 1.f, .current_sign = DUI_SIGN_NONE, - .speed_mod = 1.0, + .speed_mod = 1.f, }; void setup() { } void loop() { - unsigned char cmd = 0; + unsigned char cmd = 0x00; while ((cmd = uart_read())) handle_cmd(cmd, &g_dui_target_state); - //TODO: PID controllers + sign handlers + apply_pid(&g_dui_target_state, &g_dui_current_state); - apply_state(&g_dui_target_state); + apply_state(&g_dui_current_state); + + delay(10); } |