diff options
Diffstat (limited to 'robot')
| -rw-r--r-- | robot/modes.c | 5 | ||||
| -rw-r--r-- | robot/readme.md | 3 | ||||
| -rw-r--r-- | robot/sercomm.c | 26 | ||||
| -rw-r--r-- | robot/sim.h | 2 | ||||
| -rw-r--r-- | robot/tests/sens.bin | 1 |
5 files changed, 32 insertions, 5 deletions
diff --git a/robot/modes.c b/robot/modes.c index 8d3a099..9877f6e 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,8 +1,7 @@ -#include <stdbool.h> - +#include "modes.h" +#include "../shared/protocol.h" #include "../shared/util.h" #include "errcatch.h" -#include "modes.h" #include "sercomm.h" /** function pointer to current mode */ diff --git a/robot/readme.md b/robot/readme.md index c1cae72..b27c06f 100644 --- a/robot/readme.md +++ b/robot/readme.md @@ -99,6 +99,9 @@ this list will probably get updated from time to time: - general constants should be placed in `consts.h` - don't import `<pololu/orangutan.h>` directly, instead use `"orangutan_shim.h"` to keep code compatible with the simulator +- don't use `<stdbool.h>`, instead use `"../shared/protocol.h"`. this makes + sure that `bool` values are equal to `uint8_t`. they're functionally + identical. ## todo diff --git a/robot/sercomm.c b/robot/sercomm.c index 2786b85..2736030 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 "io.h" #include "mode_dirc.h" #include "modes.h" #include "orangutan_shim.h" @@ -93,7 +94,30 @@ void w2_cmd_sres_rx(w2_s_bin *data) { return; } void w2_cmd_mcfg_rx(w2_s_bin *data) { return; } -void w2_cmd_sens_rx(w2_s_bin *data) { return; } +void w2_cmd_sens_rx(w2_s_bin *data) { + w2_s_cmd_sens_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); + memcpy(message, data->data, data->bytes); + + size_t return_size = sizeof(w2_s_cmd_sens_tx); + w2_s_cmd_sens_tx *return_message = malloc(return_size); + return_message->opcode = W2_CMD_SENS | W2_CMDDIR_TX; + memcpy((uint8_t *)&return_message->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all)); + + for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&return_message->io.qtr[i].range); + w2_bin_repl_hton16(&return_message->io.front_distance.detection); + w2_bin_repl_hton16(&return_message->io.side_distance.detection); + w2_bin_repl_hton16(&return_message->io.battery.charge_level); + w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_left.speed); + w2_bin_repl_hton16((uint16_t *)&return_message->io.motor_right.speed); + + 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_cmd_info_rx(w2_s_bin *data) { w2_s_cmd_info_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes)); diff --git a/robot/sim.h b/robot/sim.h index f087517..ab1cd77 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -3,9 +3,9 @@ #include <stdio.h> #include <stdlib.h> #include <unistd.h> -#include <stdbool.h> #include "../shared/bin.h" +#include "../shared/protocol.h" extern bool g_w2_sim_headless; diff --git a/robot/tests/sens.bin b/robot/tests/sens.bin new file mode 100644 index 0000000..0f21683 --- /dev/null +++ b/robot/tests/sens.bin @@ -0,0 +1 @@ +ÿ
\ No newline at end of file |