def update_motion_model():
robot.send() #send the robot data before the update
current_pose, delta_d = robot.odom.update(
robot.x[2]) #update the odometry data
robot.update_pose(current_pose) #update the robot's pose
update_motion_jacobians(current_pose,
delta_d) #update the motion jacobians
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)