TEAM 3 |
6.270 - Autonomous Robot Design Competition - IAP 2005 |
Home | Strategy | Design | Code | Results | 6.270
The Code
/* Sensor Ports*/ int frontLeft=27, frontRight=29, backRight=31; /* Servo Ports */ int frontServo=5, backServo=0; /* Motor Ports */ int leftMotor = 1, rightMotor = 3; /* Gyro Port */ int gyro_port = 19; /* Position of my and opponent coordinates */ int myX, myY, oppoX, oppoY; /*e=distance tolerance*/ /* Speed of forward progress */ int speed=100; /* Threshold between black and white */ int flThreshold=80, frThreshold=80, brThreshold=20; int flColor, frColor, brColor; /* color of our bot 0=white, 1=black, and the rfID of our bot */ int selfColor=0, rfID=0; int process_num, numTests=5; /* Gyro variables */ int gyro_type = 300; /* 150 for ADXRS150, 300 for ADXRS300 */ float lsb_ms_per_deg=256.0; /* nominal scale factor depending on gyro type */ float scale_factor = 1.; /* adjust this for your particular gyro! */ float theta; /* current angle, units explained in update_angle() */ float offset; /* gyro offset */ long time_ms; void main() { rf_team=3; rf_enable(); calibrateInfrared(); start_machine(); /* printf("%d %d %d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight)); start_press(); /* /* start_press();*/ process_num = start_process(update_angle()); /*start_press();*/ orient(); enable_servos(); step1(); step2(); step3(); motorStop(); } void orient() { calibrate_gyro(); /*determines the angle(theta) and color of itself*/ if(bwColor(frontLeft)) { if(bwColor(frontRight)) { if(bwColor(backRight)) { selfColor = 1; } else { selfColor = 1; turn90ccw(); } } else { if(bwColor(backRight)) { printf("here"); sleep(5.0); turn180(); selfColor = 1; } else { turn180(); selfColor = 0; } } } else { if(bwColor(frontRight)) { if(bwColor(backRight)) { turn90cw(); selfColor=1; } else { turn90ccw(); selfColor=0; } } else { if(bwColor(backRight)) { selfColor=0; } else { turn90cw(); selfColor=0; printf("%d", selfColor); } } } /*figures out who it is on the rf*/ msleep(600L); if(selfColor==0) { if(rf_y0<0) rfID=0; else rfID=1; } else { if(rf_y0>0) rfID=0; else rfID=1; } } /* Return 0 if white, 1 if black */ int bwColor(int port) { if(port==frontLeft) { if(analogExp(port)>flThreshold) return 1; else return 0; } if(port==frontRight) { if(analogExp(port)>frThreshold) return 1; else return 0; } if(port==backRight) { if(analogExp(port)>brThreshold) return 1; else return 0; } } void closeFrontServo() { servo(frontServo, 3800); } void closeRearServo() { servo(backServo, 3800); } void openFrontServo() { servo(frontServo, 200); } void openRearServo() { servo(backServo, 200); } void turn90cw() { calibrate_gyro(); go_to_angle(-90.); motorStop(); } void turn90ccw() { calibrate_gyro(); go_to_angle(90.); motorStop(); } void turn180() { calibrate_gyro(); go_to_angle(180.); motorStop(); } void motorStop() { motor(leftMotor, 0); motor(rightMotor, 0); msleep(100L); } /* scraps one reading of the port */ int analogExp(int port) { analog(port); return analog(port); } void calibrateInfrared() { int i, flTotal, frTotal, brTotal; int flBlack, frBlack, brBlack, flWhite, frWhite, brWhite; /*CALIBRATING FOR BLACK IN ALL THREE SENSORS*/ printf("BLACK: fl=%d. fr=%d. br=%d.\n", frontLeft, frontRight, backRight); start_press(); flTotal=frTotal=brTotal=0; for(i=0; i<numTests; i++) { printf ("Values %d.%d.%d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight)); flTotal+=analogExp(frontLeft); frTotal+=analogExp(frontRight); brTotal+=analogExp(backRight); msleep(100L); } flBlack=flTotal/numTests; frBlack=frTotal/numTests; brBlack=brTotal/numTests; printf("BLACK=%d.%d.%d.\n", flBlack, frBlack, brBlack); sleep(1.); /*CALIBRATING FOR WHITE IN ALL THREE SENSORS*/ printf("WHITE: fl=%d. fr=%d. br=%d.\n", frontLeft, frontRight, backRight); start_press(); flTotal=frTotal=brTotal=0; for(i=0; i<numTests; i++) { printf ("Values %d.%d.%d\n", analogExp(frontLeft), analogExp(frontRight), analogExp(backRight)); flTotal+=analogExp(frontLeft); frTotal+=analogExp(frontRight); brTotal+=analogExp(backRight); msleep(100L); } flWhite=flTotal/numTests; frWhite=frTotal/numTests; brWhite=brTotal/numTests; printf("WHITE=%d.%d.%d.\n", flWhite, frWhite, brWhite); sleep(1.); flThreshold=(flBlack+flWhite)/2; frThreshold=(frBlack+frWhite)/2; brThreshold=(brBlack+brWhite)/2; } /* At the end of which front faces green*/ void step1() { closeFrontServo(); closeRearServo(); calibrate_gyro(); go_straight(0., speed, 1000L); /*CHANGED SO THAT IT DOES NOT RECALIBRATE AFTER PUSHING THE 4 BALLS IN*/ motorStop(); msleep(500L); /* calibrate_gyro();*/ if (selfColor==0) { /* turn90cw();*/ go_to_angle(-90.); motorStop(); } else { /*turn90ccw();*/ go_to_angle(90.); motorStop(); } openFrontServo(); openRearServo(); } /* Ends with the bot facing vertical */ void step2() { /* calibrate_gyro();*/ float a; if(selfColor==0) a=-90.; if(selfColor==1) a=90.; if (rfID==0) { if (rf_x1>=0) { go_straight(a, -speed, 3000L); calibrate_gyro(); go_straight(0., speed, 6000L); motorStop(); go_straight(0., -speed, 250L); if (rf_y0>0) { turn90ccw(); go_straight(90., -speed, 2000L); calibrate_gyro(); go_straight(0., speed, 8500L); } else { turn90cw(); go_straight(-90., -speed, 2000L); calibrate_gyro(); go_straight(0., speed, 8500L); } } else { go_straight(a, speed, 3000L); calibrate_gyro(); go_straight(0., -speed, 6000L); motorStop(); go_straight(0., speed, 250L); if (rf_y0 >0) { turn90cw(); go_straight(-90., speed, 2000L); calibrate_gyro(); go_straight(0., -speed, 8500L); } else { turn90ccw(); go_straight(90., speed, 2000L); calibrate_gyro(); go_straight(0., -speed, 8500L); } } } else { if (rf_x0>=0) { go_straight(a, -speed, 3000L); calibrate_gyro(); go_straight(0., speed, 6000L); motorStop(); go_straight(0., -speed, 250L); if (rf_y1 >0) { turn90ccw(); go_straight(90., -speed, 2000L); calibrate_gyro(); go_straight(0., speed, 8500L); } else { turn90cw(); go_straight(-90., -speed, 2000L); calibrate_gyro(); go_straight(0., speed, 8500L); } } else { go_straight(a, speed, 3000L); calibrate_gyro(); go_straight(0., -speed, 6000L); motorStop(); go_straight(0., speed, 250L); if (rf_y1 >0) { turn90cw(); go_straight(-90., speed, 2000L); calibrate_gyro(); go_straight(0., -speed, 8500L); } else { turn90ccw(); go_straight(90., speed, 2000L); calibrate_gyro(); go_straight(0., -speed, 8500L); } } } motorStop(); } /* Dumps the front compartment into our scoring area and vote with the four of the other color, */ void step3() { calibrate_gyro(); if (rfID==0) { if (rf_y0>=0 && rf_x0>=0) { closeRearServo(); msleep(500L); go_straight(0., speed, 2800L); openRearServo(); turn90ccw(); calibrate_gyro(); closeFrontServo(); msleep(1000L); go_straight(0., -speed, 1000L); go_straight(0., speed, 4000L); } if (rf_y0<0 && rf_x0>=0) { closeRearServo(); msleep(500L); go_straight(0., speed, 2800L); openRearServo(); turn90cw(); calibrate_gyro(); closeFrontServo(); msleep(1000L); go_straight(0., -speed, 1000L); go_straight(0., speed, 4000L); } if (rf_y0>=0 && rf_x0<0) { closeFrontServo(); msleep(500L); go_straight(0., -speed, 2800L); openFrontServo(); turn90cw(); calibrate_gyro(); closeRearServo(); msleep(1000L); go_straight(0., speed, 1000L); go_straight(0., -speed, 4000L); } if (rf_y0<0 && rf_x0<0) { closeFrontServo(); msleep(500L); go_straight(0., -speed, 2800L); openFrontServo(); turn90ccw(); calibrate_gyro(); closeRearServo(); msleep(1000L); go_straight(0., speed, 1000L); go_straight(0., -speed, 4000L); } } if (rfID==1) { if (rf_y1>=0 && rf_x1>=0) { closeRearServo(); msleep(500L); go_straight(0., speed, 2800L); openRearServo(); turn90ccw(); calibrate_gyro(); closeFrontServo(); msleep(1000L); go_straight(0., -speed, 1000L); go_straight(0., speed, 4000L); } if (rf_y1<0 && rf_x1>=0) { closeRearServo(); msleep(500L); go_straight(0., speed, 2800L); openRearServo(); turn90cw(); calibrate_gyro(); closeFrontServo(); msleep(1000L); go_straight(0., -speed, 1000L); go_straight(0., speed, 4000L); } if (rf_y1>=0 && rf_x1<0) { closeFrontServo(); msleep(500L); go_straight(0., -speed, 2800L); openFrontServo(); turn90cw(); calibrate_gyro(); closeRearServo(); msleep(1000L); go_straight(0., speed, 1000L); go_straight(0., -speed, 4000L); } if (rf_y1<0 && rf_x1<0) { closeFrontServo(); msleep(500L); go_straight(0., -speed, 2800L); openFrontServo(); turn90ccw(); calibrate_gyro(); closeRearServo(); msleep(1000L); go_straight(0., speed, 1000L); go_straight(0., -speed, 4000L); } } } /* Gyro support code */ void calibrate_gyro(){ /* Averaging a number of readings only works when the gyro noise is greater than the converter resolution. Since the xrs300 noise is far lower than the 20mV resolution of the 8-bit converter, a noise source has been added to the gyro board. You should average for about 5 seconds to minimize the effects of the added noise. */ int samples; float sum; long start_time_ms; long int_time_ms = 1000L; kill_process(process_num); printf("Stabilizing...\n"); msleep(200L); /* Wait for robot to stabilize mechanically */ printf("Calibrating offset...\n"); /* average some samples for offset */ sum = 0.0; samples = 0; start_time_ms = mseconds(); while( ( mseconds() - start_time_ms ) < int_time_ms ){ sum += (float)analog(gyro_port); samples++; } offset = sum / (float)samples; theta = 0.0; process_num = start_process(update_angle()); } /* calibrate_gyro() */ void update_angle(){ /* The units of theta are converter LSBs * ms 1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for ADXRS150 1 deg = 0.256LSB * 1000ms = 256 (stored in lsb_ms_per_deg) To convert theta to degrees, divide by lsb_ms_per_deg. */ long delta_t_ms, new_time_ms; int i; time_ms = mseconds(); for(;;){ new_time_ms = mseconds(); delta_t_ms = (new_time_ms - time_ms); /* This does the Riemann sum; CCW gyro output polarity is negative when the gyro is visible from the top of the robot. */ theta -= ((float)analog(gyro_port) - offset) * (float)delta_t_ms; time_ms = new_time_ms; defer(); } } /* update_angle() */ void go_to_angle(float angle) { /* To convert degrees to units of theta, multiply by lsb_ms_per_deg. Demo robot turns at about 120 deg/s when speed = 100 */ float angle_target; float error, tolerance; int speed; tolerance = 1.0 * lsb_ms_per_deg; angle_target = angle * lsb_ms_per_deg; error = theta - angle_target; /*MAKES THE ROBOT TURN THE SHORTEST ARC*/ while(fabs(theta-angle_target-360.*lsb_ms_per_deg) < fabs(error)) { angle_target+=360.*lsb_ms_per_deg; error=theta-angle_target; } while(fabs(theta-angle_target+360.*lsb_ms_per_deg) < fabs(error)) { angle_target-=360.*lsb_ms_per_deg; error=theta-angle_target; } if ( fabs(error) > tolerance ) { spin(90 * -sign(error)); while(fabs(error) > (80.0 * lsb_ms_per_deg)){ error = theta - angle_target; } spin(75 * -sign(error)); while(fabs(error) > (60.0 * lsb_ms_per_deg)){ error = theta - angle_target; } spin(70 * -sign(error)); while(fabs(error) > (10.0 * lsb_ms_per_deg)){ error = theta - angle_target; } /* Slam on the brakes */ spin(100 * sign(error)); msleep(10L); /* Fine adjust either direction as necessary */ spin(50 * -sign(error)); while(fabs(error) > tolerance){ error = (theta - angle_target); spin(50 * -sign(error)); } } /* if ( fabs(error)... */ } /* go_to_angle() */ void spin(int speed){ motor(leftMotor, -speed); motor(rightMotor, speed); } /* spin() */ int sign(float a){ if ( a > 0.0 ) return 1; else return -1; } /* sign() */ float fabs(float a){ if ( a > 0.0 ) return a; else return -a; } /* fabs() */ void go_straight(float angle, int speed, long duration_ms){ /* The robot turns to the specified angle, then goes straight for the specified duration. Note that speed must be greater than tweak, and less than 100 - tweak. */ long start_time; int tweak = 30; float angle_target; float tolerance; tolerance = lsb_ms_per_deg * 1.; /* +/-0.5 degree accuracy ***************CHANGED */ /* first turn the robot */ go_to_angle(angle) ; /* then go forward under gyro control for the specified time */ start_time = mseconds(); angle_target = angle * lsb_ms_per_deg; while ((mseconds() - start_time) < duration_ms){ if (theta < (angle_target - tolerance)) { /* Turn CCW a little */ motor(leftMotor, speed - tweak); motor(rightMotor, speed + tweak); } else if (theta > (angle_target + tolerance)){ /* Turn CW a little */ motor(leftMotor, speed + tweak); motor(rightMotor, speed - tweak); } else { motor(leftMotor, speed); motor(rightMotor, speed); } /* You can check a sensor here and terminate the while loop */ } /* while */ motorStop(); } /* void go_straight() */