diff options
| -rw-r--r-- | zumo/main.cpp | 13 | ||||
| -rw-r--r-- | zumo/pid.cpp | 36 | ||||
| -rw-r--r-- | zumo/pid.h | 7 | ||||
| -rw-r--r-- | zumo/protocol.cpp | 39 | ||||
| -rw-r--r-- | zumo/protocol.h | 35 | ||||
| -rw-r--r-- | zumo/zumo.ino | 34 |
6 files changed, 151 insertions, 13 deletions
diff --git a/zumo/main.cpp b/zumo/main.cpp deleted file mode 100644 index 23d13db..0000000 --- a/zumo/main.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include <Zumo32U4.h> -#include <Arduino.h> -#include <Wire.h> - -void setup() { -} - -void loop() { - ledRed(1); - delay(1000); - ledRed(0); - delay(1000); -} 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 new file mode 100644 index 0000000..fea8a00 --- /dev/null +++ b/zumo/protocol.cpp @@ -0,0 +1,39 @@ +#include <Zumo32U4Motors.h> + +#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 +#define DUI_CMD_STEER_START 0x10 +#define DUI_CMD_STEER_END 0x1f +#define DUI_CMD_SPEED_START 0x20 +#define DUI_CMD_SPEED_END 0xff + +void handle_cmd(unsigned char cmd, dui_state_t *state) { + if (cmd == DUI_CMD_NULL) return; + else if (DUI_CMD_SIGN_START <= cmd && cmd <= DUI_CMD_SIGN_END) { + state->current_sign = (dui_e_sign) (cmd - DUI_CMD_SIGN_START); + } else if (DUI_CMD_STEER_START <= cmd && cmd <= DUI_CMD_STEER_END) { + state->steer = (float) (cmd - DUI_CMD_STEER_START) / (float) (DUI_CMD_STEER_END - DUI_CMD_STEER_START); + } else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) { + state->speed = ((float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_START - DUI_CMD_SPEED_END) * (float) 2 - (float) 1); + } +} + +void apply_state(dui_state_t *state) { + 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); + + // TODO: print sign on OLED screen +} + +unsigned char uart_read() { + return 0x00; +} diff --git a/zumo/protocol.h b/zumo/protocol.h new file mode 100644 index 0000000..662a5ce --- /dev/null +++ b/zumo/protocol.h @@ -0,0 +1,35 @@ +#pragma once + +typedef enum { + DUI_CMD_NULL, + DUI_CMD_SIGN, + DUI_CMD_SPEED, + DUI_CMD_STEER, +} dui_e_cmd; + +typedef enum { + DUI_SIGN_NONE, /** @brief no sign */ + DUI_SIGN_STOP, /** @brief stop (set speed to 0) */ + DUI_SIGN_LEFT, /** @brief turn left (set steer to -1) */ + DUI_SIGN_RIGHT, /** @brief turn right (set steer to +1) */ + DUI_SIGN_SPEED_LIMIT_LOW, /** @brief slow down (speed limit 0.5) */ + DUI_SIGN_SPEED_LIMIT_HIGH, /** @brief full speed (speed limit 1.0) */ + DUI_SIGN_LIGHT_STOP, /** @brief traffic light red (set speed to 0) */ + DUI_SIGN_LIGHT_FLOOR_IT, /** @brief traffic light orange (set speed to 2 temporarily) */ + DUI_SIGN_LIGHT_GO, /** @brief traffic light green (keep current speed) */ +} dui_e_sign; + +typedef struct { + float steer; /** @brief steer value (-1 is left, +1 is right) */ + float speed; /** @brief speed (0-15) */ + dui_e_sign current_sign; /** @brief last seen sign */ + float speed_mod; /** @brief global speed multiplier */ +} dui_state_t; + +/** @brief non blocking read byte */ +unsigned char uart_read(); +/** @brief read and apply cmd to state */ +void handle_cmd(unsigned char cmd, dui_state_t *state); +/** @brief apply state to motors */ +void apply_state(dui_state_t* state); + diff --git a/zumo/zumo.ino b/zumo/zumo.ino new file mode 100644 index 0000000..f59bd36 --- /dev/null +++ b/zumo/zumo.ino @@ -0,0 +1,34 @@ +#include <Zumo32U4.h> +#include <Arduino.h> +#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 = 1.f, +}; +dui_state_t g_dui_current_state = { + .steer = 0.f, + .speed = 1.f, + .current_sign = DUI_SIGN_NONE, + .speed_mod = 1.f, +}; + +void setup() { +} + +void loop() { + unsigned char cmd = 0x00; + while ((cmd = uart_read())) + handle_cmd(cmd, &g_dui_target_state); + + apply_pid(&g_dui_target_state, &g_dui_current_state); + + apply_state(&g_dui_current_state); + + delay(10); +} |