Milestone #3

#pragma config(Sensor,in1,right_quad,sensorQuadEncoder,int5)
#pragma config(Sensor,in2,left_quad,sensorQuadEncoder,int6)
#pragma config(Motor,port1,right_motor,tmotorNormal,openLoop)
#pragma config(Motor,port2,left_motor,tmotorNormal,openLoop)
#pragma config(Motor,port8,arm_motor,tmotorNormal,openLoop)
#pragma config(Sensor,in3,right_button,sensorTouch)
#pragma config(Sensor,in4,left_button,sensorTouch)
#pragma config(Sensor,in5,arm_limit_switch,sensorTouch)
//  Sensor and motor configuration


/*
 *  CONSTANTS
 */


//  QUEUE

//  Maximum number of motions in the queue
#define QUEUE_SIZE 4
//  Number of integers per element in the queue
#define QUEUE_NUM_PER_ELEMENT 2


//  MOTOR CONTROL

//  Maximum power level of a motor
#define MOTOR_MAX 100
//  Default power level of a motor
#define MOTOR_ON MOTOR_MAX
//  Off power level of a motor
#define MOTOR_OFF 0
//  Scalar that will be used to obtain
//  the actual power level of the right
//  motor
#define RIGHT_SCALAR 1
//  Scalar that will be used to obtain
//  the actual power level of the left
//  motor
#define LEFT_SCALAR (-1)
//  Braking power
#define MOTOR_BRAKE 25
//  Scalar for obtaining reverse power
//  levels
#define BACKWARDS_SCALAR (-1)


//  BRAKING

//  The timer to use for brake polling
#define BRAKE_TIMER T1
//  The interval at which to poll while braking in milliseconds
#define BRAKE_INTERVAL 50


//  ADAPTIVE ANGULAR VELOCITY ALGORITHM

//  The timer to use for power adjustment polling
#define TRIM_TIMER T1
//  The interval at which to recalculate angular velocity
//  and adjust power levels accordings in milliseconds
#define TRIM_INTERVAL 100
//  The amount a motor's power level will be stepped up
//  or down each time it's found to be necessary
#define TRIM_STEP 1


//  ANGULAR ACCELERATION COLLISION DETECTION ALGORITHM

//  Minimum number of deltas needed to calculate acceleration
#define MIN_DELTAS 2
//  The number of subsequent times an axle must have a negative
//  acceleration before a collision is detected
#define COLLISION_THRESHOLD 5


//  MOVEMENT PARAMETERS

//  Distance to move forwards in response
//  to button push in millimeters
#define FORWARD_DISTANCE 1000
//  Distance to move backwards in response
//  to button push in millimeters
#define BACKWARD_DISTANCE 1000


/*
 *  MACROS
 */
#define LEFT_QUAD abs(SensorValue[left_quad])
#define RIGHT_QUAD abs(SensorValue[right_quad])


/*
 *  TYPE DEFINITIONS/DECLARATIONS
 */


/*
 *  Types of motions the robot
 *  may execute
 */
typedef enum {

    Stopped,        //  Robot is not moving
    TurnLeft,       //  Robot is turning left as part of a correction
    TurnRight,      //  Robot is turning right as part of a correction
    Straight,       //  Full speed ahead
    Approach,       //  Ahead at low power
    ScanLeft,       //  Robot is turning to the left, scanning
    ScanRight,      //  Robot is turning to the right, scanning
    CorrectLeft,    //  Robot is making a fine correction to the left
    CorrectRight,   //  Robot is making a fine correction to the left
    Retreat         //  Robot has encountered an obstacle and is backing off

} MotionType;


/*
 *  Encapsulates all information about
 *  a motion.
 */
typedef struct {

    //  Type of motion
    MotionType type;
    //  How long the motion is to be executed for
    int distance;

} Motion;


/*
 *  Encapsulates all information about
 *  a motor.
 */
