/* 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