summaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/mode_chrg.c11
-rw-r--r--robot/mode_grid.c26
-rw-r--r--robot/mode_grid.h4
-rw-r--r--robot/sercomm.c15
4 files changed, 20 insertions, 36 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 5b733af..0f02f37 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -15,16 +15,12 @@ void w2_short_drive() {
void w2_home() {
set_motors(0, 0);
delay_ms(150);
- clear();
- print("CHARGING");
set_motors(30, 30);
delay_ms(600);
set_motors(0, 0);
- play_frequency(300, 500, 7);
delay_ms(600);
g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
g_w2_charged_status = 1;
- clear();
delay_ms(2000);
}
@@ -40,8 +36,6 @@ void w2_charge_cross_walk() {
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);
- // clear();
- print("WALK");
g_w2_transition++;
if (g_w2_transition == 3) { // TODO: document g_w2_transition
set_motors(40, 40);
@@ -62,9 +56,6 @@ void w2_charge_cross_walk() {
void w2_mode_chrg() {
unsigned int last_proportional = 0;
long integral = 0;
- // initialize();
- clear();
- print("CHARGE");
while (1) {
// Get the position of the line. Note that we *must* provide
@@ -113,13 +104,11 @@ void w2_mode_chrg() {
w2_maze_rotation_full();
w2_short_drive();
} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
- clear();
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) {
- clear();
w2_maze_rotation_half_left();
} else {
if (power_difference < 0 &&
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 16dbef3..235ac66 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -7,24 +7,12 @@ int g_w2_order_number;
int g_w2_maze_status = 0;
-w2_s_grid_coordinate g_w2_order[4] = {
- {0, 0},
- {1, 1},
- {2, 2},
- {3, 3},
-};
+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;
-void w2_location_message() {
- clear();
- print_long(g_w2_location.x);
- print(",");
- print_long(g_w2_location.y);
- delay(200);
-}
-
int g_w2_detection = 0;
int g_w2_transition;
char g_w2_x_location = 0;
@@ -39,8 +27,6 @@ void w2_crosswalk_stroll() {
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);
- clear();
- print("WALK");
g_w2_transition++;
if (g_w2_transition == 3) {
set_motors(40, 40);
@@ -206,8 +192,6 @@ void w2_turn_east() {
void w2_arrived_message() {
if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
- clear();
- print("ORDER ");
print_long(g_w2_order_number);
play_frequency(400, 500, 7);
delay(500);
@@ -256,27 +240,23 @@ void w2_go_to_y() {
void w2_mode_grid() {
set_motors(0, 0);
- clear();
- print("GRID");
delay(500);
w2_begin_location();
// TODO: orders read here
- for (int i = 0; i < 4; i++) {
+ for (int i = 0; i < g_w2_order_index; i++) {
g_w2_order_number = i + 1;
g_w2_destination.x = g_w2_order[i].x;
g_w2_destination.y = g_w2_order[i].y;
- w2_location_message();
delay(1000);
w2_go_to_x();
w2_go_to_y();
}
w2_end_destination();
- w2_location_message();
delay(1000);
w2_go_to_y();
w2_go_to_x();
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index 2df0d33..408a8ff 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -22,4 +22,6 @@ typedef struct {
int y;
} w2_s_grid_coordinate;
-extern w2_s_grid_coordinate g_w2_order[4];
+extern w2_s_grid_coordinate g_w2_order[16];
+extern unsigned int g_w2_order_index;
+
diff --git a/robot/sercomm.c b/robot/sercomm.c
index efad449..d12fcca 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -4,6 +4,7 @@
#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"
@@ -117,7 +118,18 @@ void w2_cmd_dirc_rx(w2_s_bin *data) {
void w2_cmd_cord_rx(w2_s_bin *data) { return; }
-void w2_cmd_bomd_rx(w2_s_bin *data) { return; }
+// #include <stdio.h>
+void w2_cmd_bomd_rx(w2_s_bin *data) {
+ /* W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req);
+
+ 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++; */
+}
void w2_cmd_sres_rx(w2_s_bin *data) {
W2_CAST_BIN(w2_s_cmd_sres_rx, data, req);
@@ -149,6 +161,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());
w2_sercomm_append_msg(res_bin);
free(res_bin);