/*
 * Create first satellite removal
 * Started 3/22/08 by Stephen Carlson
 * Comments 3/24/08 by Kevin Heithaus 
 */

/*
 * This program is beginning to test out our create code and how we will
 * score using our prototype claw. We will be picking up the first cup
 * (satellite) and dropping it off the table.
 */

void main() {
    msleep(1000L); //Wait after start
    create_connect(); //Opens the link between the XBC and Create
    create_spin_block(200, -80); //Turn towards the first cup 
    create_mrp(200, 400L); //Move forward to the cup
    arm_down(); //Lowers the arm from starting position
    close_claw(); //Close the claw around the cup
    arm_up(); //Lift it up
    create_mrp(200, 450L); //Move forwards to the edge of the table
    open_claw(); //Drop the cup
    create_mrp(-200, 450L); //Move backwards from the pipe
    arm_down(); //Put the arm back down
    create_spin_block(200, 80); //Turn back towards the rest of the table
    create_disconnect(); //Close the connection
}

/*
 * Pre-condition: claw is open
 * Post-condition: claw has been closed after method call
 */
void close_claw() {
    motor(1, -75);
    msleep(300L);
    off(1);
    msleep(50L);
}

/*
 * Pre-condition: arm is up
 * Post-condition: arm has been lowered after method call
 */
void arm_down() {
    enable_servos();
    set_servo_position(3, 83);
    msleep(1000L);
    disable_servos();
}

/*
 * Pre-condition: arm is down
 * Post-condition: arm has been raised after method call
 */
void arm_up() {
    enable_servos();
    set_servo_position(3, 130);
    msleep(1000L);
}

/*
 * Pre-condition: claw is closed
 * Post-condition: claw has been opened after method call
 */
void open_claw() {
    motor(1, 100);
    msleep(600L);
    off(1);
    msleep(50L);
}

/*
 * Pre-condition: speed is a valid (int) speed for create motors, dist is a
 * valid (long) distance for the create to travel
 * Post-condition: create has mooved dist distance across the table at 
 * speed velocity.
 */
void create_mrp(int speed, long dist) {
    create_drive_straight(speed);
    if (speed < 0) dist = -dist;
    msleep(dist * 1000L / (long)speed);
    create_stop();
    msleep(50L);
}