From 40f6164ba6187a0160af9fe200bbd1d729e8c03b Mon Sep 17 00:00:00 2001 From: lonkaars Date: Fri, 24 Jun 2022 11:42:23 +0200 Subject: added TARQ to robot and client --- client/commands.c | 10 ++++++++++ client/commands.h | 2 ++ client/i18n.h | 1 - client/i18n/en_us.h | 3 +++ client/makefile | 2 +- client/serial.c | 1 + client/ui_modes.c | 4 ++++ protocol.md | 22 ++++++++++++++++++---- robot/hypervisor.c | 2 ++ robot/hypervisor.h | 3 +++ robot/mode_chrg.c | 8 ++++---- robot/mode_grid.c | 32 ++++++++++++++------------------ robot/mode_grid.h | 1 - robot/mode_halt.c | 2 +- robot/mode_maze.c | 17 +++++++---------- robot/mode_scal.c | 2 +- robot/mode_spin.c | 3 +-- robot/modes.c | 2 +- robot/movement.c | 17 ++++++++--------- robot/sercomm.c | 16 ++++++++++------ shared/protocol.c | 3 +++ shared/protocol.h | 17 ++++++++++++++++- 22 files changed, 110 insertions(+), 60 deletions(-) diff --git a/client/commands.c b/client/commands.c index d2e53c8..768289e 100644 --- a/client/commands.c +++ b/client/commands.c @@ -64,3 +64,13 @@ void w2_send_mode(w2_e_mode mode) { free(msg_bin); } } + +void w2_send_tarq(w2_e_target_area target_area) { + W2_CREATE_MSG_BIN(w2_s_cmd_tarq_rx, msg, msg_bin); + + msg->opcode = W2_CMD_TARQ | W2_CMDDIR_RX; + msg->target_area = target_area; + + w2_send_bin(msg_bin); + free(msg_bin); +} diff --git a/client/commands.h b/client/commands.h index 02ae313..8917f99 100644 --- a/client/commands.h +++ b/client/commands.h @@ -2,6 +2,7 @@ #include "../shared/bin.h" #include "../shared/modes.h" +#include "../shared/protocol.h" #include "serial.h" void w2_send_bin(w2_s_bin *data); @@ -10,3 +11,4 @@ void w2_send_info(); void w2_send_ping(); void w2_send_mode(w2_e_mode mode); void w2_send_dirc(uint16_t left, uint16_t right); +void w2_send_tarq(w2_e_target_area target_area); diff --git a/client/i18n.h b/client/i18n.h index a8a6d9c..7d1fcbd 100644 --- a/client/i18n.h +++ b/client/i18n.h @@ -9,4 +9,3 @@ #define W2_LANG_DEFAULT #include "i18n/en_us.h" #endif - diff --git a/client/i18n/en_us.h b/client/i18n/en_us.h index d198296..2a24c15 100644 --- a/client/i18n/en_us.h +++ b/client/i18n/en_us.h @@ -110,6 +110,9 @@ "4 - set to charging station mode\n" \ "5 - set to spinning mode (wet floor simulation)\n" \ "6 - calibrate sensors\n" \ + "7 - set target area to maze\n" \ + "8 - set target area to grid\n" \ + "9 - set target area to charging station\n" \ "\n" \ "0 - previous\n" \ diff --git a/client/makefile b/client/makefile index d89c186..e72b585 100644 --- a/client/makefile +++ b/client/makefile @@ -1,7 +1,7 @@ CC = gcc LD = gcc RM = rm -f -CFLAGS = -DW2_LANG_NL_NL +CFLAGS = -DW2_LANG_EN_US LDFLAGS = -lncursesw EXECNAME = main diff --git a/client/serial.c b/client/serial.c index 2a4d26f..e52939f 100644 --- a/client/serial.c +++ b/client/serial.c @@ -54,3 +54,4 @@ void w2_cmd_info_rx(w2_s_bin *data) { return; } void w2_cmd_disp_rx(w2_s_bin *data) { return; } void w2_cmd_play_rx(w2_s_bin *data) { return; } void w2_cmd_cled_rx(w2_s_bin *data) { return; } +void w2_cmd_tarq_rx(w2_s_bin *data) { return; } diff --git a/client/ui_modes.c b/client/ui_modes.c index 3048ba9..bb34d3c 100644 --- a/client/ui_modes.c +++ b/client/ui_modes.c @@ -10,6 +10,10 @@ void w2_ui_onkey_modes(int ch) { if (ch == '5') w2_send_mode(W2_M_SPIN); if (ch == '6') w2_send_mode(W2_M_SCAL); + if (ch == '7') w2_send_tarq(W2_AREA_MAZE); + if (ch == '8') w2_send_tarq(W2_AREA_GRID); + if (ch == '9') w2_send_tarq(W2_AREA_CHRG); + if (ch == '0') w2_send_mode(W2_M_PREV); } diff --git a/protocol.md b/protocol.md index b627913..4139a22 100644 --- a/protocol.md +++ b/protocol.md @@ -25,10 +25,10 @@ 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 (`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 shared/protocol.h for code -readability. +indicate a transfer from robot to client (`tx`) (with exception of the +[PING](#ping) command). 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 shared/protocol.h for code readability. |code|name|implemented|directions|full name| |--:|---|:-:|:-:|---| @@ -46,6 +46,7 @@ readability. |`0x16`|[DISP](#disp)|no|`r <-- c`|display control |`0x18`|[PLAY](#play)|no|`r <-- c`|play midi |`0x1a`|[CLED](#cled)|no|`r <-- c`|control leds +|`0x1c`|[TARQ](#tarq)|no|`r <-- c`|target area request the DISP, PLAY, and CLED commands have low implementation priority, and should be considered extra features @@ -321,4 +322,17 @@ robot info response - DWBM display write bitmap --> +### TARQ + +#### set target area (`r <-- c`) (1 byte) + +|type|description| +|-:|-| +|`uint8_t`|opcode (`0x1c + 0`)| +|`uint8_t`|target area| + +_target area_ is one of: +- 0: maze +- 1: grid +- 2: charging station diff --git a/robot/hypervisor.c b/robot/hypervisor.c index be3fb77..50dc1ac 100644 --- a/robot/hypervisor.c +++ b/robot/hypervisor.c @@ -23,6 +23,8 @@ bool g_w2_ping_received = true; bool g_w2_ping_timeout = false; bool g_w2_connected = false; +w2_e_target_area g_w2_target_area = W2_AREA_CHRG; + void w2_hypervisor_main() { #ifdef W2_SIM w2_sim_cycle_begin(); diff --git a/robot/hypervisor.h b/robot/hypervisor.h index e9699cf..154ae58 100644 --- a/robot/hypervisor.h +++ b/robot/hypervisor.h @@ -5,6 +5,7 @@ #include #include "../shared/bool.h" +#include "../shared/protocol.h" /** amount of parallel timers */ #define W2_HYPERVISOR_TIMER_COUNT 2 @@ -26,6 +27,8 @@ extern bool g_w2_ping_received; extern bool g_w2_ping_timeout; extern bool g_w2_connected; +extern w2_e_target_area g_w2_target_area; + /** * backbone of all other modules * diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c index a5910f2..c41c0f7 100644 --- a/robot/mode_chrg.c +++ b/robot/mode_chrg.c @@ -4,7 +4,7 @@ #include "movement.h" #include "orangutan_shim.h" -int g_w2_charged_status; //used to detect the charging station (once) +int g_w2_charged_status; // used to detect the charging station (once) void w2_short_drive() { set_motors(50, 50); @@ -12,7 +12,7 @@ void w2_short_drive() { set_motors(0, 0); } -//charging station +// charging station void w2_home() { set_motors(0, 0); delay_ms(150); @@ -25,7 +25,7 @@ void w2_home() { delay_ms(2000); } -//crosswalk from charging station back to maze +// crosswalk from charging station back to maze void w2_charge_cross_walk() { if (g_w2_transition == 0) { set_motors(-30, 30); @@ -55,7 +55,7 @@ void w2_charge_cross_walk() { } } -//main function for charge mode +// main function for charge mode void w2_mode_chrg() { unsigned int last_proportional = 0; long integral = 0; diff --git a/robot/mode_grid.c b/robot/mode_grid.c index 4364b67..7e6db49 100644 --- a/robot/mode_grid.c +++ b/robot/mode_grid.c @@ -19,12 +19,11 @@ w2_s_grid_coordinate g_w2_destination; w2_e_orientation g_w2_direction; int g_w2_detection = 0; -int g_w2_transition; //used for the crosswalk, used to count black lines -char g_w2_x_location = 0; //current location in grid +int g_w2_transition; // used for the crosswalk, used to count black lines +char g_w2_x_location = 0; // current location in grid char g_w2_y_location = 0; - -//used for the crosswalk from maze to grid +// used for the crosswalk from maze to grid void w2_crosswalk_stroll() { while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { @@ -57,7 +56,7 @@ void w2_grid_crossway_detection() { g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } -//main function for grid mode +// main function for grid mode void w2_grid_follow_line() { unsigned int last_proportional = 0; long integral = 0; @@ -75,9 +74,10 @@ void w2_grid_follow_line() { if (power_difference < -max) power_difference = -max; if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && - g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { //crossways/intersections + g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { // crossways/intersections break; - } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { //left corners + } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && + g_w2_sensors[4] < 100) { // left corners break; } else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[0] < 100) { // for the south and west borders of the grid @@ -99,22 +99,20 @@ void w2_grid_follow_line() { } } -//begin location when entering the grid +// begin location when entering the grid void w2_begin_location() { g_w2_location.x = 4; g_w2_location.y = 0; g_w2_direction = W2_ORT_WEST; } - -//location of grid exit +// location of grid exit void w2_end_destination() { g_w2_destination.x = 4; g_w2_destination.y = 4; } - -//turns are used to get the correct orientation when picking up orders +// turns are used to get the correct orientation when picking up orders void w2_turn_north() { switch (g_w2_direction) { case W2_ORT_NORTH: @@ -201,8 +199,7 @@ void w2_turn_east() { g_w2_direction = W2_ORT_EAST; } - -//signals when the product is picked +// signals when the product is picked void w2_arrived_message() { if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) { play_frequency(400, 500, 7); @@ -210,8 +207,7 @@ void w2_arrived_message() { } } - -//go to correct x coordinate +// go to correct x coordinate void w2_go_to_x() { if (g_w2_location.x != g_w2_destination.x) { while (g_w2_location.x != g_w2_destination.x) { @@ -232,7 +228,7 @@ void w2_go_to_x() { } } -//go to correct y coordinate +// go to correct y coordinate void w2_go_to_y() { if (g_w2_location.y != g_w2_destination.y) { while (g_w2_location.y != g_w2_destination.y) { @@ -253,7 +249,7 @@ void w2_go_to_y() { } } -//main function for grid mode +// main function for grid mode void w2_mode_grid() { set_motors(0, 0); delay(500); diff --git a/robot/mode_grid.h b/robot/mode_grid.h index 408a8ff..79a6475 100644 --- a/robot/mode_grid.h +++ b/robot/mode_grid.h @@ -24,4 +24,3 @@ typedef struct { extern w2_s_grid_coordinate g_w2_order[16]; extern unsigned int g_w2_order_index; - diff --git a/robot/mode_halt.c b/robot/mode_halt.c index 8bd051e..acd55ee 100644 --- a/robot/mode_halt.c +++ b/robot/mode_halt.c @@ -1,5 +1,5 @@ #include "mode_halt.h" #include "orangutan_shim.h" -//emergency stop +// emergency stop void w2_mode_halt() { set_motors(0, 0); } diff --git a/robot/mode_maze.c b/robot/mode_maze.c index 0350205..72478e8 100644 --- a/robot/mode_maze.c +++ b/robot/mode_maze.c @@ -6,8 +6,7 @@ unsigned int g_w2_last_proportional = 0; long g_w2_integral = 0; - -//main function for maze mode +// main function for maze mode void w2_mode_maze() { // PID controller g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); @@ -22,17 +21,15 @@ void w2_mode_maze() { if (power_difference < -max) power_difference = -max; if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && - g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { //dead ends + g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { // dead ends w2_crosswalk_stroll(); - } - else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && - g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { //crossways or intersection + } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && + g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { // crossways or intersection w2_maze_rotation_half_left(); - } - else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { //left corners + } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && + g_w2_sensors[4] < 100) { // left corners w2_maze_rotation_half_left(); - } - else { //normal line following + } else { // normal line following if (power_difference < 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) set_motors(max + power_difference, max); diff --git a/robot/mode_scal.c b/robot/mode_scal.c index 8835183..e4e282c 100644 --- a/robot/mode_scal.c +++ b/robot/mode_scal.c @@ -2,7 +2,7 @@ #include "modes.h" #include "orangutan_shim.h" -//callibrates the robot +// callibrates the robot void w2_mode_scal() { for (int counter = 0; counter < 80; counter++) { if (counter < 20 || counter >= 60) { diff --git a/robot/mode_spin.c b/robot/mode_spin.c index 79f9dea..5065e13 100644 --- a/robot/mode_spin.c +++ b/robot/mode_spin.c @@ -1,6 +1,5 @@ #include "mode_spin.h" #include "orangutan_shim.h" - -//wet floor simulation +// wet floor simulation void w2_mode_spin() { set_motors(255, -255); } diff --git a/robot/modes.c b/robot/modes.c index aedc02a..559d81c 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -16,7 +16,7 @@ w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE]; uint8_t g_w2_mode_history_index = 0; void (*g_w2_modes[W2_MODE_COUNT])(); -//all of the different modi +// all of the different modi void w2_modes_init() { g_w2_modes[W2_M_CHRG] = &w2_mode_chrg; g_w2_modes[W2_M_DIRC] = &w2_mode_dirc; diff --git a/robot/movement.c b/robot/movement.c index 37d66d5..e348410 100644 --- a/robot/movement.c +++ b/robot/movement.c @@ -1,11 +1,10 @@ #include "movement.h" #include "orangutan_shim.h" -unsigned int g_w2_sensors[5] = {0}; //IR sensors on the bottom of the robot -unsigned int g_w2_position = 0; //position on the black line +unsigned int g_w2_sensors[5] = {0}; // IR sensors on the bottom of the robot +unsigned int g_w2_position = 0; // position on the black line - -//full rotation maze/charge +// full rotation maze/charge void w2_maze_rotation_full() { set_motors(0, 0); delay_ms(500); @@ -16,7 +15,7 @@ void w2_maze_rotation_full() { delay_ms(500); } -//left turn maze/charge +// left turn maze/charge void w2_maze_rotation_half_left() { set_motors(0, 0); set_motors(50, 50); @@ -28,7 +27,7 @@ void w2_maze_rotation_half_left() { delay_ms(500); } -//right turn maze/charge +// right turn maze/charge void w2_maze_rotation_half_right() { set_motors(0, 0); set_motors(50, 50); @@ -42,7 +41,7 @@ void w2_maze_rotation_half_right() { delay_ms(500); } -//180 turn in grid +// 180 turn in grid void w2_grid_rotation_full() { set_motors(60, -60); delay_ms(540); @@ -50,7 +49,7 @@ void w2_grid_rotation_full() { g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } -//left turn in grid +// left turn in grid void w2_grid_rotation_half_left() { set_motors(-30, 30); delay_ms(600); @@ -58,7 +57,7 @@ void w2_grid_rotation_half_left() { g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } -//right turn in grid +// right turn in grid void w2_grid_rotation_half_right() { set_motors(30, -30); delay_ms(600); diff --git a/robot/sercomm.c b/robot/sercomm.c index d12fcca..195c477 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -4,10 +4,10 @@ #include "../shared/bin.h" #include "../shared/errcatch.h" #include "../shared/serial_parse.h" -#include "mode_grid.h" #include "hypervisor.h" #include "io.h" #include "mode_dirc.h" +#include "mode_grid.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" @@ -124,10 +124,9 @@ void w2_cmd_bomd_rx(w2_s_bin *data) { char buf[32]; clear(); - sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position / W2_MAP_DEFAULT_WIDTH); - print(buf); - g_w2_order[g_w2_order_index].x = req->position % W2_MAP_DEFAULT_WIDTH; - g_w2_order[g_w2_order_index].y = req->position / W2_MAP_DEFAULT_WIDTH; + sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position / + W2_MAP_DEFAULT_WIDTH); print(buf); g_w2_order[g_w2_order_index].x = req->position % + W2_MAP_DEFAULT_WIDTH; g_w2_order[g_w2_order_index].y = req->position / W2_MAP_DEFAULT_WIDTH; g_w2_order_index++; */ } @@ -161,7 +160,7 @@ void w2_cmd_info_rx(w2_s_bin *data) { res_msg->mode_ms = (uint8_t)g_w2_hypervisor_ema_mode_ms; res_msg->uptime_s = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3)); res_msg->mode = g_w2_mode_history[g_w2_mode_history_index]; - res_msg->battery_mv = w2_bin_hton16(read_battery_millivolts()); + res_msg->battery_mv = w2_bin_hton16(read_battery_millivolts()); w2_sercomm_append_msg(res_bin); free(res_bin); @@ -173,6 +172,11 @@ void w2_cmd_play_rx(w2_s_bin *data) { return; } void w2_cmd_cled_rx(w2_s_bin *data) { return; } +void w2_cmd_tarq_rx(w2_s_bin *data) { + W2_CAST_BIN(w2_s_cmd_tarq_rx, data, req); + g_w2_target_area = req->target_area; +} + #pragma GCC diagnostic pop void w2_cmd_expt_tx(w2_s_bin *data) {} diff --git a/shared/protocol.c b/shared/protocol.c index 02d746a..8aa4a3f 100644 --- a/shared/protocol.c +++ b/shared/protocol.c @@ -23,6 +23,7 @@ void w2_cmd_setup_handlers() { g_w2_cmd_handlers[W2_CMD_DISP | W2_CMDDIR_RX] = w2_cmd_disp_rx; g_w2_cmd_handlers[W2_CMD_PLAY | W2_CMDDIR_RX] = w2_cmd_play_rx; g_w2_cmd_handlers[W2_CMD_CLED | W2_CMDDIR_RX] = w2_cmd_cled_rx; + g_w2_cmd_handlers[W2_CMD_TARQ | W2_CMDDIR_RX] = w2_cmd_tarq_rx; } size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_length) { @@ -47,6 +48,8 @@ size_t w2_cmd_sizeof(uint8_t data[W2_SERIAL_READ_BUFFER_SIZE], uint8_t data_leng if (data[0] == (W2_CMD_INFO | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_info_rx); if (data[0] == (W2_CMD_INFO | W2_CMDDIR_TX)) return sizeof(w2_s_cmd_info_tx); + if (data[0] == (W2_CMD_TARQ | W2_CMDDIR_RX)) return sizeof(w2_s_cmd_tarq_rx); + w2_s_bin *copy = w2_bin_s_alloc(data_length, data); uint8_t length = 1; diff --git a/shared/protocol.h b/shared/protocol.h index ba16d56..c13e56c 100644 --- a/shared/protocol.h +++ b/shared/protocol.h @@ -17,7 +17,7 @@ #define W2_CMD_CODE_MASK (~1) #define W2_CMD_DIRECTION_MASK (1) -#define W2_CMD_COUNT 28 +#define W2_CMD_COUNT 30 typedef enum { /** ping command */ W2_CMD_PING = 0x00, @@ -47,8 +47,16 @@ typedef enum { W2_CMD_PLAY = 0x18, /** control leds command */ W2_CMD_CLED = 0x1a, + /** target area request */ + W2_CMD_TARQ = 0x1c, } w2_e_scmds; +typedef enum { + W2_AREA_MAZE = 0, + W2_AREA_GRID = 1, + W2_AREA_CHRG = 2, +} w2_e_target_area; + #pragma pack(push, 1) typedef struct { @@ -158,6 +166,11 @@ typedef struct { typedef struct { } w2_s_cmd_cled_rx; +typedef struct { + uint8_t opcode; + w2_e_target_area target_area; +} w2_s_cmd_tarq_rx; + #pragma pack(pop) /** stores message handlers in array with opcode as index */ @@ -206,6 +219,8 @@ void w2_cmd_disp_rx(w2_s_bin *data); void w2_cmd_play_rx(w2_s_bin *data); /** handler for cled_rx (on complete message) */ void w2_cmd_cled_rx(w2_s_bin *data); +/** handler for tarq_rx (on complete message) */ +void w2_cmd_tarq_rx(w2_s_bin *data); /** calculate message length for expt_tx (incomplete message) */ size_t w2_cmd_expt_tx_sizeof(w2_s_bin *data); -- cgit v1.2.3