def drive_relative(x, y, robot):
'''
Drive relative to the robot's coordinate frame
inputs: x, y
'''
finished = False
#have we reached our goal?
if not (((x - 0.05) <= robot.x[0] <=
(x + 0.05)) and ((y - 0.05) <= robot.x[1] <= (y + 0.05))):
#We shouldn't need to contain this angle as it is simply for a driving system and PID
heading = (np.arctan2(y - robot.x[1],
x - robot.x[0])) - robot.x[2] #in radians
# the output of our pid represents a signed number depending on the direction we are turning
pid_return = abs(robot.PID.update(
heading)) #abs so that our motors dont travel in reverse direction
#limit to robots maximum angular velocity
if pid_return > robot.std_steer:
pid_return = robot.std_steer
pid_wheel = int(
robot.std_vel -
pid_return) #integer as our motor function only accepts an integer
#basic drive loop
if heading > 0.0:
#pass
Motors.driveMotors(pid_wheel, robot.std_vel)
if heading <= 0.0:
#pass
Motors.driveMotors(robot.std_vel, pid_wheel)
else:
Motors.driveMotors(0, 0)
return True
return False
def shutdown(robot):
robot.stream.stop()
print('[SLAMBOT] Shutting down...')
robot.client.sock.close()
Motors.driveMotors(0, 0)
sys.exit()
def run_localization(robot):
robot.odom.set_initial() #set initial odom
#robot.state = 20
if robot.state == 0:
#DRIVE TO CENTER OF MAP
path = drive_relative(0.5, 0.5, robot)
if path:
robot.state = 1
Motors.driveMotors(0, 0)
# robot.stored_theta = float(robot.u[2])
time.sleep(robot.dt)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 1:
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
#if len(robot.landmarks) > 0:
#robot.ekf_update(sensor) #call the ekf_update for each landmark
if sensor[2] == 4:
print('im here')
robot.state = 2
if len(robot.landmarks) >= 5:
robot.state = 100
Motors.driveMotors(40, 0)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 2:
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
#if len(robot.landmarks) > 0:
#robot.ekf_update(sensor) #call the ekf_update for each landmark
if sensor[2] == 0:
print('now here')
robot.state = 1
if len(robot.landmarks) >= 5:
robot.state = 100
Motors.driveMotors(0, 40)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 100:
Motors.driveMotors(0, 0)
time.sleep(robot.dt)
robot.send()
if robot.state == 20:
Motors.driveMotors(-30, 30)
if robot.u[2] > 1.57:
Motors.driveMotors(0, 0)
shutdown(robot)
def run_localization(robot):
robot.odom.set_initial() #set initial odom
#robot.state = 20
if robot.state == 0:
#DRIVE TO CENTER OF MAP
path = drive_relative(0.5, 0.5, robot)
if path:
robot.state = 1
Motors.driveMotors(0, 0)
# robot.stored_theta = float(robot.u[2])
time.sleep(robot.dt)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 1:
for i in range(5):
sensor = find_landmark(robot, i)
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
landmark_init(robot, sensor) #check for any new landmarks
if sensor[2] == 0:
robot.state = 2
Motors.driveMotors(40, 0)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 2:
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
if sensor[2] == 4:
robot.state = 3
Motors.driveMotors(0, 40)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 3:
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
if sensor[2] == 4:
robot.state = 3
if robot.u[2] > 0.8:
Motors.driveMotors(40, 0)
elif robot.u[2] < 0.6:
Motors.driveMotors(0, 40)
else:
Motors.driveMotors(0, 0)
robot.state = 4
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 4:
path = drive_relative(0.8, 0.8, robot)
if path:
robot.state = 5
Motors.driveMotors(0, 0)
# robot.stored_theta = float(robot.u[2])
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
if sensor[2] == 4:
robot.state = 5
time.sleep(robot.dt)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 5:
for i in range(5):
sensor = find_landmark(robot, i)
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
landmark_init(robot, sensor) #check for any new landmarks
if sensor[2] == 0:
robot.state = 6
Motors.driveMotors(40, 0)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 6:
for i in range(5):
sensor = find_landmark(robot, i)
landmark_init(robot, sensor) #check for any new landmarks
if (len(robot.landmarks) > 0) and sensor[0] > 0:
robot.ekf_update(
sensor) #call the ekf_update for each landmark
if sensor[2] == 4:
robot.state = 5
Motors.driveMotors(0, 40)
time.sleep(robot.dt)
Motors.driveMotors(0, 0)
update_motion_model()
robot.ekf_predict() #run the prediction step
if robot.state == 100:
Motors.driveMotors(0, 0)
time.sleep(robot.dt)
robot.send()
if robot.state == 20:
Motors.driveMotors(-30, 30)
if robot.u[2] > 1.57:
Motors.driveMotors(0, 0)
shutdown(robot)
