From 365dcc18fbd98645585cdbe009f537ecdaa90c1a Mon Sep 17 00:00:00 2001 From: lonkaars Date: Tue, 17 May 2022 21:21:00 +0200 Subject: WIP sercomm implementation - moved some module-specific constants to their respective header files - changed .clang-tidy to ignore global private global constants (starting with `_`) - suppressed some GCC warnings in bin.c and all pololu library warnings - added function signatures for sercomm protocol data generators - added endianness check in setup.c --- protocol.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index f5bb0cc..d41b00f 100644 --- a/protocol.md +++ b/protocol.md @@ -25,8 +25,8 @@ is converted to a single `0xff` on the receiving end, so these duplicated bytes and the starting byte don't count towards message length. opcodes are picked sequentially, but the direction bit (LSB) is reserved to -indicate a transfer from robot to client. this means that the opcode for a -sensor data request would be `0x12`, but the response opcode would be `0x13`. +indicate a transfer from robot to client (`tx`). this means that the opcode for +a sensor data request would be `0x12`, but the response opcode would be `0x13`. these opcodes are stored as enum constants inside consts.h for code readability. @@ -54,6 +54,9 @@ a double stroke arrow means that the command can be initiated from either the robot or the client, while a single arrow indicates a request-response structure. +in *both* the robot and client code `r <-- c` is referred to as `rx` and `r +--> c` as `tx` (from the *robots* view). + ### PING #### ping (`r <=> c`) (2 bytes) -- cgit v1.2.3 From 931cf5f2c3a0fdf947ba2c4b839bc886a43b4701 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 25 May 2022 19:16:41 +0200 Subject: implement ping command --- protocol.md | 4 ++-- robot/sercomm.c | 18 +++++++++++++++++- 2 files changed, 19 insertions(+), 3 deletions(-) (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index d41b00f..4db4032 100644 --- a/protocol.md +++ b/protocol.md @@ -27,12 +27,12 @@ and the starting byte don't count towards message length. opcodes are picked sequentially, but the direction bit (LSB) is reserved to indicate a transfer from robot to client (`tx`). this means that the opcode for a sensor data request would be `0x12`, but the response opcode would be `0x13`. -these opcodes are stored as enum constants inside consts.h for code +these opcodes are stored as enum constants inside shared/protocol.h for code readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| -|`0x00`|[PING](#ping)|no|`r <=> c`|ping +|`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|no|`r --> c`|exception |`0x04`|[MODE](#mode)|no|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed diff --git a/robot/sercomm.c b/robot/sercomm.c index da632b5..44bf5f9 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -47,7 +47,23 @@ void w2_sercomm_append_msg(w2_s_bin *data) { g_w2_sercomm_index = next_index; } -void w2_scmd_ping_rx(w2_s_bin *data) { return; } +void w2_scmd_ping_rx(w2_s_bin *data) { + w2_s_cmd_ping_rx *message = malloc(w2_scmd_length(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + size_t return_size = sizeof(w2_s_cmd_ping_tx); + w2_s_cmd_ping_tx *return_message = malloc(return_size); + return_message->opcode = (message->opcode & W2_CMD_DIRECTION_MASK) | W2_CMDDIR_TX; + return_message->id = message->id; + + w2_s_bin *return_message_bin = w2_bin_s_alloc(return_size, (uint8_t *)return_message); + + w2_sercomm_append_msg(return_message_bin); + + free(message); + free(return_message); + free(return_message_bin); +} void w2_scmd_mode_rx(w2_s_bin *data) { return; } -- cgit v1.2.3 From 4487ce5c3083d95fad26ebca790b0f849821d736 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 12:43:11 +0200 Subject: update implementation status in protocol.md --- protocol.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index 4db4032..915fa65 100644 --- a/protocol.md +++ b/protocol.md @@ -42,7 +42,7 @@ readability. |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset |`0x10`|[MCFG](#mcfg)|no|`r <-- c`|map config |`0x12`|[SENS](#sens)|no|`r <-> c`|sensor data -|`0x14`|[INFO](#info)|no|`r <-> c`|info +|`0x14`|[INFO](#info)|yes|`r <-> c`|info |`0x16`|[DISP](#disp)|no|`r <-- c`|display control |`0x18`|[PLAY](#play)|no|`r <-- c`|play midi |`0x1a`|[CLED](#cled)|no|`r <-- c`|control leds -- cgit v1.2.3 From f7387fd6af14a740f474620555de379bc9ba69db Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 13:04:35 +0200 Subject: forward errors to sercomm (implement expt command --- protocol.md | 10 +++++----- robot/errcatch.c | 13 ++++++++++++- robot/sim.c | 8 ++++++-- robot/sim.h | 4 +++- 4 files changed, 26 insertions(+), 9 deletions(-) (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index 915fa65..7084d14 100644 --- a/protocol.md +++ b/protocol.md @@ -33,7 +33,7 @@ readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| |`0x00`|[PING](#ping)|yes|`r <=> c`|ping -|`0x02`|[EXPT](#expt)|no|`r --> c`|exception +|`0x02`|[EXPT](#expt)|yes|`r --> c`|exception |`0x04`|[MODE](#mode)|no|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed |`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control @@ -294,10 +294,10 @@ requests robot info |-:|-| |`uint8_t`|opcode (`0x14 + 1`)| |`uint8_t[32]`|build string| -|`uint8_t`|errcatch module cycle time (ms)| -|`uint8_t`|io module cycle time (ms)| -|`uint8_t`|sercomm module cycle time (ms)| -|`uint8_t`|modes module cycle time (ms)| +|`uint8_t`|exponential moving average errcatch module cycle time (ms)| +|`uint8_t`|exponential moving average io module cycle time (ms)| +|`uint8_t`|exponential moving average sercomm module cycle time (ms)| +|`uint8_t`|exponential moving average modes module cycle time (ms)| |`uint32_t`|total robot uptime (s)| robot info response diff --git a/robot/errcatch.c b/robot/errcatch.c index 4bdbaef..99f1063 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -5,6 +5,7 @@ #include "halt.h" #include "modes.h" #include "orangutan_shim.h" +#include "sercomm.h" w2_s_error *g_w2_error_buffer[W2_ERROR_BUFFER_SIZE] = {}; uint8_t g_w2_error_index = 0; @@ -66,7 +67,17 @@ void w2_errcatch_handle_error(w2_s_error *error) { } } - // TODO: forward error to sercomm + // forward error to sercomm + size_t msg_size = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length; + w2_s_cmd_expt_tx *msg = malloc(msg_size); + msg->opcode = W2_CMD_EXPT | W2_CMDDIR_TX; + msg->error = error->code; + msg->length = error->message_length; + memcpy(msg->message, error->message, error->message_length); + w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); + w2_sercomm_append_msg(msg_bin); + free(msg); + free(msg_bin); return; } diff --git a/robot/sim.c b/robot/sim.c index 0cde6a0..baf8a8a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -9,6 +9,7 @@ #include "../shared/consts.h" #include "../shared/protocol.h" #include "sercomm.h" +#include "errcatch.h" struct timespec reference_time; // NOLINT bool g_w2_sim_headless = false; @@ -74,10 +75,10 @@ void serial_send(char* message, unsigned int length) { putc(message[byte] & 0xff, stdout); return; } - if (!DBG_ENABLE_PRINTFUNC) return; - simprintfunc("serial_send", ", %u", length); + if (DBG_ENABLE_PRINTFUNC) simprintfunc("serial_send", ", %u", length); + if (!DBG_ENABLE_SERIAL) return; w2_s_bin *bin = w2_bin_s_alloc(length, (uint8_t*) message); w2_sim_print_serial(bin); free(bin); @@ -103,6 +104,9 @@ void w2_sim_setup(int argc, char **argv) { term.c_cc[VTIME] = 0; term.c_cc[VMIN] = 0; tcsetattr(STDIN_FILENO, 0, &term); + + // debug error + // w2_errcatch_throw(W2_E_WARN_BATTERY_LOW); } void w2_sim_cycle_begin() { diff --git a/robot/sim.h b/robot/sim.h index 6e2498d..25cc713 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -10,10 +10,12 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (1) +#define DBG_ENABLE_PRINTFUNC (0) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) +#define DBG_ENABLE_SERIAL (1) + #define DBG_MAX_CYCLES (10) // debug print options -- cgit v1.2.3 From d70f4f44f927281d6c9bfff64264bd754d682dc8 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 14:05:53 +0200 Subject: implement mode command --- protocol.md | 13 +++++++++++-- robot/calibration.c | 1 - robot/calibration.h | 9 --------- robot/errcatch.c | 3 +-- robot/grid.c | 1 - robot/grid.h | 8 -------- robot/halt.c | 5 ----- robot/halt.h | 8 -------- robot/maze.c | 1 - robot/maze.h | 8 -------- robot/mode_grid.c | 3 +++ robot/mode_grid.h | 8 ++++++++ robot/mode_halt.c | 3 +++ robot/mode_halt.h | 8 ++++++++ robot/mode_lcal.c | 1 + robot/mode_lcal.h | 9 +++++++++ robot/mode_maze.c | 3 +++ robot/mode_maze.h | 8 ++++++++ robot/modes.c | 20 +++++++++++++++++++- robot/modes.h | 30 ++++++++++++++++++++++++++++++ robot/readme.md | 2 +- robot/sercomm.c | 8 +++++++- robot/setup.c | 1 - robot/tests/mode.bin | 1 + 24 files changed, 113 insertions(+), 49 deletions(-) delete mode 100644 robot/calibration.c delete mode 100644 robot/calibration.h delete mode 100644 robot/grid.c delete mode 100644 robot/grid.h delete mode 100644 robot/halt.c delete mode 100644 robot/halt.h delete mode 100644 robot/maze.c delete mode 100644 robot/maze.h create mode 100644 robot/mode_grid.c create mode 100644 robot/mode_grid.h create mode 100644 robot/mode_halt.c create mode 100644 robot/mode_halt.h create mode 100644 robot/mode_lcal.c create mode 100644 robot/mode_lcal.h create mode 100644 robot/mode_maze.c create mode 100644 robot/mode_maze.h create mode 100644 robot/tests/mode.bin (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index 7084d14..345fe50 100644 --- a/protocol.md +++ b/protocol.md @@ -34,7 +34,7 @@ readability. |--:|---|:-:|:-:|---| |`0x00`|[PING](#ping)|yes|`r <=> c`|ping |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception -|`0x04`|[MODE](#mode)|no|`r <=> c`|mode +|`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed |`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate @@ -96,7 +96,16 @@ message, and can be 0 in case of no message. |`uint8_t`|mode code| when initiated from the client, the **mode** command forces the robot to change -execution mode. the mode codes are undetermined as of now. +execution mode. **mode** can be one of: + +- 0: mode_maze +- 1: mode_grid +- 2: mode_halt +- 3: mode_lcal +- 4: mode_chrg +- 5: mode_dirc +- 6: mode_spin +- 7: mode_scal #### get mode (`r --> c`) (2 bytes) diff --git a/robot/calibration.c b/robot/calibration.c deleted file mode 100644 index 7788532..0000000 --- a/robot/calibration.c +++ /dev/null @@ -1 +0,0 @@ -#include "calibration.h" diff --git a/robot/calibration.h b/robot/calibration.h deleted file mode 100644 index 5c1af38..0000000 --- a/robot/calibration.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -/** - * calibration mode - * - * turns robot on its own axis 360 degress, and aligns the front sensors with - * the line if found, else triggers halt mode (emergency) - */ -void w2_mode_calb(); diff --git a/robot/errcatch.c b/robot/errcatch.c index 99f1063..cf06f5d 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -2,7 +2,6 @@ #include #include "errcatch.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -52,7 +51,7 @@ void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; // trigger emergency mode for critical errors - if ((severity ^ W2_E_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt; + if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT); // TODO: handle more error types switch (error->code) { diff --git a/robot/grid.c b/robot/grid.c deleted file mode 100644 index 0c83272..0000000 --- a/robot/grid.c +++ /dev/null @@ -1 +0,0 @@ -#include "grid.h" diff --git a/robot/grid.h b/robot/grid.h deleted file mode 100644 index fcf9100..0000000 --- a/robot/grid.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * warehouse mode - * - * processes orders from the order buffer - */ -void w2_mode_grid(); diff --git a/robot/halt.c b/robot/halt.c deleted file mode 100644 index 2f159f0..0000000 --- a/robot/halt.c +++ /dev/null @@ -1,5 +0,0 @@ -#include - -#include "halt.h" - -void w2_mode_halt() { return; } diff --git a/robot/halt.h b/robot/halt.h deleted file mode 100644 index d92905e..0000000 --- a/robot/halt.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * halt (emergency) mode - * - * stops all execution until emergency status is manually cleared by the user - */ -void w2_mode_halt(); diff --git a/robot/maze.c b/robot/maze.c deleted file mode 100644 index a27414f..0000000 --- a/robot/maze.c +++ /dev/null @@ -1 +0,0 @@ -#include "maze.h" diff --git a/robot/maze.h b/robot/maze.h deleted file mode 100644 index 9fbeb8c..0000000 --- a/robot/maze.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -/** - * maze mode - * - * finds route out of maze - */ -void w2_mode_maze(); diff --git a/robot/mode_grid.c b/robot/mode_grid.c new file mode 100644 index 0000000..8526499 --- /dev/null +++ b/robot/mode_grid.c @@ -0,0 +1,3 @@ +#include "mode_grid.h" + +void w2_mode_grid() {} diff --git a/robot/mode_grid.h b/robot/mode_grid.h new file mode 100644 index 0000000..fcf9100 --- /dev/null +++ b/robot/mode_grid.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * warehouse mode + * + * processes orders from the order buffer + */ +void w2_mode_grid(); diff --git a/robot/mode_halt.c b/robot/mode_halt.c new file mode 100644 index 0000000..88d6183 --- /dev/null +++ b/robot/mode_halt.c @@ -0,0 +1,3 @@ +#include "mode_halt.h" + +void w2_mode_halt() { return; } diff --git a/robot/mode_halt.h b/robot/mode_halt.h new file mode 100644 index 0000000..d92905e --- /dev/null +++ b/robot/mode_halt.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * halt (emergency) mode + * + * stops all execution until emergency status is manually cleared by the user + */ +void w2_mode_halt(); diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c new file mode 100644 index 0000000..6b4e736 --- /dev/null +++ b/robot/mode_lcal.c @@ -0,0 +1 @@ +#include "mode_lcal.h" diff --git a/robot/mode_lcal.h b/robot/mode_lcal.h new file mode 100644 index 0000000..dd373f0 --- /dev/null +++ b/robot/mode_lcal.h @@ -0,0 +1,9 @@ +#pragma once + +/** + * calibration mode + * + * turns robot on its own axis 360 degress, and aligns the front sensors with + * the line if found, else triggers halt mode (emergency) + */ +void w2_mode_lcal(); diff --git a/robot/mode_maze.c b/robot/mode_maze.c new file mode 100644 index 0000000..6ae62be --- /dev/null +++ b/robot/mode_maze.c @@ -0,0 +1,3 @@ +#include "mode_maze.h" + +void w2_mode_maze() {} diff --git a/robot/mode_maze.h b/robot/mode_maze.h new file mode 100644 index 0000000..9fbeb8c --- /dev/null +++ b/robot/mode_maze.h @@ -0,0 +1,8 @@ +#pragma once + +/** + * maze mode + * + * finds route out of maze + */ +void w2_mode_maze(); diff --git a/robot/modes.c b/robot/modes.c index c22875c..1f36703 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,6 +1,24 @@ #include "modes.h" -#include "halt.h" +#include "sercomm.h" void (*g_w2_current_mode)() = &w2_mode_halt; void w2_modes_main() { (*g_w2_current_mode)(); } + +void w2_modes_switch(w2_e_mode new_mode) { + if (new_mode == W2_M_PREV) { + // TODO implement previous mode buffer + } else { + g_w2_current_mode = W2_MODES[new_mode]; + } + + // forward mode change to sercomm + size_t msg_size = sizeof(w2_s_cmd_mode_tx); + w2_s_cmd_mode_tx *msg = malloc(msg_size); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; + msg->mode = (uint8_t)new_mode; + w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); + w2_sercomm_append_msg(msg_bin); + free(msg); + free(msg_bin); +} diff --git a/robot/modes.h b/robot/modes.h index dd34690..fe81043 100644 --- a/robot/modes.h +++ b/robot/modes.h @@ -1,5 +1,11 @@ #pragma once +#include "mode_grid.h" +#include "mode_halt.h" +#include "mode_lcal.h" +#include "mode_maze.h" + +/** function pointer to current mode */ extern void (*g_w2_current_mode)(); /** @@ -8,3 +14,27 @@ extern void (*g_w2_current_mode)(); * executes mode in g_w2_current_mode */ void w2_modes_main(); + +/** mode constants */ +typedef enum { + W2_M_PREV = -1, + W2_M_MAZE = 0, + W2_M_GRID = 1, + W2_M_HALT = 2, + W2_M_LCAL = 3, + W2_M_CHRG = 4, + W2_M_DIRC = 5, + W2_M_SPIN = 6, + W2_M_SCAL = 7, +} w2_e_mode; + +/** array that maps w2_e_mode to mode function pointers */ +static const void(*const W2_MODES[]) = { + &w2_mode_maze, + &w2_mode_grid, + &w2_mode_grid, + &w2_mode_halt, +}; + +/** switch the current mode */ +void w2_modes_switch(w2_e_mode new_mode); diff --git a/robot/readme.md b/robot/readme.md index 2de14db..31acafc 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -62,7 +62,7 @@ what they're supposed to do: |maze |`mode_maze `|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module| |warehouse |`mode_grid `|Loek| controls robot during warehouse portion of map; hands off control to maze module| |emergency stop |`mode_halt `|Fiona| stops all execution until emergency mode is reset by software or user| -|calibration |`mode_calb `|Fiona| find line by turning on own axis if lost| +|calibration |`mode_lcal `|Fiona| find line by turning on own axis if lost| ## some standards diff --git a/robot/sercomm.c b/robot/sercomm.c index a3ccdb0..e972f28 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -66,7 +67,12 @@ void w2_cmd_ping_rx(w2_s_bin *data) { free(return_message_bin); } -void w2_cmd_mode_rx(w2_s_bin *data) { return; } +void w2_cmd_mode_rx(w2_s_bin *data) { + w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + w2_modes_switch(message->mode); +} void w2_cmd_sped_rx(w2_s_bin *data) { return; } diff --git a/robot/setup.c b/robot/setup.c index d075c41..1ecda43 100644 --- a/robot/setup.c +++ b/robot/setup.c @@ -2,7 +2,6 @@ #include "../shared/bin.h" #include "../shared/consts.h" -#include "halt.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin new file mode 100644 index 0000000..35ebd35 --- /dev/null +++ b/robot/tests/mode.bin @@ -0,0 +1 @@ +ÿ \ No newline at end of file -- cgit v1.2.3 From f073c9d3848dab915bed4844e9d13684aa5e23eb Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 26 May 2022 15:34:58 +0200 Subject: implement direct control --- protocol.md | 2 +- robot/mode_dirc.c | 6 +++++- robot/mode_dirc.h | 5 +++++ robot/sercomm.c | 9 ++++++++- robot/sim.c | 3 +++ robot/sim.h | 4 +++- robot/tests/dirc.bin | Bin 0 -> 6 bytes robot/tests/mode.bin | 2 +- 8 files changed, 26 insertions(+), 5 deletions(-) create mode 100644 robot/tests/dirc.bin (limited to 'protocol.md') diff --git a/protocol.md b/protocol.md index 345fe50..c6cb3fb 100644 --- a/protocol.md +++ b/protocol.md @@ -36,7 +36,7 @@ readability. |`0x02`|[EXPT](#expt)|yes|`r --> c`|exception |`0x04`|[MODE](#mode)|yes|`r <=> c`|mode |`0x06`|[SPED](#sped)|no|`r <-- c`|speed -|`0x08`|[DIRC](#dirc)|no|`r <-- c`|direct control +|`0x08`|[DIRC](#dirc)|yes|`r <-- c`|direct control |`0x0a`|[CORD](#cord)|no|`r <=> c`|coordinate |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|backorder modify |`0x0e`|[SRES](#sres)|no|`r <-- c`|soft reset diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 6c5d2bb..0bbf3cb 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,3 +1,7 @@ #include "mode_dirc.h" +#include "orangutan_shim.h" -void w2_mode_dirc() {} +int16_t g_w2_mode_dirc_motor_l = 0; +int16_t g_w2_mode_dirc_motor_r = 0; + +void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); } diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h index 25a664a..5b9bbf4 100644 --- a/robot/mode_dirc.h +++ b/robot/mode_dirc.h @@ -1,5 +1,10 @@ #pragma once +#include + +extern int16_t g_w2_mode_dirc_motor_l; +extern int16_t g_w2_mode_dirc_motor_r; + /** * direct control mode * diff --git a/robot/sercomm.c b/robot/sercomm.c index c9c6194..2786b85 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,6 +4,7 @@ #include "../shared/bin.h" #include "../shared/serial_parse.h" #include "hypervisor.h" +#include "mode_dirc.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -76,7 +77,13 @@ void w2_cmd_mode_rx(w2_s_bin *data) { void w2_cmd_sped_rx(w2_s_bin *data) { return; } -void w2_cmd_dirc_rx(w2_s_bin *data) { return; } +void w2_cmd_dirc_rx(w2_s_bin *data) { + w2_s_cmd_dirc_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + g_w2_mode_dirc_motor_l = w2_bin_ntoh16(message->left); + g_w2_mode_dirc_motor_r = w2_bin_ntoh16(message->right); +} void w2_cmd_cord_rx(w2_s_bin *data) { return; } diff --git a/robot/sim.c b/robot/sim.c index baf8a8a..ddc208a 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -123,3 +123,6 @@ void w2_sim_print_serial(w2_s_bin *data) { printf("\n"); } +void set_motors(int left, int right) { + simprintfunc("set_motors", "%i, %i", left, right); +} diff --git a/robot/sim.h b/robot/sim.h index 25cc713..a595042 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -10,7 +10,7 @@ extern bool g_w2_sim_headless; // debug fine-tuning -#define DBG_ENABLE_PRINTFUNC (0) +#define DBG_ENABLE_PRINTFUNC (1) #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) @@ -54,6 +54,8 @@ void serial_set_baud_rate(unsigned int rate); // NOLINT void serial_send(char *message, unsigned int length); // NOLINT void serial_receive_ring(char *buffer, unsigned char size); // NOLINT unsigned char serial_get_received_bytes(); // NOLINT +void set_motors(int left, int right); // NOLINT + void w2_sim_setup(int argc, char **argv); void w2_sim_cycle_begin(); void w2_sim_print_serial(w2_s_bin *data); diff --git a/robot/tests/dirc.bin b/robot/tests/dirc.bin new file mode 100644 index 0000000..1aea35c Binary files /dev/null and b/robot/tests/dirc.bin differ diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin index 35ebd35..9cd9175 100644 --- a/robot/tests/mode.bin +++ b/robot/tests/mode.bin @@ -1 +1 @@ -ÿ \ No newline at end of file +ÿ \ No newline at end of file -- cgit v1.2.3