aboutsummaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/errcatch.c6
-rw-r--r--robot/hypervisor.c23
-rw-r--r--robot/hypervisor.h14
-rw-r--r--robot/io.c59
-rw-r--r--robot/io.h7
-rw-r--r--robot/mode_dirc.c13
-rw-r--r--robot/mode_halt.c3
-rw-r--r--robot/mode_lcal.c3
-rw-r--r--robot/mode_lcal.h11
-rw-r--r--robot/mode_scal.c17
-rw-r--r--robot/mode_spin.c3
-rw-r--r--robot/modes.c4
-rw-r--r--robot/readme.md8
-rw-r--r--robot/sercomm.c22
-rw-r--r--robot/setup.c3
15 files changed, 93 insertions, 103 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 17c96fa..3830d54 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -16,6 +16,12 @@ void w2_errcatch_handle_error(w2_s_error *error) {
case W2_E_WARN_UNCAUGHT_ERROR: {
break;
}
+ case W2_E_WARN_OBSTACLE_DETECTED:
+ break;
+ case W2_E_CRIT_OBSTACLE_STUCK:
+ break;
+ case W2_E_WARN_MODE_HISTORY_BUFFER_IOB:
+ break;
default: {
g_w2_error_uncaught = true;
#ifdef W2_SIM
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 2a6120b..be3fb77 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -14,6 +14,9 @@ unsigned long g_w2_hypervisor_ema_io_ms = 0;
unsigned long g_w2_hypervisor_ema_mode_ms = 0;
uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0};
+uint64_t g_w2_hypervisor_ms_timer_offset = 0;
+uint64_t g_w2_hypervisor_ms_timer_cpu_ticks = 0;
+
unsigned int g_w2_ping_ms = 0;
uint8_t g_w2_ping_id = 0;
bool g_w2_ping_received = true;
@@ -26,17 +29,17 @@ void w2_hypervisor_main() {
if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n");
#endif
- g_w2_hypervisor_uptime_ms += get_ms();
- time_reset();
+ g_w2_hypervisor_uptime_ms += w2_get_ms();
+ w2_time_reset();
w2_sercomm_main();
- unsigned long sercomm_time = get_ms();
+ unsigned long sercomm_time = w2_get_ms();
w2_errcatch_main();
- unsigned long errcatch_time = get_ms() - sercomm_time;
- // w2_io_main();
- unsigned long io_time = get_ms() - errcatch_time;
+ unsigned long errcatch_time = w2_get_ms() - sercomm_time;
+ w2_io_main();
+ unsigned long io_time = w2_get_ms() - errcatch_time;
w2_modes_main();
- unsigned long mode_time = get_ms() - io_time;
+ unsigned long mode_time = w2_get_ms() - io_time;
// calculate exponential moving averages
g_w2_hypervisor_ema_sercomm_ms =
@@ -58,6 +61,12 @@ void w2_hypervisor_main() {
g_w2_hypervisor_cycles++;
}
+uint64_t w2_get_ms() {
+ return ticks_to_microseconds(get_ticks() - g_w2_hypervisor_ms_timer_offset) / 1e3;
+}
+
+void w2_time_reset() { g_w2_hypervisor_ms_timer_offset = get_ticks(); }
+
void w2_hypervisor_time_start(uint8_t label) {
g_w2_hypervisor_timers[label] = g_w2_hypervisor_uptime_ms;
}
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index 4b1ed9b..e9699cf 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -7,9 +7,10 @@
#include "../shared/bool.h"
/** amount of parallel timers */
-#define W2_HYPERVISOR_TIMER_COUNT (1)
+#define W2_HYPERVISOR_TIMER_COUNT 2
-#define W2_TIMER_PING (0)
+#define W2_TIMER_PING 0
+#define W2_TIMER_OBJECT_DETECTION 1
extern uint64_t g_w2_hypervisor_cycles;
extern uint64_t g_w2_hypervisor_uptime_ms;
@@ -32,6 +33,15 @@ extern bool g_w2_connected;
*/
void w2_hypervisor_main();
+/**
+ * pololu's `get_ms()` estimates run time by only adding the CPU time taken up
+ * by pololu library functions. this function uses the `get_ticks()` function
+ * to calculate elapsed milliseconds, and should therefore be more accurate.
+ */
+uint64_t w2_get_ms();
+/** reset time returned by `w2_get_ms()` */
+void w2_time_reset();
+
/** start timer with label `label` */
void w2_hypervisor_time_start(uint8_t label);
/** stop timer with label `label` */
diff --git a/robot/io.c b/robot/io.c
index 8b34aba..717778a 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -1,35 +1,36 @@
-#include <string.h>
-
-#include "../shared/consts.h"
#include "io.h"
+#include "../shared/consts.h"
+#include "../shared/errcatch.h"
+#include "hypervisor.h"
#include "modes.h"
#include "orangutan_shim.h"
-w2_s_io_all g_w2_io;
+#include <stdio.h>
+
+bool g_w2_io_object_detected = false;
+
+void w2_io_object_detection() {
+ unsigned int front_distance = analog_read(W2_FRONT_SENSOR_PIN);
+
+ if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_HALT) return;
+
+ if (!g_w2_io_object_detected && front_distance >= W2_IO_DISTANCE_CLOSE_THRESHOLD) {
+ g_w2_io_object_detected = true;
+ w2_hypervisor_time_start(W2_TIMER_OBJECT_DETECTION);
+ w2_errcatch_throw(W2_E_WARN_OBSTACLE_DETECTED);
+ }
+
+ if (g_w2_io_object_detected) {
+ 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_io_main() {
- g_w2_io.button[0].pressed = get_single_debounced_button_press(BUTTON_A);
- g_w2_io.button[1].pressed = get_single_debounced_button_press(BUTTON_B);
- g_w2_io.button[2].pressed = get_single_debounced_button_press(BUTTON_C);
- g_w2_io.button[3].pressed = get_single_debounced_button_press(TOP_BUTTON);
- g_w2_io.button[4].pressed = get_single_debounced_button_press(BOTTOM_BUTTON);
-
- unsigned int sensor_values[5];
- qtr_read(sensor_values, QTR_EMITTERS_ON);
- for (int i = 0; i < 5; i++) g_w2_io.qtr[i].range = sensor_values[i];
-
- // TODO average voltage over mutiple samples sensor
- g_w2_io.battery.charge_level = analog_read(W2_BATTERY_PIN);
- g_w2_io.front_distance.detection = analog_read(W2_FRONT_SENSOR_PIN);
- g_w2_io.side_distance.detection = analog_read(W2_SIDE_SENSOR_PIN);
-
- set_motors(g_w2_io.motor_left.speed, g_w2_io.motor_right.speed);
- red_led(g_w2_io.led_red.on);
- green_led(g_w2_io.led_green.on);
-
- // TODO don't do this every cycle
- char text_copy[17];
- memcpy((char *)text_copy, g_w2_io.lcd.text, 16);
- text_copy[16] = 0x00;
- print(text_copy);
-};
+ w2_io_object_detection();
+ // TODO: battery status
+
+ return;
+}
diff --git a/robot/io.h b/robot/io.h
index 64ce293..31fe410 100644
--- a/robot/io.h
+++ b/robot/io.h
@@ -1,11 +1,10 @@
#pragma once
+#include "../shared/bool.h"
+
/** @file io.h */
-#include "../shared/io.h"
+extern bool g_w2_io_object_detected;
/** @brief i/o module main */
void w2_io_main();
-
-/** @brief global struct containing all i/o */
-extern w2_s_io_all g_w2_io;
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 7021721..d9c0d17 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -10,16 +10,5 @@
int16_t g_w2_mode_dirc_motor_l = 0;
int16_t g_w2_mode_dirc_motor_r = 0;
-uint8_t g_w2_mode_dirc_power = 100;
-void w2_mode_dirc() {
- if (g_w2_connected == 1)
- g_w2_mode_dirc_power = 100;
- else
- g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 1);
-
- if (g_w2_mode_dirc_power == 0) w2_modes_call(W2_M_HALT);
-
- set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power / 100,
- g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power / 100);
-}
+void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); }
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index 88d6183..083d45b 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,3 +1,4 @@
#include "mode_halt.h"
+#include "orangutan_shim.h"
-void w2_mode_halt() { return; }
+void w2_mode_halt() { set_motors(0, 0); }
diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c
deleted file mode 100644
index 896d0f0..0000000
--- a/robot/mode_lcal.c
+++ /dev/null
@@ -1,3 +0,0 @@
-#include "mode_lcal.h"
-
-void w2_mode_lcal() {}
diff --git a/robot/mode_lcal.h b/robot/mode_lcal.h
deleted file mode 100644
index 5a43701..0000000
--- a/robot/mode_lcal.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#pragma once
-
-/** @file mode_lcal.h */
-
-/**
- * calibration mode
- *
- * turns robot on its own axis 360 degress, and aligns the front sensors with
- * the line if found, else triggers halt mode (emergency)
- */
-void w2_mode_lcal();
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index f3178d7..53cbf67 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -1,19 +1,20 @@
#include "mode_scal.h"
+#include "modes.h"
+#include "orangutan_shim.h"
void w2_mode_scal() {
- // TODO ???
- /* pololu_3pi_init(2000);
+ pololu_3pi_init(2000);
for (int counter = 0; counter < 80; counter++) {
if (counter < 20 || counter >= 60) {
- g_w2_io.motor_left.speed = 40;
- g_w2_io.motor_right.speed = -40;
+ set_motors(40, -40);
} else {
- g_w2_io.motor_left.speed = -40;
- g_w2_io.motor_right.speed = 40;
+ set_motors(-40, 40);
}
calibrate_line_sensors(IR_EMITTERS_ON);
- delay_ms(20); // TODO foei
- } */
+ delay_ms(20);
+ }
+
+ w2_modes_call(W2_M_PREV);
}
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
index 9145eb3..9ee83b0 100644
--- a/robot/mode_spin.c
+++ b/robot/mode_spin.c
@@ -1,3 +1,4 @@
#include "mode_spin.h"
+#include "orangutan_shim.h"
-void w2_mode_spin() {}
+void w2_mode_spin() { set_motors(255, -255); }
diff --git a/robot/modes.c b/robot/modes.c
index 7decf47..4995d6f 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -8,7 +8,6 @@
#include "mode_dirc.h"
#include "mode_grid.h"
#include "mode_halt.h"
-#include "mode_lcal.h"
#include "mode_maze.h"
#include "mode_scal.h"
#include "mode_spin.h"
@@ -22,7 +21,6 @@ void w2_modes_init() {
g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
g_w2_modes[W2_M_GRID] = &w2_mode_grid;
g_w2_modes[W2_M_HALT] = &w2_mode_halt;
- g_w2_modes[W2_M_LCAL] = &w2_mode_lcal;
g_w2_modes[W2_M_MAZE] = &w2_mode_maze;
g_w2_modes[W2_M_SCAL] = &w2_mode_scal;
g_w2_modes[W2_M_SPIN] = &w2_mode_spin;
@@ -48,7 +46,7 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
// forward mode change to sercomm
W2_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
- msg->mode = new_mode;
+ msg->mode = g_w2_mode_history[g_w2_mode_history_index];
w2_sercomm_append_msg(msg_bin);
free(msg_bin);
diff --git a/robot/readme.md b/robot/readme.md
index f54af21..e8316e3 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -50,8 +50,7 @@ organizational and form more of a software 'skeleton', while the 'maze' and
Maze ─┤
Warehouse ─┤
Emergency stop ─┤
- *logic modes* -> Line finding ─┤
- Charge station ─┤
+ *logic modes* -> Charge station ─┤
Direct control ─┤
Wet floor ─┤
Sensor calibration ─┘
@@ -74,11 +73,10 @@ what they're supposed to do:
|maze |`mode_maze `|done|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module|
|warehouse |`mode_grid `|may 31|Loek| controls robot during warehouse portion of map; hands off control to maze module|
|emergency stop |`mode_halt `|done|Fiona| stops all execution until emergency mode is reset by software or user|
-|line finding |`mode_lcal `|may 31|Fiona| find line by turning on own axis if lost|
|charge station |`mode_chrg `|may 31|Fiona| go to the charging station transition in the grid, and continue until a black circle is found|
|direct control |`mode_dirc `|done|Loek| respond to [DIRC](../protocol.md#DIRC) commands|
-|wet floor |`mode_spin `|may 31|Fiona| spin uncontrollably (simulating wet floor??)|
-|sensor calibration|`mode_scal `|may 31|Jorn & Abdullaahi| calibrate underside uv sensors|
+|wet floor |`mode_spin `|done|Fiona| spin uncontrollably (simulating wet floor??)|
+|sensor calibration|`mode_scal `|done|Jorn & Abdullaahi| calibrate underside uv sensors|
## some standards
diff --git a/robot/sercomm.c b/robot/sercomm.c
index c50dd15..efad449 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -99,7 +99,11 @@ void w2_cmd_ping_rx(w2_s_bin *data) { w2_sercomm_append_msg(data); }
void w2_cmd_mode_rx(w2_s_bin *data) {
W2_CAST_BIN(w2_s_cmd_mode_rx, data, req);
- w2_modes_swap(req->mode);
+ if (req->mode == W2_M_SCAL) {
+ w2_modes_call(req->mode);
+ } else {
+ w2_modes_swap(req->mode);
+ }
}
void w2_cmd_sped_rx(w2_s_bin *data) { return; }
@@ -135,22 +139,6 @@ void w2_cmd_sres_rx(w2_s_bin *data) {
void w2_cmd_mcfg_rx(w2_s_bin *data) { return; }
-void w2_cmd_sens_rx(w2_s_bin *data) {
- W2_CREATE_MSG_BIN(w2_s_cmd_sens_tx, res_msg, res_bin);
- res_msg->opcode = W2_CMD_SENS | W2_CMDDIR_TX;
- memcpy((uint8_t *)&res_msg->io, (uint8_t *)&g_w2_io, sizeof(w2_s_io_all));
-
- for (int i = 0; i < 5; i++) w2_bin_repl_hton16(&res_msg->io.qtr[i].range);
- w2_bin_repl_hton16(&res_msg->io.front_distance.detection);
- w2_bin_repl_hton16(&res_msg->io.side_distance.detection);
- w2_bin_repl_hton16(&res_msg->io.battery.charge_level);
- w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_left.speed);
- w2_bin_repl_hton16((uint16_t *)&res_msg->io.motor_right.speed);
-
- w2_sercomm_append_msg(res_bin);
- free(res_bin);
-}
-
void w2_cmd_info_rx(w2_s_bin *data) {
W2_CREATE_MSG_BIN(w2_s_cmd_info_tx, res_msg, res_bin);
res_msg->opcode = W2_CMD_INFO | W2_CMDDIR_TX;
diff --git a/robot/setup.c b/robot/setup.c
index 4706d64..dfd88bd 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -34,6 +34,9 @@ void w2_setup_main() {
w2_modes_swap(W2_M_MAZE);
w2_modes_call(W2_M_HALT);
+ // send info
+ w2_cmd_info_rx(NULL);
+
// indicate startup done
play("L50 c>c");
}