From d0a1cc366e1657b4d65bd5c3bef35f4173ef2b15 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 24 May 2023 20:51:23 +0200 Subject: fix protocol implementation --- zumo/control.cpp | 37 +++++++++++++++++++++++++++++ zumo/control.h | 12 ++++++++++ zumo/protocol.cpp | 48 ++++++-------------------------------- zumo/protocol.h | 7 ------ zumo/protocoltest/.gitignore | 1 + zumo/protocoltest/makefile | 21 +++++++++++++++++ zumo/protocoltest/protocoltest.cpp | 35 +++++++++++++++++++++++++++ 7 files changed, 113 insertions(+), 48 deletions(-) create mode 100644 zumo/control.cpp create mode 100644 zumo/control.h create mode 100644 zumo/protocoltest/.gitignore create mode 100644 zumo/protocoltest/makefile create mode 100644 zumo/protocoltest/protocoltest.cpp diff --git a/zumo/control.cpp b/zumo/control.cpp new file mode 100644 index 0000000..263c145 --- /dev/null +++ b/zumo/control.cpp @@ -0,0 +1,37 @@ +#include +#include +#include + +#include "protocol.h" + +#define DUI_SPEED_MOD 96.0f +#define DUI_MOTOR_DIFF 0.6f + +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 +} + +inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); } + +unsigned char uart_read() { + if (rx() == true) return 0x00; // return immediately if line is idle + + delayMicroseconds(1500); // wait out start bit + + unsigned char byte = 0x00; + for (unsigned int i = 0; i < 8; i++) { + byte = (byte << 1) | rx(); + delayMicroseconds(1000); + } + + delayMicroseconds(1000); // wait out stop bit + + return byte; +} + diff --git a/zumo/control.h b/zumo/control.h new file mode 100644 index 0000000..12ecbd7 --- /dev/null +++ b/zumo/control.h @@ -0,0 +1,12 @@ +#pragma once + +#include "protocol.h" + +#define DUI_PINOUT_NICLA_TX 13 +#define DUI_PINOUT_NICLA_RX 14 + +/** @brief non blocking read byte */ +unsigned char uart_read(); +/** @brief apply state to motors */ +void apply_state(dui_state_t* state); + diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp index f11827c..ab8397a 100644 --- a/zumo/protocol.cpp +++ b/zumo/protocol.cpp @@ -1,55 +1,21 @@ -#include -#include -#include - #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 +#define DUI_CMD_SPEED_START 0x10 +#define DUI_CMD_SPEED_END 0x1f +#define DUI_CMD_STEER_START 0x20 +#define DUI_CMD_STEER_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); + state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_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) * (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 -} - -inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); } - -unsigned char uart_read() { - if (rx() == true) return 0x00; // return immediately if line is idle - - delayMicroseconds(1500); // wait out start bit - - unsigned char byte = 0x00; - for (unsigned int i = 0; i < 8; i++) { - byte = (byte << 1) | rx(); - delayMicroseconds(1000); - } - - delayMicroseconds(1000); // wait out stop bit - - return byte; -} diff --git a/zumo/protocol.h b/zumo/protocol.h index 9db7902..a1f9951 100644 --- a/zumo/protocol.h +++ b/zumo/protocol.h @@ -1,8 +1,5 @@ #pragma once -#define DUI_PINOUT_NICLA_TX 13 -#define DUI_PINOUT_NICLA_RX 14 - typedef enum { DUI_CMD_NULL, DUI_CMD_SIGN, @@ -29,10 +26,6 @@ typedef struct { 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/protocoltest/.gitignore b/zumo/protocoltest/.gitignore new file mode 100644 index 0000000..fb5d35f --- /dev/null +++ b/zumo/protocoltest/.gitignore @@ -0,0 +1 @@ +protocoltest diff --git a/zumo/protocoltest/makefile b/zumo/protocoltest/makefile new file mode 100644 index 0000000..7a69352 --- /dev/null +++ b/zumo/protocoltest/makefile @@ -0,0 +1,21 @@ +CPP = g++ +LD = g++ +RM = rm -f +CFLAGS = +LFLAGS = +TARGET = protocoltest + +SRCS := $(TARGET).cpp ../protocol.cpp +OBJS := $(TARGET).o ../protocol.o + +all: $(TARGET) + +%.o: %.cpp + $(CPP) -c $(CFLAGS) $< -o $@ + +$(TARGET): $(OBJS) + $(LD) $^ $(LFLAGS) -o $@ + +clean: + $(RM) $(TARGET) $(OBJS) + diff --git a/zumo/protocoltest/protocoltest.cpp b/zumo/protocoltest/protocoltest.cpp new file mode 100644 index 0000000..461221e --- /dev/null +++ b/zumo/protocoltest/protocoltest.cpp @@ -0,0 +1,35 @@ +#include +#include + +#include "../protocol.h" + +void print_state(dui_state_t* s) { + printf("{ steer = %1.3f\n speed = %1.3f }\n", s->steer, s->speed); +} + +void test_case(unsigned char cmd, dui_state_t* s) { + printf("\ncmd in = 0x%02x\n", cmd); + handle_cmd(cmd, s); + print_state(s); +} + +int main() { + dui_state_t state = { + .steer = 0.f, + .speed = 1.f, + .current_sign = DUI_SIGN_NONE, + .speed_mod = 1.f, + }; + + test_case(0x00, &state); + test_case(0x01, &state); + test_case(0x02, &state); + test_case(0x10, &state); + test_case(0x15, &state); + test_case(0x1f, &state); + test_case(0x20, &state); + test_case(0x25, &state); + test_case(0x2f, &state); + test_case(0x88, &state); + test_case(0xff, &state); +} -- cgit v1.2.3