Embedded Files
State machine
State machine
Key Sub tasks pseudocode
Key Sub tasks pseudocode
Bumper Following (hug[Left/Right]Wall)
Bumper Following (hug[Left/Right]Wall)
hugLeftWall( ideal forward position, ultrasonic threshold, bias) :
// continuously runs while driving against wall, if returns true: we've reached another wall, else returns false
ideal forward position adjusted = ideal forward position - bias
if correct_orientation_flag is true (correction needed):
turn angle from savedPosition to ideal forward position adjusted (angle_to_turn)
set correct_orientation_flag to false
else
if front US threshold reached:
return true
// to check orientation
if currentPosition < ideal forward position adjusted - left threshold :
// we are turning away from wall
set correct_orientation_flag to true
save current position as savedPosition
calculate angle_to_turn = (ideal forward position adjusted - currentPosition)*0.035*0.6 <- encoder ticks to angle
else if currentPosition > ideal forward position adjusted + right threshold:
// we are turning towards wall
set correct_orientation_flag to true
save current position as savedPosition
calculate angle_to_turn = (ideal forward position adjusted - currentPosition)*0.035*0.6
return false //otherwise
Wall Ultrasonic Following (MOVE_TO_WALL, CORRECT_TO_RIGHT, CORRECT_TO_LEFT)
Wall Ultrasonic Following (MOVE_TO_WALL, CORRECT_TO_RIGHT, CORRECT_TO_LEFT)
In state: MOVE_TO_WALL:
check left ultrasonic (US) distance
if correct_timer (1500 ms) expired:
if left US distance < 26 cm:
reset correct_turn_timer
change state to CORRECT_TO_RIGHT
else if left US distance > 38 cm:
reset correct_turn_timer
change state to CORRECT_TO_LEFT
else:
set motor speeds to driveSpeedA, driveSpeedB (regular driving straight forwards)
drive motors forward
else:
set motor speeds to driveSpeedA, driveSpeedB (regular driving straight forwards)
drive motors forward
if front US distance < wall threshold distance:
stop motors
change to next state
In state: CORRECT_TO_RIGHT:
if correct_turn_timer has NOT expired:
change motor speeds to 1*driveSpeedA, 0.85*driveSpeedB (adjust speeds with faster left motor)
drive motors forward
else:
change state to MOVE_TO_WALL
reset correct_timer
In state: CORRECT_TO_LEFT:
if correct_turn_timer has NOT expired:
change motor speeds to 0.85*driveSpeedA, 1*driveSpeedB (adjust speeds with faster right motor)
drive motors forward
else:
change state to MOVE_TO_WALL
reset correct_timer
FUll Code Listing
FUll Code Listing
Page updated
Google Sites
Report abuse