/* This is our real, game-day program.   */

int TRUE = 1; 
int FALSE = 0; 
int pid;      /* the process ID of the "mymain" process */
int START_SENSOR1 = 20;
int START_SENSOR2 = 21;
int START_LIGHT = 30; 
float GAME_HALF_TIME = 30.0;  /* 1/2 the game time (sleep doesn't 
                                work well for long times. */
int LEFT_REAR_MOTOR = 5;
int RIGHT_REAR_MOTOR = 4;
int LEFT_MIDDLE_MOTOR = 3;
int RIGHT_MIDDLE_MOTOR = 2;
int LEFT_FRONT_MOTOR = 1;
int RIGHT_FRONT_MOTOR = 0;

int IR_PORT1 = 2;
int IR_PORT2 = 3;
int IR_PORT3 = 4;

int color;



void main(){
    ao();
    /* dip 1 signifies white start side*/

    if (dip_switch(1))  {
        set_ir_transmit_frequency(100);
        set_ir_receive_frequency(125);
        color = 1;
    } else {
        set_ir_transmit_frequency(125);
        set_ir_receive_frequency(100);
        color = 0;
    }
    ir_transmit_on();

    Prepare_Robot();  /* Routine to help set up over start light */
                  /* returns when Choose Button pushed */
    printf("Starting\n");

    while((analog(START_SENSOR1) >= START_LIGHT) && (analog(START_SENSOR2) >= START_LIGHT)) {}
    pid = start_process(go_pimping());
    sleep(GAME_HALF_TIME);
    sleep(GAME_HALF_TIME);

    kill_process(pid);        /* Time has elapsed, so kill everything */
    printf("Did we win?/n");
    ao();
    beep();
}


void Prepare_Robot(){
    int left, middle, right;
    /* Routine to help set up over start light; push choose to stop 
        assumes START_SENSOR is in a digital port (even if it 
        is an analog sensor).  If you use 
        an analog port, modify this accordingly */
    while(!choose_button()){
        ir_receive_on();
        sleep(.25);
        ir_receive_off();
        left = (ir_counts(IR_PORT1) > 20);
        middle = (ir_counts(IR_PORT2) > 20);
        right = (ir_counts(IR_PORT3) > 20);
        if (color) {
            printf("White  (L  M  R)%d %d (%d,%d,%d)\n",analog(START_SENSOR1),analog(START_SENSOR2),left,middle,right);
        } else {
            printf("Black  (L  M  R)%d %d (%d,%d,%d)\n",analog(START_SENSOR1),analog(START_SENSOR2),left,middle,right);
        }
    }
} 

void apply_speeds(int rightSpeed, int leftSpeed) {
    if (dip_switch(3)) {
        rightSpeed = 8;
        leftSpeed = 8;
    }
    motor(RIGHT_FRONT_MOTOR,rightSpeed);
    motor(RIGHT_MIDDLE_MOTOR,rightSpeed);
    motor(RIGHT_REAR_MOTOR,rightSpeed);
    motor(LEFT_FRONT_MOTOR,leftSpeed);
    motor(LEFT_MIDDLE_MOTOR,leftSpeed);
    motor(LEFT_REAR_MOTOR,leftSpeed);
}


/* The actual functioning of our robot*/
void go_pimping() {
int a = 8; /* forward speed */
int b = 8; /* turn speed */
int e = 4; /* long turn following forward movement speed*/
long c = 25L; /* short turn duration */
long d = 25L; /* long turn duration */

    long COUNT_TIME = 40L;     /* Default Counting Time */
    int IR_COUNT_CONST = 2;

    if (dip_switch(4)) { /* disable forward movemet if dip4 set - used for testing*/
        a = 0;
        e = 0;
    }
    while(1) {
        int left = (ir_counts(IR_PORT1) >= IR_COUNT_CONST);
        int middle = (ir_counts(IR_PORT2) >= IR_COUNT_CONST);
        int right = (ir_counts(IR_PORT3) >= IR_COUNT_CONST);
        ir_receive_on();
        msleep(COUNT_TIME);
        printf("IR Counts:      %d,%d\n",left, right );
        ir_receive_off();

        /* determine which way to turn*/
        if (left && !right) {
            /* turn left*/
            apply_speeds(b,-b);
            if (middle) {
                msleep(c);
                apply_speeds(a,a);
            } else {
                msleep(d);
                apply_speeds(e,e);
            }
        } else if (right && !left) {
            /* turn right*/
            apply_speeds(-b,b);
            if (middle) {
                msleep(c);
                apply_speeds(a,a);
            } else {
                msleep(d);
                apply_speeds(e,e);
            }
        } else {
            /* continue forward*/
            apply_speeds(a,a);
        }
    }

}

Back to Software