/* ######################################### * * behaviors.c -- some behavior definitions * * *######################################### */ /** ** behaviors.c ** ** Copyright 1993, 1994, 1995 by Kurt Konolige ** ** The author hereby grants to SRI permission to use this software. ** The author also grants to SRI permission to distribute this software ** to schools for non-commercial educational use only. ** ** The author hereby grants to other individuals or organizations ** permission to use this software for non-commercial ** educational use only. This software may not be distributed to others ** except by SRI, under the conditions above. ** ** Other than these cases, no part of this software may be used or ** distributed without written permission of the author. ** ** Neither the author nor SRI make any representations about the ** suitability of this software for any purpose. It is provided ** "as is" without express or implied warranty. ** ** Kurt Konolige ** Senior Computer Scientist ** SRI International ** 333 Ravenswood Avenue ** Menlo Park, CA 94025 ** E-mail: konolige@ai.sri.com ** **/ #include "saphira.h" /* * for reference pointers */ static int Accel = ACCEL; static int Decel = DECEL; static int Turn_left = TURN_LEFT; static int Turn_right = TURN_RIGHT; /* * Constant velocity -- achieve and maintain a constant velocity * */ #define CV_MAX_SPEED 500.0 BeginBehavior sfConstantVelocity Params sfFLOAT speed Rules Maintain_Speed If cv_too_slow Or cv_too_fast Then Speed cv_speed Update float prog; /* this should probably be declared a global by Activity section */ cv_too_slow = f_smaller(sfTargetVel(),MIN(speed,CV_MAX_SPEED),speed*.2); cv_too_fast = f_greater(sfTargetVel(),speed,speed*.2); cv_speed = MIN(speed,CV_MAX_SPEED); prog = f_eq(sfRobot.tv,sfTargetVel(),20.0); Activity Speed 1.0 Turn 0.0 Progress prog Goal 0.0 EndBehavior /* * STOP -- stop the robot * * No external parameters. */ BeginBehavior sfStop Params Rules stop If 1.0 Then Speed 0.0 Update Activity Speed 1.0 Turn 1.0 EndBehavior /* * Avoid collision --- avoid obstacles close to the robot * * Parameters: AC_FRONT_SN front sensitivity * AC_SIDE_SN side sensitivity * AC_TURN_SN turn strength * AC_STANDOFF min obstacle approach distance * */ int sfPreferredTurnDir = TURN_LEFT; #define ESCAPE_COUNT 8 BeginBehavior sfAvoidCollision Params sfFLOAT front_sense sfFLOAT side_sense sfFLOAT turn_gain sfFLOAT standoff Rules Collision_right If ac_right Then Turn Left ac_turn_depth Collision_left If ac_left Then Turn Right ac_turn_depth Collision_front If ac_front Then Speed ac_speed Collision_blocked If ac_blocked Then Turn ac_preferred_turn ac_turn_depth Init ac_preferred_turn = 0; ac_speed = -30.0; Update int fincd = 200, fincs = 70, fstart = 120, fedge1, fedge2, sincd = 100, sincs = 100, sstart = 200, sedge1 = -50, sedge2 = 300, fstand, sstand, fsense = 50; static int cul_de_sac = -1; float cf, crs, crf, cls, clf, vel, fdist, sdist; float right, front_right, left, front_left, front; fedge1 = 0; fedge2 = 1.2 * flakey_radius; fstand = standoff + 50; sstand = standoff + 50; fedge2 += fsense*side_sense; vel = sfRobot.tv; clf = occupied2(fstart,fedge1,fedge2,1); cls = MIN(occupied4(sstart,sedge1,sedge2,2), MIN(occupied4(sstart+sincd, sedge1, sedge2+sincs, 2), occupied4(sstart+sincd+sincd, sedge1, sedge2+sincs+sincs, 2))); fedge1 = -fedge1; fedge2 = -fedge2; fincs = -fincs; crs = MIN(occupied4(sstart,sedge1,sedge2,-2), MIN(occupied4(sstart+sincd, sedge1, sedge2+sincs, -2), occupied4(sstart+sincd+sincd, sedge1, sedge2+sincs+sincs, -2))); crf = occupied2(fstart,fedge1,fedge2,1); cf = occupied2(100,-flakey_radius,flakey_radius,1); fdist = front_sense*MAX(150,vel); /* distance of front sensitivity, 900mm @ 300mm/sec */ sdist = side_sense*MAX(120,vel); /* distance of side sensitivity, 300mm @ 300mm/sec */ right = straight_down(crs,sstand,sstand+sdist); front_right = straight_down(crf,fstand,fstand+fdist); left = straight_down(cls,sstand,sstand+sdist); front_left = straight_down(clf,fstand,fstand+fdist); front = straight_down(cf,fstand,fstand+fdist); front = MAX(front, MAX(front_right*.5,front_left*.5)); ac_right = f_and(f_or(right,front_right), f_not(left)); ac_left = f_and(f_or(left,front_left), f_not(right)); ac_front = f_or(front, f_or(front_left,front_right)); /* we're block if front is blocked, l/r equal or l/r both very strong */ ac_blocked = f_and(ac_front,f_or( f_and(f_greater(front_left,0.5,0.2), f_greater(front_right,0.5,0.2)), f_and(f_greater(0.5,ac_left,0.2), f_greater(0.5,ac_right,0.2)))); if (ac_blocked > 0.5) cul_de_sac = ESCAPE_COUNT; /* start escape sequence */ if (--cul_de_sac >= 0) ac_blocked = 1.0; else cul_de_sac = -1; if (ac_blocked > 0.6) /* front is blocked, turn in default dir */ { if (ac_preferred_turn == 0) /* ac_preferred_turn = AC_BLOCKED_TURN(params); */ ac_preferred_turn = sfPreferredTurnDir; ac_left = ac_right = 0.0; } else if (ac_blocked < 0.1) { ac_preferred_turn = 0; ac_blocked = 0.0; } else if (ac_preferred_turn == 0) ac_blocked = 0.0; if (ac_left > ac_right) ac_right = 0.0; if (ac_right > ac_left) ac_left = 0.0; if (front_right > .2) sfDrawCenteredRect(crf*.5,-30.0+crf*-.5,crf,crf); if (front_left > .2) sfDrawCenteredRect(clf*.5,30.0+clf*.5,clf,clf); if (right > .2) sfDrawCenteredRect(100.0,-30.0+crs*-.5,100.0,crs); if (left > .2) sfDrawCenteredRect(100.0,30.0+cls*.5,100.0,cls); c_act_t = MAX(ac_right, MAX(ac_left, MAX(ac_front, ac_blocked))); c_act_v = c_act_t; ac_turn_depth = turn_gain; ac_act = MAX(ac_right, MAX(ac_left, MAX(ac_front, ac_blocked))); Activity Turn ac_act Speed ac_act Progress 1.0 Goal 0.0 EndBehavior /* * Stop collision --- stop when obstacles close to the robot * * Parameters: SC_FRONT_SN front sensitivity * SC_SIDE_SN side sensitivity * SC_STANDOFF min obstacle approach distance * */ BeginBehavior sfStopCollision Params sfFLOAT front_sense sfFLOAT side_sense sfFLOAT standoff Rules Collision_front If front Or front_left Or front_right Then Speed 0.0 Update int fincd = 200, fincs = 70, fstart = 120, fedge1, fedge2, sincd = 100, sincs = 100, sstart = 200, sedge1 = 50, sedge2 = 300, fstand, sstand, fsense = 50; float cf, crs, crf, cls, clf, vel, fdist, sdist; float right, front_right, left, front_left, front; fedge1 = 0; fedge2 = 1.2 * flakey_radius; fstand = standoff + 50; sstand = standoff + 50; fedge2 += fsense*side_sense; vel = sfRobot.tv; clf = occupied2(fstart,fedge1,fedge2,1); cls = MIN(occupied3(sstart,sedge1,sedge2,2), MIN(occupied3(sstart+sincd, sedge1, sedge2+sincs, 2), occupied3(sstart+sincd+sincd, sedge1, sedge2+sincs+sincs, 2))); fedge1 = -fedge1; fedge2 = -fedge2; fincs = -fincs; crs = MIN(occupied3(sstart,sedge1,sedge2,-2), MIN(occupied3(sstart+sincd, sedge1, sedge2+sincs, -2), occupied3(sstart+sincd+sincd, sedge1, sedge2+sincs+sincs, -2))); crf = occupied2(fstart,fedge1,fedge2,1); cf = occupied2(100,-flakey_radius,flakey_radius,1); fdist = front_sense*MAX(150,vel); /* distance of front sensitivity, 900mm @ 300mm/sec */ sdist = side_sense*MAX(120,vel); /* distance of side sensitivity, 300mm @ 300mm/sec */ right = straight_down(crs,sstand,sstand+sdist); front_right = straight_down(crf,fstand,fstand+fdist); left = straight_down(cls,sstand,sstand+sdist); front_left = straight_down(clf,fstand,fstand+fdist); front = straight_down(cf,fstand,fstand+fdist); front = MAX(front, MAX(front_right*.5,front_left*.5)); if (front_right > .2) sfDrawCenteredRect(crf*.5,-30.0+crf*-.5,crf,crf); if (front_left > .2) sfDrawCenteredRect(clf*.5,30.0+clf*.5,clf,clf); if (right > .2) sfDrawCenteredRect(100.0,-30.0+crs*-.5,100.0,crs); if (left > .2) sfDrawCenteredRect(100.0,30.0+cls*.5,100.0,cls); sc_act = MAX(front_right, MAX(front_left,front)); Activity Speed sc_act Turn 0.0 Progress 1.0 Goal sc_act EndBehavior /* * KEEP OFF * * Veers robot away from obstacles at longer distances * * Parameters: CAUTION_SPEED * SENSITIVITY * BLOCKED_TURN (TURN_LEFT, TURN_RIGHT, 0 = none) * */ BeginBehavior sfKeepOff Params sfFLOAT speed sfFLOAT sensitivity sfINT turn /* turn direction not used; sfPreferredTurnDir */ Rules Obstacle_Right If ob_right Then Turn Left turning_depth Obstacle_Left If ob_left Then Turn Right turning_depth Obstacle_Front If ob_front Then Turn preferred_turn turning_depth Obstacle_Caution If ob_caution Then Speed caution_speed Update static int last_turn = 0; int lat_vel, safe_front, safe_right, safe_left, front, front_middle, front_right, front_left, side_right, side_left, back_right, back_left, front_diff, right_diff, left_diff; float sv = sensitivity * sfRobot.tv; float ob_right_side, ob_left_side, ob_near, ob_in_front, ob_angled_right, ob_angled_left, ob_straight, too_fast_for_ob, approaching_ob; /* preferred_turn = KO_BLOCKED_TURN(params); */ preferred_turn = sfPreferredTurnDir; lat_vel = sfRobot.tv * sfRobot.rv * DEG_TO_RAD * 0.5; safe_front = 400 + sv * 5.0; safe_right = 400 + sv - sensitivity * lat_vel; safe_left = 400 + sv + sensitivity * lat_vel; front_right = MIN(occupied(1900,-300,1200,600,0), occupied(800,-200,1000,400,0)); front_left = MIN(occupied(1900,300,1200,600,0), occupied(800,200,1000,400,0)); front_middle = occupied(1000,0,1400,400,0); side_right = occupied(350,-700,300,800,1); side_left = occupied(350,700,300,800,1); back_right = occupied(-200,-700,200,800,1); back_left = occupied(-200, 700,200,800,1); front = MIN(front_right,front_left); if (front_right < 4000) sfDrawCenteredRect(front_right, -300, 100, 600); if (front_left < 4000) sfDrawCenteredRect(front_left , 300, 100, 600); if (side_right < 4000) sfDrawCenteredRect(350, -side_right, 300, 100); if (side_left < 4000) sfDrawCenteredRect(350, side_left, 300, 100); front_diff = front_right - front_left; right_diff = (back_right > 2000) ? 0 : back_right - side_right; left_diff = (back_left > 2000) ? 0 : back_left - side_left; ob_right_side = MAX(straight_down(side_right,400,safe_right), (right_diff <= 0) ? 0.0 : straight_down(400*(side_right+lat_vel)/MIN(right_diff,300), 400, safe_front)); ob_left_side = MAX(straight_down(side_left,400,safe_left), (left_diff <= 0) ? 0.0 : straight_down(400*(side_left-lat_vel)/MIN(left_diff,300), 400,safe_front)); ob_near = (front > 4000) ? 0.0 : straight_down(front, safe_front, 2700.0); ob_in_front = (front > 4000) ? 0.0 : MAX(straight_down(front_middle,1000.0,2000.0), MAX(straight_down(front,800.0,1600.0), MIN(straight_down(front,0.0,2500.0), straight_down(ABS(front_diff),0.0,300.0)))); ob_angled_right = MIN(up_straight(-front_diff,250.0,350.0), MAX(straight_down(-front_diff,40.0,300.0), straight_down(front,400,2500.0))); ob_angled_left = MIN(up_straight(front_diff,250.0,350.0), MAX(straight_down(front_diff,40.0,300.0), straight_down(front,400.0,2500.0))); ob_straight = (front > 4000) ? 0.0 : straight_down(ABS(front_diff),250.0,350.0); too_fast_for_ob = f_greater(sfRobot.tv, speed, 200.0); approaching_ob = MIN(up_straight(sfRobot.tv, 0.0, speed), MAX(ob_near, MAX(ob_right_side, ob_left_side))); if (preferred_turn == 0) { if ((side_right > 4000) == (side_left > 4000)) { if (last_turn == -1) { if (front_diff > 30) preferred_turn = TURN_RIGHT; else preferred_turn = TURN_LEFT; } else { if (front_diff < -30) preferred_turn = TURN_LEFT; else preferred_turn = TURN_RIGHT; } } else if (side_right > 4000) preferred_turn = TURN_RIGHT; else if (side_left > 4000) preferred_turn = TURN_LEFT; } last_turn = preferred_turn; if (preferred_turn == 0) preferred_turn = TURN_LEFT; turning_depth = MAX(5.0,12.0*(up_straight(sfRobot.tv,100.0,500.0))); caution_speed = speed; ob_caution = f_and(too_fast_for_ob, f_or(ob_in_front, f_and(ob_left_side,ob_right_side))); ob_right = f_or(ob_right_side, f_and(ob_angled_right, f_and(ob_near, f_not(ob_right_side)))); ob_left = f_or(ob_left_side, f_and(ob_angled_left, f_and(ob_near, f_not(ob_left_side)))); ob_front = f_and(ob_in_front, /* was ob_straight... */ f_and(f_not(ob_left_side), f_not(ob_right_side))); ko_act = MIN(sensitivity*10.0, up_straight(approaching_ob, MAX(0.0,0.9-sensitivity), 1.0/MAX(sensitivity,1.0))); Activity Turn ko_act Speed ko_act Progress 1.0 Goal 0.0 EndBehavior /* * GO TO POS * * PURPOSE: To go to an X,Y position * PARAMETERS: speed, target-point, radius * */ #define GP_TURN_FACTOR 6.0 float gt_max_speed = 400.0; BeginBehavior sfGoToPos Params sfFLOAT speed /* how fast we go */ sfPTR pos /* goal position point */ sfFLOAT radius /* success radius (mm) */ Rules Goal_speed If gt_too_fast Or gt_too_slow Then Speed gt_speed Pos_on_left If gt_pos_on_left Then Turn Left gt_turn_amount Pos_on_right If gt_pos_on_right Then Turn Right gt_turn_amount Pos_here If gt_pos_achieved Then Speed 0.0 Angle_way_off If gt_angle_way_off Then Speed 0.0 Update float x, y, phi, delta; point *p; int lhs_clear, rhs_clear; float ach, left, right, bleft, bright, wayoff; float too_slow, too_fast; gt_speed = MIN(gt_max_speed,speed); too_fast = f_greater(sfTargetVel(),gt_speed,gt_speed*.2); too_slow = f_smaller(sfTargetVel(),gt_speed,gt_speed*.2); p = (point *)pos; /* get target position */ x = p->x; y = p->y; phi = sfPointPhi(p); delta = sfTargetHead(); if (phi < 0.0) { if (delta > phi) delta = delta - phi; else delta = 0.0; } else { if (delta < phi) delta = phi - delta; else delta = 0.0; } gt_turn_amount = GP_TURN_FACTOR * f_not(f_near(delta,6.0)); rhs_clear = occupied(300,-600,400,600,1); lhs_clear = occupied(300,600,400,600,1); ach = straight_down(MAX(ABS(x),ABS(y)),radius,radius * 0.5); left = up_straight(phi,10.0,30.0); right = up_straight(-phi,10.0,30.0); bleft = straight_down(lhs_clear,100.0,300.0); bright = straight_down(rhs_clear,100.0,300.0); wayoff = up_straight(ABS(phi),50.0,70.0); gt_pos_on_left = f_and(left,f_and(f_not(ach), f_or(wayoff,f_not(bleft)))); gt_pos_on_right = f_and(right,f_and(f_not(ach), f_or(wayoff,f_not(bright)))); gt_pos_achieved = f_very(f_very(ach)); gt_angle_way_off = wayoff; gt_too_slow = f_and(too_slow, f_and(f_not(ach), f_not(wayoff))); gt_too_fast = f_and(too_fast, f_not(wayoff)); Activity Speed 1.0 Turn 1.0 Progress 1.0 Goal ach EndBehavior /* * ATTEND AT POS * * very similar to GO TO POS, but keeps a standoff * * PURPOSE: To go near an X,Y position * PARAMETERS: speed, target-point, radius * */ static float at_max_speed = 400.0; #define GP_TURN_FACTOR 6.0 BeginBehavior sfAttendAtPos Params sfFLOAT speed sfPTR pos sfFLOAT radius Rules Approach_speed If at_too_slow Or at_too_fast Then Speed at_speed Pos_on_left If at_pos_on_left Then Turn Left at_turn_amount Pos_on_right If at_pos_on_right Then Turn Right at_turn_amount Pos_here If at_pos_achieved Then Speed 0.0 Angle_way_off If at_angle_way_off Then Speed 0.0 Update float x, y, phi, delta; point *p; int lhs_clear, rhs_clear; float ach, left, right, bleft, bright, wayoff; float too_slow, too_fast; at_speed = MIN(speed,at_max_speed); too_fast = f_greater(sfTargetVel(),speed,speed*.2); too_slow = f_smaller(sfTargetVel(),at_speed,at_speed*.2); p = (point *)pos; /* get target position */ x = p->x; y = p->y; phi = sfPointPhi(p); delta = sfTargetHead(); if (phi < 0.0) { if (delta > phi) delta = delta - phi; else delta = 0.0; } else { if (delta < phi) delta = phi - delta; else delta = 0.0; } at_turn_amount = GP_TURN_FACTOR * f_not(f_near(delta,6.0)); rhs_clear = occupied(300,-600,400,600,1); lhs_clear = occupied(300,600,400,600,1); ach = straight_down(MAX(ABS(x),ABS(y)),radius,radius * 0.5); left = up_straight(phi,10.0,30.0); right = up_straight(-phi,10.0,30.0); bleft = straight_down(lhs_clear,100.0,300.0); bright = straight_down(rhs_clear,100.0,300.0); wayoff = up_straight(ABS(phi),50.0,70.0); at_pos_on_left = f_and(left, f_or(wayoff,f_not(bleft))); at_pos_on_right = f_and(right, f_or(wayoff,f_not(bright))); at_pos_achieved = f_very(f_very(ach)); at_angle_way_off = wayoff; at_too_slow = f_and(too_slow, f_and(f_not(ach), f_not(wayoff))); at_too_fast = f_and(too_fast, f_not(wayoff)); Activity Speed 1.0 Turn 1.0 Progress 1.0 Goal ach EndBehavior /* * FOLLOW (a lane) * Follows a point artifact (corridor, usually), keeping in the center * Goes to a target point * * Parameters: * FOL_LANE (lane artifact) * FOL_TARGET (goal point artifact) * FOL_RIGHT_EDGE (distance to right boundary) * FOL_LEFT_EDGE (distance to left boundary) * FOL_IN_LANE_SPEED * FOL_OFF_LANE_SPEED * * The real hard part of this is determining all the angles * */ boolean fol_draw_lane = true; BeginBehavior sfFollow Params sfPTR f_lane sfPTR f_target sfFLOAT right_edge sfFLOAT left_edge sfFLOAT in_lane_speed sfFLOAT off_lane_speed sfFLOAT turn_ratio Rules Follow_speed If fol_too_slow Or fol_too_fast Then Speed fol_speed Angled_right If fol_angled_right Then Turn Left fol_angle_param Angled_left If fol_angled_left Then Turn Right fol_angle_param Wayoff_right If fol_wayoff_right Then Turn Left fol_angle_param Wayoff_left If fol_wayoff_left Then Turn Right fol_angle_param On_left If fol_on_left Then Turn Right fol_close_param On_right If fol_on_right Then Turn Left fol_close_param Init fol_angle_param = 8.0; /* amount to turn if angled wrong */ Update point *lane, *target; point p; float offset, width, inner_border, outer_border, distance, angle, theta; int lhs_occ, rhs_occ; float near_lane, angled_right, angled_left, angled_lcenter, angled_rcenter, aligned, on_right, on_left, in_lane, rhs_clear, lhs_clear, ach; /* lane setup */ lane = f_lane; target = f_target; offset = (left_edge - right_edge) * 0.5; width = left_edge + right_edge; inner_border = width * 0.125; outer_border = width * 0.5; theta = lane->th; /* points towards target from robot */ if (sfPointXoPoint(lane,target) < sfPointXo(lane)) theta = sfAddAngle(theta,180); angle = sfNorm2Angle(theta); p.x = lane->x; p.y = lane->y; p.th = theta; distance = offset + sfPointYo(&p); /* what's near */ lhs_occ = occupied(300, 700, 600, 800, 1); rhs_occ = occupied(300, -700, 600, 800, 1); /* fuzzy vars */ near_lane = straight_down(ABS(distance),outer_border,width*1.5); angled_right = up_straight(angle,0.0,30); angled_left = up_straight(-angle,0.0,30); aligned = straight_down(ABS(angle),30,60); on_right = up_straight(-distance,inner_border,outer_border); on_left = up_straight(distance,inner_border,outer_border); if (on_left > on_right) theta = sfAddAngle(theta,90); else theta = sfSubAngle(theta,90); angled_rcenter = straight_down(theta, 120, 180); angled_lcenter = up_straight(theta, 180, 240); in_lane = f_not(f_or(on_right, f_or(on_left, f_or(angled_right,angled_left)))); rhs_clear = up_straight((float)rhs_occ,500.0,1000.0); lhs_clear = up_straight((float)lhs_occ,500.0,1000.0); fol_speed = f_and(in_lane,aligned) * in_lane_speed + f_not(f_and(in_lane,aligned)) * off_lane_speed; ach = sfPointXo(lane) - sfPointXoPoint(lane,target); /* achievment */ ach = straight_down(ABS(ach),100.0,500.0); fol_speed = MIN(fol_speed,CV_MAX_SPEED); fol_too_slow = f_smaller(sfTargetVel(),fol_speed,fol_speed*.2); fol_too_fast = f_greater(sfTargetVel(),fol_speed,fol_speed*.2); if (angled_left > angled_right) sfPreferredTurnDir = TURN_RIGHT; else sfPreferredTurnDir = TURN_LEFT; fol_angled_left = f_and(rhs_clear, f_and(angled_left,near_lane)); fol_angled_right = f_and(lhs_clear, f_and(angled_right,near_lane)); fol_on_left = f_and(rhs_clear, f_and(on_left, f_and(near_lane, f_not(angled_right)))); fol_on_right = f_and(lhs_clear, f_and(on_right, f_and(near_lane, f_not(angled_left)))); /* fol_wayoff_left = f_and(rhs_clear, f_and(f_not(near_lane), angled_rcenter)); fol_wayoff_right = f_and(lhs_clear, f_and(f_not(near_lane), angled_lcenter)); */ fol_wayoff_left = f_and(rhs_clear, f_not(near_lane)); fol_wayoff_right = f_and(lhs_clear, f_not(near_lane)); if (fol_wayoff_left > 0.5) sfPreferredTurnDir = TURN_RIGHT; if (fol_wayoff_right > 0.5) sfPreferredTurnDir = TURN_LEFT; /* parameters */ fol_close_param = turn_ratio * fol_angle_param; fl_act = f_and(f_not(ach), f_or(fol_angled_left, f_or(fol_angled_right, f_or(fol_wayoff_left, f_or(fol_wayoff_right, f_or(fol_on_left,fol_on_right)))))); if (fol_draw_lane) /* draw the controlling lane inner width */ draw_lane_to_target(lane,target,inner_border*2.0); /* to the target point */ Activity Turn fl_act Speed Not ach Goal ach Progress 1.0 EndBehavior /* * FOLLOW_DOOR * * Go through a door to a distance inside/outside the door. * * Params: * 0. Door point * 1. Direction: IN or OUT * 2. Distance inside/outside to achieve */ point fol_door_point; float fol_door_speed = 200.0; BeginBehavior sfFollowDoor Params sfPTR f_door sfINT f_dir sfFLOAT dist Rules Follow_speed If fol_too_slow Or fol_too_fast Then Speed fol_speed Angled_right If fol_angled_right Then Turn Left fol_angle_param Angled_left If fol_angled_left Then Turn Right fol_angle_param Wayoff_right If fol_wayoff_right Then Turn Left fol_angle_param Wayoff_left If fol_wayoff_left Then Turn Right fol_angle_param On_left If fol_on_left Then Turn Right fol_close_param On_right If fol_on_right Then Turn Left fol_close_param Init door *d = f_door; dir dr = f_dir; /* door setup */ fol_door_point.y = 0.0; if (dr == D_IN) /* door direction vector points in */ fol_door_point.x = dist; else fol_door_point.x = -dist; sfUnchangeVP((point *)d,&fol_door_point,&fol_door_point); sfAddPointCheck(&fol_door_point); fol_angle_param = 8.0; /* amount to turn if angled wrong */ Update point *lane, *target; point p; float offset, width, inner_border, outer_border, distance, angle, theta; int lhs_occ, rhs_occ; float near_lane, angled_right, angled_left, angled_lcenter, angled_rcenter, aligned, on_right, on_left, in_lane, rhs_clear, lhs_clear, ach, left_edge, right_edge, in_lane_speed, off_lane_speed, turn_ratio; /* lane setup */ lane = (point *)f_door; target = &fol_door_point; left_edge = ((door *)lane)->width * 0.15; right_edge = ((door *)lane)->width * 0.15; in_lane_speed = fol_door_speed; off_lane_speed = fol_door_speed - 50.0; turn_ratio = 0.7; offset = (left_edge - right_edge) * 0.5; width = left_edge + right_edge; inner_border = width * 0.125; outer_border = width * 0.5; theta = lane->th; /* points towards target from robot */ if (sfPointXoPoint(lane,target) < sfPointXo(lane)) theta = sfAddAngle(theta,180); angle = sfNorm2Angle(theta); p.x = lane->x; p.y = lane->y; p.th = theta; distance = offset + sfPointYo(&p); /* what's near */ lhs_occ = occupied(300, 700, 600, 800, 1); rhs_occ = occupied(300, -700, 600, 800, 1); /* fuzzy vars */ near_lane = straight_down(ABS(distance),outer_border,width*1.5); angled_right = up_straight(angle,0.0,30); angled_left = up_straight(-angle,0.0,30); aligned = straight_down(ABS(angle),30,60); on_right = up_straight(-distance,inner_border,outer_border); on_left = up_straight(distance,inner_border,outer_border); if (on_left > on_right) theta = sfAddAngle(theta,90); else theta = sfSubAngle(theta,90); angled_rcenter = straight_down(theta, 120, 180); angled_lcenter = up_straight(theta, 180, 240); in_lane = f_not(f_or(on_right, f_or(on_left, f_or(angled_right,angled_left)))); rhs_clear = up_straight((float)rhs_occ,500.0,1000.0); lhs_clear = up_straight((float)lhs_occ,500.0,1000.0); fol_speed = f_and(in_lane,aligned) * in_lane_speed + f_not(f_and(in_lane,aligned)) * off_lane_speed; ach = sfPointXo(lane) - sfPointXoPoint(lane,target); /* achievment */ ach = straight_down(ABS(ach),100.0,500.0); fol_speed = MIN(fol_speed,CV_MAX_SPEED); fol_too_slow = f_smaller(sfTargetVel(),fol_speed,fol_speed*.2); fol_too_fast = f_greater(sfTargetVel(),fol_speed,fol_speed*.2); if (angled_left > angled_right) sfPreferredTurnDir = TURN_RIGHT; else sfPreferredTurnDir = TURN_LEFT; fol_angled_left = f_and(rhs_clear, f_and(angled_left,near_lane)); fol_angled_right = f_and(lhs_clear, f_and(angled_right,near_lane)); fol_on_left = f_and(rhs_clear, f_and(on_left, f_and(near_lane, f_not(angled_right)))); fol_on_right = f_and(lhs_clear, f_and(on_right, f_and(near_lane, f_not(angled_left)))); /* fol_wayoff_left = f_and(rhs_clear, f_and(f_not(near_lane), angled_rcenter)); fol_wayoff_right = f_and(lhs_clear, f_and(f_not(near_lane), angled_lcenter)); */ fol_wayoff_left = f_and(rhs_clear, f_not(near_lane)); fol_wayoff_right = f_and(lhs_clear, f_not(near_lane)); if (fol_wayoff_left > 0.5) sfPreferredTurnDir = TURN_RIGHT; if (fol_wayoff_right > 0.5) sfPreferredTurnDir = TURN_LEFT; /* parameters */ fol_close_param = turn_ratio * fol_angle_param; fl_act = f_and(f_not(ach), f_or(fol_angled_left, f_or(fol_angled_right, f_or(fol_wayoff_left, f_or(fol_wayoff_right, f_or(fol_on_left,fol_on_right)))))); if (fol_draw_lane) /* draw the controlling lane inner width */ draw_lane_to_target(lane,target,inner_border*2.0); /* to the target point */ Activity Turn fl_act Speed Not ach Goal ach Progress 1.0 EndBehavior /* * FOLLOW_CORRIDOR * * Follow a corridor to a specified X-position along it. * A generalized follow, with a narrow lane for the doorway * width. * * Param: * 0. Corridor point * 1. Target point */ float fol_corr_speed = 250.0; BeginBehavior sfFollowCorridor Params sfPTR f_corr sfPTR f_target Rules Follow_speed If (fol_too_slow Or fol_too_fast) And aligned Then Speed fol_speed Off_speed If Not aligned Then Speed 50.0 Angled_right If fol_angled_right Then Turn Left fol_angle_param Angled_left If fol_angled_left Then Turn Right fol_angle_param Wayoff_right If fol_wayoff_right Then Turn Left fol_angle_param Wayoff_left If fol_wayoff_left Then Turn Right fol_angle_param On_left If fol_on_left Then Turn Right fol_close_param On_right If fol_on_right Then Turn Left fol_close_param Init fol_angle_param = 8.0; /* amount to turn if angled wrong */ Update point *lane, *target; point p; float offset, width, inner_border, outer_border, distance, angle, theta; int lhs_occ, rhs_occ; float near_lane, angled_right, angled_left, angled_lcenter, angled_rcenter, aligned, on_right, on_left, in_lane, rhs_clear, lhs_clear, ach, left_edge, right_edge, in_lane_speed, off_lane_speed, turn_ratio; /* lane setup */ lane = (point *)f_corr; target = (point *)f_target; left_edge = 50.0 + ((corridor *)lane)->width * 0.5; right_edge = 50.0 + ((corridor *)lane)->width * 0.5; in_lane_speed = fol_corr_speed; off_lane_speed = fol_corr_speed - 50.0; turn_ratio = 0.5; offset = (left_edge - right_edge) * 0.5; width = left_edge + right_edge; inner_border = width * 0.125; outer_border = width * 0.5; theta = lane->th; /* points towards target from robot */ if (sfPointXoPoint(lane,target) < sfPointXo(lane)) theta = sfAddAngle(theta,180); angle = sfNorm2Angle(theta); p.x = lane->x; p.y = lane->y; p.th = theta; distance = offset + sfPointYo(&p); /* what's near */ lhs_occ = occupied(300, 700, 600, 800, 1); rhs_occ = occupied(300, -700, 600, 800, 1); /* fuzzy vars */ near_lane = straight_down(ABS(distance),outer_border,width*1.5); angled_right = up_straight(angle,0.0,30); angled_left = up_straight(-angle,0.0,30); aligned = straight_down(ABS(angle),30,60); on_right = up_straight(-distance,inner_border,outer_border); on_left = up_straight(distance,inner_border,outer_border); if (on_left > on_right) theta = sfAddAngle(theta,90); else theta = sfSubAngle(theta,90); angled_rcenter = straight_down(theta, 120, 180); angled_lcenter = up_straight(theta, 180, 240); in_lane = f_not(f_or(on_right, f_or(on_left, f_or(angled_right,angled_left)))); rhs_clear = up_straight((float)rhs_occ,500.0,1000.0); lhs_clear = up_straight((float)lhs_occ,500.0,1000.0); fol_speed = f_and(in_lane,aligned) * in_lane_speed + f_not(f_and(in_lane,aligned)) * off_lane_speed; ach = sfPointXo(lane) - sfPointXoPoint(lane,target); /* achievment */ ach = straight_down(ABS(ach),100.0,500.0); if (angled_left > angled_right) sfPreferredTurnDir = TURN_RIGHT; else sfPreferredTurnDir = TURN_LEFT; fol_angled_left = f_and(rhs_clear, f_and(angled_left,near_lane)); fol_angled_right = f_and(lhs_clear, f_and(angled_right,near_lane)); fol_on_left = f_and(rhs_clear, f_and(on_left, f_and(near_lane, f_not(angled_right)))); fol_on_right = f_and(lhs_clear, f_and(on_right, f_and(near_lane, f_not(angled_left)))); fol_wayoff_left = f_and(rhs_clear, f_and(f_not(near_lane), angled_rcenter)); fol_wayoff_right = f_and(lhs_clear, f_and(f_not(near_lane), angled_lcenter)); /* fol_wayoff_left = f_and(rhs_clear, f_not(near_lane)); fol_wayoff_right = f_and(lhs_clear, f_not(near_lane)); */ if (fol_wayoff_left > 0.5) sfPreferredTurnDir = TURN_RIGHT; if (fol_wayoff_right > 0.5) sfPreferredTurnDir = TURN_LEFT; fol_speed = MIN(fol_speed,CV_MAX_SPEED); fol_too_slow = f_smaller(sfTargetVel(),fol_speed,fol_speed*.2); fol_too_fast = f_greater(sfTargetVel(),fol_speed,fol_speed*.2); /* parameters */ fol_close_param = turn_ratio * fol_angle_param; fl_act = f_and(f_not(ach), f_or(fol_angled_left, f_or(fol_angled_right, f_or(fol_wayoff_left, f_or(fol_wayoff_right, f_or(fol_on_left,fol_on_right)))))); if (fol_draw_lane) /* draw the controlling lane inner width */ draw_lane_to_target(lane,target,inner_border*2.0); /* to the target point */ Activity Turn fl_act Speed Not ach Goal ach Progress 1.0 EndBehavior /* * TURN_TO * * Turn to face a given target point within delta degrees * * Params * 0. Target point * 1. Delta angle of acceptance (in radians) * 2. Turning speed */ BeginBehavior sfTurnTo Params sfPTR t_target sfFLOAT delta sfFLOAT turn_fraction Rules Totally_off If tt_totally_off Then Turn Left tt_turn_amount Heading_left If tt_heading_left Then Turn Right tt_turn_amount Heading_right If tt_heading_right Then Turn tt_turn_dir tt_turn_amount Slow_down If tt_too_fast Then Speed 0.0 Update point *target; float phi, dphi, ang, not_near, too_fast, too_right, too_left, heading_ok; target = (point *)t_target; /* target point */ phi = sfNorm2Angle(sfPointPhi(target)); tt_turn_dir = TURN_LEFT; if (phi > 10.0) tt_turn_dir = TURN_LEFT; else if (phi < -10.0) tt_turn_dir = TURN_RIGHT; dphi = (sfRobot.control > 10.0) ? 0.0 : sfRobot.control; dphi = sfSub2Angle(phi,dphi); ang = 0.0; if (phi > 0.0) { if (dphi > 0.0) ang = dphi; } else if (dphi < 0.0) ang = -dphi; not_near = f_not(f_eq(ang,0.0,10.0)); tt_turn_amount = 6.0 * not_near * turn_fraction; tt_too_fast = f_greater(sfTargetVel(), 0.0, 10.0); too_right = up_straight(phi,0.0,delta); too_left = up_straight((- phi),0.0,delta); tt_totally_off = up_straight(ABS(phi), delta*2.0, delta*8.0); heading_ok = straight_down(ABS(phi), delta*0.5, delta*2.0); tt_heading_left = f_and(too_left, f_not(tt_totally_off)); tt_heading_right = f_and(too_right, f_not(tt_totally_off)); /* tt_wrong_angle = f_and(tt_too_fast,f_not(heading_ok)); */ tt_act = f_or(tt_totally_off,f_or(too_right,too_left)); Activity Turn tt_act Speed tt_act Goal heading_ok Progress 1.0 EndBehavior /* * TURN_LEFT * * Keep turning left * * Params * Turning speed fraction -- 0.0 to 1.0 */ BeginBehavior sfTurnLeft Params sfFLOAT turn_fraction Rules Go_left If 1.0 Then Turn Left tl_turn_amount Slow_down If 1.0 Then Speed 0.0 Update tl_turn_amount = 8.0 * turn_fraction; Activity Turn 1.0 Speed 1.0 Progress 1.0 Goal 0.0 EndBehavior /* * Global initialization fn -- add behaviors as schemas */ void init_behaviors(void) { sfRegisterSchema("sfConstantVelocity", sfBEHAVIOR, sfConstantVelocity); sfRegisterSchema("sfStop", sfBEHAVIOR, sfStop); sfRegisterSchema("sfAvoidCollision", sfBEHAVIOR, sfAvoidCollision); sfRegisterSchema("sfStopCollision", sfBEHAVIOR, sfStopCollision); sfRegisterSchema("sfKeepOff", sfBEHAVIOR, sfKeepOff); sfRegisterSchema("sfGoToPos", sfBEHAVIOR, sfGoToPos); sfRegisterSchema("sfAttendAtPos", sfBEHAVIOR, sfAttendAtPos); sfRegisterSchema("sfFollow", sfBEHAVIOR, sfFollow); sfRegisterSchema("sfFollowDoor", sfBEHAVIOR, sfFollowDoor); sfRegisterSchema("sfFollowCorridor", sfBEHAVIOR, sfFollowCorridor); sfRegisterSchema("sfTurnTo", sfBEHAVIOR, sfTurnTo); sfRegisterSchema("sfTurnLeft", sfBEHAVIOR, sfTurnLeft); }