typedef struct {

    //  The maximum power level that this motor
    //  may take on.  This is learned by observation
    //  of the motors' angular velocity with respect
    //  to the angular velocity of the opposing motor.
    int max_power;
    //  The current speed this motor is set to.  This
    //  will be scaled by max_power so that speed=MOTOR_MAX
    //  corresponds to max_power.
    int power;
    //  The motor's angular displacement as last measured.
    int old;
    //  The motor's angular velocity as last measured.
    int delta;
    //  The motor's angular acceleration as last measured.
    int delta_delta;
    //  The motor's last delta (used for acceleration
    //  calculations).
    int old_delta;
    //  Whether the motor is being braked
    bool brake;
    //  The number of subsequent polls which have
    //  detected a collision
    int collision_count;

} MotorInfo;


/*
 *  GLOBALS
 */


//  QUEUE

int queue_mem [QUEUE_SIZE*QUEUE_NUM_PER_ELEMENT];
int queue_begin;
int queue_end;


//  Information about the right motor
MotorInfo right;
//  Information about the left motor
MotorInfo left;
//  Information about the current motion
Motion curr;
//  How many times the delta of the motors
//  has been recorded since this motion began.
//
//  Maximum value: 2.
//  Default value: 0
int num_deltas;
//  Whether a push of the left button
//  has had the corresponding event
//  dispatched.
//
//  Default value: false
bool left_push_handled;
//  Whether a push of the right button
//  has had the corresponding event
//  dispatched.
//
//  Default value: true
bool right_push_handled;
//  Whether the robot is currently
//  in the process of coming to stop
//  after a motion completes.
//
//  Default value: false
bool braking;
//  Set if the robot should skip
//  the current motion immediately.
//
//  Default value: false
bool skip;
/*
 *  There are ~1.13599117689mm per degree
 *  of rotation of the 5 1/8" wheels used
 *  for the robot.
 *
 *  Since the VEX PIC 0.5 doesn't support
 *  floating point arithmetic, if we simply
 *  rounded this number to the nearest integer
 *  we would be ~15% under -- a large margin.
 *
 *  However, multiplying by arbitrary powers
 *  of ten to "push" decimals into whole numbers
 *  and thereby increase the precision is also
 *  not an option, due to the extremely limited
 *  (-32 768 to 32 767) 16-bit signed (unsigned
 *  integers also unsupported mysteriously) range
 *  of integers on the VEX PIC 0.5.
 *
 *  Therefore we have the variable "periods" and
 *  the variable "nums" which are arrays of const
 *  integers.  "periods" specifies the number of
 *  degrees after which a component of the
 *  floating point number given above shall
 *  "accumulate" into some whole number of millimeters.
 *  "nums" specifies the number of whole millimeters that
 *  shall be accumulated after the specified period.
 */
const int periods []={
    1,      //  Ones                (1*1)/      1
            //                                  .
    10,     //  Tens                (1*10)/     1
    100,    //  Hundreds            (3*100)/    3
    500     //  Thousands           (3*1000)/   6   Rounded off here
            //                                  .
            //                                  .
            //                                  .
};
const int nums []={
    1,
    1,
    3,
    3
};
#define NUM_ACCUMULATE 4


/*
 *  FUNCTIONS
 */


/*
 *  Prepares the queue for use.
 */
void init_queue () {

    queue_begin=0;
    queue_end=0;

}


/*
 *  Enqueues a new motion, if possible.
 *
 *  Returns false if the motion could not
 *  be enqueued.
 */
bool enqueue (MotionType type, int distance) {

    //  Bounds check
    if (queue_end==(QUEUE_SIZE*QUEUE_NUM_PER_ELEMENT)) return false;

    //  Enqueue
    queue_mem[queue_end]=(int)type;
    ++queue_end;
    queue_mem[queue_end]=distance;
    ++queue_end;

    //  Succeed
    return true;

}


/*
 *  Attempts to dequeue a motion from the
 *  queue and store it in the global "curr".
 *
 *  Returns false if there are no motions
 *  to dequeue.
 */
