#define XXX -1 #define TRUE 1 #define FALSE 0 #define CW -1 #define CCW 1 /* Colors */ #define BLUE 2 #define SAND 3 #define LIGHT_SENSOR_LEFT 1 #define LIGHT_SENSOR_RIGHT 2 #define LIGHT_SENSOR_MID 0 #define LIGHT_SENSOR_BACK 3 int COLOR_PORT[4] = {20,16,18,22}; #define LED_SWITCH 6 #define LED_WAIT 0.01 #define READ_LIGHT_TIME 4 int LIGHT_TRESHOLD[4]={37,74,88,128} ; /* Driving wheels */ #define MOTOR_LEFT_FRONT 0 #define MOTOR_LEFT_BACK 1 #define MOTOR_RIGHT_FRONT 2 #define MOTOR_RIGHT_BACK 3 #define SHAFT_LEFT 8 #define SHAFT_RIGHT 7 /* Front wheel */ #define SERVO_WHEEL 0 #define FRONT_WHEEL_STRAIGHT 2000 #define FRONT_WHEEL_WAIT 0.9 /* Arm */ #define SERVO_ARM 1 #define ARM_HOLD 3500 #define ARM_RELEASE 0 #define ARM_RELEASE_TIME 4 #define ARM_RELEASE_WAIT 1.0 #define ARM_BUMP_RIGHT 9 #define ARM_BUMP_LEFT 10 #define ELBOW_LEFT 30 /* Orientation */ #define TO_SELF 0 #define TO_PLATFORM 1 #define TO_OPPONENT 2 #define TO_WALL 3 #define CIRCUM_TO_PLATFORM 97 #define CIRCUM_TO_WALL 100 #define CIRCUM_TO_SELF 224 /* Following Wall */ #define FRONT_WHEEL_BIAS_1 60 #define FRONT_WHEEL_BIAS_2 30 #define FOL_WALL_SPEED_1 100 #define DIST_FOL_WALL_1 560 #define TURN_AWAY_DIST_1 1 #define TURN_AWAY_DIST_2 1 #define TURN_AWAY_SPEED 100 #define FOL_WALL_BACK_OFF 15 #define FOL_WALL_TURN 10 /* Detect Line */ #define DETECT_LINE_SPEED 100 #define READ_LIGHT_WAIT 0.2 /* Following Line */ #define VEER_WAIT 0.3 #define CHECK_LINE_LOOP_WAIT 0.3 #define WHEEL_VEER 200 #define RIGHT_WALL_DIST 63 #define RIGHT_DIST_SENSOR 5 /* Turning */ #define CIRCUM_FIRST_1 78 #define CIRCUM_FIRST_2 320 #define N_VOLLEY 8 #define FIRST_TURN_SPEED 100 #define CIRCUM_CORNER_1 120 #define CIRCUM_CORNER_2 100 #define CIRCUM_CORNER_ON_LINE 33 #define CIRCUM_CORNER_1_ON_LINE 85 #define TURN_SPEED 100 #define TURN_SPEED_CORNER_2 100 #define TURN_CAREFUL_BACK_OFF 30 /* Stall detection */ #define STALL_TIMEOUT 0.5 #define STALL_BEND 20 #define STALL_TOL 10 #define STALL_BACK_OFF 40 /* Align with Wall */ #define ALIGN_SPEED 100 #define ALIGN_F_SPEED 100 #define ALIGN_DIST 200 #define ALIGN_WAIT 0.3 #define BACK_BUMP_LEFT 26 #define BACK_BUMP_RIGHT 24 /* Beacon */ #define BEACON_NORTH 12 #define BEACON_EAST 13 #define BEACON_SOUTH 14 #define BEACON_WEST 15 /* TIME OUT !!! */ #define INFINITE_TIME 60.0 #define FOL_WALL_1_TIMEOUT 20.0 #define DETECT_LINE_1_TIMEOUT 1.0 #define FOL_LINE_1_TIMEOUT 5.0 #define CORNER_2_TIMEOUT 13.0 #define FOL_WALL_2_TIMEOUT 20.0 #define FOL_WALL_TURN_TIMEOUT 5.0 #define ALIGN_TIMEOUT 2.5 #define BACK_OFF_TIMEOUT 8.0 #define TURN_TIMEOUT 8.0 /* Avoid Opponent */ #define FRONT_DIST 4 #define THING_LINE_DIST 33 #define THING_START_DIST 0 /* Global Variables */ int myColor=XXX; int colorBeneath[4]; int orientation; int bigThingLine = FALSE; /* There is a big thing on the line */ int bigThingStart = FALSE; /* There is a big thing on the start point */ int folLineDist,frontDist; int folWallDone = FALSE; /***************************************************/ void main(){ ir_transmit_off(); enable_servos(); servo(SERVO_ARM,ARM_HOLD); servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT); sleep(4.0); disable_servos(); /* Getting Oriented */ start_machine(COLOR_PORT[LIGHT_SENSOR_MID]); initialize(); orientation=detectOrientation(); /* printf("l=%d r=%d b=%d\n",colorBeneath[LIGHT_SENSOR_LEFT], colorBeneath[LIGHT_SENSOR_RIGHT], colorBeneath[LIGHT_SENSOR_BACK]); if(myColor==SAND)printf("WHITE "); else if(myColor==BLUE)printf("BLUE "); else printf("NO COLOR "); if(orientation == 0)printf("to SELF\n"); else if(orientation == 1) printf("to PLATform\n"); else if(orientation == 2) printf("to OPPonent\n"); else if(orientation == 3) printf("to WALL\n"); else printf("!!!Aiya???\n"); */ turnToOpponent(); start_process(releaseArm()); /* The First Trip */ followWall(ARM_BUMP_RIGHT,ARM_BUMP_LEFT,CCW,TURN_AWAY_DIST_1,DIST_FOL_WALL_1,FOL_WALL_SPEED_1,FRONT_WHEEL_BIAS_1,FOL_WALL_1_TIMEOUT); alignWithFrontWall(); straightFront(); backOff(60,100); turnSpecial(); /* Deliver the first ball */ followWallAggressive(ARM_BUMP_LEFT,ARM_BUMP_RIGHT,CW,TURN_AWAY_DIST_2,760,100,FRONT_WHEEL_BIAS_2,FOL_WALL_2_TIMEOUT); ao(); straightFront(); servo(SERVO_WHEEL,2025); printf("Back to corner 1\n"); backOff(500,100); /* BEGIN CHECKING OPPONENT */ if(opponentBack()){ botInBack(); }else{ /* nothing at corner 1 */ backOff(650,100); alignWithBackWall(); servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT); sleep(FRONT_WHEEL_WAIT/2.0); followWall(ARM_BUMP_LEFT,ARM_BUMP_RIGHT,CW,TURN_AWAY_DIST_2,130,100,0,FOL_WALL_2_TIMEOUT); ao(); turn(CW,CIRCUM_CORNER_1,TURN_SPEED); straightFront(); if(opponentFront()){ /* a bot on the line */ botOnLine(); }else{ /* nothing on the line */ followLine(RIGHT_DIST_SENSOR,ARM_BUMP_RIGHT,CCW,RIGHT_WALL_DIST,390,100,FOL_LINE_1_TIMEOUT); alignWithFrontWall(); if(opponentLeft()){ /* There is a big thing at start point */ botAtStart(); }else{ /* Normal case */ normalCase(); } } } finalize(); } void botInBack(){ backOff(400,100); backOff(50,-100); turnCareful(CW,100,100,ARM_BUMP_LEFT,13.0); servo(SERVO_WHEEL,2350); sleep(FRONT_WHEEL_WAIT); backOff(180,-100); turn(CCW,50,100); straightFront(); followLine(RIGHT_DIST_SENSOR,ARM_BUMP_RIGHT,CCW,RIGHT_WALL_DIST,390,100,FOL_LINE_1_TIMEOUT); alignWithFrontWall(); straightFront(); backOff(70,100); turnCareful(CCW,CIRCUM_CORNER_2,TURN_SPEED_CORNER_2,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); followWall(ARM_BUMP_RIGHT,ARM_BUMP_LEFT,CCW,TURN_AWAY_DIST_1,1500,100,FRONT_WHEEL_BIAS_1,INFINITE_TIME); } void botOnLine(){ sleep(0.2); frontDist=(analog(FRONT_DIST)+analog(FRONT_DIST)+analog(FRONT_DIST))/3; if(frontDist > 43) folLineDist=100; else if(frontDist > 35) folLineDist=200; else if(frontDist > 25) folLineDist=300; else folLineDist=350; followLine(RIGHT_DIST_SENSOR,ARM_BUMP_RIGHT,CCW,RIGHT_WALL_DIST,folLineDist,100,FOL_LINE_1_TIMEOUT); straightFront(); backOff(70,100); turnCareful(CCW,100,100,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); sleep(0.6); turnCareful(CCW,125,100,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); straightFront(); backOff(300,-100); alignWithFrontWall(); straightFront(); backOff(70,100); turnCareful(CW,115,100,ARM_BUMP_LEFT,CORNER_2_TIMEOUT); followWallAggressive(ARM_BUMP_LEFT,ARM_BUMP_RIGHT,CW,TURN_AWAY_DIST_2,1500,100,0,INFINITE_TIME); } void botAtStart(){ printf("A bot is still at the start point\n"); straightFront(); backOff(70,100); turnCareful(CCW,100,100,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); sleep(0.6); turnCareful(CCW,125,100,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); straightFront(); backOff(300,-100); alignWithFrontWall(); straightFront(); backOff(70,100); turnCareful(CW,115,100,ARM_BUMP_LEFT,CORNER_2_TIMEOUT); followWallAggressive(ARM_BUMP_LEFT,ARM_BUMP_RIGHT,CW,TURN_AWAY_DIST_2,1500,100,0,INFINITE_TIME); } void normalCase(){ printf("Nothing is on my way -> normal case\n"); straightFront(); backOff(70,100); turnCareful(CCW,CIRCUM_CORNER_2,TURN_SPEED_CORNER_2,ARM_BUMP_RIGHT,CORNER_2_TIMEOUT); followWallAggressive(ARM_BUMP_RIGHT,ARM_BUMP_LEFT,CCW,TURN_AWAY_DIST_1,1500,100,FRONT_WHEEL_BIAS_1,INFINITE_TIME); }