summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--robot/io.c6
-rw-r--r--robot/io.h3
-rw-r--r--robot/mode_chrg.c151
-rw-r--r--robot/mode_dirc.c2
-rw-r--r--robot/mode_grid.c26
-rw-r--r--robot/mode_halt.c3
-rw-r--r--robot/mode_maze.c5
-rw-r--r--robot/mode_scal.c5
-rw-r--r--robot/mode_spin.c3
-rw-r--r--robot/movement.c37
-rw-r--r--robot/sercomm.c16
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 <stdio.h>
+
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 <stdio.h>
+#include <stdio.h>
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) {