bool dequeue () {

    //  Bounds check
    if (queue_end==queue_begin) return false;

    //  Dequeue
    curr.type=(MotionType)queue_mem[queue_begin];
    ++queue_begin;
    curr.distance=queue_mem[queue_begin];
    ++queue_begin;

    //  Reset pointers to the beginning
    //  to reuse memory
    if (queue_end==queue_begin) {

        queue_end=0;
        queue_begin=0;

    }

    //  Succeed
    return true;

}


/*
 *  Clears all motions from the queue.
 */
void clear_queue () {

    queue_end=0;
    queue_begin=0;

}


/*
 *  Retrieves a count of the number
 *  of motions in the queue.
 */
int count_queue () {

    return (queue_end-queue_begin)/QUEUE_NUM_PER_ELEMENT;

}


/*
 *  Resets all applicable values
 *  after the completion or abortion
 *  of a motion.
 */
void motion_reset () {

    right.old=0;
    left.old=0;

    right.collision_count=0;
    left.collision_count=0;

    SensorValue[right_quad]=0;
    SensorValue[left_quad]=0;

    num_deltas=0;

    ClearTimer(TRIM_TIMER);

}


/*
 *  Commits all changes to speeds
 *  to the motors.
 */
void motors_commit () {

    motor[right_motor]=RIGHT_SCALAR*((right.max_power*right.power)/MOTOR_MAX);
    motor[left_motor]=LEFT_SCALAR*((left.max_power*left.power)/MOTOR_MAX);

}


/*
 *  Polls the quadrature encoders
 *  and thereby obtains information
 *  about the angular displacement,
 *  velocity, and acceleration
 *  of the axles.
 *
 *  Using this information the motors'
 *  power levels are evened out such
 *  that the robot goes straight, and
 *  collisions are detected (when a
 *  gross negative angular acceleration
 *  occurs).
 */
void motor_maintenance () {

    //  Only execute at each expiration
    //  of the polling interval
    if (time1[TRIM_TIMER]>=TRIM_INTERVAL) {

        //  Reset timer for next poll
        ClearTimer(TRIM_TIMER);

        //  Acquire and calculate values

        int new_right=RIGHT_QUAD;
        int new_left=LEFT_QUAD;

        //  Increment delta counter if
        //  it's low enough (i.e. under 2)
        //  that it would be meaningful
        if (num_deltas<MIN_DELTAS) ++num_deltas;

        //  Store previous velocities
        right.old_delta=right.delta;
        left.old_delta=left.delta;

        //  Calculate angular velocities
        left.delta=abs(new_left-left.old);
        right.delta=abs(new_right-right.old);

        right.old=new_right;
        left.old=new_left;

        //  Right wheel is spinning faster
        if (right.delta>left.delta) {

            //  Adjust power of left wheel up
            //  unless it's already at maximum
            //  power, in which case decrease
            //  the power of the right wheel,
            //  unless it's turned off

            if (left.max_power!=MOTOR_MAX) left.max_power+=TRIM_STEP;
            else if (right.max_power!=MOTOR_OFF) right.max_power-=TRIM_STEP;

        //  Left wheel is spinning faster
        } else if (left.delta>right.delta) {

            //  Adjust power of right wheel up
            //  unless it's already at maximum
            //  power, in which case decrease
            //  the power of the left wheel,
            //  unless it's turned off
            if (right.max_power!=MOTOR_MAX) right.max_power+=TRIM_STEP;
            else if (left.max_power!=MOTOR_OFF) left.max_power-=TRIM_STEP;

        }

        //  If we have at least two deltas
        //  we can calculate the angular
        //  acceleration
        if (num_deltas>=MIN_DELTAS) {

            //  Calculate angular acceleration
            right.delta_delta=right.delta-right.old_delta;
            left.delta_delta=left.delta-left.old_delta;

            //  Has either axle inexplicably stopped completely?
            //  If so trigger a collision straight away
            if (
                //  Right axle
                (
                    (right.power!=0) &&
                    (right.delta==0)
                ) ||
                //  Left axle
                (
                    (left.power!=0) &&
                    (left.delta==0)
                )
            ) goto collision;

            //  Check for negative angular acceleration on
            //  the right axle
            if (
                (right.power!=0) &&
                (right.delta_delta<0)
            ) ++right.collision_count;
            else right.collision_count=0;

            //  Check for negative angular acceleration on
            //  the left axle
            if (
                (left.power!=0) &&
                (left.delta_delta<0)
            ) ++left.collision_count;
            else left.collision_count=0;

            //  See if either collision count is over the threshold
            if (
                (left.collision_count>=COLLISION_THRESHOLD) ||
                (right.collision_count>=COLLISION_THRESHOLD)
            ) {

                //  Entry from above
                collision:

                for (;;) {

                    motor[right_motor]=MOTOR_OFF;
                    motor[left_motor]=MOTOR_OFF;

                }

            }

        }

    }

}


