From 6472b77eb6582e5133a28f61a448eb6a00cd25b6 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Sun, 14 May 2023 14:05:03 +0200 Subject: initial robot driver code --- zumo/main.cpp | 13 ------------- zumo/protocol.cpp | 37 +++++++++++++++++++++++++++++++++++++ zumo/protocol.h | 35 +++++++++++++++++++++++++++++++++++ zumo/zumo.ino | 31 +++++++++++++++++++++++++++++++ 4 files changed, 103 insertions(+), 13 deletions(-) delete mode 100644 zumo/main.cpp create mode 100644 zumo/protocol.cpp create mode 100644 zumo/protocol.h create mode 100644 zumo/zumo.ino (limited to 'zumo') 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 -#include -#include - -void setup() { -} - -void loop() { - ledRed(1); - delay(1000); - ledRed(0); - delay(1000); -} diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp new file mode 100644 index 0000000..a0c72f0 --- /dev/null +++ b/zumo/protocol.cpp @@ -0,0 +1,37 @@ +#include + +#include "protocol.h" + +#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) { + 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; + + 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..ed63f6e --- /dev/null +++ b/zumo/zumo.ino @@ -0,0 +1,31 @@ +#include +#include +#include + +#include "protocol.h" + +dui_state_t g_dui_target_state = { + .steer = 1.0f, + .speed = 1.0f, + .current_sign = DUI_SIGN_NONE, + .speed_mod =96.0, +}; +dui_state_t g_dui_current_state = { + .steer = 0, + .speed = 0, + .current_sign = DUI_SIGN_NONE, + .speed_mod = 1.0, +}; + +void setup() { +} + +void loop() { + unsigned char cmd = 0; + while ((cmd = uart_read())) + handle_cmd(cmd, &g_dui_target_state); + + //TODO: PID controllers + sign handlers + + apply_state(&g_dui_target_state); +} -- cgit v1.2.3