/***********************************************************************/ /** @name driveglobal.c Contains global functions to init and release motors, control overall robot movement and set and read robot position. @author Birgit Graf, UWA, 1998, modified 2000 (Mk3/4) */ /***********************************************************************/ /*@{*/ #include "driveglobal.h" #include "general.h" /** simulate motors (position is still being read)? */ int sim_motors = FALSE; /** Handle for motors */ VWHandle vw; /** maximum driving speeds */ meterPerSec LIN_SPEED = 0.2; radPerSec ROT_SPEED = 0.6; /* meterPerSec LIN_SPEED = 0.1; radPerSec ROT_SPEED = 0.3; */ /** 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 /* distance beneath/behind ball where robot should drive */ #define arounddist 0.1 /* distance below which robot should drive straight instead of curve to ball */ #define STRAIGHT_DISTANCE 0.1 /***********************************************************************/ /** Init VW interface. Called by main(). @see main() */ /***********************************************************************/ void Init_Drive() { if (!(vw = VWInit(VW_DRIVE, 1))) /* initialize motors and encoders */ error("VWInit!\n"); if (sim_motors) /* only need encoder readings, no motion */ return; if (VWStartControl(vw, v_lin, t_lin , v_rot, t_rot) == -1) { LCDClear(); LCDMenu("", "", "", "OK"); LCDPutString("VWStart \nerror!\nSwitching to\nsim mode "); KEYWait(KEY4); LCDClear(); sim_motors = TRUE; } 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: VWSetPosition(vw, 0, 0, 0); /* 0 position */ break; } } /***********************************************************************/ /** Stop VW interface. */ /***********************************************************************/ void End_Drive() { VWRelease(vw); } /***********************************************************************/ /** 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(); if (sim_motors) { LCDMenu("", "", " ", "OK"); LCDPrintf("Motors \ndeactivated!"); KEYWait(KEY4); LCDClear(); return; } 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"); 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, 1.0, 0.05); break; case 1: ROT_SPEED = set_fparam("Rot. speed", 0.0, ROT_SPEED, 1.0, 0.05); break; case 2: player_pos = set_iparam("Player pos.\n(1=goalie, \n2=lb, 3=rb,\n4=lf, 5=rf)", 1, player_pos, 5, 1); break; default: break; } LCDMenu("CHG", "NXT", " ", "END"); break; case KEY2: LCDSetChar(2 + ind, 15, ' '); ind ++; 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 position. Interface function to vw. */ /***********************************************************************/ void set_pos(meter x, meter y, radians phi) { VWSetPosition(vw, x, y, phi); } /***********************************************************************/ /** Return robot position in referenced parameter. Interface function to vw. */ /***********************************************************************/ void get_pos(PositionType* req_pos) { VWGetPosition(vw, req_pos); } /***********************************************************************/ /** Set robot coordinates. Set robot x and y coordinates. (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", "0", "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, 0.0, 0.0, 0.0); break; case KEY4: LCDClear(); end_proc2 = TRUE; break; default: break; } } } case KEY4: LCDClear(); end_proc = TRUE; break; default: break; } } } /***********************************************************************/ /** Stop. Interface function to vw. */ /***********************************************************************/ void drive_stop() { /* LCDPrintf("Stop"); */ VWSetSpeed(vw, 0.0, 0.0); } /***********************************************************************/ /** Did robot wheels stall? Interface function to vw. */ /***********************************************************************/ int wheels_stalled() { return VWStalled(vw); } /***********************************************************************/ /** Get drive speed. Get drive speed depending on distance to goal @param dist distance to current goal @return speed */ /***********************************************************************/ float get_drive_speed(float dist) { float speed = 2.0 * dist * LIN_SPEED; if (speed < LIN_SPEED / 2.0) speed = LIN_SPEED / 2.0; if (speed > LIN_SPEED) speed = LIN_SPEED; return speed; } /***********************************************************************/ /** 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 0 and 2*PI */ if (diffphi >= 2.0 * PI) diffphi -= 2.0 * PI; if (diffphi <= 0) diffphi += 2.0 * PI; /* angle always between -PI and PI */ if (diffphi >= PI) diffphi -= 2.0 * PI; /* if (fabs(diffphi) > PI/2) */ /* if (USE_AUDIO) */ /* AUTone(1200,200); */ 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=0, diff_y=0; /* distance and angle from robot to ball */ float ball_angle=0, ball_dist=0; /* speed depending on distance of ball */ float drive_speed=0, drive_dist=0; 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 = get_drive_speed(drive_dist); /* KEYWait(ANYKEY); LCDClear(); LCDPrintf("d:%f\na:%f\ns:%f\nt:%d\n", ball_dist, ball_angle, drive_speed, turn); KEYWait(ANYKEY); LCDClear(); */ if ((ball_dist < STRAIGHT_DISTANCE) && !turn) VWDriveStraight(vw, ball_dist, drive_speed); else /* drive curve to ball, angle = ball_angle, distance on curve = straight line distance * angle * sin(angle) */ 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; } /*@}*/