diff options
Diffstat (limited to 'robot/mode_chrg.c')
| -rw-r--r-- | robot/mode_chrg.c | 133 |
1 files changed, 132 insertions, 1 deletions
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c index 81808d5..27e17d9 100644 --- a/robot/mode_chrg.c +++ b/robot/mode_chrg.c @@ -1,3 +1,134 @@ #include "mode_chrg.h" +#include "orangutan_shim.h" +#include "movement.h" +#include "modes.h" +#include "transition.h" -void w2_mode_chrg() {} +int g_w2_charged_status; + +void w2_short_drive() { + set_motors(50, 50); + delay(150); + set_motors(0, 0); +} + +void w2_home() { + set_motors(0, 0); + delay_ms(150); + clear(); + print("CHARGING"); + set_motors(30, 30); + delay_ms(600); + set_motors(0, 0); + play_frequency(300, 500, 7); + delay_ms(600); + g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); + g_w2_charged_status = 1; + clear(); + delay_ms(2000); +} + + +void w2_charge_cross_walk() { + 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); + delay(500); + 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); + clear(); + print("WALK"); + g_w2_transition++; + if (g_w2_transition == 3) { //TODO: document g_w2_transition + set_motors(40, 40); + delay(600); + set_motors(0, 0); + + g_w2_transition = 0; + // g_w2_maze_status = 1; + w2_modes_swap(W2_M_MAZE); + break; + } + } else { + g_w2_transition = 0; + w2_full_rotation(); + } + } +} + + +void w2_mode_chrg() { + unsigned int last_proportional = 0; + long integral = 0; + // initialize(); + g_w2_charged_status = 0; + clear(); + print("CHARGE"); + + 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; + } + } + + 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_full_rotation(); + w2_short_drive(); + } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { + clear(); + w2_half_rotation_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) { + clear(); + w2_crossway_detection(); + } 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); + } + } + } +} |