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)