Diff: programs.cpp

Revision:

10:1b09d4bb847b

Child:

11:7b3ee540ba56

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/programs.cpp	Mon Oct 26 23:58:08 2015 +0000
@@ -0,0 +1,303 @@
+/// PsiSwarm Beautiful Meme Project Source Code
+/// Version 0.1
+/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
+/// University of York
+
+// Programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
+
+#include "main.h"
+
+float battery_low_threshold = 3.63;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
+float battery_high_threshold = 3.97;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
+
+char was_turning = 0;
+enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
+enum random_walk_state internal_state = random_walk;
+char action_timeout = 0;
+char interaction_timeout = 0;
+char random_walk_timeout = 0;
+float previous_left_motor_speed = 0.5;
+float previous_right_motor_speed = 0.5;
+int obstacle_avoidance_threshold = 300;
+char recharge_power_check_count = 0;
+char battery_low_count = 0;
+
+///The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
+void head_to_bearing_program(int target_bearing)
+{
+    if(step_cycle == 0 || was_turning == 0) {
+        // Check if we are heading in the right bearing (+- 25 degrees)
+        int current_bearing = (360 - beacon_heading) % 360;
+        // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
+        char bearing_ok = 0;
+        int lower_bound = target_bearing - 25;
+        int upper_bound = target_bearing + 25;
+        if(lower_bound < 0) {
+            if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
+        } else if(upper_bound > 359) {
+            if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
+        } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
+        // Check if there is an obstacle in front of us
+        if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
+        else {
+            // Now move forward if we are facing correct bearing, otherwise turn
+            if(bearing_ok == 1) {
+                //Check if anything is in front of us to determine speed - if it is, move slowly
+                int t_time = 6 * BEACON_PERIOD;
+                float t_speed = 1.0;
+                if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
+                    t_time = 4 * BEACON_PERIOD;
+                    t_speed = 0.6;
+                }
+                if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
+                    t_time = 3 * BEACON_PERIOD;
+                    t_speed = 0.4;
+                }
+                if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
+                    t_time = 2 * BEACON_PERIOD;
+                    t_speed = 0.2;
+                }
+                time_based_forward(t_speed,t_time,0);
+                was_turning = 0;
+            } else {
+                turn_to_bearing(target_bearing);
+                was_turning = 1;
+            }
+        }
+    }
+}
+
+/// Recharging program
+void recharging_program()
+{
+    switch(recharging_state) {
+        case 0:
+            // We are not currently recharging, check the battery state
+            if(get_battery_voltage() < battery_low_threshold) {
+                // Battery is below low threshold
+                battery_low_count ++;
+                // We don't immediately start recharging routine in case as battery level can fluctuate a little due to load; if we get a low value for 4 continuous timesteps we start recharging routine
+                if(battery_low_count > 3) {
+                    // Set recharging state to 'looking for charger'
+                    recharging_state = 1;
+                    strcpy(prog_name,"CHARGING PROGRAM");
+                    set_program_info("HEAD TO BEACON");
+                }
+            } else battery_low_count = 0;
+            break;
+            // State 1:  Head to beacon [as this is where battery charger is]
+        case 1:
+            target_reached = 0;
+            head_to_bearing_program(0);
+            if(target_reached == 1) {
+                recharging_state = 2;
+                set_program_info("TURN 90 DEGREES");
+            }
+            break;
+            // Stage 2:  Turn 90 degrees to align with charging pads
+        case 2:
+            disable_ir_emitters = 1;
+            time_based_turn_degrees(0.8, 70.0, 1);
+            recharge_power_check_count = 0;
+            recharging_state = 3;
+            break;
+            // Stage 3:  Wait for turn to complete
+        case 3:
+            if (time_based_motor_action != 1) {
+                recharging_state = 4;
+                set_program_info("CHECK FOR POWER");
+            }
+            break;
+            // Stage 4:  Check if charging
+        case 4:
+            recharge_power_check_count++;
+            if(get_dc_status() == 1) {
+                recharging_state = 5;
+            } else {
+                if(recharge_power_check_count < 10)recharging_state = 6;
+                else {
+                    recharging_state = 7;
+                    set_program_info("NO POWER - RETRY");
+                }
+            }
+            break;
+            // Stage 5:  Charging.  Wait until threshold voltage exceeded
+        case 5:
+            if(get_battery_voltage() > battery_high_threshold) {
+                set_program_info("LEAVE CHARGER");
+                recharging_state = 7;
+            } else {
+                char b_voltage[16];
+                sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
+                set_program_info(b_voltage);
+            }
+            break;
+            // Stage 6:  We didn't find power, keep turning a couple of degrees at a time a recheck
+        case 6:
+            time_based_turn_degrees(0.5,4,1);
+            recharging_state = 4;
+            break;
+            // Stage 7:  Charge may be finished.  Turn 90 degrees then move away and resume previous program
+        case 7:
+            time_based_turn_degrees(0.8, 90.0, 1);
+            recharging_state = 8;
+            break;
+
+            // Stage 8:  Wait for turn to complete
+        case 8:
+            if (time_based_motor_action != 1) recharging_state = 9;
+            break;
+            // Stage 9:  Move away
+        case 9:
+            time_based_forward(0.5, 1000000, 1);
+            recharging_state = 10;
+            break;
+            // Stage 10:  Wait for move to complete
+        case 10:
+            if (time_based_motor_action != 1) recharging_state = 11;
+            break;
+            // Stage 11:  Check if battery is below low threshold; if it is, start over, else end charge cycle
+        case 11:
+            disable_ir_emitters = 0;
+            if (get_battery_voltage() < battery_low_threshold) {
+                recharging_state = 1;
+            } else {
+                recharging_state = 0;
+                //Restore name of old program on display
+                set_program(main_program_state);
+            }
+            break;
+    }
+}
+
+///Alan's Random Walk\Obstacle Avoid and Robot Interaction Program
+void curved_random_walk_with_interaction_program()
+{
+    if(internal_state == random_walk) {
+        if(interaction_timeout < 4)
+            interaction_timeout++;
+
+        int closest_robot = -1;
+        unsigned short shortest_distance = 0;
+
+        // Check whether there are any other robots within range
+        for(int i = 0; i < 8; i++) {
+            if(robots_found[i]) {
+                if(robots_distance[i] > shortest_distance) {
+                    shortest_distance = robots_distance[i];
+                    closest_robot = i;
+                }
+            }
+        }
+
+        // Turn towards the closest robot
+        if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
+            time_based_turn_degrees(1, robots_heading[closest_robot], 1);
+
+            action_timeout = 0;
+            internal_state = turn_towards;
+            char temp_message[17];
+            sprintf(temp_message,"FACE ROBOT %d",closest_robot);
+            set_program_info(temp_message);
+        } else { // Otherwise, do a random walk
+            // Check the front sensors for obstacles
+            if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
+                    reflected_sensor_data[1] > obstacle_avoidance_threshold ||
+                    reflected_sensor_data[6] > obstacle_avoidance_threshold ||
+                    reflected_sensor_data[7] > obstacle_avoidance_threshold) {
+                // Ignore the rear sensors when calculating the heading
+                reflected_sensor_data[2] = 0;
+                reflected_sensor_data[3] = 0;
+                reflected_sensor_data[4] = 0;
+                reflected_sensor_data[5] = 0;
+
+                // Turn 180 degrees away from the sensed obstacle
+                int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
+
+                // Normalise
+                heading %= 360;
+
+                if(heading < -180)
+                    heading += 360;
+
+                if(heading > 180)
+                    heading -= 360;
+                set_program_info("AVOID OBSTACLE");
+                time_based_turn_degrees(1, heading, 1);
+
+                action_timeout = 0;
+                internal_state = turn_away;
+            } else {
+                // Change motor speeds every 1s
+                if(random_walk_timeout >= 2) {
+                    float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
+
+                    float left_motor_speed = previous_left_motor_speed - random_offset;
+                    float right_motor_speed = previous_right_motor_speed + random_offset;
+
+                    float threshold = 0.25;
+
+                    if(left_motor_speed < threshold)
+                        left_motor_speed = threshold;
+
+                    if(right_motor_speed < threshold)
+                        right_motor_speed = threshold;
+
+                    set_left_motor_speed(left_motor_speed);
+                    set_right_motor_speed(right_motor_speed);
+
+                    random_walk_timeout = 0;
+                }
+
+                random_walk_timeout++;
+            }
+        }
+    } else if(internal_state == turn_towards) {
+        if(action_timeout < 4)
+            action_timeout++;
+        else {
+            set_program_info("SAY HELLO");
+            vibrate();
+
+            action_timeout = 0;
+            internal_state = interact;
+        }
+    } else if(internal_state == interact) {
+        if(action_timeout < 4)
+            action_timeout++;
+        else {
+            set_program_info("TURN AROUND");
+            time_based_turn_degrees(1, 180, 1);
+
+            action_timeout = 0;
+            internal_state = turn_away;
+        }
+
+    } else if(internal_state == turn_away) {
+        if(action_timeout < 4)
+            action_timeout++;
+        else {
+            set_program_info("RANDOM WALK");
+            interaction_timeout = 0;
+            internal_state = random_walk;
+        }
+    } else if(internal_state == avoid_obstacle) {
+        if(action_timeout < 4)
+            action_timeout++;
+        else
+            set_program_info("RANDOM WALK");
+            internal_state = random_walk;
+    }
+}
+
+
+void straight_random_walk_with_interaction_program()
+{
+
+}
+
+
+void find_space_program()
+{
+
+}
\ No newline at end of file