From 4d446fb2c1d14142304971095ee60668ba9c6bc5 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 29 Jun 2022 09:34:54 +0200 Subject: [WIP] last touches for second assessment --- robot/io.c | 6 ++- robot/io.h | 3 ++ robot/mode_chrg.c | 151 +++++++++++++++++++++++++++--------------------------- robot/mode_dirc.c | 2 +- robot/mode_grid.c | 26 ++++------ robot/mode_halt.c | 3 +- robot/mode_maze.c | 5 +- robot/mode_scal.c | 5 +- robot/mode_spin.c | 3 +- robot/movement.c | 37 ++++++------- robot/sercomm.c | 16 +++--- 11 files changed, 133 insertions(+), 124 deletions(-) diff --git a/robot/io.c b/robot/io.c index 8add1e9..9977e2f 100644 --- a/robot/io.c +++ b/robot/io.c @@ -27,11 +27,13 @@ void w2_io_object_detection() { if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) g_w2_io_object_detected = false; if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK); - - set_motors(0, 0); } } +void w2_set_motors(int left, int right) { + set_motors(left * (1 - g_w2_io_object_detected), right * (1 - g_w2_io_object_detected)); +} + void w2_io_battery_logic() { if (w2_hypervisor_time_end(W2_TIMER_BATTERY_MEASUREMENT) <= W2_BATTERY_MEAS_FREQ) return; w2_hypervisor_time_start(W2_TIMER_BATTERY_MEASUREMENT); diff --git a/robot/io.h b/robot/io.h index 5e469bb..5b79c52 100644 --- a/robot/io.h +++ b/robot/io.h @@ -10,3 +10,6 @@ extern bool g_w2_io_object_detected; /** @brief i/o module main */ void w2_io_main(); + +/** @brief pololu library `set_motors` override */ +void w2_set_motors(int left, int right); diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c index 47391a1..c7349ab 100644 --- a/robot/mode_chrg.c +++ b/robot/mode_chrg.c @@ -1,4 +1,5 @@ #include "mode_chrg.h" +#include "io.h" #include "../shared/bool.h" #include "hypervisor.h" #include "mode_grid.h" @@ -6,32 +7,34 @@ #include "movement.h" #include "orangutan_shim.h" +#include + bool g_w2_chrg_aligned; void w2_short_drive() { - set_motors(50, 50); + w2_set_motors(50, 50); delay(150); - set_motors(0, 0); + w2_set_motors(0, 0); } // crosswalk from charging station back to maze void w2_charge_cross_walk() { if (g_w2_transition == 0) { - set_motors(-30, 30); + w2_set_motors(-30, 30); delay(50); } 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) { - set_motors(15, 15); + w2_set_motors(15, 15); delay(550); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) { - set_motors(0, 0); + w2_set_motors(0, 0); g_w2_transition++; if (g_w2_transition == 3) { // TODO: document g_w2_transition - set_motors(40, 40); + w2_set_motors(40, 40); delay(600); - set_motors(0, 0); + w2_set_motors(0, 0); g_w2_transition = 0; w2_modes_swap(W2_M_MAZE); @@ -48,80 +51,76 @@ void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; } // main function for charge mode void w2_mode_chrg() { - unsigned int last_proportional = 0; - long integral = 0; - - while (1) { - // Get the position of the line. Note that we *must* provide - // the "sensors" argument to read_line() here, even though we - // are not interested in the individual sensor readings. - g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); - - // The "proportional" term should be 0 when we are on the line. - int proportional = ((int)g_w2_position) - 2000; - - // Compute the derivative (change) and integral (sum) of the - // position. - int derivative = proportional - last_proportional; - integral += proportional; - - // Remember the last position. - last_proportional = proportional; - - // Compute the difference between the two motor power settings, - // m1 - m2. If this is a positive number the robot will turn - // to the right. If it is a negative number, the robot will - // turn to the left, and the magnitude of the number determines - // the sharpness of the turn. - int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2; - - // Compute the actual motor settings. We never set either motor - // to a negative value. - - const int max = 60; - if (power_difference > max) power_difference = max; - 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) { - w2_charge_cross_walk(); - if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) { - break; - } + static unsigned int last_proportional = 0; + static long integral = 0; + + g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); + + // The "proportional" term should be 0 when we are on the line. + int proportional = ((int)g_w2_position) - 2000; + + // Compute the derivative (change) and integral (sum) of the + // position. + int derivative = proportional - last_proportional; + integral += proportional; + + // Remember the last position. + last_proportional = proportional; + + // Compute the difference between the two motor power settings, + // m1 - m2. If this is a positive number the robot will turn + // to the right. If it is a negative number, the robot will + // turn to the left, and the magnitude of the number determines + // the sharpness of the turn. + int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2; + + // Compute the actual motor settings. We never set either motor + // to a negative value. + + const int max = 60; + if (power_difference > max) power_difference = max; + 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) { + w2_charge_cross_walk(); + if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) { + return; } + } - else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 && - g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) { - if (g_w2_target_area == W2_AREA_CHRG) { - if (!g_w2_chrg_aligned) { - set_motors(0, 0); - delay_ms(150); - set_motors(30, 30); - delay_ms(600); - set_motors(0, 0); - delay_ms(600); - g_w2_chrg_aligned = true; - } - } else { - delay(200); - w2_maze_rotation_full(); - w2_short_drive(); + else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 && + g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) { + if (g_w2_target_area == W2_AREA_CHRG) { + if (!g_w2_chrg_aligned) { + w2_set_motors(0, 0); + delay_ms(150); + w2_set_motors(30, 30); + delay_ms(600); + w2_set_motors(0, 0); + delay_ms(600); + g_w2_chrg_aligned = true; } - } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { - w2_maze_rotation_half_left(); + } else { + delay(200); + w2_maze_rotation_full(); + w2_short_drive(); } + } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { + w2_maze_rotation_half_left(); + } - 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) { - w2_maze_rotation_half_left(); - } else { - 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); - } else if (power_difference > 0 && - (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { - set_motors(max, max - power_difference); - } + 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) { + w2_maze_rotation_half_left(); + } else { + if (power_difference < 0 && + (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { + w2_set_motors(max + power_difference, max); + } else if (power_difference > 0 && + (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { + w2_set_motors(max, max - power_difference); } } } + diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index d9c0d17..86756f6 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -11,4 +11,4 @@ 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); } +void w2_mode_dirc() { w2_set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); } diff --git a/robot/mode_grid.c b/robot/mode_grid.c index cfecae5..ff80dd2 100644 --- a/robot/mode_grid.c +++ b/robot/mode_grid.c @@ -1,5 +1,6 @@ #include "mode_grid.h" #include "hypervisor.h" +#include "io.h" #include "modes.h" #include "movement.h" #include "orangutan_shim.h" @@ -12,13 +13,8 @@ int g_w2_order_number; int g_w2_maze_status = 0; -w2_s_grid_coordinate g_w2_order[16] = { - {0, 0}, - {3, 4}, - {2, 1}, - {4, 2}, -}; -unsigned int g_w2_order_index = 4; +w2_s_grid_coordinate g_w2_order[16]; +unsigned int g_w2_order_index = 0; w2_s_grid_coordinate g_w2_location; w2_s_grid_coordinate g_w2_destination; w2_e_orientation g_w2_direction; @@ -32,15 +28,15 @@ char g_w2_y_location = 0; 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) { - set_motors(15, 15); + w2_set_motors(15, 15); delay(290); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); if ((g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) && (g_w2_target_area != W2_AREA_MAZE)) { - set_motors(0, 0); + w2_set_motors(0, 0); g_w2_transition++; if (g_w2_transition == 3) { - set_motors(40, 40); + w2_set_motors(40, 40); delay(600); g_w2_transition = 0; w2_modes_swap(W2_M_GRID); @@ -56,9 +52,9 @@ void w2_crosswalk_stroll() { } void w2_grid_crossway_detection() { - set_motors(50, 50); + w2_set_motors(50, 50); delay_ms(150); - set_motors(10, 10); + w2_set_motors(10, 10); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } @@ -96,10 +92,10 @@ void w2_grid_follow_line() { else { 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); + w2_set_motors(max + power_difference, max); } else if (power_difference > 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) { - set_motors(max, max - power_difference); + w2_set_motors(max, max - power_difference); } } } @@ -257,7 +253,7 @@ void w2_go_to_y() { // main function for grid mode void w2_mode_grid() { - set_motors(0, 0); + w2_set_motors(0, 0); delay(500); w2_begin_location(); diff --git a/robot/mode_halt.c b/robot/mode_halt.c index acd55ee..268a922 100644 --- a/robot/mode_halt.c +++ b/robot/mode_halt.c @@ -1,5 +1,6 @@ #include "mode_halt.h" #include "orangutan_shim.h" +#include "io.h" // emergency stop -void w2_mode_halt() { set_motors(0, 0); } +void w2_mode_halt() { w2_set_motors(0, 0); } diff --git a/robot/mode_maze.c b/robot/mode_maze.c index 72478e8..a7f76ec 100644 --- a/robot/mode_maze.c +++ b/robot/mode_maze.c @@ -2,6 +2,7 @@ #include "mode_grid.h" #include "movement.h" #include "orangutan_shim.h" +#include "io.h" unsigned int g_w2_last_proportional = 0; long g_w2_integral = 0; @@ -32,9 +33,9 @@ void w2_mode_maze() { } 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); + w2_set_motors(max + power_difference, max); else if (power_difference > 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) - set_motors(max, max - power_difference); + w2_set_motors(max, max - power_difference); } } diff --git a/robot/mode_scal.c b/robot/mode_scal.c index e4e282c..245fec4 100644 --- a/robot/mode_scal.c +++ b/robot/mode_scal.c @@ -1,14 +1,15 @@ #include "mode_scal.h" #include "modes.h" #include "orangutan_shim.h" +#include "io.h" // callibrates the robot void w2_mode_scal() { for (int counter = 0; counter < 80; counter++) { if (counter < 20 || counter >= 60) { - set_motors(40, -40); + w2_set_motors(40, -40); } else { - set_motors(-40, 40); + w2_set_motors(-40, 40); } calibrate_line_sensors(IR_EMITTERS_ON); diff --git a/robot/mode_spin.c b/robot/mode_spin.c index 5065e13..1c7cc11 100644 --- a/robot/mode_spin.c +++ b/robot/mode_spin.c @@ -1,5 +1,6 @@ #include "mode_spin.h" #include "orangutan_shim.h" +#include "io.h" // wet floor simulation -void w2_mode_spin() { set_motors(255, -255); } +void w2_mode_spin() { w2_set_motors(255, -255); } diff --git a/robot/movement.c b/robot/movement.c index e348410..b800464 100644 --- a/robot/movement.c +++ b/robot/movement.c @@ -1,41 +1,42 @@ #include "movement.h" #include "orangutan_shim.h" +#include "io.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 // full rotation maze/charge void w2_maze_rotation_full() { - set_motors(0, 0); + w2_set_motors(0, 0); delay_ms(500); - set_motors(60, -60); + w2_set_motors(60, -60); delay_ms(540); - set_motors(0, 0); + w2_set_motors(0, 0); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); delay_ms(500); } // left turn maze/charge void w2_maze_rotation_half_left() { - set_motors(0, 0); - set_motors(50, 50); + w2_set_motors(0, 0); + w2_set_motors(50, 50); delay_ms(150); - set_motors(-30, 30); + w2_set_motors(-30, 30); delay_ms(600); - set_motors(0, 0); + w2_set_motors(0, 0); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); delay_ms(500); } // right turn maze/charge void w2_maze_rotation_half_right() { - set_motors(0, 0); - set_motors(50, 50); + w2_set_motors(0, 0); + w2_set_motors(50, 50); delay_ms(150); - set_motors(30, -30); + w2_set_motors(30, -30); delay_ms(600); - set_motors(0, 0); - set_motors(50, 50); + w2_set_motors(0, 0); + w2_set_motors(50, 50); delay_ms(150); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); delay_ms(500); @@ -43,24 +44,24 @@ void w2_maze_rotation_half_right() { // 180 turn in grid void w2_grid_rotation_full() { - set_motors(60, -60); + w2_set_motors(60, -60); delay_ms(540); - set_motors(10, 10); + w2_set_motors(10, 10); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } // left turn in grid void w2_grid_rotation_half_left() { - set_motors(-30, 30); + w2_set_motors(-30, 30); delay_ms(600); - set_motors(10, 10); + w2_set_motors(10, 10); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } // right turn in grid void w2_grid_rotation_half_right() { - set_motors(30, -30); + w2_set_motors(30, -30); delay_ms(600); - set_motors(10, 10); + w2_set_motors(10, 10); g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); } diff --git a/robot/sercomm.c b/robot/sercomm.c index 0f251d2..16134d1 100644 --- a/robot/sercomm.c +++ b/robot/sercomm.c @@ -118,16 +118,20 @@ void w2_cmd_dirc_rx(w2_s_bin *data) { void w2_cmd_cord_rx(w2_s_bin *data) { return; } -// #include +#include void w2_cmd_bomd_rx(w2_s_bin *data) { - /* W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req); + W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req); + + req->position = w2_bin_hton32(req->position); 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; - g_w2_order_index++; */ + // sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position / W2_MAP_DEFAULT_WIDTH); + sprintf(buf, "%lu", req->position); + 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++; } void w2_cmd_sres_rx(w2_s_bin *data) { -- cgit v1.2.3