// 6.270 team 12 main code /////////////// //DEFINITIONS// /////////////// // Include headers from OS #include #include //port numbers #define GYRO_PORT 11 #define ENCODER_PORT 24 //servo port numbers #define SERVO_LEVER 0 #define SERVO_FENCE 2 //motor port numbers #define MOTOR_LEFT 0 #define MOTOR_RIGHT 1 #define MOTOR_GEAR 2 //servo positions #define SERVO_UP_LEVER 370 #define SERVO_UP_LEVER_QUICK 200 #define SERVO_UP_LEVER_UP 420 #define SERVO_DOWN_LEVER 90 #define SERVO_CLOSE_FENCE 200 #define SERVO_OPEN_FENCE 70 //magic numbers #define MOTOR_MAX_ROTATE_VEL 160 #define MOTOR_MIN_ROTATE_VEL 100 #define MOTOR_ROTATE_VEL 180 #define MOTOR_MAX_DRIVE_VEL 255 #define MOTOR_MIN_DRIVE_VEL 60 #define DRIVE_FACTOR_FORWARD 5 //proportional gain for driving forward #define DRIVE_FACTOR_BACKWARD 5 //proportional gain for driving backward #define DRIVE_FACTOR 5 //proportional gain for driving #define DRIVE_BOUND 20 //threshold before robotRotate is called during moveToPointXXX #define DRIVE_BOUND_TIME 3 #define DRIVE_SLOW 190 #define DRIVE_FAST 230 #define ROTATE_MARGIN 0.5 //allowed error margin for rotation #define ROTATE_MARGIN_AFTER 20 #define ROTATE_FACTOR 135 #define MOTION_DRIVE 1 #define MOTION_ROTATE 2 #define MOTION_ERROR 3 #define MOTION_STATIONARY 0 #define DIRECTION_FORWARD 1 #define DIRECTION_BACKWARD 0 #define TIME_FOR_ACCELERATION 700 #define DRIVE_TIMEOUT 4000 #define ROTATE_TIMEOUT 2500 #define ENCODER_TIMEOUT_TIME 200 #define VPS_PER_FOOT 443.4 #define DRIVE_TOLERANCE_ERROR 150 #define DRIVE_TOLERANCE_ERROR_AFTER 200 #define DRIVE_DECELERATION_DISTANCE 305 #define DRIVE_DECELERATION_FACTOR 0.97 #define VPS_RATIO (0.922480620150) #define ERROR 0 #define NO_ERROR 1 #define DRIVE_ERROR 2 #define ROTATE_ERROR 3 #define DEPOSIT_YES 1 #define DEPOSIT_NO 0 #define GEAR_TIME 1600 #define LEVER_TIME 1300 #define DEPOSIT_TIME 1250 #define STAGE_DETERMINE 0 #define STAGE_CAPTURE 1 #define STAGE_MINE 2 #define STAGE_DEPOSIT 3 #define DEPOSIT_0 1 #define DEPOSIT_3 4 #define ROUND_CLOCKWISE -1 #define ROUND_COUNTER_CLOCKWISE 1 #define ROUND_NEUTRAL 0 #define SECONDS_MINE 7 #define SECONDS_CAPTURE 5 #define SECONDS_BETWEEN_TERRITORY 3 #define SECONDS_DEPOSIT 4 ////////////////// //INITIALIZATION// ////////////////// //structure for point typedef struct point { int x; int y; float theta; } point ; //for VPS extern volatile uint8_t robot_id; //initialize variables int16_t motor_left_vel = 0, motor_right_vel = 0; int8_t motion_state; int8_t direction_state; int8_t round_direction = ROUND_NEUTRAL; int8_t skip_territory = 0; int8_t avoid_territory = 6; int8_t encoder_error = 0; uint8_t total_balls_count = 0; uint8_t capture_give_up = 0; uint8_t mine_give_up = 0; point current; point pivots[6]; point levers[6]; point gears[6]; point gears_error[6]; point levers_error[6]; point deposit_points[6]; int8_t gear_direction; int8_t deposit_places; int32_t game_start_time; ////////////////////////////////// //BASIC CALCULATIONS/ASSIGNMENTS// ////////////////////////////////// //Function to determine sign of number int sign(float num){ if (num > 0) return 1; else if (num < 0) return -1; else return 0; } //limits absolute values of velocity between MOTOR_MIN_XXX_VEL and MOTOR_MAX_XXX_VEL int16_t limitVelocity(int16_t velocity, int8_t state) { int16_t speed = fabs(velocity); if (state == MOTION_DRIVE) { if (speed <= MOTOR_MAX_DRIVE_VEL && speed >= MOTOR_MIN_DRIVE_VEL) return velocity; else if (speed > MOTOR_MAX_DRIVE_VEL) return MOTOR_MAX_DRIVE_VEL * sign(velocity); else return MOTOR_MIN_DRIVE_VEL * sign(velocity); } else if (state == MOTION_ROTATE) { if (speed <= MOTOR_MAX_ROTATE_VEL && speed >= MOTOR_MIN_ROTATE_VEL) return velocity; else if (speed > MOTOR_MAX_ROTATE_VEL) return MOTOR_MAX_ROTATE_VEL * sign(velocity); else return MOTOR_MIN_ROTATE_VEL * sign(velocity); } else return velocity; } //sets velocity of motors void setVelocity(int16_t left_velocity, int16_t right_velocity, int8_t state) { motor_left_vel = limitVelocity(left_velocity, state); motor_right_vel = limitVelocity(right_velocity, state); motor_set_vel( MOTOR_LEFT, motor_left_vel ); motor_set_vel( MOTOR_RIGHT, motor_right_vel ); } //brake robot and set motion_state to MOTION_STATIONARY void robotBrake (void) { motor_brake (MOTOR_LEFT); motor_brake (MOTOR_RIGHT); motor_left_vel = 0; motor_right_vel = 0; motion_state=MOTION_STATIONARY; } //determines delta_angle (between -180 and 180) float determineRotationAngle(float desired_angle, float act_bearing) { float angle = fmod(desired_angle, 360) - fmod(act_bearing, 360); angle = fmod(angle, 360); if (angle <= 180 && angle >= -180) return angle; else if (angle > 180) return angle - 360; else if (angle < -180) return angle + 360; else return angle; } //calculates distance between two points float distanceTo (int16_t x1, int16_t y1, int16_t x2, int16_t y2) { float distX = x2 - x1; float distY = y2 - y1; float answer = sqrt( distX * distX + distY * distY ); return answer; } ///////////////////////// //VPS/TERRITORY UPDATES// ///////////////////////// //update coordinates multiplied by VPS_RATIO to account for height of robot void update(){ copy_objects(); current.x = (float) (game.coords[0].x * VPS_RATIO); current.y = (float) (game.coords[0].y * VPS_RATIO); } //align gyro to VPS theta void align(){ copy_objects(); float init_angle = (float) game.coords[0].theta/2047.0*180.0; gyro_set_degrees(init_angle); } //determine which territory any point is uint8_t determineTerritory(point position) { float angle = (float) (atan2(position.y , position.x) / M_PI * 180.0); if(angle >= -30 && angle < 30) return 0; else if(angle >= 30 && angle < 90) return 1; else if(angle >= 90 && angle < 150) return 2; else if(angle >= 150 || angle < -150) return 3; else if(angle >= -150 && angle < -90) return 4; else if(angle >= -90 && angle < -30) return 5; else return 6; //this is an error!!! } //get the pivot point for a particular territory point getTerritoryPivotPoint(uint8_t territory) { point result; result.x = pivots[territory].x; result.y = pivots[territory].y; return result; } //////////////// //ROBOT MOTION// //////////////// //rotate left, move forward, rotate right //hopefully gets robot out of stuck position void robotWiggle(int8_t direction) { update(); avoid_territory = determineTerritory(current); if(direction == DIRECTION_FORWARD) { setVelocity(-230,-250,MOTION_DRIVE); pause(400); setVelocity(-250,-230,MOTION_DRIVE); pause(400); setVelocity(0,-250,MOTION_DRIVE); pause(400); setVelocity(-250,0,MOTION_DRIVE); pause(700); } else { setVelocity(230,250,MOTION_DRIVE); pause(400); setVelocity(250,230,MOTION_DRIVE); pause(400); setVelocity(0,250,MOTION_DRIVE); pause(400); setVelocity(250,0,MOTION_DRIVE); pause(700); } robotBrake(); update(); int8_t territory = determineTerritory(current); point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[1].y; float angle = (float) (atan2(current.y,current.x)/M_PI*180.0); pause(400); update(); align(); } //rotates robot to given direction, with facing direction in consideration int8_t robotRotate (float target_angle, int end_velocity, int8_t direction) { motion_state = MOTION_ROTATE; float desired_angle = fmod(target_angle, 360); uint32_t start_time = get_time(); float delta_angle = determineRotationAngle(desired_angle, gyro_get_degrees()); while(fabs(delta_angle) >= ROTATE_MARGIN && get_time() - start_time < ROTATE_TIMEOUT) { motor_left_vel = (float) ( (- delta_angle / ROTATE_FACTOR ) * MOTOR_ROTATE_VEL ) ; motor_right_vel = (float) ( ( delta_angle / ROTATE_FACTOR ) * MOTOR_ROTATE_VEL ) ; setVelocity(motor_left_vel, motor_right_vel, motion_state); delta_angle = determineRotationAngle(desired_angle,gyro_get_degrees()); } robotBrake(); pause(150); if (end_velocity==0) motion_state = MOTION_STATIONARY; else motion_state = MOTION_DRIVE; if (fabs(fmod(gyro_get_degrees()+360,360) - fmod((desired_angle+360),360)) > ROTATE_MARGIN_AFTER && fabs(fmod(gyro_get_degrees()+360,360) - fmod((desired_angle+360),360) + 360) > ROTATE_MARGIN_AFTER && fabs(fmod(gyro_get_degrees()+360,360) - fmod((desired_angle+360),360) - 360) > ROTATE_MARGIN_AFTER) { return ERROR; } else { return NO_ERROR; } } //circleMove and circleRotate are essentially the same as moveToPointLong and robotRotate, but with less precision //for the first ten seconds of exploration void circleRotate (float target_angle, int end_velocity, int8_t direction) { motion_state = MOTION_ROTATE; uint32_t start_time = get_time(); float desired_angle = target_angle; float delta_angle = determineRotationAngle(desired_angle, gyro_get_degrees()); while(fabs(delta_angle) >= 15 && get_time() - start_time < 2000) { motor_left_vel = (float) ( (- delta_angle / ROTATE_FACTOR ) * 250 ) ; motor_right_vel = (float) ( ( delta_angle / ROTATE_FACTOR ) * 250 ) ; setVelocity(motor_left_vel, motor_right_vel, motion_state); delta_angle = determineRotationAngle(desired_angle,gyro_get_degrees()); } if (end_velocity==0) motion_state = MOTION_STATIONARY; else motion_state = MOTION_DRIVE; } int8_t circleMove(int16_t goal_x, int16_t goal_y, int16_t velocity, int8_t direction) { motion_state = MOTION_DRIVE; update(); if(direction == DIRECTION_FORWARD) { float desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); float delta_heading = determineRotationAngle(desired_heading, gyro_get_degrees()); //first rotate then go forward if angle too large if (fabs(delta_heading) > DRIVE_BOUND) { circleRotate(desired_heading, velocity, direction); update(); desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); motion_state = MOTION_DRIVE; } uint32_t start_time = get_time(); while( distanceTo(current.x,current.y,goal_x,goal_y) > 200 && get_time() - start_time < 4000) { desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); motor_left_vel = (velocity - DRIVE_FACTOR * delta_heading); motor_right_vel = (velocity + DRIVE_FACTOR * delta_heading); setVelocity(motor_left_vel, motor_right_vel, motion_state); if( distanceTo(current.x,current.y,goal_x,goal_y) < DRIVE_DECELERATION_DISTANCE ) { velocity = velocity * DRIVE_DECELERATION_FACTOR; } update(); } } else { float desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; float delta_heading = determineRotationAngle(desired_heading, gyro_get_degrees()); //first rotate then go forward if angle too large if (fabs(delta_heading) > DRIVE_BOUND) { circleRotate(desired_heading, velocity, direction); update(); desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; motion_state = MOTION_DRIVE; } uint32_t start_time = get_time(); while( distanceTo(current.x,current.y,goal_x,goal_y) > 200 && get_time() - start_time < 4000) { desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); motor_left_vel = (-velocity - DRIVE_FACTOR * delta_heading); motor_right_vel = (-velocity + DRIVE_FACTOR * delta_heading); setVelocity(motor_left_vel, motor_right_vel, motion_state); if( distanceTo(current.x,current.y,goal_x,goal_y) < DRIVE_DECELERATION_DISTANCE ) { velocity = velocity * DRIVE_DECELERATION_FACTOR; } update(); } } motion_state = MOTION_STATIONARY; update(); if(distanceTo(current.x, current.y, goal_x, goal_y) < 300) { return NO_ERROR; } else { robotWiggle(direction); return ERROR; } } //moves to point using coordinates / VPS int8_t moveToPointLong(int16_t goal_x, int16_t goal_y, int16_t velocity, int8_t direction) { motion_state = MOTION_DRIVE; update(); //for timeout encoder_reset(ENCODER_PORT); int previous_encoder = 0; int encoder_tries = 0; int8_t track_change = 0; uint32_t track_time = 0; if(direction == DIRECTION_FORWARD) { float desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); float delta_heading = determineRotationAngle(desired_heading, gyro_get_degrees()); //first rotate then go forward if angle too large //if rotate returns error, rotates back to angle started from (assume that it is able to do it), and return error if (fabs(delta_heading) > DRIVE_BOUND) { if(robotRotate(desired_heading, velocity, direction) == ERROR) { robotWiggle(direction); return ERROR; } update(); desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); motion_state = MOTION_DRIVE; } uint32_t start_time = get_time(); while( distanceTo(current.x,current.y,goal_x,goal_y) > DRIVE_TOLERANCE_ERROR && get_time() - start_time < DRIVE_TIMEOUT) { //for timeout //if encoder doesn't change for ENCODER_TIMEOUT_TIME milliseconds, returns errors if(encoder_read(ENCODER_PORT) == previous_encoder && encoder_error == 0) { if(track_change == 1 && get_time() - track_time > ENCODER_TIMEOUT_TIME && get_time() - start_time > TIME_FOR_ACCELERATION) { point opponent;//ronald tuesday 3AM opponent.x = game.coords[1].x; //ronald tuesday 3AM opponent.y = game.coords[1].y; //ronald tuesday 3AM if (encoder_tries == 3 || distanceTo(current.x,current.y,opponent.x,opponent.y) < 300) { //ronald tuesday 3AM robotWiggle(direction); return ERROR; } else {//ronald tuesday 3AM encoder_tries++;//ronald tuesday 3AM track_change = 0;//ronald tuesday 3AM previous_encoder = encoder_read(ENCODER_PORT);//ronald tuesday 3AM } } else if(track_change == 0) { track_time = get_time(); track_change = 1; } } else track_change = 0; encoder_tries = 0;//ronald tuesday 7am previous_encoder = encoder_read(ENCODER_PORT); desired_heading = (float) (atan2(goal_y - current.y , goal_x - current.x ) / M_PI * 180.0); delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); if (fabs(delta_heading) > DRIVE_BOUND) { if(robotRotate(desired_heading, velocity, direction) == ERROR) { robotWiggle(direction); return ERROR; } delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); motion_state = MOTION_DRIVE; } //added this. if(fabs(delta_heading) < 0.5) { motor_left_vel = (velocity - 3 * delta_heading); motor_right_vel = (velocity + 3 * delta_heading); } else if(fabs(delta_heading) >= 0.5 && fabs(delta_heading) < 2) { motor_left_vel = velocity - 5 * delta_heading; motor_right_vel = velocity + 5 * delta_heading; } else if(fabs(delta_heading) >= 2 && fabs(delta_heading) <=5) { motor_left_vel = (velocity - 5.5 * delta_heading); motor_right_vel = (velocity + 5.5 * delta_heading); } else { motor_left_vel = (velocity -6 * delta_heading); motor_right_vel = (velocity + 6 * delta_heading); } setVelocity(motor_left_vel, motor_right_vel, motion_state); if( distanceTo(current.x,current.y,goal_x,goal_y) < DRIVE_DECELERATION_DISTANCE ) { velocity = velocity * DRIVE_DECELERATION_FACTOR; } update(); } } else { float desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; float delta_heading = determineRotationAngle(desired_heading, gyro_get_degrees()); //first rotate then go forward if angle too large if (fabs(delta_heading) > DRIVE_BOUND) { if(robotRotate(desired_heading, velocity, direction) == ERROR){ robotWiggle(direction); return ERROR; } update(); desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; motion_state = MOTION_DRIVE; } uint32_t start_time = get_time(); while( distanceTo(current.x,current.y,goal_x,goal_y) > DRIVE_TOLERANCE_ERROR && get_time() - start_time < DRIVE_TIMEOUT) { //for timeout //if encoder doesn't change for ENCODER_TIMEOUT_TIME milliseconds, returns errors if(encoder_read(ENCODER_PORT) == previous_encoder && encoder_error == 0) { if(track_change == 1 && get_time() - track_time > ENCODER_TIMEOUT_TIME && get_time() - start_time > TIME_FOR_ACCELERATION) { point opponent;//ronald tuesday 7am opponent.x = game.coords[1].x; //ronald tuesday 7am opponent.y = game.coords[1].y; //ronald tuesday 7am //changed from 1 try to 2 tries if (encoder_tries == 3 || distanceTo(current.x,current.y,opponent.x,opponent.y) < 300) { //ronald tuesday 7am robotWiggle(direction); return ERROR; } else {//ronald tuesday 7am encoder_tries++;//ronald tuesday 7am track_change = 0;//ronald tuesday 7am previous_encoder = (ENCODER_PORT);//ronald tuesday 7am } } else if(track_change == 0) { track_time = get_time(); track_change = 1; } } else track_change = 0; encoder_tries = 0;//ronald tuesday 7am previous_encoder = encoder_read(ENCODER_PORT); desired_heading = (float) (M_PI + (atan2(goal_y - current.y , goal_x - current.x ))) / M_PI * 180.0; delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); if (fabs(delta_heading) > DRIVE_BOUND) { if(robotRotate(desired_heading, velocity, direction) == ERROR) { robotWiggle(direction); return ERROR; } delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); motion_state = MOTION_DRIVE; } if(get_time() - start_time < 200) { motor_left_vel = -150 - DRIVE_FACTOR_FORWARD * delta_heading; motor_right_vel = -150 + DRIVE_FACTOR_FORWARD * delta_heading; } else { motor_left_vel = (-velocity - DRIVE_FACTOR_BACKWARD * delta_heading); motor_right_vel = (-velocity + DRIVE_FACTOR_BACKWARD * delta_heading); } setVelocity(motor_left_vel, motor_right_vel, motion_state); if( distanceTo(current.x,current.y,goal_x,goal_y) < DRIVE_DECELERATION_DISTANCE ) { velocity = velocity * DRIVE_DECELERATION_FACTOR; } update(); } } //returns error if ending position too far from target point if(distanceTo(current.x, current.y, goal_x, goal_y) > DRIVE_TOLERANCE_ERROR_AFTER) { robotWiggle(direction); return ERROR; } motion_state = MOTION_STATIONARY; motor_left_vel = 0; motor_right_vel = 0; setVelocity(motor_left_vel, motor_right_vel, motion_state); return NO_ERROR; } int8_t moveToPointTime(int16_t time, float desired_heading, int16_t velocity, int8_t direction) { float start_angle = gyro_get_degrees(); if (direction == DIRECTION_BACKWARD) { desired_heading = desired_heading + 180; } motion_state = MOTION_DRIVE; float delta_heading = determineRotationAngle(desired_heading, gyro_get_degrees()); if (fabs(delta_heading) > DRIVE_BOUND_TIME) { if(robotRotate(desired_heading, velocity, direction) == ERROR) { robotWiggle(direction); return ERROR; } motion_state = MOTION_DRIVE; } pause(300);//ronald tuesday 3pm uint32_t start_time = get_time(); //for timeout encoder_reset(ENCODER_PORT); int16_t previous_encoder = 0; int8_t track_change = 0; uint32_t track_time = 0; int encoder_tries = 0; while( get_time() - start_time < time ){ if(encoder_read(ENCODER_PORT) == previous_encoder && encoder_error == 0) { if(track_change == 1 && get_time() - track_time > ENCODER_TIMEOUT_TIME && get_time() - start_time > TIME_FOR_ACCELERATION) { update(); point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[1].y; if (encoder_tries == 3 || distanceTo(current.x,current.y,opponent.x,opponent.y)<300) { robotBrake(); break; } else { encoder_tries++; track_change = 0; previous_encoder = encoder_read(ENCODER_PORT); } } else if(track_change == 0) { track_time = get_time(); track_change = 1; } } else track_change = 0; encoder_tries = 0; previous_encoder = encoder_read(ENCODER_PORT); delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); if (fabs(delta_heading) > DRIVE_BOUND) { if(robotRotate(desired_heading, velocity, direction) == ERROR) { robotWiggle(direction); return ERROR; } delta_heading = determineRotationAngle(desired_heading,gyro_get_degrees()); motion_state = MOTION_DRIVE; } if(direction == DIRECTION_FORWARD) { if(fabs(delta_heading) < 0.5) { motor_left_vel = (velocity - 3 * delta_heading); motor_right_vel = (velocity + 3 * delta_heading); } else if(fabs(delta_heading) >= 0.5 && fabs(delta_heading) < 2) { motor_left_vel = velocity - 5 * delta_heading; motor_right_vel = velocity + 5 * delta_heading; } else if(fabs(delta_heading) >= 2 && fabs(delta_heading) <=5) { motor_left_vel = (velocity - 5.5 * delta_heading); motor_right_vel = (velocity + 5.5 * delta_heading); } else { motor_left_vel = (velocity -6 * delta_heading); motor_right_vel = (velocity + 6 * delta_heading); } } else { motor_left_vel = (-velocity - DRIVE_FACTOR_BACKWARD * delta_heading); motor_right_vel = (-velocity + DRIVE_FACTOR_BACKWARD * delta_heading); } setVelocity(motor_left_vel, motor_right_vel, motion_state); if((get_time() - start_time) > 0.5 * time) { velocity = 100; } } robotBrake(); return NO_ERROR; } /////////////////////////////// //HIGH LEVEL ACTION COMMANDS// ////////////////////////////// int8_t goToTerritory(uint8_t target_territory, int8_t direction) { update(); int8_t current_territory = determineTerritory(current); point current_territory_point = pivots[current_territory]; if (distanceTo(current.x, current.y, current_territory_point.x, current_territory_point.y) > DRIVE_TOLERANCE_ERROR_AFTER && skip_territory == 0) { if(moveToPointLong(current_territory_point.x, current_territory_point.y, DRIVE_FAST, direction) == ERROR) return ERROR; } point target; //changed this. still need to find somewhere else to reset round_direction to avoid confusion //two places round_direction should be set: determinecapture, and when you call deposit(territory) - neutral if(round_direction == ROUND_NEUTRAL) { if(target_territory == (current_territory + 1) % 6 || target_territory == (current_territory + 2) % 6 || target_territory == (current_territory + 3) % 6) { while(current_territory != target_territory) { target = getTerritoryPivotPoint((current_territory + 1) % 6); if (moveToPointLong(target.x,target.y,DRIVE_FAST,direction) == ERROR) return ERROR; current_territory = (current_territory + 1) % 6; } } else if(target_territory != current_territory) { while(current_territory != target_territory) { if(current_territory - 1 < 0) current_territory = current_territory + 6; target = getTerritoryPivotPoint((current_territory - 1) % 6); if (moveToPointLong(target.x,target.y,DRIVE_FAST,direction) == ERROR) return ERROR; current_territory = (current_territory - 1) % 6; } } } if(round_direction == ROUND_COUNTER_CLOCKWISE) { while(current_territory != target_territory) { target = getTerritoryPivotPoint((current_territory + 1) % 6); if (moveToPointLong(target.x,target.y,DRIVE_FAST,direction) == ERROR) { return ERROR; } current_territory = (current_territory + 1) % 6; } } else if(round_direction == ROUND_CLOCKWISE) { while(current_territory != target_territory) { target = getTerritoryPivotPoint((current_territory + 5) % 6); if (moveToPointLong(target.x,target.y,DRIVE_FAST,direction) == ERROR) { return ERROR; } current_territory = (current_territory + 5) % 6; } } return NO_ERROR; } //spin gearbox int8_t captureTerritory(uint8_t target_territory, int8_t capture_error) { //added this update(); point opponent; opponent.x = game.coords[1].x * VPS_RATIO; opponent.y = game.coords[1].y * VPS_RATIO; float opponent_angle = (float) (atan2(opponent.y - current.y, opponent.x - current.x) / M_PI * 180.0); if (fmod(opponent_angle + 360, 360) < 60 * (target_territory + 1) && fmod(opponent_angle + 360, 360) > target_territory * 60 ) { avoid_territory = target_territory; return DRIVE_ERROR; } else if(game.territories[target_territory].owner != robot_id) { pause(500); update(); align(); point pivot = getTerritoryPivotPoint(target_territory); if(distanceTo(current.x, current.y, pivot.x, pivot.y) > 300 && capture_error == 0) { if(moveToPointLong(pivot.x, pivot.y, DRIVE_FAST, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } float angle = (float) (atan2(gears[target_territory].y - current.y, gears[target_territory].x - current.x) / M_PI * 180.0 ); if(moveToPointTime(GEAR_TIME, angle, DRIVE_SLOW, DIRECTION_FORWARD) == ERROR) return DRIVE_ERROR; motor_set_vel(MOTOR_GEAR, gear_direction * 250); uint32_t start_time = get_time(); int time; if (game.territories[target_territory].owner == 0) time = 1300; //jessie, tuesday 8:30 changed from 1000 else time = 2400; //jessie tuesday 8:30 changed from 1200 while(game.territories[target_territory].owner != robot_id && get_time() - start_time < time) { copy_objects(); yield(); } motor_brake(MOTOR_GEAR); copy_objects(); if(game.territories[target_territory].owner == robot_id) { if(moveToPointLong(pivot.x, pivot.y, DRIVE_FAST, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; else return NO_ERROR; } else { if(capture_error < 1) { if (moveToPointLong(gears_error[target_territory].x, gears_error[target_territory].y, DRIVE_SLOW, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } else { if(moveToPointLong(pivot.x, pivot.y, DRIVE_FAST, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } return ERROR; } } else return NO_ERROR; } //goes to lever and pulls int8_t mineBalls(int8_t target_territory, int8_t mine_error) { update(); point opponent; opponent.x = game.coords[1].x * VPS_RATIO; opponent.y = game.coords[1].y * VPS_RATIO; int8_t opponent_territory = determineTerritory(opponent); point pivot = getTerritoryPivotPoint(target_territory); if(opponent_territory == target_territory) { avoid_territory = target_territory; return DRIVE_ERROR; } servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER); servo_set_pos(SERVO_FENCE, SERVO_CLOSE_FENCE); //added this if (game.territories[target_territory].owner == robot_id && (game.territories[target_territory].remaining > 0 || game.territories[target_territory].rate_limit < SECONDS_MINE / 2)) { robotBrake();//ronald monday 3pm pause(600);//jessie tuesday 5am point pivot = getTerritoryPivotPoint(target_territory); update(); align(); float angle = (float) (atan2(levers[target_territory].y - current.y, levers[target_territory].x - current.x) / M_PI * 180.0); if(mine_error<1){ if(moveToPointTime(LEVER_TIME, angle, DRIVE_SLOW, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } else { if(moveToPointTime(LEVER_TIME, angle, DRIVE_SLOW-50, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } copy_objects(); uint16_t previous_score = game.coords[0].score; while(game.territories[target_territory].remaining > 0 && game.territories[target_territory].owner == robot_id) { servo_set_pos(SERVO_LEVER, SERVO_DOWN_LEVER); pause(400); servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER_QUICK); pause(300); copy_objects(); if(game.coords[0].score > previous_score) { total_balls_count++; copy_objects(); previous_score = game.coords[0].score; } else { servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER_UP);//up up: ronald tuesday 1pm pause(500); //changed from 250 and from 200 and from 400 (jessie 8:30) if (game.coords[0].score > previous_score) { total_balls_count++; copy_objects(); previous_score = game.coords[0].score; } else { if(moveToPointLong(pivots[target_territory].x, pivots[target_territory].y, DRIVE_FAST, DIRECTION_FORWARD) == ERROR) return DRIVE_ERROR; return ERROR; } } } servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER); pause(400);//jessie tuesday 8:30, changed from 200 to solve timeout problem if(moveToPointLong(pivot.x, pivot.y, DRIVE_FAST, DIRECTION_FORWARD) == ERROR) return DRIVE_ERROR; else return NO_ERROR; } else return NO_ERROR; } //determine if we should deposit balls int8_t determineDepositPeriod2() { update(); int8_t territory = determineTerritory(current); //ronald tuesday 10am point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[2].y; int8_t opponent_territory = determineTerritory(opponent); //changed this if(total_balls_count >= 15 && (territory == deposit_places || territory == deposit_places+1 || territory == (deposit_places+2)%6) && opponent_territory!=territory) { return DEPOSIT_YES; } else return DEPOSIT_NO; } int8_t determineDepositPeriod3() { update(); int8_t territory = determineTerritory(current); point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[2].y; int8_t opponent_territory = determineTerritory(opponent); //changed this if(total_balls_count >= 5 && (territory == deposit_places || territory == deposit_places+1 || territory == (deposit_places+2)%6) && opponent_territory!=territory) { return DEPOSIT_YES; } else return DEPOSIT_NO; } //dump balls into center int8_t depositBalls(int8_t deposit_territory) { pause(450); update(); align(); point pivot = getTerritoryPivotPoint(deposit_territory); if(distanceTo(current.x, current.y, pivot.x, pivot.y) > DRIVE_TOLERANCE_ERROR_AFTER || distanceTo(current.x, current.y, deposit_points[deposit_territory].x, deposit_points[deposit_territory].y) < DRIVE_TOLERANCE_ERROR_AFTER) { if(goToTerritory(deposit_territory,DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR;//ronald tuesday 3pm } int8_t current_territory_deposit = determineTerritory(current); update(); point opponent; opponent.x = game.coords[1].x * VPS_RATIO; opponent.y = game.coords[1].y * VPS_RATIO; float opponent_angle = (atan2(opponent.y - current.y, opponent.x - current.x) / M_PI * 180.0); float angle = (atan2(deposit_points[deposit_territory].y-current.y,deposit_points[deposit_territory].x-current.x) / M_PI * 180.0); if (fabs(opponent_angle - angle) < 30.0 && determineTerritory(opponent) == deposit_territory) { avoid_territory = deposit_territory; return DRIVE_ERROR; } if(moveToPointTime(DEPOSIT_TIME, angle, DRIVE_SLOW, DIRECTION_FORWARD) == ERROR) { return DRIVE_ERROR; } if(distanceTo(current.x,current.y,0,0) > 700) { robotBrake(); pause(250); update(); align(); angle = (atan2(0-current.y,0-current.x) / M_PI * 180.0); if(moveToPointTime(1000, angle, DRIVE_SLOW, DIRECTION_FORWARD) == ERROR) { return DRIVE_ERROR; } } servo_set_pos(SERVO_FENCE, SERVO_OPEN_FENCE); int time = (ceil((float) (total_balls_count / 5.0)) * 600)/3; //to be determined pause(time); setVelocity(-100,-100,MOTION_DRIVE); pause(70); setVelocity(100,100,MOTION_DRIVE); pause(70); robotBrake(); pause(50); pause(time); setVelocity(-180,-180,MOTION_DRIVE); pause(50); setVelocity(180,180,MOTION_DRIVE); pause(50); robotBrake(); pause(50); pause(time); servo_set_pos(SERVO_FENCE, SERVO_CLOSE_FENCE); total_balls_count = 0; if(moveToPointLong(pivot.x, pivot.y, DRIVE_FAST, DIRECTION_BACKWARD) == ERROR) return DRIVE_ERROR; } ////////////// //STRATEGIES// ////////////// //determine the territory to be captured for Period 2 (10th - 90th second) (TO BE CHANGED) int8_t determineCaptureTerritoryPeriod2() { round_direction = ROUND_NEUTRAL; update(); point opponent_position; opponent_position.x = game.coords[1].x * VPS_RATIO; opponent_position.y = game.coords[1].y * VPS_RATIO; uint8_t opponent_territory = determineTerritory(opponent_position); uint8_t territory = determineTerritory(current); if(distanceTo(opponent_position.x, opponent_position.y, current.x, current.y) > 350) { //ronald tuesday 3AM: this whole section if (fabs(opponent_territory-territory)<2) { if(game.territories[territory].owner != robot_id && capture_give_up == 0 && avoid_territory != territory && opponent_territory != territory) { return territory; //added error } else if(game.territories[territory].remaining > 0 && mine_give_up == 0 && game.territories[territory].owner == robot_id && avoid_territory != territory && opponent_territory != territory) { return territory; } int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - territory < 3 && opponent_territory - territory > 0) || opponent_territory - territory < -3) { preference = -1; round_direction = ROUND_CLOCKWISE; } else if((opponent_territory - territory >= -3 && opponent_territory - territory < 0) || opponent_territory - territory >= 3) { preference = 1; round_direction = ROUND_COUNTER_CLOCKWISE; } round_direction = preference; if((game.territories[(territory + preference * 1 + 6) % 6].remaining > 0 || game.territories[(territory + preference * 1 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + preference * 1 + 6) % 6) && (territory + preference * 1 + 6) % 6 != opponent_territory) { return (territory + preference * 1 + 6) % 6; } else if((game.territories[(territory + preference * 2 + 6) % 6].remaining > 0 || game.territories[(territory + preference * 2 + 6) % 6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + preference * 2 + 6) % 6)) { return (territory + preference * 2 + 6) % 6; } else if(game.territories[(territory + preference * 1 + 6) % 6].owner != robot_id && avoid_territory != ((territory + preference * 1 + 6) % 6)) { return (territory + preference * 1 + 6) % 6; } else if(game.territories[(territory + preference * 2 + 6) % 6].owner != robot_id && avoid_territory != ((territory + preference * 2 + 6) % 6)) { return (territory + preference * 2 + 6) % 6; } else if(game.territories[(territory + 9) % 6].owner != robot_id && avoid_territory != ((territory + 9) % 6)) { return (territory + 9) % 6; } else if((game.territories[(territory + 9) % 6].remaining > 0 || game.territories[(territory + 9) % 6].rate_limit < 3 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + 9) % 6)) { return (territory + 9) % 6; } else if (1) { round_direction = -preference; if((game.territories[(territory - preference * 2 + 6) % 6].remaining > 0 || game.territories[(territory - preference * 2 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory - preference * 2 + 6) % 6)) { return (territory - preference * 2 + 6) % 6; } else if(game.territories[(territory - preference * 2 + 6) % 6].owner != robot_id && avoid_territory != ((territory - preference * 2 + 6) % 6)) { return (territory - preference * 2 + 6) % 6; } else if((game.territories[(territory - preference * 1 + 6) % 6].remaining > 0 || game.territories[(territory - preference * 1 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory - preference * 1 + 6) % 6) && (territory - preference * 1 + 6) % 6 != opponent_territory) { return (territory - preference * 1 + 6) % 6; } else if(game.territories[(territory - preference * 1 + 6) % 6].owner != robot_id && avoid_territory != ((territory - preference * 1 + 6) % 6)) { return (territory - preference * 1 + 6) % 6; } } else { round_direction = preference; return (territory + preference * 1 + 6) % 6; } } if(game.territories[territory].owner != robot_id && capture_give_up == 0 && avoid_territory != territory && opponent_territory != territory) { return territory; } else if(game.territories[territory].remaining > 0 && mine_give_up == 0 && game.territories[territory].owner == robot_id && avoid_territory != territory && opponent_territory != territory) { return territory; } int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - territory < 3 && opponent_territory - territory > 0) || opponent_territory - territory < -3) { preference = -1; } else if((opponent_territory - territory >= -3 && opponent_territory - territory < 0) || opponent_territory - territory >= 3) { preference = 1; } if((game.territories[(territory + preference * 1 + 6) % 6].remaining > 0 || game.territories[(territory + preference * 1 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + preference * 1 + 6) % 6) && (territory + preference * 1 + 6) % 6 != opponent_territory) { return (territory + preference * 1 + 6) % 6; } else if((game.territories[(territory - preference * 1 + 6) % 6].remaining > 0 || game.territories[(territory - preference * 1 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory - preference * 1 + 6) % 6) && (territory - preference * 1 + 6) % 6 != opponent_territory) { return (territory - preference * 1 + 6) % 6; } else if((game.territories[(territory + preference * 2 + 6) % 6].remaining > 0 || game.territories[(territory + preference * 2 + 6) % 6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + preference * 2 + 6) % 6)) { return (territory + preference * 2 + 6) % 6; } else if((game.territories[(territory - preference * 2 + 6) % 6].remaining > 0 || game.territories[(territory - preference * 2 + 6) % 6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory - preference * 2 + 6) % 6)) { return (territory - preference * 2 + 6) % 6; } else if(game.territories[(territory + preference * 1 + 6) % 6].owner != robot_id && avoid_territory != ((territory + preference * 1 + 6) % 6)) { return (territory + preference * 1 + 6) % 6; } else if(game.territories[(territory - preference * 1 + 6) % 6].owner != robot_id && avoid_territory != ((territory - preference * 1 + 6) % 6)) { return (territory - preference * 1 + 6) % 6; } else if(game.territories[(territory + preference * 2 + 6) % 6].owner != robot_id && avoid_territory != ((territory + preference * 2 + 6) % 6)) { return (territory + preference * 2 + 6) % 6; } else if(game.territories[(territory - preference * 2 + 6) % 6].owner != robot_id && avoid_territory != ((territory - preference * 2 + 6) % 6)) { return (territory - preference * 2 + 6) % 6; } else if(game.territories[(territory + 9) % 6].owner != robot_id && avoid_territory != ((territory + 9) % 6)) { return (territory + 9) % 6; } else if((game.territories[(territory + 9) % 6].remaining > 0 || game.territories[(territory + 9) % 6].rate_limit < 3 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory + 9) % 6)) { return (territory + 9) % 6; } else return (territory + preference * 1 + 6) % 6; } else { float angle = (float) (atan2(current.y, current.x) / M_PI * 180.0); float opponent_angle = (float) (atan2(current.y, current.x) / M_PI * 180.0); if(angle - opponent_angle > 0) { round_direction = ROUND_COUNTER_CLOCKWISE; if((game.territories[(territory+1)%6].remaining > 0 || game.territories[(territory+1)%6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2 ) && avoid_territory != ((territory+1)%6)) { return (territory+1)%6; } else if(game.territories[(territory+1)%6].owner != robot_id && avoid_territory != ((territory + 1) % 6)) { return (territory + 1) % 6; } else if((game.territories[(territory+2)%6].remaining > 0 || game.territories[(territory+2)%6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+2)%6)){ return (territory+2)%6; } else if(game.territories[(territory+2)%6].owner != robot_id && avoid_territory != ((territory + 2) % 6)) { return (territory + 2) % 6; } else if((game.territories[(territory+3)%6].remaining > 0 || game.territories[(territory+3)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+3)%6)) { return (territory+3)%6; } else if((game.territories[(territory+4)%6].remaining > 0 || game.territories[(territory+4)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+4)%6)) { return (territory+4)%6; } else if(game.territories[(territory+3)%6].owner != robot_id && avoid_territory != ((territory + 3) % 6)) { return (territory + 3) % 6; } else if(game.territories[(territory+4)%6].owner != robot_id && avoid_territory != ((territory + 4) % 6)) { return (territory + 4) % 6; } else if((game.territories[(territory+5)%6].remaining > 0 || game.territories[(territory+5)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+5)%6)) { return (territory+5)%6; } else if(game.territories[(territory+5)%6].owner != robot_id && avoid_territory != ((territory + 5) % 6)) { return (territory + 5) % 6; } else return (territory + 1) % 6; } else { round_direction = ROUND_CLOCKWISE; if((game.territories[(territory+5)%6].remaining > 0 || game.territories[(territory+5)%6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2 ) && avoid_territory != ((territory+5)%6)) { return (territory+5)%6; } else if((game.territories[(territory+4)%6].remaining > 0 || game.territories[(territory+4)%6].rate_limit < SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+4)%6)){ return (territory+4)%6; } else if(game.territories[(territory+5)%6].owner != robot_id && avoid_territory != ((territory + 5) % 6)) { return (territory + 5) % 6; } else if(game.territories[(territory+4)%6].owner != robot_id && avoid_territory != ((territory + 4) % 6)) { return (territory + 4) % 6; } else if((game.territories[(territory+3)%6].remaining > 0 || game.territories[(territory+3)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+3)%6)) { return (territory+3)%6; } else if(game.territories[(territory+3)%6].owner != robot_id && avoid_territory != ((territory + 3) % 6)) { return (territory + 3) % 6; } else if((game.territories[(territory+2)%6].remaining > 0 || game.territories[(territory+2)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+2)%6)) { return (territory+2)%6; } else if(game.territories[(territory+2)%6].owner != robot_id && avoid_territory != ((territory + 2) % 6)) { return (territory + 2) % 6; } else if(game.territories[(territory+1)%6].owner != robot_id && avoid_territory != ((territory + 1) % 6)) { return (territory + 1) % 6; } else if((game.territories[(territory+1)%6].remaining > 0 || game.territories[(territory+1)%6].rate_limit < 2 * SECONDS_BETWEEN_TERRITORY + SECONDS_MINE / 2) && avoid_territory != ((territory+1)%6)) { return (territory+1)%6; } else return (territory + 5) % 6; } } } int8_t determineCaptureTerritoryPeriod3() { update(); int8_t territory = determineTerritory(current); point opponent_position; opponent_position.x = game.coords[1].x * VPS_RATIO; opponent_position.y = game.coords[1].y * VPS_RATIO; uint8_t opponent_territory = determineTerritory(opponent_position); int8_t count; //potential points for each territory within time limit. int16_t potential_points[6]; int balls_current = total_balls_count; for(count = 0; count < 6; count++) { potential_points[count] = 0; int8_t distance_away = abs(count - territory); uint16_t time_taken = distance_away * SECONDS_BETWEEN_TERRITORY; //minus 5 seconds to account for possibility of crashes... uint16_t time_left = 120 - (get_time() - game_start_time) / 1000 - 5; // //capture if(game.territories[count].owner != robot_id) { potential_points[count] = potential_points[count] + 100; time_taken = time_taken + SECONDS_CAPTURE; } //mine int8_t balls_number; if(time_taken + SECONDS_MINE / 2 < time_left && game.territories[count].remaining > 0) { balls_number = balls_current + game.territories[count].remaining; potential_points[count] = potential_points[count] + game.territories[count].remaining * 40; time_taken = time_taken + SECONDS_MINE; } else if(time_taken + SECONDS_MINE / 2 < time_left && game.territories[count].rate_limit < SECONDS_BETWEEN_TERRITORY * distance_away + SECONDS_CAPTURE) { balls_number = balls_current + 5; potential_points[count] = potential_points[count] + 200; time_taken = time_taken + SECONDS_MINE; } //deposit if(time_taken + SECONDS_DEPOSIT < time_left && (count == deposit_places || count == deposit_places + 1 || count == (deposit_places + 2) % 6) && opponent_territory != count) { potential_points[count] = potential_points[count] + balls_number * 40; } if (count == opponent_territory && fabs((territory - opponent_territory + 6) % 6) <= 1) potential_points[count] = 0; } int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - territory < 3 && opponent_territory - territory > 0) || opponent_territory - territory < -3) { preference = -1; } else if((opponent_territory - territory >= -3 && opponent_territory - territory < 0) || opponent_territory - territory >= 3) { preference = 1; } int16_t max_points = -1; int8_t go_territory = 6; if((capture_give_up == 0 && avoid_territory != territory) || (mine_give_up == 0 && avoid_territory != territory && game.territories[territory].owner == robot_id)) { max_points = potential_points[territory]; go_territory = territory; round_direction = ROUND_NEUTRAL; } for(count = 1; count <= 3; count++) { int8_t temp_territory = ((territory + preference * count) + 6) % 6; if(potential_points[temp_territory] > max_points && temp_territory != avoid_territory) { max_points = potential_points[temp_territory]; go_territory = temp_territory; round_direction = preference; } temp_territory = ((territory - preference * count) + 6) % 6; if(potential_points[temp_territory] > max_points && temp_territory != avoid_territory) { max_points = potential_points[temp_territory]; go_territory = temp_territory; round_direction = preference; } } if(go_territory == 6 || go_territory == territory) { return (territory + 1 ) % 6; //changed this } return go_territory; } void goCircle() { update(); align(); int8_t start = determineTerritory(current); int count; for(count = 1; count<=5; count++) { if(get_time() - game_start_time < 10000) { if(count <= 4) { if(circleMove(pivots[(start+count)%6].x, pivots[(start+count)%6].y, 248, DIRECTION_BACKWARD) == ERROR) { robotWiggle(DIRECTION_FORWARD); //added this 1/30 break; } } else if (count == 5) { //ronald tuesday 6am if(moveToPointLong(pivots[(start + 5) % 6].x,pivots[(start + 5) % 6].y,DRIVE_FAST,DIRECTION_BACKWARD) == ERROR) { robotWiggle(DIRECTION_FORWARD); break; } } } else break; } robotBrake(); } //10th-90th second, capture, mine, deposit void goPeriod2() { uint8_t territory = 0; //current/target territory uint8_t stage_count = 0; //tracks stage of logic int16_t ultimate_count = 0; //worst-case scenario point previous; update(); int8_t current_territory = determineTerritory(current); previous.x = current.x; previous.y = current.y; //changed to 120000 for testing purposes while(get_time() - game_start_time < 90000) { //encoder error if((get_time() - game_start_time > 30000 && game.coords[0].score < 350) || (get_time() - game_start_time > 60000 && game.coords[0].score < 500)) encoder_error = 1; ultimate_count++; if (ultimate_count == 3) { update(); if (distanceTo (current.x, current.y, previous.x, previous.y) < 200) { setVelocity(250,250,MOTION_DRIVE); motor_set_vel(MOTOR_GEAR,250); setVelocity(250,-250,MOTION_DRIVE); pause(1000); setVelocity(-250,-250,MOTION_DRIVE); pause(1000); setVelocity(-250,250,MOTION_DRIVE); pause(1000); setVelocity(250,250,MOTION_DRIVE); pause(1000); motor_brake(MOTOR_GEAR); robotBrake(); } previous.x = current.x; previous.y = current.y; } if (ultimate_count == 6) { update(); if (distanceTo (current.x, current.y, previous.x, previous.y) < 200) { setVelocity(-250,-250,MOTION_DRIVE); motor_set_vel(MOTOR_GEAR,-250); setVelocity(-250,250,MOTION_DRIVE); pause(1000); setVelocity(250,250,MOTION_DRIVE); pause(1000); setVelocity(250,-250,MOTION_DRIVE); pause(1000); setVelocity(-250,-250,MOTION_DRIVE); pause(1000); motor_brake(MOTOR_GEAR); robotBrake(); pause(300); update(); align(); } ultimate_count = 0; previous.x = current.x; previous.y = current.y; } update(); //if balls more than 20, first deposit //need to change this if(total_balls_count >= 13) { //ronald tuesday 8am current_territory = determineTerritory(current); point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[1].y; int8_t opponent_territory = determineTerritory(opponent); int8_t go_territory = 6; int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - territory < 3 && opponent_territory - territory > 0) || opponent_territory - territory < -3) { preference = -1; round_direction = ROUND_CLOCKWISE; } else if((opponent_territory - territory >= -3 && opponent_territory - territory < 0) || opponent_territory - territory >= 3) { preference = 1; round_direction = ROUND_COUNTER_CLOCKWISE; } if((current_territory == deposit_places || current_territory == deposit_places+1 || current_territory == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = current_territory; } else if((((current_territory+preference*1+6)%6) == deposit_places || ((current_territory+preference*1+6)%6) == deposit_places+1 || ((current_territory+preference*1+6)%6) == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = ((current_territory+preference*1+6)%6); } else if((((current_territory+preference*2+6)%6) == deposit_places || ((current_territory+preference*2+6)%6) == deposit_places+1 || ((current_territory+preference*2+6)%6) == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = ((current_territory+preference*2+6)%6); } else if((((current_territory+3*preference+6)%6) == deposit_places || ((current_territory+3*preference+6)%6) == deposit_places+1 || ((current_territory+3*preference+6)%6) == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = ((current_territory+3*preference+6)%6); } depositBalls(go_territory); } //determine territory to go to //haven't implemented timeout at this level yet if(stage_count == STAGE_DETERMINE) { territory = determineCaptureTerritoryPeriod2(); if (goToTerritory(territory, DIRECTION_BACKWARD) == ERROR) { skip_territory = 1; round_direction = ROUND_NEUTRAL; // } else { stage_count = STAGE_CAPTURE; skip_territory = 0; avoid_territory = 6; round_direction = ROUND_NEUTRAL;// ultimate_count = 0;// } } //captures territory, gives up after three unsuccessful tries - will not mine if failed, but will check deposit else if(stage_count == STAGE_CAPTURE) { //printf("af\n"); int error_capture = 0; while(error_capture < 2) { int8_t capture = captureTerritory(territory, error_capture); if(capture== ERROR) { error_capture = error_capture + 1; ultimate_count = 0; pause(200); } else if(capture == DRIVE_ERROR) { stage_count = STAGE_DETERMINE; skip_territory = 1; break; } else { capture_give_up = 0; //added this stage_count = STAGE_MINE; avoid_territory = 6; ultimate_count = 0; break; } } if(error_capture >= 2) { stage_count = STAGE_DEPOSIT; capture_give_up = 1; //added this avoid_territory = 6; } } //mine territory balls, also gives up after three unsuccessful tries else if(stage_count == STAGE_MINE) { int error_mine = 0; while(error_mine < 2) { int8_t mine = mineBalls(territory, error_mine); if(mine == ERROR) { error_mine = error_mine + 1; ultimate_count = 0; pause(200); } else if(mine == DRIVE_ERROR) { stage_count = STAGE_DETERMINE; skip_territory = 1; break; } else { mine_give_up = 0; stage_count = STAGE_DEPOSIT; avoid_territory = 6; ultimate_count = 0; break; } } if(error_mine >= 2) { mine_give_up = 1; stage_count = STAGE_DEPOSIT; avoid_territory = 6; } } else if(stage_count == STAGE_DEPOSIT) { if(determineDepositPeriod2() == DEPOSIT_YES) { if(depositBalls(territory) == DRIVE_ERROR) { skip_territory = 1; } else ultimate_count = 0; } stage_count = STAGE_DETERMINE; avoid_territory = 6; } } } void goPeriod3() { update(); int8_t current_territory = determineTerritory(current); point pivot = getTerritoryPivotPoint(current_territory); if (distanceTo(current.x,current.y,pivot.x,pivot.y) > 350) goToTerritory(current_territory,DIRECTION_BACKWARD); //if balls more than 10, first deposit //need to change this if(total_balls_count >= 10) { int8_t go_territory = 6; point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[1].y; int8_t opponent_territory = determineTerritory(opponent); int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - current_territory < 3 && opponent_territory - current_territory > 0) || opponent_territory - current_territory < -3) { preference = -1; round_direction = ROUND_CLOCKWISE; } else if((opponent_territory - current_territory >= -3 && opponent_territory - current_territory < 0) || opponent_territory - current_territory >= 3) { preference = 1; round_direction = ROUND_COUNTER_CLOCKWISE; } if((current_territory == deposit_places || current_territory == deposit_places+1 || current_territory == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = current_territory; } else if((((current_territory+1*preference+6)%6) == deposit_places || ((current_territory+1*preference+6)%6) == deposit_places+1 || ((current_territory+1*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+1*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+1*preference+6)%6); } else if((((current_territory+5*preference+6)%6) == deposit_places || ((current_territory+5*preference+6)%6) == deposit_places+1 || ((current_territory+5*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+5*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+5*preference+6)%6); } else if((((current_territory+2*preference+6)%6) == deposit_places || ((current_territory+2*preference+6)%6) == deposit_places+1 || ((current_territory+2*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+2*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+2*preference+6)%6); } else if((((current_territory+4*preference+6)%6) == deposit_places || ((current_territory+4*preference+6)%6) == deposit_places+1 || ((current_territory+4*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+4*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+4*preference+6)%6); } else if((((current_territory+3*preference+6)%6) == deposit_places || ((current_territory+3*preference+6)%6) == deposit_places+1 || ((current_territory+3*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+3*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+3*preference+6)%6); } depositBalls(go_territory); } uint8_t territory = 0; //current/target territory uint8_t stage_count = 0; //tracks stage of logic while(get_time() - game_start_time < 120000) { update(); //if balls more than 10, first deposit //need to change this if(total_balls_count >= 5) { int8_t current_territory = determineTerritory(current); int8_t go_territory = 6; point opponent; opponent.x = game.coords[1].x; opponent.y = game.coords[1].y; int8_t opponent_territory = determineTerritory(opponent); round_direction = ROUND_NEUTRAL; int8_t preference = 1; //initialize to 1 instead of 0 to be safe if((opponent_territory - territory < 3 && opponent_territory - territory > 0) || opponent_territory - territory < -3) { preference = -1; } else if((opponent_territory - territory >= -3 && opponent_territory - territory < 0) || opponent_territory - territory >= 3) { preference = 1; } if((current_territory == deposit_places || current_territory == deposit_places+1 || current_territory == (deposit_places+2)%6) && current_territory != opponent_territory) { go_territory = current_territory; } else if((((current_territory+1*preference+6)%6) == deposit_places || ((current_territory+1*preference+6)%6) == deposit_places+1 || ((current_territory+1*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+1*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+1*preference+6)%6); } else if((((current_territory+5*preference+6)%6) == deposit_places || ((current_territory+5*preference+6)%6) == deposit_places+1 || ((current_territory+5*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+5*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+5*preference+6)%6); } else if((((current_territory+2*preference+6)%6) == deposit_places || ((current_territory+2*preference+6)%6) == deposit_places+1 || ((current_territory+2*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+2*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+2*preference+6)%6); } else if((((current_territory+4*preference+6)%6) == deposit_places || ((current_territory+4*preference+6)%6) == deposit_places+1 || ((current_territory+4*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+4*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+4*preference+6)%6); } else if((((current_territory+3*preference+6)%6) == deposit_places || ((current_territory+3*preference+6)%6) == deposit_places+1 || ((current_territory+3*preference+6)%6) == (deposit_places+2)%6) && ((current_territory+3*preference+6)%6) != opponent_territory) { go_territory = ((current_territory+3*preference+6)%6); } depositBalls(go_territory); } //determine territory to go to //haven't implemented timeout at this level yet if(stage_count == STAGE_DETERMINE) { territory = determineCaptureTerritoryPeriod3(); if (goToTerritory(territory, DIRECTION_BACKWARD) == ERROR) { skip_territory = 1; round_direction = ROUND_NEUTRAL; // } else { stage_count = STAGE_CAPTURE; skip_territory = 0; avoid_territory = 6; round_direction = ROUND_NEUTRAL; // } } //captures territory, gives up after three unsuccessful tries - will not mine if failed, but will check deposit else if(stage_count == STAGE_CAPTURE) { int error_capture = 0; while(error_capture < 3) { int8_t capture = captureTerritory(territory, error_capture); if(capture/*captureTerritory(territory)*/ == ERROR) { error_capture = error_capture + 1; pause(200); } else if(capture == DRIVE_ERROR) { stage_count = STAGE_DETERMINE; skip_territory = 1; break; } else { capture_give_up = 0; //added this stage_count = STAGE_MINE; avoid_territory = 6; break; } } if(error_capture >= 3) { stage_count = STAGE_DEPOSIT; capture_give_up = 1; //added this avoid_territory = 6; } } //mine territory balls, also gives up after three unsuccessful tries else if(stage_count == STAGE_MINE) { int error_mine = 0; while(error_mine < 3) { int8_t mine = mineBalls(territory, error_mine);//ronald tuesday 1pm if(mine/*mineBalls(territory)*/ == ERROR) { error_mine = error_mine + 1; pause(200); } else if(mine == DRIVE_ERROR) { stage_count = STAGE_DETERMINE; skip_territory = 1; break; } else { mine_give_up = 0; stage_count = STAGE_DEPOSIT; avoid_territory = 6; break; } } if(error_mine == 3) { mine_give_up = 1; stage_count = STAGE_DEPOSIT; avoid_territory = 6; } } else if(stage_count == STAGE_DEPOSIT) { if(determineDepositPeriod3() == DEPOSIT_YES) { if(depositBalls(territory) == DRIVE_ERROR) { skip_territory = 1; } } stage_count = STAGE_DETERMINE; avoid_territory = 6; } } } /////////////////////////////////// //ULTIMATE LEVEL OF ABSTRACTION//// /////////////////////////////////// void winGame() { goCircle(); goPeriod2(); goPeriod3(); robotBrake(); } //////////////////////////////////// // Entry point to contestant code.// //////////////////////////////////// int umain (void) { ////////////////////////////////////// //INITIALIZATION, DO NOT COMMENT OUT// ////////////////////////////////////// game_start_time = get_time(); update(); align(); //initiate start territory int8_t start_territory = determineTerritory(current); if(start_territory <= 4 && start_territory >= 2) { gear_direction = -1; deposit_places = DEPOSIT_3; } else { gear_direction = 1; deposit_places = DEPOSIT_0; } ///////////////////////// //END OF INITIALIZATION// ///////////////////////// winGame(); return 0; } // usetup is called during the calibration period. void usetup (void) { printf("\nPlace robot. Press go to calibrate."); go_click(); int test = frob_read_range(0,10); if (test == 10) { //test functions pause(100); servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER); pause(300); servo_set_pos(SERVO_LEVER, SERVO_DOWN_LEVER); pause(300); servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER_QUICK); pause(300); servo_set_pos(SERVO_LEVER, SERVO_UP_LEVER_UP); pause(300); servo_set_pos(SERVO_FENCE, SERVO_CLOSE_FENCE); pause(300); servo_set_pos(SERVO_FENCE, SERVO_OPEN_FENCE); pause(300); servo_set_pos(SERVO_FENCE, SERVO_CLOSE_FENCE); pause(300); motor_set_vel(MOTOR_GEAR, 100); pause(300); motor_brake(MOTOR_GEAR); motor_set_vel(MOTOR_LEFT, 250); motor_set_vel(MOTOR_RIGHT, -250); pause(50); motor_set_vel(MOTOR_LEFT,-250); motor_set_vel(MOTOR_RIGHT,250); pause(50); motor_brake(MOTOR_LEFT); motor_brake(MOTOR_RIGHT); go_click(); pause(1000); gyro_init(GYRO_PORT, 1435105 , 2000L); encoder_reset(ENCODER_PORT); while(1) { if(gyro_get_degrees() > 60 && gyro_get_degrees() < 120) { motor_set_vel(MOTOR_GEAR, 100); pause(200); motor_brake(MOTOR_GEAR); pause(200); } } } else { encoder_reset(ENCODER_PORT); //calibrate gyro pause(1000); gyro_init(GYRO_PORT, 1435105 , 2000L); //update and align robot_id = 12; servo_set_pos(SERVO_LEVER,SERVO_UP_LEVER); servo_set_pos(SERVO_FENCE,SERVO_CLOSE_FENCE); //initiate coords //pivot point coordinates pivots[0].x = 1043; pivots[1].x = 533; pivots[2].x = -497; pivots[3].x = -1026; pivots[4].x = -540; pivots[5].x = 499; pivots[0].y = -30; pivots[1].y = 863; pivots[2].y = 885; pivots[3].y = 6; pivots[4].y = -909; pivots[5].y = -918; //LEVER COODRINATES levers[0].x = 1791; levers[1].x = 1045; levers[2].x = -497; levers[3].x = -1551; levers[4].x = -1041; levers[5].x = 499; levers[0].y = -443; levers[1].y = 1159; levers[2].y = 1470; levers[3].y = 292; levers[4].y = -1139; levers[5].y = -1521; gears[0].x = 1495; gears[1].x = 533; gears[2].x = -946; gears[3].x = -1487; gears[4].x = -540; gears[5].x = 963; gears[0].y = 231; gears[1].y = 1396; gears[2].y = 1144; gears[3].y = -244; gears[4].y = -1435; gears[5].y = -1186; gears_error[0].x = 1279; gears_error[1].x = 533; gears_error[2].x = -729; gears_error[3].x = -1271; gears_error[4].x = -540; gears_error[5].x = 747; gears_error[0].y = 106; gears_error[1].y = 1146; gears_error[2].y = 1019; gears_error[3].y = -119; gears_error[4].y = -1185; gears_error[5].y = -1061; levers_error[0].x = 1531; levers_error[1].x = 785; levers_error[2].x = -497; levers_error[3].x = -1291; levers_error[4].x = -781; levers_error[5].x = 499; levers_error[0].y = -293; levers_error[1].y = 1009; levers_error[2].y = 1170; levers_error[3].y = 142; levers_error[4].y = -989; levers_error[5].y = -1221; deposit_points[0].x = 669; deposit_points[1].x = 371; deposit_points[2].x = -297; deposit_points[3].x = -650; deposit_points[4].x = -364; deposit_points[5].x = 333; deposit_points[0].y = -31; deposit_points[1].y = 537; deposit_points[2].y = 570; deposit_points[3].y = 27; deposit_points[4].y = -555; deposit_points[5].y = -585; } }