diff options
| -rw-r--r-- | client/main.c | 19 | ||||
| -rw-r--r-- | client/serial.c | 10 | ||||
| -rw-r--r-- | client/ui_dirc.c | 7 | ||||
| -rw-r--r-- | robot/errcatch.c | 10 | ||||
| -rw-r--r-- | robot/hypervisor.c | 6 | ||||
| -rw-r--r-- | robot/mode_dirc.c | 13 | ||||
| -rw-r--r-- | robot/mode_dirc.h | 1 | ||||
| -rw-r--r-- | robot/sercomm.c | 20 | ||||
| -rw-r--r-- | robot/sim.h | 4 | ||||
| -rw-r--r-- | shared/consts.h | 2 | ||||
| -rw-r--r-- | shared/serial_parse.c | 2 |
11 files changed, 55 insertions, 39 deletions
diff --git a/client/main.c b/client/main.c index 00f686a..b5af0e8 100644 --- a/client/main.c +++ b/client/main.c @@ -1,15 +1,13 @@ #include "main.h" -#include "../shared/errcatch.h" #include "../shared/consts.h" +#include "../shared/errcatch.h" +#include "commands.h" #include "serial.h" #include "setup.h" -#include "ui.h" #include "time.h" -#include "commands.h" +#include "ui.h" -w2_s_client_state g_w2_state = { - .ping_received = true -}; +w2_s_client_state g_w2_state = {.ping_received = true}; int main(int argc, char **argv) { w2_client_setup(argc, argv); @@ -21,12 +19,13 @@ int main(int argc, char **argv) { if (!g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_TIMEOUT) { g_w2_state.ping_timeout = true; - g_w2_state.connected = false; + g_w2_state.connected = false; w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT); } - - if ((g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || g_w2_state.ping_timeout) { - g_w2_state.ping_timeout = false; + + if ((g_w2_state.ping_received && w2_timer_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || + g_w2_state.ping_timeout) { + g_w2_state.ping_timeout = false; g_w2_state.ping_received = false; w2_send_ping(); } diff --git a/client/serial.c b/client/serial.c index 743fe76..112fd81 100644 --- a/client/serial.c +++ b/client/serial.c @@ -2,10 +2,10 @@ #include "../shared/protocol.h" #include "../shared/serial_parse.h" +#include "commands.h" #include "main.h" #include "serial.h" #include "time.h" -#include "commands.h" void w2_serial_main() { int temp; @@ -20,13 +20,11 @@ void w2_cmd_ping_rx(w2_s_bin *data) { g_w2_state.ping = w2_timer_end(W2_TIMER_PING); g_w2_state.ping_received = true; - g_w2_state.ping_timeout = false; - g_w2_state.connected = true; + g_w2_state.ping_timeout = false; + g_w2_state.connected = true; } -void w2_cmd_ping_tx(w2_s_bin *data) { - w2_send_bin(data); -} +void w2_cmd_ping_tx(w2_s_bin *data) { w2_send_bin(data); } void w2_cmd_expt_tx(w2_s_bin *data) {} void w2_cmd_mode_tx(w2_s_bin *data) { diff --git a/client/ui_dirc.c b/client/ui_dirc.c index 675913a..ed69cd2 100644 --- a/client/ui_dirc.c +++ b/client/ui_dirc.c @@ -81,7 +81,9 @@ void w2_ui_dirc_paint(int left, int right) { "\n" " <q> <w> <e> forward\n" " <a> <s> <d> backward\n" - "left both right\n"); + "left both right\n" + "\n" + "<space> send dirc mode command"); } void w2_ui_dirc(bool first) { @@ -96,6 +98,7 @@ void w2_ui_dirc(bool first) { if (ch == 'd' || ch == 's') lb++; if (ch == 'q' || ch == 'w') rf++; if (ch == 'a' || ch == 's') rb++; + if (ch == ' ') w2_send_mode(W2_M_DIRC); } int drive_l = w2_dirc_motor_l(lf, lb); @@ -104,6 +107,6 @@ void w2_ui_dirc(bool first) { drive_l += drive_r * W2_DIRC_STP; drive_r += drive_l * W2_DIRC_STP; - // w2_send_dirc(drive_l, drive_r); + w2_send_dirc(drive_l, drive_r); w2_ui_dirc_paint(drive_l, drive_r); } diff --git a/robot/errcatch.c b/robot/errcatch.c index 0f809f9..77fcfd3 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -19,21 +19,21 @@ void w2_errcatch_handle_error(w2_s_error *error) { default: { g_w2_error_uncaught = true; #ifdef W2_SIM - simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code); + simwarn("Uncaught/unhandled error found with code 0x%02x", error->code); + if (error->message_length > 0) fprintf(stderr, " and message \"%.*s\"", error->message_length, error->message); + fprintf(stderr, "\n"); #endif } } // 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); + W2_CREATE_MSG_SIZE_BIN(w2_s_cmd_expt_tx, sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length, msg, msg_bin); 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/hypervisor.c b/robot/hypervisor.c index 478d3a5..efdceec 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -14,6 +14,12 @@ unsigned long g_w2_hypervisor_ema_io_ms = 0; unsigned long g_w2_hypervisor_ema_mode_ms = 0; uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0}; +unsigned int g_w2_ping_ms = 0; +uint8_t g_w2_ping_id = 0; +bool g_w2_ping_received = true; +bool g_w2_ping_timeout = false; +bool g_w2_connected = false; + void w2_hypervisor_main() { #ifdef W2_SIM w2_sim_cycle_begin(); diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 5988816..397d44d 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,13 +1,18 @@ #include "mode_dirc.h" #include "io.h" - +#include "hypervisor.h" +#include "modes.h" +#include "../shared/util.h" #include "orangutan_shim.h" int16_t g_w2_mode_dirc_motor_l = 0; int16_t g_w2_mode_dirc_motor_r = 0; +double g_w2_mode_dirc_power = 1.0; void w2_mode_dirc() { - set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); - g_w2_io.motor_left.speed = g_w2_mode_dirc_motor_l; - g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r; + // if (g_w2_connected) g_w2_mode_dirc_power = 1.0; + // else g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 0.01); + + // if (g_w2_mode_dirc_power == 0.0) w2_modes_call(W2_M_HALT); + set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power, g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power); } diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h index 12c967a..bf02270 100644 --- a/robot/mode_dirc.h +++ b/robot/mode_dirc.h @@ -6,6 +6,7 @@ extern int16_t g_w2_mode_dirc_motor_l; extern int16_t g_w2_mode_dirc_motor_r; +extern double g_w2_mode_dirc_power; /** * direct control mode diff --git a/robot/sercomm.c b/robot/sercomm.c index afde48a..6e191d2 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -20,12 +20,6 @@ char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0}; uint8_t g_w2_serial_buffer_index = 0; uint8_t g_w2_serial_buffer_head = 0; -unsigned int g_w2_ping_ms = 0; -uint8_t g_w2_ping_id = 0; -bool g_w2_ping_received = true; -bool g_w2_ping_timeout = false; -bool g_w2_connected = false; - void w2_sercomm_main() { #ifdef W2_SIM simprintfunc("w2_sercomm_main", ""); @@ -40,10 +34,11 @@ void w2_sercomm_main() { // check time-out if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) { g_w2_ping_timeout = true; + g_w2_connected = false; w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT); } // send ping every W2_TIMER_PING ms - if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > 1000) || g_w2_ping_timeout) { + if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || g_w2_ping_timeout) { g_w2_ping_timeout = false; g_w2_ping_received = false; g_w2_ping_id = (uint8_t) rand(); @@ -62,7 +57,7 @@ void w2_sercomm_main() { while (g_w2_sercomm_offset != g_w2_sercomm_index) { w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset]; #ifdef W2_SIM - w2_sim_print_serial(data); + if (DBG_ENABLE_SERIAL) w2_sim_print_serial(data); #endif serial_send_blocking("\xff", 1); for (uint8_t i = 0; i < data->bytes; i++) { @@ -89,10 +84,19 @@ void w2_sercomm_append_msg(w2_s_bin *data) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Warray-bounds" +#include <stdlib.h> +#include <string.h> + + void w2_cmd_ping_tx(w2_s_bin *data) { g_w2_ping_ms = w2_hypervisor_time_end(W2_TIMER_PING); g_w2_ping_received = true; g_w2_ping_timeout = false; + g_w2_connected = true; + + char buf[32]; + sprintf(buf, "rec: %i, tim: %i, con: %i", g_w2_ping_received, g_w2_ping_timeout, g_w2_connected); + w2_errcatch_throw_msg(0x69, strlen(buf), buf); } void w2_cmd_ping_rx(w2_s_bin *data) { diff --git a/robot/sim.h b/robot/sim.h index 14f0f74..7d7c091 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -14,9 +14,9 @@ #define DBG_ENABLE_SIMWARN (1) #define DBG_ENABLE_SIMINFO (1) #define DBG_ENABLE_CYCLEINFO (0) -#define DBG_ENABLE_SERIAL (1) +#define DBG_ENABLE_SERIAL (0) -#define DBG_CYCLE_DELAY (1e3) +#define DBG_CYCLE_DELAY (10e3) #define DBG_MAX_CYCLES (-1) // debug print options diff --git a/shared/consts.h b/shared/consts.h index 3f792b4..cd6dff1 100644 --- a/shared/consts.h +++ b/shared/consts.h @@ -22,7 +22,7 @@ /** size of the serial communication buffer (in messages, not bytes) */ #define W2_SERCOMM_BUFFER_SIZE (16) /** size of mode history buffer */ -#define W2_MODE_HISTORY_BUFFER_SIZE (4) +#define W2_MODE_HISTORY_BUFFER_SIZE (8) /** max logic module execution time in milliseconds */ #define W2_MAX_MODULE_CYCLE_MS (20) diff --git a/shared/serial_parse.c b/shared/serial_parse.c index 54b9ab5..d07c67f 100644 --- a/shared/serial_parse.c +++ b/shared/serial_parse.c @@ -51,7 +51,7 @@ void w2_cmd_handler(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_lengt w2_errcatch_throw(W2_E_WARN_SERIAL_NOISY); } else { #ifdef W2_SIM - w2_sim_print_serial(copy); + if (DBG_ENABLE_SERIAL) w2_sim_print_serial(copy); #endif handler(copy); } |