/* dynamic_object.ic
 * Copyright Dean F. Hougen 2003
 * Freely available for educational use.  All other rights reserved.
 */

/* This Interactive C code is for use with the dynamic object robot for
 * project 3 of CS 4970-001/5973-002 at the University of Oklahoma during
 * Spring Semester 2003.
 *
 * The constraints on this robot's design are:
 * (1) that it should not move the static cube objects in the environment
 * too far from their previous locations too rapidly (to provide only a
 * small amount of dynamism in the overall environment),
 * (2) that it move itself more rapidly through the environment without
 * getting stuck for long periods of time (to provide a more dynamic
 * component with which the students' robots interact), 
 * (3) that it move in relatively straight lines most of the time (to
 * provide some level of predictability in its actions), 
 * (4) that it back up while turning when necessary after collisions (to
 * allow the robot to keep moving through the environment), 
 * (5) that it have bumpers on each side that cause it to stop running when
 * contacted by a student robot (to allow the student robot to score points
 * by "disabling the dynamic object"), and 
 * (6) that it be made from the same robot kits provided to the students
 * without using parts that are likely to be in high demand by the students
 * for their own robots (so that the students may duplicate the dynamic
 * object robot from their kits while still having sufficient components
 * available to construct their own robots to complete the
 * objectives of project 3).
 * 
 * To satisfy constraint 1, the robot has a wedge-shaped front bumper.  The
 * shape of this bumper should push a cube slightly ahead and off to one
 * side, letting the robot pass without moving any cube object very far
 * during a single encounter.
 *
 * To satisfy constraint 2, the robot has two sensors: A switch attached to
 * the front bumper and the Lego light sensor mounted to point at the right
 * rear wheel, which is passively driven.  The switch a micro-switch from
 * the electronics kit.  The switch should cause the robot to register a
 * firm impact, triggering its backup-while-turning maneuver as described
 * in the project description and below.  However, if the contact is not
 * firm (such as when the robot contacts a wall at an angle close to
 * parallel and grinds to a halt), the robot may become stuck without
 * depressing the contact switch.  In these cases, the light sensor code,
 * which has been monitoring changes in the reflectance as the passive
 * wheel turns, will record an extended period of time in which no changes
 * occur (because the wheel is not turning), which will also trigger the
 * backup- while-turning maneuver.  (If the light sensor checking code
 * notices no changes for an extended period while backing, it will halt
 * the backing.)
 *
 * To satisfy constraints 3 and 4, the robot has a single drive motor and a
 * simple steering mechanism, similar to those found on many inexpensive
 * radio-controlled cars.  The robot has four wheels.  The rear two wheels
 * face forward and are attached to separate axles.  The drive motor is
 * attached to the left rear wheel and the right rear wheel is passive (as
 * mentioned above).  The front two wheels are attached to a common axle
 * that is hinged to the body near the right wheel.  When the robot is
 * moving forward, the left end of this axle presses back against the body
 * of the robot such that the axle is relatively perpendicular to the
 * direction of motion, resulting in the robot moving relatively straight
 * ahead.  When the robot backs up, however, the front axle rotates around
 * the attachment point near the right wheel until the left end of the axle
 * contacts a new point on the robot body, at which time the axle is about
 * 15 degrees clockwise of its previous orientation.  Thus the robot turns
 * anytime it is backing up.
 *
 * To satisfy constraint 5, a single Lego contact switch is mechanically
 * connected to two side bumpers.
 *
 * Constraint 6 is satisfied by using the RCX brick for the on-board
 * computer, rather than the HandyBoard, the overall simple design (for
 * example, using only a single drive motor and passive steering), and by
 * the other hardware choices made (such as using the small wheels).  Note
 * that I could slightly simplify the design and reduce the number of
 * uncommon parts used by eliminating the switch on the front bumper.
 * However, to minimize damage to the motor by reducing stall time, and to
 * reduce the chances of the dynamic object robot pushing a student robot,
 * the front bumper switch is retained.
*/

/* CONSTANTS:
 *  
 * LIGHT, FRONT_BUMPER, and SIDE_BUMPERS denote the port numbers of the
 * sensors (with names that should be self-explanatory).
 * MOTOR likewise denotes the port number for the motor output (where 1
 * corresponds to A, 2 to B, and 3 to C).
 * MAX_RECORD is the number of light sensor readings to record.  A higher
 * number means a longer time window for monitoring.  A longer time window,
 * in turn, means a lower chance of a false stall reading but a longer
 * delay before a real stall is recognized.
 * FORWARD_POWER and BACKUP_POWER are the values passed to the motor
 * commands for going forward and backward, respectively.
 * BACKTIME is the number of seconds to back up.
 */

