From b12d6b2ecee0be03122a4bdba8ebbc91112fae3f Mon Sep 17 00:00:00 2001 From: lonkaars Date: Thu, 2 Jun 2022 12:12:47 +0200 Subject: dirc working --- client/commands.c | 43 ++++++++++++++++++++---------- client/commands.h | 7 +++-- client/ui.c | 10 ++++--- client/ui.h | 4 +-- client/ui_dirc.c | 78 +++++++++++++++++++++++++++++++++++++++++------------- robot/hypervisor.c | 2 +- robot/main.c | 1 - robot/mode_dirc.c | 3 +++ robot/modes.c | 10 +++---- robot/sercomm.c | 5 ++-- robot/sim.c | 2 +- robot/sim.h | 2 +- shared/util.c | 2 ++ shared/util.h | 1 + 14 files changed, 118 insertions(+), 52 deletions(-) diff --git a/client/commands.c b/client/commands.c index 1ab79fb..62867dd 100644 --- a/client/commands.c +++ b/client/commands.c @@ -14,26 +14,41 @@ void w2_send_bin(w2_s_bin *data) { } } -w2_s_bin *w2_send_info() { - size_t msg_size = sizeof(w2_s_cmd_info_rx); - w2_s_cmd_info_rx *msg = malloc(msg_size); - msg->opcode = W2_CMD_INFO | W2_CMDDIR_RX; - w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); +void w2_send_info() { + W2_CREATE_MSG_BIN(w2_s_cmd_info_rx, msg, msg_bin); + msg->opcode = W2_CMD_INFO | W2_CMDDIR_RX; + w2_send_bin(msg_bin); - free(msg); free(msg_bin); } -w2_s_bin *w2_send_ping() { - g_w2_state.ping_id = (uint8_t)rand(); - size_t msg_size = sizeof(w2_s_cmd_ping_rx); - w2_s_cmd_ping_rx *msg = malloc(msg_size); - msg->opcode = W2_CMD_PING | W2_CMDDIR_RX; - msg->id = g_w2_state.ping_id; - w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg); +void w2_send_ping() { + g_w2_state.ping_id = (uint8_t)rand(); + W2_CREATE_MSG_BIN(w2_s_cmd_ping_rx, msg, msg_bin); + msg->opcode = W2_CMD_PING | W2_CMDDIR_RX; + msg->id = g_w2_state.ping_id; + w2_send_bin(msg_bin); - free(msg); free(msg_bin); w2_timer_start(W2_TIMER_PING); } + +void w2_send_mode(w2_e_mode mode) { + W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX; + msg->mode = mode; + + w2_send_bin(msg_bin); + free(msg_bin); +} + +void w2_send_dirc(int left, int right) { + W2_CREATE_MSG_BIN(w2_s_cmd_dirc_rx, msg, msg_bin); + msg->opcode = W2_CMD_DIRC | W2_CMDDIR_RX; + msg->left = w2_bin_hton16((uint16_t)left); + msg->right = w2_bin_hton16((uint16_t)right); + + w2_send_bin(msg_bin); + free(msg_bin); +} diff --git a/client/commands.h b/client/commands.h index 949360e..6768ea1 100644 --- a/client/commands.h +++ b/client/commands.h @@ -1,7 +1,10 @@ #pragma once #include "../shared/bin.h" +#include "../shared/modes.h" #include "serial.h" -w2_s_bin *w2_send_info(); -w2_s_bin *w2_send_ping(); +void w2_send_info(); +void w2_send_ping(); +void w2_send_mode(w2_e_mode mode); +void w2_send_dirc(int left, int right); diff --git a/client/ui.c b/client/ui.c index ca96efb..e6e73f0 100644 --- a/client/ui.c +++ b/client/ui.c @@ -11,9 +11,10 @@ #include "ui.h" WINDOW *g_w2_ui_win; -unsigned int g_w2_ui_width = 0; -unsigned int g_w2_ui_height = 0; -void (*g_w2_ui_current_tab)() = &w2_ui_dirc; +unsigned int g_w2_ui_width = 0; +unsigned int g_w2_ui_height = 0; +void (*g_w2_ui_current_tab)(bool first) = &w2_ui_dirc; +void (*g_w2_ui_last_tab)(bool first) = NULL; void w2_ui_main() { g_w2_ui_width = getmaxx(g_w2_ui_win); @@ -25,7 +26,8 @@ void w2_ui_main() { void w2_ui_paint() { w2_ui_paint_statusbar(); if (w2_timer_end(W2_TIMER_UPDATE) >= (1000 / W2_UI_UPDATE_FPS)) { - (*g_w2_ui_current_tab)(); + (*g_w2_ui_current_tab)(g_w2_ui_last_tab != g_w2_ui_current_tab); + g_w2_ui_last_tab = g_w2_ui_current_tab; w2_timer_start(W2_TIMER_UPDATE); } refresh(); diff --git a/client/ui.h b/client/ui.h index f749fa0..0d375de 100644 --- a/client/ui.h +++ b/client/ui.h @@ -8,7 +8,7 @@ extern WINDOW *g_w2_ui_win; extern unsigned int g_w2_ui_width; extern unsigned int g_w2_ui_height; -extern void (*g_w2_ui_current_tab)(); +extern void (*g_w2_ui_current_tab)(bool first); /** update terminal props */ void w2_ui_update(); @@ -24,4 +24,4 @@ void w2_ui_paint_statusbar(); /** draw tab bar */ void w2_ui_paint_tabbar(); -void w2_ui_dirc(); +void w2_ui_dirc(bool first); diff --git a/client/ui_dirc.c b/client/ui_dirc.c index 118a3ea..7720a4c 100644 --- a/client/ui_dirc.c +++ b/client/ui_dirc.c @@ -1,21 +1,18 @@ +#include "../shared/protocol.h" #include "../shared/util.h" +#include "commands.h" #include "ui.h" -void w2_ui_bar_graph(unsigned int percent) { - unsigned int width = g_w2_ui_width - 7; - char bar[width]; - for (unsigned int i = 0; i < width - 2; i++) { - bar[i + 1] = i > (width - 2) * percent / 100 ? ' ' : '*'; - } - bar[0] = '|'; - bar[width - 1] = '|'; - mvaddnstr(4, 7, bar, width); -} - +/** decay modifier */ #define W2_DIRC_MOD ((double)0.95) -#define W2_DIRC_ADD ((double)13.0) -#define W2_DIRC_PAD ((double)1.10) -#define W2_DIRC_SPL ((unsigned int)20) +/** add value per key press */ +#define W2_DIRC_ADD ((double)17.0) +/** padding */ +#define W2_DIRC_PAD ((double)3.00) +/** average samples */ +#define W2_DIRC_SPL ((unsigned int)14) +/** steering padding */ +#define W2_DIRC_STP ((double)0.2) int w2_avg(int *samples, unsigned int sample_count) { double total = 0; @@ -45,7 +42,50 @@ int w2_avg(int *samples, unsigned int sample_count) { W2_DIRC_MOTOR_DRIVER(l); W2_DIRC_MOTOR_DRIVER(r); -void w2_ui_dirc() { +void w2_ui_dirc_init() { w2_send_mode(W2_M_DIRC); } + +void w2_ui_bar_graph(unsigned int y, unsigned int x, unsigned int width, double value) { + char temp[width]; + temp[0] = '|'; + temp[width - 1] = '|'; + for (unsigned int i = 0; i < width - 2; i++) temp[i + 1] = i < width * value ? '*' : ' '; + + mvaddnstr(y, x, temp, width); +} + +void w2_ui_bar_graph_pm(unsigned int y, unsigned int x, unsigned int width, double value) { + char temp[width]; + temp[0] = '|'; + temp[width - 1] = '|'; + width -= 2; + unsigned int hw = width / 2; + if (value >= 0) { + for (unsigned int i = 0; i < width; i++) + temp[i + 1] = i < hw ? ' ' : (i - hw) < (hw * value) ? '*' : ' '; + } else { + for (unsigned int i = 0; i < width; i++) + temp[i + 1] = i < hw ? ' ' : (i - hw) < (hw * value) ? '*' : ' '; + } + + mvaddnstr(y, x, temp, width); +} + +void w2_ui_dirc_paint(int left, int right) { + mvaddstr(4, 0, "left drive: "); + w2_ui_bar_graph_pm(4, 13, g_w2_ui_width - 14, (double)left / 255); + mvaddstr(5, 0, "right drive: "); + w2_ui_bar_graph_pm(5, 13, g_w2_ui_width - 14, (double)right / 255); + + mvaddstr(7, 0, + " controls:\n" + "\n" + " forward\n" + " backward\n" + "left both right\n"); +} + +void w2_ui_dirc(bool first) { + if (first) w2_ui_dirc_init(); int ch = 0; unsigned int lb = 0; unsigned int lf = 0; @@ -61,7 +101,9 @@ void w2_ui_dirc() { int drive_l = w2_dirc_motor_l(lf, lb); int drive_r = w2_dirc_motor_r(rf, rb); - char temp[32] = {0}; - sprintf(temp, "l: %04i, r: %04i", drive_l, drive_r); - mvaddstr(4, 0, temp); + drive_l += drive_r * W2_DIRC_STP; + drive_r += drive_l * W2_DIRC_STP; + + w2_send_dirc(drive_l, drive_r); + w2_ui_dirc_paint(drive_l, drive_r); } diff --git a/robot/hypervisor.c b/robot/hypervisor.c index 4f994a8..478d3a5 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -27,7 +27,7 @@ void w2_hypervisor_main() { unsigned long sercomm_time = get_ms(); w2_errcatch_main(); unsigned long errcatch_time = get_ms() - sercomm_time; - w2_io_main(); + // w2_io_main(); unsigned long io_time = get_ms() - errcatch_time; w2_modes_main(); unsigned long mode_time = get_ms() - io_time; diff --git a/robot/main.c b/robot/main.c index b0302bd..2f15e97 100644 --- a/robot/main.c +++ b/robot/main.c @@ -5,7 +5,6 @@ #include "sim.h" #endif -#include int main() { #ifdef W2_SIM w2_sim_setup(); diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 9ed9fef..5988816 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,10 +1,13 @@ #include "mode_dirc.h" #include "io.h" +#include "orangutan_shim.h" + 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); g_w2_io.motor_left.speed = g_w2_mode_dirc_motor_l; g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r; } diff --git a/robot/modes.c b/robot/modes.c index f73a809..7decf47 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -46,13 +46,11 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) { } // 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_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin); + msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX; + msg->mode = new_mode; + w2_sercomm_append_msg(msg_bin); - free(msg); free(msg_bin); } diff --git a/robot/sercomm.c b/robot/sercomm.c index deef39f..519568d 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -37,10 +37,11 @@ void w2_sercomm_main() { #ifdef W2_SIM w2_sim_print_serial(data); #endif - serial_send("\xff", 1); + serial_send_blocking("\xff", 1); for (uint8_t i = 0; i < data->bytes; i++) { uint8_t byte = data->data[i]; - byte == 0xff ? serial_send("\xff\xff", 2) : serial_send((char *)&byte, 1); + byte == 0xff ? serial_send_blocking("\xff\xff", 2) + : serial_send_blocking((char *)&byte, 1); } g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE; } diff --git a/robot/sim.c b/robot/sim.c index 5956fbb..9cce12f 100644 --- a/robot/sim.c +++ b/robot/sim.c @@ -74,7 +74,7 @@ void serial_set_baud_rate(unsigned int rate) { simprintfunc("serial_set_baud_rate", "%u", rate); } -void serial_send(char* message, unsigned int length) { +void serial_send_blocking(char* message, unsigned int length) { for (unsigned int byte = 0; byte < length; byte++) putc(message[byte] & 0xff, stdout); fflush(stdout); diff --git a/robot/sim.h b/robot/sim.h index 9e250da..9d73585 100644 --- a/robot/sim.h +++ b/robot/sim.h @@ -71,7 +71,7 @@ void green_led(unsigned char on); // NOLINT void clear(); // NOLINT void play(const char *melody); // NOLINT void serial_set_baud_rate(unsigned int rate); // NOLINT -void serial_send(char *message, unsigned int length); // NOLINT +void serial_send_blocking(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 diff --git a/shared/util.c b/shared/util.c index 55f3491..68503e8 100644 --- a/shared/util.c +++ b/shared/util.c @@ -4,3 +4,5 @@ unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_m return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) + (((double)(new_meas)) * ((double)(W2_EMA_WEIGHT)))); } + +int w2_sign(int n) { return (n > 0) - (n < 0); } diff --git a/shared/util.h b/shared/util.h index 465e043..230c3e4 100644 --- a/shared/util.h +++ b/shared/util.h @@ -7,3 +7,4 @@ #define W2_RANGE(min, val, max) W2_MIN(max, W2_MAX(val, min)) unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas); +int w2_sign(int n); -- cgit v1.2.3