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 --- 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 ++++++++++------ 12 files changed, 52 insertions(+), 53 deletions(-) (limited to 'robot') 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) {} -- cgit v1.2.3