summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorlonkaars <loek@pipeframe.xyz>2022-06-24 12:22:49 +0200
committerlonkaars <loek@pipeframe.xyz>2022-06-24 12:22:49 +0200
commitc47518a99d8c8a49a8a97100bdff5b06cfa125ae (patch)
treee59f79b106f31a21ae54991af682ba2cf80ebd47
parent40f6164ba6187a0160af9fe200bbd1d729e8c03b (diff)
mode_chrg & mode_maze respect g_w2_target_area
-rw-r--r--robot/mode_chrg.c43
-rw-r--r--robot/mode_chrg.h3
-rw-r--r--robot/mode_grid.c8
-rw-r--r--robot/modes.c2
4 files changed, 34 insertions, 22 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index c41c0f7..47391a1 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,10 +1,12 @@
#include "mode_chrg.h"
+#include "../shared/bool.h"
+#include "hypervisor.h"
#include "mode_grid.h"
#include "modes.h"
#include "movement.h"
#include "orangutan_shim.h"
-int g_w2_charged_status; // used to detect the charging station (once)
+bool g_w2_chrg_aligned;
void w2_short_drive() {
set_motors(50, 50);
@@ -12,19 +14,6 @@ void w2_short_drive() {
set_motors(0, 0);
}
-// charging station
-void w2_home() {
- set_motors(0, 0);
- delay_ms(150);
- set_motors(30, 30);
- delay_ms(600);
- set_motors(0, 0);
- delay_ms(600);
- g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
- g_w2_charged_status = 1;
- delay_ms(2000);
-}
-
// crosswalk from charging station back to maze
void w2_charge_cross_walk() {
if (g_w2_transition == 0) {
@@ -55,6 +44,8 @@ void w2_charge_cross_walk() {
}
}
+void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; }
+
// main function for charge mode
void w2_mode_chrg() {
unsigned int last_proportional = 0;
@@ -99,13 +90,23 @@ void w2_mode_chrg() {
}
}
- 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) &&
- g_w2_charged_status == 0) {
- w2_home();
- 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) {
+ 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] >= 200 && g_w2_sensors[4] < 100) {
w2_maze_rotation_half_left();
}
diff --git a/robot/mode_chrg.h b/robot/mode_chrg.h
index a870e58..1015813 100644
--- a/robot/mode_chrg.h
+++ b/robot/mode_chrg.h
@@ -9,3 +9,6 @@
* black circle is found
*/
void w2_mode_chrg();
+
+/** called when mode is switched to charge station mode */
+void w2_mode_chrg_onswitch();
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 7e6db49..cfecae5 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,8 +1,13 @@
#include "mode_grid.h"
+#include "hypervisor.h"
#include "modes.h"
#include "movement.h"
#include "orangutan_shim.h"
+/**
+ * TODO: mode_grid g_w2_target_area laten volgen
+ */
+
int g_w2_order_number;
int g_w2_maze_status = 0;
@@ -30,7 +35,8 @@ void w2_crosswalk_stroll() {
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) {
+ 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);
g_w2_transition++;
if (g_w2_transition == 3) {
diff --git a/robot/modes.c b/robot/modes.c
index 559d81c..a54bc43 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -44,6 +44,8 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
g_w2_mode_history[g_w2_mode_history_index] = new_mode;
}
+ if (new_mode == W2_M_CHRG) w2_mode_chrg_onswitch();
+
// 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;