/*
 *  Ends a motion by bringing
 *  the robot to a stop as
 *  quickly as possible.
 */
void brake () {

    //  If both motors are powered
    //  down and we're not braking,
    //  pass
    if (
        (right.power==MOTOR_OFF) &&
        (left.power==MOTOR_OFF) &&
        !braking
    ) return;

    //  If we weren't already braking
    //  get setup
    if (!braking) {

        //  Set defaults
        right.brake=false;
        left.brake=false;

        //  If the left motor is running, brake it
        if (left.power!=MOTOR_OFF) {

            left.brake=true;

            //  Set braking power in the opposite direction
            left.power=
                //  Obtains the sign of the current power
                (left.power/abs(left.power))*
                //  Reverses it
                BACKWARDS_SCALAR*
                //  Scales it up to braking power
                MOTOR_BRAKE;

        }

        //  If the right motor is running, brake it
        if (right.power!=MOTOR_OFF) {

            right.brake=true;

            //  Set braking power in the opposite direction
            right.power=
                //  Obtains the sign of the current power
                (right.power/abs(right.power))*
                //  Reverses it
                BACKWARDS_SCALAR*
                //  Scales it up to braking power
                MOTOR_BRAKE;

        }

        //  We are now braking
        braking=true;

        //  Reset the poll timer
        ClearTimer(BRAKE_TIMER);

        //  We use raw quad values because
        //  sign matters
        right.old=SensorValue[right_quad];
        left.old=SensorValue[left_quad];

    }

    //  Pass until it's time to poll
    if (time1[BRAKE_TIMER]<BRAKE_INTERVAL) return;

    //  Begin next poll interval
    ClearTimer(BRAKE_TIMER);

    //  Calculate deltas
    right.delta=SensorValue[right_quad]-right.old;
    left.delta=SensorValue[left_quad]-left.old;

    //  Update values
    right.old=SensorValue[right_quad];
    left.old=SensorValue[left_quad];

    //  Check right axle
    if (right.brake) {

        if (
            //  Axle has completely stopped
            (right.delta==0) ||
            (
                //  Avoid divide by zero
                (right.old!=0) &&
                //  Motor moved just slightly in the opposite
                //  direction (we're just comparing the signs)
                ((right.delta/abs(right.delta))!=(right.old/abs(right.old)))
            )
        ) {

            //  Power down
            right.power=MOTOR_OFF;

        }

    }

    //  Check left axle
    if (left.brake) {

        if (
            //  Axle has completely stopped
            (left.delta==0) ||
            (
                //  Avoid divide by zero
                (left.old!=0) &&
                //  Axle moved just slightly in the opposite
                //  direction (we're just comparing the signs)
                ((left.delta/abs(left.delta))!=(left.old/abs(left.old)))
            )
        ) {

            //  Power down
            left.power=MOTOR_OFF;

        }

    }

    //  If both axles have completely stopped
    //  moving, braking is over
    if (
        (right.delta==0) &&
        (left.delta==0)
    ) braking=false;

}


