diff options
Diffstat (limited to 'robot/mode_dirc.c')
| -rw-r--r-- | robot/mode_dirc.c | 19 |
1 files changed, 17 insertions, 2 deletions
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c index 9ed9fef..7021721 100644 --- a/robot/mode_dirc.c +++ b/robot/mode_dirc.c @@ -1,10 +1,25 @@ #include "mode_dirc.h" +#include "../shared/util.h" +#include "hypervisor.h" #include "io.h" +#include "modes.h" +#include "orangutan_shim.h" + +#include "../shared/errcatch.h" +#include <string.h> 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() { - g_w2_io.motor_left.speed = g_w2_mode_dirc_motor_l; - g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r; + 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); } |