diff options
| -rw-r--r-- | protocol.md | 2 | ||||
| -rw-r--r-- | robot/mode_dirc.c | 6 | ||||
| -rw-r--r-- | robot/mode_dirc.h | 5 | ||||
| -rw-r--r-- | robot/sercomm.c | 9 | ||||
| -rw-r--r-- | robot/sim.c | 3 | ||||
| -rw-r--r-- | robot/sim.h | 4 | ||||
| -rw-r--r-- | robot/tests/dirc.bin | bin | 0 -> 6 bytes | |||
| -rw-r--r-- | robot/tests/mode.bin | 2 |
8 files changed, 26 insertions, 5 deletions
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`|<u>ex</u>ce<u>pt</u>ion |`0x04`|[MODE](#mode)|yes|`r <=> c`|<u>mode</u> |`0x06`|[SPED](#sped)|no|`r <-- c`|<u>spe</u>e<u>d</u> -|`0x08`|[DIRC](#dirc)|no|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol +|`0x08`|[DIRC](#dirc)|yes|`r <-- c`|<u>dir</u>ect <u>c</u>ontrol |`0x0a`|[CORD](#cord)|no|`r <=> c`|<u>co</u>o<u>rd</u>inate |`0x0c`|[BOMD](#bomd)|no|`r <=> c`|<u>b</u>ack<u>o</u>rder <u>m</u>o<u>d</u>ify |`0x0e`|[SRES](#sres)|no|`r <-- c`|<u>s</u>oft <u>res</u>et 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 <stdint.h> + +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 Binary files differnew file mode 100644 index 0000000..1aea35c --- /dev/null +++ b/robot/tests/dirc.bin 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 |