def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
class MyFrontEnd(FrontEnd):
def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
def mouseup(self, x, y, button):
pass
def draw(self, surface):
# draw occupancy map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def update(self, time_delta):
# get sonar distance
if self.sparki.port == '':
# simulate rangefinder
T_sonar_map = self.robot.get_robot_map_transform(
) * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# read rangefinder
self.robot.sonar_distance = self.sparki.dist
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors(
)
# send command
self.sparki.send_command(left_speed, left_dir, right_speed, right_dir)
# update robot position
self.robot.update(time_delta)
def __init__(self,sparki,omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self,self.omap.width,self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width*0.5
self.robot.y = self.omap.height*0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
#is robot at goal
self.goalReached = True
#PID control constants
self.K_linear = 1
self.K_angular = 1
self.distanceThreshold = 1
def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
# Goal location in the robot's reference frame
self.goal = vec(self.robot.x, self.robot.y)
# Angle to goal
self.angle = 0
# Distance to goal
self.distance = 0
class MyFrontEnd(FrontEnd):
def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
# Goal location in the robot's reference frame
self.goal = vec(self.robot.x, self.robot.y)
# Angle to goal
self.angle = 0
# Distance to goal
self.distance = 0
def mouseup(self, x, y, button):
"""
:param x: mouse x coordinate
:param y: mouse y coordinate
:param button: mouse button
"""
self.goal = vec(x, y)
def draw(self, surface):
# draw occupancy map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def update(self, time_delta):
T_map_robot = self.robot.get_map_robot_transform()
# get angle and distance to the goal
goal_rf = mul(T_map_robot, self.goal)
self.angle = math.atan2(goal_rf[1], goal_rf[0])
self.distance = math.sqrt((goal_rf[0] - 0) ** 2 + (goal_rf[1] - 0) ** 2)
# Calculate linear and angular velocity as holonomic navigation
if self.angle > angular_threshold:
self.robot.lin_vel = 0
self.robot.ang_vel = K_angular * self.angle
elif self.distance > linear_threshold:
self.robot.lin_vel = K_linear * self.distance
self.robot.ang_vel = 0
else:
self.robot.lin_vel = 0
self.robot.ang_vel = 0
# get sonar distance
if self.sparki.port == '':
# simulate rangefinder
T_sonar_map = self.robot.get_robot_map_transform() * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# read rangefinder
self.robot.sonar_distance = self.sparki.dist
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors()
# send command
self.sparki.send_command(left_speed, left_dir, right_speed, right_dir)
# update robot position
self.robot.update(time_delta)
class MyFrontEnd(FrontEnd):
def __init__(self,sparki,omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self,self.omap.width,self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width*0.5
self.robot.y = self.omap.height*0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
#is robot at goal
self.goalReached = True
#PID control constants
self.K_linear = .5
self.K_angular = .5
self.distanceThreshold = 1
self.switchToBugDist = 30
def mouseup(self,x,y,button):
self.robot.set_map_goal(x,y)
self.goalReached = False
def draw(self,surface):
# draw occupancy map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def get_sonar_distance(self):
# get sonar distance
if self.sparki.port == '':
# simulate rangefinder
T_sonar_map = self.robot.get_robot_map_transform() * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# read rangefinder
self.robot.sonar_distance = self.sparki.dist
print('Sonar Distance (%d)'%(self.robot.sonar_distance))
def update(self,time_delta):
self.get_sonar_distance()
"""
PID navigation
Calculate angle to goal set linear and angular vel
Switch to bug if sonar reading is less than tolarance
Bug is defined as turning setting angular velocity to left
"""
if not self.goalReached:
if self.switchToBugDist > self.robot.sonar_distance and not self.robot.sonar_distance == 0: #bug if too close
self.robot.ang_vel = .3
self.robot.lin_vel = 0
elif self.robot.goalXr > self.distanceThreshold or self.robot.goalXr < -self.distanceThreshold or self.robot.goalYr > self.distanceThreshold or self.robot.goalYr < -self.distanceThreshold:
theta = math.atan2(self.robot.goalYr,self.robot.goalXr)
self.robot.lin_vel = self.K_linear * self.robot.goalXr
self.robot.ang_vel = self.K_angular * theta * self.sweep(time_delta)
else:
self.robot.ang_vel = 0
self.robot.lin_vel = 0
self.goalReached = True
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors()
# send command
#self.sparki.send_command(left_speed,left_dir,right_speed,right_dir)
# update robot position
self.robot.update(time_delta)
# update robot goal
self.robot.set_robot_goal()
def sweep(self, time_delta):
"""
Sweeps to the left and the right to see
where the farthest sonar distance lies
Returns -1 for left and 1 for right
"""
#Temporarily store linear and angular velocity
tempLin = self.robot.lin_vel
tempAng = self.robot.ang_vel
#Set linear velocity to 0 temporarily
self.robot.lin_vel = 0
#Set angular velocity to negative values
self.robot.ang_vel = -self.K_angular
#Update the robot's angle
self.robot.update(time_delta)
#Save the sonar distance
self.get_sonar_distance()
left_distance = self.robot.sonar_distance
#Change the angular velocity to twice the positive value
#to get to the other "side" of the angle
self.robot.ang_vel = 2 * self.K_angular
#Update the robot's angle
self.robot.update(time_delta)
#Save the sonar distance
self.get_sonar_distance()
right_distance = self.robot.sonar_distance
#Set the angular velocity to 1 * negative angular velocity
#to bring the robot back to its original position
self.robot.ang_vel = -self.K_angular
#Update the robot's angle
self.robot.update(time_delta)
#Reset the sonar distance
self.get_sonar_distance()
#Reload the initial velocity values
self.robot.lin_vel = tempLin
self.robot.ang_vel = tempAng
#Check which distance is smaller
if left_distance == 0 or right_distance == 0:
if left_distance <= right_distance:
return 1
else:
return -1
else:
if left_distance < right_distance:
return 1 * (left_distance / right_distance)
elif left_distance > right_distance:
return -1 * (right_distance / left_distance)
else:
return 1
class MyFrontEnd(FrontEnd):
def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
#is robot at goal
self.goalReached = True
#PID control constants
self.K_linear = .5
self.K_angular = .5
self.distanceThreshold = 1
self.switchToBugDist = 30
def mouseup(self, x, y, button):
self.robot.set_map_goal(x, y)
self.goalReached = False
def draw(self, surface):
# draw occupancy map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def update(self, time_delta):
# get sonar distance
if self.sparki.port == '':
# simulate rangefinder
T_sonar_map = self.robot.get_robot_map_transform(
) * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# read rangefinder
self.robot.sonar_distance = self.sparki.dist
print('Sonar Distance (%d)' % (self.robot.sonar_distance))
"""
PID navigation
Calculate angle to goal set linear and angular vel
Switch to bug if sonar reading is less than tolarance
Bug is defined as turning setting angular velocity to left
"""
if not self.goalReached:
if self.switchToBugDist > self.robot.sonar_distance and not self.robot.sonar_distance == 0: #bug if too close
self.robot.ang_vel = .3
self.robot.lin_vel = 0
elif self.robot.goalXr > self.distanceThreshold or self.robot.goalXr < -self.distanceThreshold or self.robot.goalYr > self.distanceThreshold or self.robot.goalYr < -self.distanceThreshold:
theta = math.atan2(self.robot.goalYr, self.robot.goalXr)
self.robot.lin_vel = self.K_linear * self.robot.goalXr
self.robot.ang_vel = self.K_angular * theta
else:
self.robot.ang_vel = 0
self.robot.lin_vel = 0
self.goalReached = True
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors(
)
# send command
#self.sparki.send_command(left_speed,left_dir,right_speed,right_dir)
# update robot position
self.robot.update(time_delta)
# update robot goal
self.robot.set_robot_goal()
class MyFrontEnd(FrontEnd):
def __init__(self, sparki, omap_path):
self.omap = OccupancyMap(omap_path)
FrontEnd.__init__(self, self.omap.width, self.omap.height)
self.sparki = sparki
self.robot = Robot()
# center robot
self.robot.x = self.omap.width * 0.5
self.robot.y = self.omap.height * 0.5
# zero out robot velocities
self.robot.lin_vel = 0
self.robot.ang_vel = 0
# bool check if robot is at goal
self.goalReached = True
def mouseup(self, x, y, button):
""" Gets mouse location on click prints goal passes to robot class """
print('mouse clicked at %d, %d' % (x, y))
self.robot.set_map_goal(x, y)
self.goalReached = False
def draw(self, surface):
# draw occupancy map
self.omap.draw(surface)
# draw robot
self.robot.draw(surface)
def update(self, time_delta):
# get sonar distance
if self.sparki.port == '':
# simulate rangefinder
T_sonar_map = self.robot.get_robot_map_transform(
) * self.robot.get_sonar_robot_transform()
self.robot.sonar_distance = self.omap.get_first_hit(T_sonar_map)
else:
# read rangefinder
self.robot.sonar_distance = self.sparki.dist
""" Holonomic Navigation
If 1 < Y or Y < -1 then turn
elseif x > 0 then move forward
else goal reached stop motors
"""
if not self.goalReached:
if self.robot.goalYr > 1.:
self.robot.ang_vel = .3
self.robot.lin_vel = 0
elif self.robot.goalYr < -1.:
self.robot.ang_vel = -.3
self.robot.lin_vel = 0
elif self.robot.goalXr > 0.:
self.robot.ang_vel = 0
self.robot.lin_vel = 10
else:
self.goalReached = True
self.robot.ang_vel = 0
self.robot.lin_vel = 0
# calculate motor settings
left_speed, left_dir, right_speed, right_dir = self.robot.compute_motors(
)
# send command
self.sparki.send_command(left_speed, left_dir, right_speed, right_dir)
# update robot position
self.robot.update(time_delta)
#update goal position
self.robot.set_robot_goal()