#define LIGHT          1
#define FRONT_BUMPER   2
#define SIDE_BUMPERS   3
#define MOTOR          1
#define MAX_RECORD     3
#define FORWARD_POWER 85
#define BACKUP_POWER -90
#define BACKTIME       1


/* GLOBAL VARIABLES: 
 *  
 * The global variable "done" is used to record whether the side bumpers
 * have been depressed, causing the dynamic object to be "disabled."  A
 * global is used so that it may be examined in the main loop yet be set in
 * check_side_bumpers(), which is run as a separate IC "process."
 * Similarly, the global variable "stuck" is used to record whether the
 * robot has made no progress for some time (as measured by the light
 * sensor aimed at the right rear wheel).  This is set by check_stuck() and
 * examined in both the main loop and its function backup().
 */

int done=0;
int stuck=0;

/* IC "PROCESSES":
 * 
 * The two functions that run as a separate IC "processes" (which are
 * really more like threads) are check_side_bumpers() and check_stuck().
 * The reason that these are run as separate IC processes is so that they 
 * can record sensing data that happens at any time (i.e., during backup() 
 * as well as in the main loop).
 */

int check_side_bumpers(){
    while(1){
        if (digital(SIDE_BUMPERS))
          done=1;
    }
}  // end check_side_bumpers


int check_stuck(){
    int light_value,               // current light sensor value
      old_light_value[MAX_RECORD], // circular list of previous light values
      light_counter=0,             // index to previous light sensor values
      local_stuck,                 // boolean, based on light sensor values
      local_counter;               // used only within small for loops
    
    // set previous light sensor values to zero
    for (local_counter=0; local_counter<MAX_RECORD; local_counter++)
      old_light_value[local_counter]=0;
    
    while (!done){
        light_value= light(LIGHT);  // record current light sensor value            
        printf("%d", light_value);  // print current light sensor value
        
        local_stuck=1;  // presume that we are stuck
        // check all previous light sensor values within our window
        for (local_counter=0; local_counter<MAX_RECORD; local_counter++){
            if (light_value != old_light_value[local_counter]){
                local_stuck=0;  // if any differ, we're not stuck
                stuck=0;
            }
        }
        
        if (local_stuck){  // if we're stuck
            stuck = 1;  // set global stuck value
            // reset previous light sensor values to zero
            for (local_counter=0; local_counter<MAX_RECORD; local_counter++)
                old_light_value[local_counter]=0;
            light_counter= 0;
        }
        else {
            // if we're not stuck, record current light value in circular list
            old_light_value[light_counter] = light_value;
            light_counter++;
            if (light_counter == MAX_RECORD)
                light_counter=0;
        }
    }  // end while
}  // end check_stuck


/* FUNCTIONS:
 *
 * The only function not run as a separate IC "process" is backup() which
 * runs the motor backward for a given period of time (given in seconds),
 * then shuts off the motor.  If the robot gets stuck while backing up, it
 * shuts off the motor and returns immediately.
 */

int backup(int time){
    int loop_times = 100 * time,  // loop 100 times for every second
      local_counter = 0;
    motor(MOTOR, BACKUP_POWER);
    
    while (!stuck && (local_counter<loop_times)){
        msleep(10L);  // sleep 1/100 of a second
        local_counter++;
    }
    
    motor(MOTOR, 0);  // shut off motor (don't keep backing too long)
}

/* MAIN:
 * 
 * As long as the side bumpers aren't hit, drive forward and check for
 * collisions.  If a collision happens, back up.  When the side bumpers are
 * hit, we're done.
 */

int main(void){
    start_process(check_side_bumpers());  // always check side bumpers
    start_process(check_stuck());
    
    while (!done){  // loop until side bumpers hit
        motor(MOTOR, FORWARD_POWER);  // drive forward
        
        if (stuck){
            // if we're stuck, back up
            printf("stuck");
            backup(BACKTIME);
        } 
        
        if (digital(FRONT_BUMPER)){
            // if we hit something, back up
            printf("bump");
            backup(BACKTIME);
        }
    }
    
    // side bumpers have been hit
    printf("done");
    ao();  // all off
}