/***********************************************************************/ /** @name drive.c Contains functions to init and release motors, control overall robot movement and set and read robot position. @author Birgit Graf, UWA, 1998 @author Andrew Berry, UWA, 1999 */ /***********************************************************************/ /*@{*/ #include "global.h" #include "general.h" #include "image.h" #include "servos.h" #include "sensors.h" /** values for RoBIOS PI controller (used in vw interface) */ #define v_lin 7.0 #define t_lin 0.3 #define v_rot 10.0 #define t_rot 0.1 /** accuracy parameter for robot movement (in m), angle < epsilon -> don't move */ #define epsilon 0.01 #define epsilon2 0.1 #define epsilonphi PI/5 /** flag to indicate location of ball (1 -> turn left, -1 -> turn right) */ int direction = 1; /** radius of circle for goalie (> GOALWIDTH/2) */ #define radius 0.55 /** distance goal middle to circle middle */ #define goaldist 0.5 /** goalie: maximum angle to ball where robot is still allowed to drive ( atan (goaldist / GOALWIDTH/2 */ #define anglemax 1.12 /* distance beneath/behind ball where robot should drive */ #define arounddist 0.1 int patrol_leg = 1; /* which part of the patrol circuit it has reached */ /***********************************************************************/ /** Check Goal waits for the image process to check for the goal Called by Check_goal_position() @author Andrew Berry, UWA, 1999 */ /**********************************************************************/ void check_goal() { look_for_goal_flag = TRUE; found_goal_flag = 0; while (found_goal_flag == 0) { OSReschedule(); LCDSetPos(0,0); LCDClear(); /*wait for image processing */ } } /***********************************************************************/ /** Check Goal Position checks to see if the goal is where it should be Called by PPdrive_field() @see PPdrive_field() @author Andrew Berry, UWA, 1999 */ /**********************************************************************/ int Check_goal_position() { PositionType pos; /* rotate to 0 degrees */ VWGetPosition(vw,&pos); VWDriveTurn(vw,-pos.phi,ROT_SPEED); while(!VWDriveDone(vw)) OSReschedule(); /*look to see if the goal is where it should be*/ check_goal(); if (found_goal_flag == 2) { /* it there*/ return TRUE; } else { /* it is lost*/ return FALSE; } } /***********************************************************************/ /** Reset position resets the angle based on the target goal. Called when the goal isn't where it is suppost to be . Called by PPdrive_field() @author Andrew Berry, UWA, 1999 */ /**********************************************************************/ int reset_angle() { PositionType pos; float rot_count=0.0; LCDClear(); /* turn pi/8 radians and look for goal. Repeat until it is found of you've turned 360 degrees. */ while ( (found_goal_flag != 2) && (rot_count < 2*PI)) { LCDSetPos(1,10); LCDPrintf("g srch\n"); found_goal_flag = 0; VWDriveTurn(vw,PI2/4,ROT_SPEED); move_camera(UP); check_goal(); while (!VWDriveDone(vw)) { LCDClear(); OSReschedule(); } rot_count += PI2/4; } if (rot_count != 2*PI) { VWDriveTurn(vw,-PI2/8,ROT_SPEED); while (!VWDriveDone(vw)) { /*do nothing */ } VWGetPosition(vw,&pos); VWSetPosition(vw,pos.x,pos.y,0.0); LCDClear(); return 1; } else { LCDClear(); return 0; } } /***********************************************************************/ /** Init VW interface. Called by main(). @see main() */ /***********************************************************************/ int InitDrive() { if (!(vw = VWInit(VW_DRIVE, 1))) error("VWInit!\n"); if (VWStartControl(vw, v_lin, t_lin , v_rot, t_rot) == -1) error("VWStart!\n"); switch(player_pos) { case 1: /* set default position */ VWSetPosition(vw, 0.1, 0.0, -PI/2); /* goalie */ break; case 2: VWSetPosition(vw, 0.225, 0.5, 0.0); /* lb */ break; case 3: VWSetPosition(vw, 0.225, -0.5, 0.0); /* rb */ break; case 4: VWSetPosition(vw, LENGTH-0.225, 0.5, -PI); /* lf */ break; case 5: VWSetPosition(vw, LENGTH-0.225, -0.5, PI); /* rf */ break; default: break; } return 0; } /***********************************************************************/ /** Change parameters. Changes thresholds which are used to select ball/goal coloured pixels from others and size of ball/goal. Called by main(). @see main() */ /***********************************************************************/ void set_drv_parameters() { int ind = 0; int end_proc = FALSE; LCDClear(); LCDMenu("CHG", "NXT", " ", "END"); while (!end_proc) { LCDSetPos(0, 0); LCDPrintf("Driving param.\n"); LCDSetPos(2, 0); LCDPrintf("Lin speed\n"); LCDSetPos(3, 0); LCDPrintf("Rot speed\n"); if (!I_am_goalie()) { LCDSetPos(4, 0); LCDPrintf("Player pos.\n"); } LCDSetChar(2 + ind, 15, '*'); switch (KEYGet()) { case KEY1: switch(ind) { case 0: LIN_SPEED = set_fparam("Lin speed", 0.0, LIN_SPEED, 2.0, 0.05); break; case 1: ROT_SPEED = set_fparam("Rot. speed", 0.0, ROT_SPEED, 2.0, 0.05); break; case 2: player_pos = set_iparam("Player pos.\n(2=lb, 3=rb,\n4=lf, 5=rf)", 2, player_pos, 5, 1); break; default: break; } LCDMenu("CHG", "NXT", " ", "END"); break; case KEY2: LCDSetChar(2 + ind, 15, ' '); ind ++; if (I_am_goalie()) if (ind > 1) ind = 0; if (ind > 2) ind = 0; break; case KEY4: LCDClear(); end_proc = TRUE; break; default: break; } } } /***********************************************************************/ /** Print robot position. Print x- and y-coordiantes in cm and orientation in degrees on LCD. Called by set_coordinates(). @see set_coordinates() */ /***********************************************************************/ void print_pos() { int boc[3]; PositionType pos; VWGetPosition(vw,&pos); boc[0] = (int)(100.0 * pos.x); boc[1] = (int)(100.0 * pos.y); boc[2] = (int)(180.0 * pos.phi / PI); LCDSetPos(1, 0); LCDPrintf("Robot Coord.:\n"); LCDSetPos(2, 0); LCDPrintf("x : % 4d cm\n", boc[0]); LCDSetPos(3, 0); LCDPrintf("y : % 4d cm \n", boc[1]); LCDSetPos(4, 0); LCDPrintf("phi: % 4d deg\n", boc[2]); } /***********************************************************************/ /** Set robot coordinates. Set robot x and y coordinaes. (0,0) = middle of own goal, looking towards opponents goal, y goes positive to left, x positive forwards, angle is positive to right, negative to left. Called by main(). @see main() */ /***********************************************************************/ void set_coordinates() { PositionType pos; int end_proc = FALSE, end_proc2 = FALSE; LCDClear(); if (I_am_goalie()) LCDMenu("GOAL", " ", "INFO", "END"); else LCDMenu("LBC", "RBC", "...", "END"); while (!end_proc) { VWGetPosition(vw, &pos); print_pos(); switch (KEYRead()) { case KEY1: if (I_am_goalie()) VWSetPosition(vw, 0.1, 0.0, -PI/2); else VWSetPosition(vw, 0.225, 0.5, 0.0); break; case KEY2: if (!I_am_goalie()) VWSetPosition(vw, 0.225, -0.5, 0.0); break; case KEY3: if (I_am_goalie()) { LCDClear(); LCDMenu(" "," "," ","OK"); LCDPutString("Put robot 10cm in front of middle of our "); LCDPutString("goal (GOAL) "); KEYWait(KEY4); LCDClear(); LCDMenu("GOAL", " ", "INFO", "END"); } else { LCDClear(); LCDMenu("LFC", "RFC", "MID", "END"); while (!end_proc2) { VWGetPosition(vw, &pos); print_pos(); switch (KEYRead()) { case KEY1: VWSetPosition(vw, LENGTH-0.225, 0.5, -PI); break; case KEY2: VWSetPosition(vw, LENGTH-0.225, -0.5, PI); break; case KEY3: VWSetPosition(vw, LENGTH/2, 0.0, 0.0); break; case KEY4: LCDClear(); end_proc2 = TRUE; break; default: break; } } } case KEY4: LCDClear(); end_proc = TRUE; break; default: break; } } } /***********************************************************************/ /** Avoid obstacles. Function makes robot drive back in case there is an obstacle in front of it (wheels stalled or front-PSD-distance below obstacle_thresh). Also resets the x or y co-ord when the obstacle is a wall. Called by PPdrive_field(). @see PPdrive_field() @author Andrew Berry, UWA, 1999 */ /***********************************************************************/ void avoid_obstacle() { PositionType pos, newpos; float delta_x,delta_y; float wall_dist; VWGetPosition(vw, &pos); /* ----- drive away from obstacle ----- */ if (got_ball_flag) { direction = fsign(pos.y); /* got ball -> turn to kick it towards opponent's goal */ VWDriveTurn(vw, (float) -direction * PI/4, 5.0 * ROT_SPEED); while(!VWDriveDone(vw) && !(KEYRead() == KEY3)){} } check_wall_flag = TRUE; is_wall_flag = FALSE; OSReschedule(); /* switch threads */ wall_dist = GetPSDValue(); /* if it is a wall, reset position of robot */ if (is_wall_flag) { if ( ( pos.phi > PI/4) && (pos.phi < 3*PI/4)) /* i.e. facing a left side wall */ { LCDSetPos(0,0); LCDPrintf("left wall\n"); OSWait(50); newpos.y = WIDTH2 - wall_dist; newpos.x = pos.x; newpos.phi = pos.phi; } else if ( ( pos.phi < -PI/4) && (pos.phi >- 3*PI/4) ) /* i.e. facing a right side wall */ { LCDSetPos(0,0); LCDPrintf("rght wall\n"); OSWait(50); newpos.y = -WIDTH2 + wall_dist; newpos.x = pos.x; newpos.phi = pos.phi; } else if ( ( (pos.phi <= PI/4) && (pos.phi >=0)) ||( (pos.phi<=0) && (pos.phi >= PI/4) ) ) /* i.e. facing target goal */ { LCDSetPos(0,0); LCDPrintf("my goal\n"); OSWait(50); newpos.x = -LENGTH + wall_dist; newpos.y = pos.y; newpos.phi = pos.phi; } else /* facing defending goal */ { LCDSetPos(0,0); LCDPrintf("ther goal\n"); OSWait(50); newpos.x = LENGTH - wall_dist; newpos.y = pos.y; newpos.phi = pos.phi; } /* reset original home position */ VWSetPosition(vw,newpos.x,newpos.y,newpos.phi); delta_x = pos.x - newpos.x; delta_y = pos.y - newpos.y; reset_pos.x -=delta_x; reset_pos.y -=delta_y; is_wall_flag = FALSE; found_wall_flag = TRUE; AUBeep(); AUBeep(); } /* drive back from obstacle */ LCDSetPos(0,0); LCDPrintf("back off\n"); VWDriveStraight(vw, -0.1, LIN_SPEED); while(!VWDriveDone(vw)) { OSReschedule(); if (see_ball_flag == TRUE) { VWSetSpeed(vw,0,0); } } LCDSetPos(0,0); LCDPrintf("turn \n"); if((rand()%2)==0) VWDriveTurn(vw,1.57,0.6); else VWDriveTurn(vw,-1.57,0.6); while(!VWDriveDone(vw) && !(KEYRead() == KEY3) && !VWStalled(vw)) { OSReschedule(); if (see_ball_flag == TRUE) VWSetSpeed(vw,0,0); } VWDriveStraight(vw, 0.1, LIN_SPEED); while(!VWDriveDone(vw) && !(KEYRead() == KEY3) && !VWStalled(vw)) { OSReschedule(); if (see_ball_flag == TRUE) { VWSetSpeed(vw,0,0); } } move_camera(UP); obstacle_flag = FALSE; got_ball_flag = FALSE; see_ball_flag = FALSE; /* lost ball while driving back -> search again */ OSReschedule(); } /***********************************************************************/ /** Search goal and drive to it. If ball is caught in front of robot, drive curve with fixed angle (don't want to loose ball) until facing goal, then kick it there. Kick ball as well as soon as goal is seen. Called by PPdrive_field(). @see PPdrive_field() */ /***********************************************************************/ void drive_to_goal() { PositionType pos; int x_middle = 0, y_middle = 0, goal_size = 0; /* parameters of goal */ /* angle between robot orientation and goal middle */ float angle; if (use_motors) VWDriveStraight(vw, 0.2, LIN_SPEED / 4.0); /* look for goal */ VWGetPosition(vw, &pos); angle = DiscAtan((int)(1000 * pos.y), (int)(1000.0 * (LENGTH - pos.x))) + pos.phi; if ( ((fabs(angle) < PI2) && (pos.x < WIDTH2)) || /* facing opponent's goal */ ((fabs(angle) < PI / 4) && (pos.x > WIDTH2)) ) { if (use_motors) VWDriveStraight(vw, 0.2, LIN_SPEED); kick_ball(); got_ball_flag = FALSE; see_ball_flag = FALSE; } else /* not facing opponent's goal */ { move_camera(UP); if (find_goal(&x_middle, &y_middle, &goal_size)) /* seeing opponent's goal */ { LCDSetPos(1, 10); LCDPrintf("GOAL!\n"); LCDSetPos(2, 10); LCDPrintf(" \n"); LCDSetPos(3, 10); LCDPrintf(" \n"); kick_ball(); got_ball_flag = FALSE; see_ball_flag = FALSE; } else if (use_motors) VWSetSpeed(vw, LIN_SPEED / 4.0, -fsign(angle) * LIN_SPEED); move_camera(DOWN); /* make sure ball is still there */ } } /***********************************************************************/ /** Get angle. Get angle between robot orientation phi and line to point (diffx, diffy). Called by start_curve_to_ball(), drive_home() and PPdrive_field(). @see start_curve_to_ball() @see drive_home() @see PPdrive_field() @param diffx, diffxy difference between coordinates of point to current robot position @param phi robot heading @return diffphi angle between robot heading and point (x, y) */ /***********************************************************************/ float get_angle(float diffx, float diffy, float phi) { float diffphi; /* angle to desired position */ if (fabs(diffx) < epsilon && fabs(diffy) < epsilon) diffphi = phi; /* point reached -> no angle difference */ else diffphi = DiscAtan((int)(1000.0 * diffy), (int) (1000.0 * diffx)); /* difference to current heading */ diffphi -= phi; /* angle always between -PI and PI */ if (diffphi >= PI) diffphi -= 2.0 * PI; if (diffphi <= -PI) diffphi += 2.0 * PI; return diffphi; } /***********************************************************************/ /** Drive curve directly to ball. Drive directly to ball if it is close enough. Called by PPdrive_field(). @see PPdrive_field() @param pos robot position @param ball structure containing coordinates of ball and angle from ball position to opponent's goal @param turn flag to indicate whether robot is supposed to turn at small distances */ /***********************************************************************/ void start_curve_to_ball(PositionType pos, PositionType ball, int turn) { /* difference in coordinates from robot to ball */ float diff_x, diff_y; /* distance and angle from robot to ball */ float ball_angle, ball_dist; /* speed depending on distance of ball */ float drive_speed, drive_dist; diff_x = ball.x - pos.x; /* difference to ball position */ diff_y = ball.y - pos.y; /* angle between robot orientation and line from robot middle through ball */ ball_angle = get_angle(diff_x, diff_y, pos.phi); /* distance to ball in a straight line */ ball_dist = hypot(diff_x, diff_y); if (fabs(ball_angle) == 0) drive_dist = ball_dist; else if (fabs(ball_angle) == PI) drive_dist = 0.5 * ball_dist * PI; else drive_dist = ball_dist * ball_angle / sin(ball_angle); drive_speed = 2.0 * drive_dist * LIN_SPEED; if (drive_speed < LIN_SPEED / 4.0) drive_speed = LIN_SPEED / 4.0; if (drive_speed > LIN_SPEED) drive_speed = LIN_SPEED; VWDriveCurve(vw, drive_dist, 2.0 * ball_angle, drive_speed); while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag) OSReschedule(); } /*************************************************************************/ /** Drive behind ball. Drive straight line, then turn to get behind ball. @param side side of ball where robot is driving to @param diff_x, diff_y way to middle of circle @return TRUE if robot drove all the way behind the ball */ /*************************************************************************/ int drive_around(float diff_x, float diff_y, int side, float myphi) { float diff_angle, diff_dist; float turn_angle; int done = FALSE; /* finished first driving operation */ /* angle between line through middle of circle and robot and 0 */ diff_angle = atan2(diff_y, diff_x); /* distance between robot and middle of circle */ diff_dist = hypot(diff_x, diff_y); /* turn back phi, forth angle to get to middle of circle and angle to circular path */ turn_angle = -myphi + diff_angle + side * asin(arounddist / 2 / diff_dist); if (fabs(turn_angle) > PI) turn_angle -= 2 * fsign(turn_angle) * PI; VWDriveTurn(vw, turn_angle, ROT_SPEED); while (!VWDriveDone(vw) && !end_flag && !obstacle_flag) OSReschedule(); if (VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag) { VWDriveStraight(vw, hypot(diff_dist, arounddist), LIN_SPEED); while (!(done = VWDriveDone(vw)) && !see_ball_flag && !end_flag && !obstacle_flag) OSReschedule(); } return done; } /***********************************************************************/ /** Turn and drive back to home position. Called by PPdrive_field(). @see PPdrive_field() */ /***********************************************************************/ void drive_home() { /* difference in coordinates from robot to ball */ float diff_x, diff_y; /* distance and angle from robot to ball */ float home_angle, home_dist; /* speed depending on distance of home position */ float drive_speed; PositionType pos; VWGetPosition(vw, &pos); diff_x = home[player_pos-2][next_home].x - pos.x; /* difference to home position */ diff_y = home[player_pos-2][next_home].y - pos.y; /* angle between robot orientation and line from robot middle through home position */ home_angle = get_angle(diff_x, diff_y, pos.phi); /* distance to home position in a straight line */ home_dist = hypot(diff_x, diff_y); if (start_flag) /* just starting to go -> take shortest way */ direction = home_angle; if (fsign(home_angle) == direction) VWDriveTurn(vw, home_angle, ROT_SPEED); else VWDriveTurn(vw, direction * 2.0 * PI + home_angle, ROT_SPEED); while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag) OSReschedule(); start_flag = FALSE; if (VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag) { drive_speed = 2.0 * home_dist * LIN_SPEED; if (drive_speed < LIN_SPEED / 4.0) drive_speed = LIN_SPEED / 4.0; if (drive_speed > LIN_SPEED) drive_speed = LIN_SPEED; VWDriveStraight(vw, home_dist, drive_speed); /* drive home */ while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag) OSReschedule(); if (see_ball_flag) VWSetSpeed(vw, 0.0, 0.0); if (VWDriveDone(vw)) { next_home++; if (next_home > 2) next_home = 0; } } } /**********************************************************************/ /** Drives to a point on the field Moves the robot in a straight line to a point, and gives it the desired facing. Will stop early if it spots the ball. @param TargetX the desired x co-ord @param TargetY the desired y co-ord @param TargetPhi the desired facing angle Returns 1 if completed successfully 0 if stopped early Called by PPdrive_field() @see PPdrive_field() @author Andrew Berry */ /***********************************************************************/ int DriveTo(float TargetX, float TargetY, float TargetPhi) { PositionType start_pos; float deltaX,deltaY,deltaPhi; /* determine direction to travel in */ VWGetPosition(vw,&start_pos); deltaX = TargetX - start_pos.x; deltaY = TargetY - start_pos.y; if( (fabs(deltaX)>epsilon2) || (fabs(deltaY)>epsilon2)) { OSReschedule(); deltaPhi = get_angle((float)deltaX,(float)deltaY,start_pos.phi); OSReschedule(); /* turn to face the direction of travel */ VWDriveTurn(vw,deltaPhi,ROT_SPEED); OSReschedule(); while(!VWDriveDone(vw)) { OSReschedule(); if (see_ball_flag == TRUE || obstacle_flag == TRUE) { UpdateFlags(); VWSetSpeed(vw,0,0); return 0; } } OSReschedule(); /* drive to point */ VWDriveStraight(vw,hypot(deltaX,deltaY),LIN_SPEED); OSReschedule(); while(!VWDriveDone(vw)) { OSReschedule(); if (see_ball_flag == TRUE || obstacle_flag == TRUE ) { UpdateFlags(); VWSetSpeed(vw,0,0); return 0; } } OSReschedule(); } VWGetPosition(vw,&start_pos); OSReschedule(); deltaPhi = TargetPhi-start_pos.phi; /* turn to the final facing */ if (fabs(deltaPhi)>epsilonphi) { OSReschedule(); if (deltaPhi>PI) deltaPhi = -(deltaPhi - PI); VWDriveTurn(vw,deltaPhi,ROT_SPEED); OSReschedule(); while(!VWDriveDone(vw)) { if (see_ball_flag == TRUE || obstacle_flag == TRUE) { UpdateFlags(); VWSetSpeed(vw,0,0); return 0; } OSReschedule(); } } OSReschedule(); LCDSetPos(1, 10); return 1; } /***********************************************************************/ /** Find_Lost_Ball() Attempt to find out what happened to the ball that was just lost by turning in the direction the ball was traveling in. @return true if the ball is found, false if it is not @Called by PPDriveField() @author Andrew Berry */ /***********************************************************************/ int Find_Lost_Ball() { float turn_angle; AUBeep(); /* which direction do we need to go ? */ if (BallImagePos == LEFT_EDGE) { turn_angle = PI2; LCDSetPos(4,0); LCDPrintf("Lost:Left \n"); } else { turn_angle = -PI2; LCDSetPos(4,0); LCDPrintf("Lost:Right \n"); } /* turn and look */ VWDriveTurn(vw,turn_angle,ROT_SPEED); while(!VWDriveDone(vw)) { if ((see_ball_flag == TRUE) || (obstacle_flag == TRUE)) { OSReschedule(); VWSetSpeed(vw,0,0); return 0; } OSReschedule(); } return 1; } /***********************************************************************/ /** AttackBall Robot drives to the balls position. @Called by PPDriveField() @author Andrew Berry */ /***********************************************************************/ int AttackBall() { PositionType pos, ball; int ball_count = 0; int done = FALSE; /* flag to show whether robot succesfully drove around ball */ float angle_after_turn; /* heading if robot would drive curve to the ball */ float ball_angle; /* angle to the ball */ float middle_x, middle_y, diff_x, diff_y, mid_dist, side; if (use_motors) { if (((ball.x < home[player_pos-2][0].x + LENGTH4) && /* ball outside region? */ ( (ball.x > home[player_pos-2][0].x - LENGTH4) )) || ( ((MyRole == ATTACKER) && fabs(pos.phi) < PI/8) )) /* striker facing opponent's goal */ { ball_count = 0; /* angle to ball */ ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi); /* angle in which robot would hit the ball after turning to it */ angle_after_turn = pos.phi - 2 * ball_angle; if (fabs(angle_after_turn) > PI) angle_after_turn -= 2 * fsign(angle_after_turn) * PI; /* drive in/turn to right position to drive curve to ball */ if (fabs(angle_after_turn) > PI2) /* turn to own goal */ { LCDSetPos(4, 12); LCDPrintf("be1\n"); /* side of ball where robot has to drive */ diff_y = ball.y - pos.y; side = fsign(diff_y); ball.x -= arounddist; /* drive behind ball */ /* position of middle of circle: used to check whether robot is too close to ball */ middle_x = ball.x - side * arounddist / 2 * sin(ball.phi); middle_y = ball.y - side * arounddist / 2 * cos(ball.phi); /* difference of position robot - middle of circle */ diff_x = middle_x - pos.x; diff_y = middle_y - pos.y; mid_dist = hypot(diff_x, diff_y); done = FALSE; if(mid_dist > arounddist) done = drive_around(diff_x, diff_y, side, pos.phi); else VWDriveTurn(vw, side * PI, ROT_SPEED); if (done) { VWGetPosition(vw, &pos); /* angle to ball */ ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi); /* angle in which robot would hit the ball after turning to it */ angle_after_turn = pos.phi - 2 * ball_angle; if (fabs(angle_after_turn) > PI) angle_after_turn -= 2 * fsign(angle_after_turn) * PI; LCDSetPos(4, 12); LCDPrintf("be2\n"); if (fabs(angle_after_turn) < PI / 4) { start_curve_to_ball(pos, ball, TRUE); /* drive behind ball */ if (VWDriveDone(vw) && !see_ball_flag) move_camera(DOWN); /* lost ball - maybe already in front of robot -> check */ } else VWDriveTurn(vw, -side * PI, ROT_SPEED); } ball.x += arounddist; /* real ball position */ } else { LCDSetPos(4, 12); LCDPrintf("frn\n"); VWGetPosition(vw, &pos); start_curve_to_ball(pos, ball, FALSE); /* drive behind ball */ } } else ball_count ++; } return 1; } /***********************************************************************/ /** Procedure for handling the receiving of messages and what to do with the message 'r' means reset 'b # #' is the ball co-ordinates 'g' means go @Called by PPDriveField() @author Andrew Berry */ /***********************************************************************/ void RecvMesg() { BYTE buffer[64],*bufferpointer; BYTE SenderID; int bytesReceived; float f1,f2; while(RADIOCheck()!=0) { if(RADIORecv(&SenderID,&bytesReceived,buffer)) OSPanic("Can't receive message\n"); switch(buffer[1]){ case 'r': reset_flag = TRUE; break; case 'b': if (OSMachineID() != SenderID ) { bufferpointer = strchr(buffer,' '); f1=atof(bufferpointer); bufferpointer = strrchr(bufferpointer,' '); f2=atof(bufferpointer); if ((fabs(ToldPos.x - f1)>epsilon2) || (fabs(ToldPos.y - f2)>epsilon2)) { /*update only if it's a new pos */ ToldPos.x = f1; ToldPos.y = f2; told_where_flag = TRUE; } } break; case 'g': play_flag = TRUE; break; default:{} } } free(buffer); } /***********************************************************************/ /** Patrol has the robot randomly patrol around through a set looking for the ball. Returns 1 if the ball is found, 0 if no ball is found. @author Andrew Berry Called by PPdrive_field() @see PPdrive_field() */ /***********************************************************************/ int Patrol() { static PositionType target_pos,pos; if (patrol_leg == 0 ) patrol_leg = 1; VWGetPosition(vw,&pos); patrol_leg = rand()%9; if (MyRole == DEFENDER) switch(patrol_leg){ case 0: target_pos.x = 0.4; target_pos.y = -0.3; target_pos.phi = 0; break; case 1: target_pos.x = 0.4; target_pos.y = 0.3; target_pos.phi = 0; break; case 2: target_pos.x = 0.6; target_pos.y = 0.3; target_pos.phi = 0; break; case 3: target_pos.x = 0.6; target_pos.y = -0.3; target_pos.phi = 0; break; case 4: target_pos.x = 1.2; target_pos.y = -0.3; target_pos.phi = 0; case 5: target_pos.x = 1.2; target_pos.y = 0.3; target_pos.phi = 0; case 6: target_pos.x = 0.4; target_pos.y = 0; target_pos.phi = 0; case 7: target_pos.x = 0.6; target_pos.y = 0; target_pos.phi = 0; case 8: target_pos.x = 1.2; target_pos.y = 0; target_pos.phi = 0; default: {} } else /*MyRole = ATTACKER */ switch(patrol_leg){ case 0: target_pos.x = 1.4; target_pos.y = -0.3; target_pos.phi = 0; break; case 1: target_pos.x = 1.4; target_pos.y = 0.3; target_pos.phi = 0; break; case 2: target_pos.x = 1.6; target_pos.y = 0.3; target_pos.phi = 0; break; case 3: target_pos.x = 1.6; target_pos.y = -0.3; target_pos.phi = 0; break; case 4: target_pos.x = 2.0; target_pos.y = -0.3; target_pos.phi = 0; case 5: target_pos.x = 2.0; target_pos.y = 0.3; target_pos.phi = 0; case 6: target_pos.x = 1.4; target_pos.y = 0; target_pos.phi = 0; case 7: target_pos.x = 1.6; target_pos.y = 0; target_pos.phi = 0; case 8: target_pos.x = 2.0; target_pos.y = 0; target_pos.phi = 0; default: {} } return DriveTo(target_pos.x,target_pos.y,target_pos.phi); } /***********************************************************************/ /** FindLoc() This procedure has the robot try to re-determine it's position on the field. It can't correct the angle, just x & y. Called by main() (not currently used) @see main() @author Andrew Berry */ /***********************************************************************/ int FindLoc() { PositionType pos; LCDSetPos(1,10); LCDPrintf("findlc\n"); VWGetPosition(vw,&pos); VWSetPosition(vw,pos.x,pos.y,0); /* set the y */ VWDriveTurn(vw,PI2,ROT_SPEED); while(!VWDriveDone(vw)); found_wall_flag = FALSE; while(found_wall_flag == FALSE) { if (GetPSDValue()>0.08) { VWDriveStraight(vw,0.15,LIN_SPEED); if (GetLeftPSDValue() < 0.08) { VWSetSpeed(vw,0,0); VWDriveTurn(vw,-PI/24,ROT_SPEED); while(!VWDriveDone(vw)); VWDriveStraight(vw,0.15,LIN_SPEED); } if (GetRightPSDValue() < 0.08) { VWSetSpeed(vw,0,0); VWDriveTurn(vw,PI/24,ROT_SPEED); while(!VWDriveDone(vw)); VWDriveStraight(vw,0.15,LIN_SPEED); } } else { VWSetSpeed(vw,0,0); AUBeep();AUBeep(); avoid_obstacle(); } } VWDriveStraight(vw,-0.1,LIN_SPEED); while(!VWDriveDone(vw)); /* set the x */ VWDriveTurn(vw,-PI2,ROT_SPEED); while(!VWDriveDone(vw)); found_wall_flag = FALSE; while(found_wall_flag == FALSE) { if (GetPSDValue()>0.08) { VWDriveStraight(vw,0.15,LIN_SPEED); if (GetLeftPSDValue() < 0.08) { VWSetSpeed(vw,0,0); VWDriveTurn(vw,-PI/24,ROT_SPEED); while(!VWDriveDone(vw)); VWDriveStraight(vw,0.15,LIN_SPEED); } if (GetRightPSDValue() < 0.08) { VWSetSpeed(vw,0,0); VWDriveTurn(vw,PI/24,ROT_SPEED); while(!VWDriveDone(vw)); VWDriveStraight(vw,0.15,LIN_SPEED); } } else { VWSetSpeed(vw,0,0); AUBeep();AUBeep(); avoid_obstacle(); } } DriveTo(reset_pos.x,reset_pos.y,reset_pos.phi); LCDClear(); return 0; } /***********************************************************************/ /** Set field player movement. Move robot and output text (drive status) on LCD. Activate robot movement according to flags end_flag, obstacle_flag, got_ball_flag and see_ball_flag, concerning different priorities. Process started by main(). @see main() */ /***********************************************************************/ void PPdrive_field() { PositionType pos, ball; /* float ball_dist; int ball_count = 0;*/ int new_ball; /* int done = FALSE; */ /* flag to show whether robot succesfully drove around ball */ /*float angle_after_turn;*/ /* heading if robot would drive curve to the ball */ /* float ball_angle; */ /* angle to the ball */ /* float middle_x, middle_y, diff_x, diff_y, mid_dist, side;*/ BYTE mesg_buffer[64]; /* the message buffer */ int kick_off = TRUE; int check_goal = FALSE; int need_to_check_counter = 0; /* counter to tell when to check angle */ play_flag = FALSE; reset_flag = TRUE; while(!end_flag) { LCDMenu("Go ","RSET","STOP"," "); LCDSetPos(1,0); LCDPrintf("here \n"); if (!see_ball_flag) got_ball_flag = FALSE; if (kick_off && play_flag) { kick_off = FALSE; kick_ball(); VWDriveStraight(vw,0.2,LIN_SPEED); } need_to_check_counter++; OSReschedule(); UpdateFlags(); if (comms_flag ) /* I have a message */ { LCDSetPos(1,0); LCDPrintf("chk mail\n"); if( RADIOCheck()!=0) { AUBeep(); LCDSetPos(1, 10); LCDPrintf("Mesg \n"); RecvMesg(); OSReschedule(); } else { LCDSetPos(1, 10); LCDPrintf("NoMesg\n"); } } if (obstacle_flag && !see_ball_flag && !got_ball_flag) /* check obstacles */ { LCDSetPos(0,0); LCDPrintf("obst dect\n"); LCDSetPos(1,10); LCDPrintf("Obst!\n"); move_camera(DOWN); OSReschedule(); if (!see_ball_flag) avoid_obstacle(); OSReschedule(); } else if (find_loc_flag) /* find my location */ { FindLoc(); find_loc_flag = FALSE; } else if (reset_flag) /* time to reset to starting position */ { UpdateFlags(); LCDSetPos(1, 10); LCDPrintf("reset\n"); play_flag = FALSE; while(!DriveTo(reset_pos.x,reset_pos.y,reset_pos.phi)) { /* keep driving to the reset position */ reset_flag = TRUE; OSReschedule(); } reset_flag = FALSE; see_ball_flag = FALSE; told_where_flag = FALSE; just_lost_flag = FALSE; } else if (play_flag == TRUE) /*time to play */ { LCDSetPos(1,0); LCDPrintf("playing \n"); /* double check if ball is in front of robot */ if (see_ball_flag) { LCDSetPos(1,0); LCDPrintf("I see ball\n"); VWGetPosition(vw, &pos); if (hypot(ball.x - pos.x, ball.y - pos.y) < 0.05) { move_camera(DOWN); got_ball_flag = TRUE; } LCDSetPos(1,0); LCDPrintf("looked down\n"); } if(start_flag) /* go go go */ { LCDSetPos(1,0); LCDPrintf("kick off \n"); UpdateFlags(); if (MyRole == ATTACKER) kick_ball(); /* start code here */ start_flag = FALSE; reset_flag = FALSE; } else if (got_ball_flag) /* I have the ball in front of me */ { LCDSetPos(1,0); LCDPrintf("got ball \n"); AUBeep(); told_where_flag = FALSE; UpdateFlags(); LCDSetPos(1,10); LCDPrintf("Got \n"); LCDSetPos(2,10); LCDPrintf("Ball \n"); new_ball = FALSE; move_camera(DOWN); if (MyRole == ATTACKER) { drive_to_goal(); } else /* I'm defending */ { drive_to_goal(); } } else if (see_ball_flag) /* I see the ball */ { AUBeep(); AUBeep(); LCDSetPos(1,0); LCDPrintf("seen ball \n"); told_where_flag = FALSE; UpdateFlags(); LCDSetPos(4,0); LCDPrintf(" \n"); LCDSetPos(1,10); LCDPrintf("Ball \n"); get_ball_coord(&ball); LCDSetPos(2,10); LCDPrintf("%d \n",(int)(100*ball.x)); LCDSetPos(3,10); LCDPrintf("%d \n",(int)(100*ball.y)); /* tell everyone about the ball */ LCDSetPos(1,0); LCDPrintf("tell about\n"); sprintf(mesg_buffer,"b %g %g ",ball.x,ball.y); RADIOSend(BROADCAST,strlen(mesg_buffer)+1,mesg_buffer); /* which edge info update */ if (BallImagePos == LEFT_EDGE) { LCDSetPos(3,0); LCDPrintf("l edge\n"); } if (BallImagePos == RIGHT_EDGE) { LCDSetPos(3,0); LCDPrintf("r edge\n"); } LCDSetPos(1,0); LCDPrintf("attack!!!!\n"); if (MyRole == ATTACKER) { AttackBall(); } else /* I'm defending */ { AttackBall(); } } else if (told_where_flag) /* someone else sees the ball */ { LCDSetPos(1,0); LCDPrintf("told where\n"); UpdateFlags(); /* for testing - go to where ball was last seen */ DriveTo(ToldPos.x,ToldPos.y,reset_pos.phi); /* look behind you */ DriveTo(ToldPos.x,ToldPos.y,-reset_pos.phi); /* otherwise give up & go somewhere else */ told_where_flag = FALSE; } else if (just_lost_flag) /* I had it, now it's gone */ { UpdateFlags(); if (BallImagePos == LEFT_EDGE) { LCDSetPos(3,0); LCDPrintf("l edge\n"); } if (BallImagePos == RIGHT_EDGE) { LCDSetPos(3,0); LCDPrintf("r edge\n"); } LCDSetPos(1,10); LCDPrintf("LB! \n"); Find_Lost_Ball(); } else if (teammate_has_ball) /* space for future teamwork stuff - does nothing now */ { if (MyRole == ATTACKER) { /* attack stuff here */ } else /* I'm defending */ { /* defender stuff here */ } } else /* default search activity */ { if (need_to_check_counter > 25) check_goal = TRUE; if (check_goal) { LCDSetPos(1,10); LCDPrintf("chk g\n"); need_to_check_counter = 0; if(!Check_goal_position()) { LCDSetPos(1,10); LCDPrintf("fnd g\n"); reset_angle(); } LCDPrintf("done\n"); check_goal = FALSE; } LCDSetPos(1,0); LCDPrintf("patrol \n"); LCDSetPos(1,10); LCDPrintf("Pat %d\n",patrol_leg); UpdateFlags(); if(VWDriveDone(vw)) Patrol(); } } } end_flag += 1; OSReschedule(); OSKill(0); } /***********************************************************************/ /** GOALIE: Drive to ball on circle near goal line. Drive curve near goal line while always facing the ball. Called by PPdrive_goal(). @see PPdrive_goal() @param ball structure containing coordinates of ball and angle from ball position to opponent's goal */ /***********************************************************************/ void start_goal_line(PositionType ball) { float robot_angle, diff_angle, correct_angle; float drive_speed; PositionType pos; VWGetPosition(vw, &pos); ball.phi = atan2(ball.y, goaldist + ball.x); /* ball angle */ robot_angle = atan2(pos.y, goaldist + pos.x); /* goal angle */ /* angle between lines middle of circle to robot and middle of circle to ball */ diff_angle = ball.phi - robot_angle; see_ball_flag = FALSE; /* already driving to current ball position */ /* ------------------------ correct heading ------------------------- */ /* correct heading if robot is standing at the corner of the goal and has to drive away: turn until robot is standing perpendicular to line to middle of circle */ if ((fsign(diff_angle) != fsign(pos.y)) && (fabs(pos.y) >= GOALWIDTH / 3.0)) VWDriveTurn(vw, -PI2 - pos.phi + robot_angle, ROT_SPEED); while (!VWDriveDone(vw) && !see_ball_flag && !end_flag) OSReschedule(); /* ------------------------------------------------------------------ */ if (!end_flag && !obstacle_flag) { if (fabs(ball.phi) > anglemax) diff_angle = fsign(ball.phi) * anglemax - robot_angle; /* ------------------------ drive to ball ------------------------ */ if ((fabs(diff_angle) > PI / 100.0)) /* don't react on little changes or if already facing ball */ { drive_speed = 4.0 * fabs(diff_angle) * LIN_SPEED; if (drive_speed < LIN_SPEED / 4.0) drive_speed = LIN_SPEED / 4.0; if (drive_speed > LIN_SPEED) drive_speed = LIN_SPEED; VWDriveCurve(vw, -diff_angle * radius, -fabs(diff_angle), drive_speed); while (!VWDriveDone(vw) && ((fsign(diff_angle) != fsign(pos.y)) || (fabs(pos.y) < GOALWIDTH / 3.0)) && !see_ball_flag && !obstacle_flag && !end_flag) { OSReschedule(); VWGetPosition(vw, &pos); } } VWSetSpeed(vw, 0.0, 0.0); if (!see_ball_flag) { /* ------------------------ correct heading ------------------------- */ /* turn to look at ball in case robot is standing at the corner of its goal */ correct_angle = PI2 + pos.phi - ball.phi; if (fabs(correct_angle) > PI / 200.0) VWDriveTurn(vw, -correct_angle, ROT_SPEED / 2.0); } } } /***********************************************************************/ /** GOALIE: Set movement. Move robot and output text (drive status) on LCD. Activate robot movement according to flags end_flag, obstacle_flag, got_ball_flag and see_ball_flag, concerning different priorities. Process started by main(). @see main() */ /***********************************************************************/ void PPdrive_goal() { PositionType ball, last, pos; BYTE mesg_buffer[64]; int last_flag = FALSE; /* flag whether last time ball has been detected */ int factor = 0; LCDMenu("RESET", "GO", "STOP", " "); AUBeep(); while(!end_flag) { /* LCDPrintf("control\n"); */ LCDSetPos(0, 10); LCDPrintf("o%ds%d\n", obstacle_flag, see_ball_flag); if (obstacle_flag) { LCDSetPos(1, 10); LCDPrintf("Wall!!!\n"); LCDSetPos(2, 10); LCDPrintf(" \n"); LCDSetPos(3, 10); LCDPrintf(" \n"); last_flag = FALSE; VWSetSpeed(vw, 0.0, 0.0); } else if (see_ball_flag) { AUBeep(); get_ball_coord(&ball); /* tell everyone about the ball */ sprintf(mesg_buffer,"b %g %g ",ball.x,ball.y); RADIOSend(BROADCAST,strlen(mesg_buffer)+1,mesg_buffer); LCDSetPos(1, 10); LCDPrintf("Ball:\n"); LCDSetPos(2, 10); LCDPrintf("(%d,\n", (int)(100 * ball.x)); LCDSetPos(3, 10); LCDPrintf("% 2d) \n", (int)(100 * ball.y)); factor = 3; /* allow factor pictures without ball after driving to it */ if ((ball.x < 0.3) || last_flag) /* use last ball coordinates if available */ { if (last_flag) /* propable future ball position */ { ball.x -= last.x - ball.x; ball.y -= last.y - ball.y; } VWGetPosition(vw, &pos); if (hypot(ball.x - pos.x, ball.y - pos.y) < 0.05) kick_ball(); start_goal_line(ball); } last = ball; last_flag = TRUE; } else /* no ball in sight */ { LCDSetPos(1, 10); LCDPrintf(" \n"); LCDSetPos(2, 10); LCDPrintf(" \n"); LCDSetPos(3, 10); LCDPrintf(" \n"); last_flag = FALSE; if (factor) factor --; else { ball.x = 0.1; ball.y = 0.0; start_goal_line(ball); /* lost ball -> drive back to middle of goal */ if (VWDriveDone(vw)) { LCDSetPos(2, 10); LCDPrintf("MID \n"); } } } OSReschedule(); } end_flag += 1; OSKill(0); } /*@}*/