/*
 *  Returns an approximation of the
 *  distance the robot has travelled
 *  since the last time the quadrature
 *  encoders were reset.
 *
 *  Picks whichever quad encoder has
 *  logged the most angular displacement
 *  and performs simulated floating point
 *  multiplication to convert that
 *  angular displacement into linear
 *  displacement.
 */
int distance_travelled () {

    int left_quad_val=LEFT_QUAD;
    int right_quad_val=RIGHT_QUAD;

    int quad_val=(left_quad_val<right_quad_val) ? right_quad_val : left_quad_val;

    int returnthis=0;

    int i;
    for (i=0;i<NUM_ACCUMULATE;++i) returnthis+=(quad_val/periods[i])*nums[i];

    return returnthis;

}


/*
 *  Maintains the robot's motion.
 */
void movement () {

    //  If braking is in function call
    //  the braking function
    if (braking) {

        brake();

        //  If braking just ended
        //  reset
        if (!braking) motion_reset();

    }

    //  If we're braking, pass
    if (!braking) {

        //  Whether a new movement has just begun
        bool new_movement=false;

        //  If the robot is currently stopped
        //  or if we're skipping the remainder of
        //  this motion or if this motion is
        //  complete, check for a new motion.
        if (
            (curr.type==Stopped) ||
            (distance_travelled()>=curr.distance)
        ) {

            new_movement=dequeue();

            if (!new_movement) curr.type=Stopped;

        }

        //  Reset skip so we don't skip
        //  subsequent motions
        skip=false;

        //  If we're stopped, stopping, or
        //  transitioning call the brake
        //  function and keep everything reset
        if (
            (curr.type==Stopped) ||
            new_movement
        ) {

            //  THE ORDER IN WHICH THESE ARE CALLED IS
            //  VERY SIGNIFICANT DO NOT REARRANGE
            motion_reset();
            ClearTimer(TRIM_TIMER);
            brake();

        //  Otherwise perform movement maintenance
        } else {

            //  Set speeds for the various motions
            switch (curr.type) {

                //  Straight ahead
                case Straight:{

                    right.power=MOTOR_ON;
                    left.power=MOTOR_ON;

                }break;

                //  Reverse
                case Retreat:{

                    right.power=BACKWARDS_SCALAR*MOTOR_ON;
                    left.power=BACKWARDS_SCALAR*MOTOR_ON;

                }break;

            }

        }

        motor_maintenance();

    }

    //  Commit power changes
    motors_commit();

}


/*
 *  Monitors inputs and takes appropriate
 *  action.
 */
void monitor_input () {

    //  LEFT BUTTON

    //  If the button has been released
    //  the next event will be handled
    if (SensorValue[left_button]==0) {

        left_push_handled=false;

    //  If the button is pressed and
    //  we can handle the event, do so
    } else if (!left_push_handled) {

        //  Block this event from being
        //  handled on subsequent
        //  invocations
        left_push_handled=true;

        enqueue(Straight,FORWARD_DISTANCE);

    }

    //  RIGHT BUTTON

    //  If the button has been released
    //  the next event will be handled
    if (SensorValue[right_button]==0) {

        right_push_handled=false;

    //  If the button is pressed and
    //  we can handle the event, do so
    } else if (!right_push_handled) {

        //  Block this event from being
        //  handled on subsequent
        //  invocations
        right_push_handled=true;

        enqueue(Retreat,BACKWARD_DISTANCE);

    }

}


/*
 *  MAIN
 */


task main () {

    //  Initialize and fulfill pre-conditions
    init_queue();
    motion_reset();
    right.max_power=MOTOR_MAX;
    left.max_power=MOTOR_MAX;
    right.power=MOTOR_OFF;
    left.power=MOTOR_OFF;
    curr.type=Stopped;
    left_push_handled=false;
    right_push_handled=false;
    braking=false;
    skip=false;
    motors_commit();

    //  Loop forever
    for (;;) {

        monitor_input();
        movement();

    }

}