slow down motors when disconnected

1 files changed, 9 insertions, 5 deletions

diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 397d44d..b7df974 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -5,14 +5,18 @@
 #include "../shared/util.h"
 #include "orangutan_shim.h"
 
+#include <string.h>
+#include "../shared/errcatch.h"
+
 int16_t g_w2_mode_dirc_motor_l = 0;
 int16_t g_w2_mode_dirc_motor_r = 0;
-double g_w2_mode_dirc_power = 1.0;
+uint8_t g_w2_mode_dirc_power = 100;
 
 void w2_mode_dirc() {
-	// if (g_w2_connected) g_w2_mode_dirc_power = 1.0;
-	// else g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 0.01);
+	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);
 
-	// if (g_w2_mode_dirc_power == 0.0) w2_modes_call(W2_M_HALT);
-	set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power, g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power);
+	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);
 }