def publishMotorCmd(self):
cmd = maebot_diff_drive_t()
# IMPLEMENT ME
distanceDifferenceLeft = abs(self.curDistance[0] - self.startDistance[0])
distanceDifferenceRight = abs(self.curDistance[1] - self.startDistance[1])
if distanceDifferenceLeft >= abs(self.DesiredDistance[0]) or distanceDifferenceRight >= abs(self.DesiredDistance[1]):
cmd.motor_left_speed = 0
self.leftCtrl.setpoint = self.curDistance[0]
self.leftCtrl.dotsetpoint = 0
cmd.motor_right_speed = 0
self.rightCtrl.setpoint = self.curDistance[1]
self.rightCtrl.dotsetpoint = 0
else:
lWheelDir = 1 #for setting left wheel trim value to negative or positive (backward/leftturn/rightturn)
rWheelDir = 1 #for setting left wheel trim value to negative or positive (backward/leftturn/rightturn)
if self.leftCtrl.dotsetpoint < 0 :
lWheelDir = -1
if self.rightCtrl.dotsetpoint < 0 :
rWheelDir = -1
cmd.motor_left_speed = (self.leftCtrl.output + self.trim * lWheelDir) * self.mu0
cmd.motor_right_speed = (self.rightCtrl.output + self.trim * rWheelDir) * self.mu0
#cmd.motor_left_speed = (self.leftCtrl.output + self.trim ) * self.mu0
#cmd.motor_right_speed = (self.rightCtrl.output + self.trim ) * self.mu0
#cmd.motor_left_speed = min(max(cmd.motor_left_speed,-0.4),0.4) #added to limit output
#cmd.motor_right_speed = min(max(cmd.motor_left_speed,-0.4),0.4) #added to limit output
#print "Left PID: %.2f Left Total: %.2f Right PID: %.2f Right Total: %.2f" % (self.leftCtrl.output, cmd.motor_left_speed, self.rightCtrl.output, cmd.motor_right_speed)
self.lc.publish("MAEBOT_DIFF_DRIVE", cmd.encode())
def signal_handler(self,signal, frame):
print("Terminating!")
for i in range(5):
cmd = maebot_diff_drive_t()
cmd.motor_right_speed = 0.0
cmd.motor_left_speed = 0.0
self.lc.publish("MAEBOT_DIFF_DRIVE",cmd.encode())
exit(1)
def publishMotorCmd(self):
cmd = maebot_diff_drive_t()
# IMPLEMENT ME
cmd.motor_left_speed = (self.leftCtrl.output + self.trim) * self.mu0
cmd.motor_right_speed = (self.rightCtrl.output + self.trim) * self.mu0
print "Left : %.2f"%(self.leftCtrl.output)
print "Right : %.2f"%(self.rightCtrl.output)
self.lc.publish("MAEBOT_DIFF_DRIVE", cmd.encode())
def publishMotorCmd(self):
cmd = maebot_diff_drive_t()
cmd.motor_right_speed = self.rightSpeed
cmd.motor_left_speed = self.leftSpeed
self.lc.publish("MAEBOT_DIFF_DRIVE", cmd.encode())
def publishMotorCmd(self):
cmd = maebot_diff_drive_t()
cmd.motor_left_speed = self.leftCtrl.output
cmd.motor_right_speed = self.rightCtrl.output
#print "Motor outputs:", cmd.motor_left_speed, cmd.motor_right_speed
self.lc.publish("MAEBOT_DIFF_DRIVE", cmd.encode())
def MainLoop(self):
pygame.key.set_repeat(1, 20)
vScale = 0.5
# Prepare Text to Be output on Screen
font = pygame.font.SysFont("DejaVuSans Mono",14)
mapCounter = 0
while 1:
leftVel = 0
rightVel = 0
for event in pygame.event.get():
if event.type == pygame.QUIT:
sys.exit()
elif event.type == KEYDOWN:
if ((event.key == K_ESCAPE)
or (event.key == K_q)):
sys.exit()
key = pygame.key.get_pressed()
if key[K_RIGHT]:
leftVel = leftVel + 0.40
rightVel = rightVel - 0.40
elif key[K_LEFT]:
leftVel = leftVel - 0.40
rightVel = rightVel + 0.40
elif key[K_UP]:
leftVel = leftVel + 0.65
rightVel = rightVel + 0.65
elif key[K_DOWN]:
leftVel = leftVel - 0.65
rightVel = rightVel - 0.65
else:
leftVel = 0.0
rightVel = 0.0
cmd = maebot_diff_drive_t()
cmd.motor_right_speed = vScale * rightVel
cmd.motor_left_speed = vScale * leftVel
self.lc.publish("MAEBOT_DIFF_DRIVE",cmd.encode())
elif event.type == pygame.MOUSEBUTTONDOWN:
command = velocity_cmd_t()
command.Distance = 987654321.0
if event.button == 1:
if ((event.pos[0] > 438) and (event.pos[0] < 510) and
(event.pos[1] > 325) and (event.pos[1] < 397)):
command.FwdSpeed = 80.0
command.Distance = 1000.0
command.AngSpeed = 0.0
command.Angle = 0.0
print "Commanded PID Forward One Meter!"
elif ((event.pos[0] > 438) and (event.pos[0] < 510) and
(event.pos[1] > 400) and (event.pos[1] < 472)):
command.FwdSpeed = -100.0
command.Distance = -1000.0
command.AngSpeed = 0.0
command.Angle = 0.0
print "Commanded PID Backward One Meter!"
elif ((event.pos[0] > 363) and (event.pos[0] < 435) and
(event.pos[1] > 400) and (event.pos[1] < 472)):
command.FwdSpeed = 0.0
command.Distance = 0.0
command.AngSpeed = 25.0
command.Angle = 90.0
print "Commanded PID Left One Meter!"
elif ((event.pos[0] > 513) and (event.pos[0] < 585) and
(event.pos[1] > 400) and (event.pos[1] < 472)):
command.FwdSpeed = 0.0
command.Distance = 0.0
command.AngSpeed = -25.0
command.Angle = -90.0
print "Commandsed PID Right One Meter!"
elif ((event.pos[0] > 513) and (event.pos[0] < 585) and
(event.pos[1] > 325) and (event.pos[1] < 397)):
pid_cmd = pid_init_t()
#pid_cmd.kp = 0.00225 # CHANGE FOR YOUR GAINS!
pid_cmd.kp = 0.003
pid_cmd.ki = 0.00001 # See initialization
# pid_cmd.kd = 0.000045
pid_cmd.kd = 0.00045
pid_cmd.iSat = 0.0
self.lc.publish("GS_PID_INIT",pid_cmd.encode())
print "Commanded PID Reset!"
if (command.Distance != 987654321.0):
self.lc.publish("GS_VELOCITY_CMD",command.encode())
# #Handle doing guidance
# if self.guidance.state == -1:
# if not self.failed_path or (time.time() - self.last_failed_path) > 5:
# #Generate a new path
# startpos = world_to_astar(self.odometry[1], self.odometry[0])
# endpos = (250, 350)
# path = astar(self.datamatrix.getMapMatrix(), startpos, endpos)
# if len(path) > 0:
# print "Found a path"
# self.failed_path = False
# pruned_path = [path[0], path[min(5, len(path)-1)]]
# self.guidance.plan = map(lambda x: astar_to_world(x[1], x[0]), pruned_path)
# #print path
# print self.guidance.plan
# self.guidance.start()
# else:
# print "Failed to find a path"
# self.failed_path = True
# self.last_failed_path = time.time()
# else:
# self.guidance.guide_and_command((self.odometry[1], self.odometry[0], math.radians(self.odometry[2])))
#Handle doing guidance - Kevin
if self.guidance.state == -1:
if not self.failed_path or (time.time() - self.last_failed_path) > 1:
#Generate a new path
startpos = world_to_astar(self.odometry[1], self.odometry[0]) #(x,y) -> y*,x*
#endpos=(250.250)
#robot will start at (3500,3500) (x,y)
#endpos = world_to_astar(3030,5500) #x, y - >y*,x*
endpos = world_to_astar(2500,3500)
print "Starting at: ", startpos, "Ending at: ", endpos
#print "Checking for a path. Printing..."
self.odom_path.append(world_to_astar(self.odometry[1], self.odometry[0]))
#print "odom:", self.odom_path
path = astar(self.datamatrix.getMapMatrix(), startpos, endpos, False)
if len(path) > 0:
print "Found a path. This is the guidance:"
self.failed_path = False
#pruned_path = [path[min(2, len(path)-1)], path[min(5, len(path)-1)]]
#pruned_path = [path[min(5, len(path)-2)], path[min(10, len(path)-2)]] #
#self.guidance.plan = map(lambda x: astar_to_world(x[1], x[0]), pruned_path) #y*,x* -> x, y
#print self.guidance.plan
#print self.guidance.plan
next_point = path[min(len(path)-1, 5)]
print next_point
self.guidance.next_point = astar_to_world(next_point[1], next_point[0])
print self.guidance.next_point
#print self.guidance.next_point
#self.taken_path.append(next_point)
self.guidance.start()
#print "Planned:", self.taken_path
#numpy.save("failed_path.npy", self.datamatrix.getMapMatrix())
else:
print "Failed to find a path"
self.failed_path = True
self.last_failed_path = time.time()
#numpy.save("temp_path_%d.npy"%(self.count), self.datamatrix.getMapMatrix())
#print self.odometry
pid_cmd = pid_init_t()
#pid_cmd.kp = 0.00225 # CHANGE FOR YOUR GAINS!
pid_cmd.kp = 0.003
pid_cmd.ki = 0.00001 # See initialization
# pid_cmd.kd = 0.000045
pid_cmd.kd = 0.00045
pid_cmd.iSat = 0.0
self.lc.publish("GS_PID_INIT",pid_cmd.encode())
# else:
## print "ODO"
## print (self.odometry[1], self.odometry[0])
## print "Guidance"
## print self.guidance.plan
## self.guidance.guide_and_command((self.odometry[1], self.odometry[0], math.radians(self.odometry[2])))
# self.path_failed = True
# self.last_failed_path = time.time()
self.screen.fill((255,255,255))
# Plot Lidar Scans
plt.cla()
self.ax.plot(self.thetas,self.ranges,'or',markersize=2)
self.ax.set_rmax(1.5)
self.ax.set_theta_direction(-1)
self.ax.set_theta_zero_location("N")
self.ax.set_thetagrids([0,45,90,135,180,225,270,315],
labels=['','','','','','','',''],
frac=None,fmt=None)
self.ax.set_rgrids([0.5,1.0,1.5],labels=['0.5','1.0',''],
angle=None,fmt=None)
canvas = agg.FigureCanvasAgg(self.fig)
canvas.draw()
renderer = canvas.get_renderer()
raw_data = renderer.tostring_rgb()
size = canvas.get_width_height()
surf = pygame.image.fromstring(raw_data, size, "RGB")
self.screen.blit(surf, (320,0))
#Put map on screen
if self.map_init == True:
image = pygame.image.load("current_map.png")
image = pygame.transform.scale(image, (320, 320))
self.screen.blit(image, (0,0))
# Position and Velocity Feedback Text on Screen
self.lc.handle()
pygame.draw.rect(self.screen,(0,0,0),(5,350,300,120),2)
text = font.render(" POSITION ",True,(0,0,0))
self.screen.blit(text,(10,360))
text = font.render("x: %.2f [mm]" % (self.odometry[1]),True,(0,0,0))
self.screen.blit(text,(10,390))
text = font.render("y: %.2f [mm]" % (self.odometry[0]),True,(0,0,0))
self.screen.blit(text,(10,420))
text = font.render("t: %.2f [rad]" % (self.odometry[2]),True,(0,0,0))
self.screen.blit(text,(10,450))
text = font.render(" VELOCITY ",True,(0,0,0))
self.screen.blit(text,(150,360))
text = font.render("dxy/dt: %.2f [mm/us]" % (self.dxy / self.dt * 1000000),True,(0,0,0))
self.screen.blit(text,(150,390))
text = font.render("dth/dt: %.2f [deg/us]" % (self.dtheta / self.dt * 1000000),True,(0,0,0))
self.screen.blit(text,(150,420))
text = font.render("dt: %d [s]" % (self.dt/1000000),True,(0,0,0))
self.screen.blit(text,(150,450))
# Plot Buttons
self.screen.blit(self.arrowup,(438,325))
self.screen.blit(self.arrowdown,(438,400))
self.screen.blit(self.arrowleft,(363,400))
self.screen.blit(self.arrowright,(513,400))
self.screen.blit(self.resetbut,(513,325))
pygame.display.flip()
def __init__(self, width=640, height=480, FPS=10):
pygame.init()
self.width = width
self.height = height
self.screen = pygame.display.set_mode((self.width, self.height))
self.currOdoPos = (0,0,0)
self.currOdoVel = (0,0,0)
self.goalPos = (0,0)
self.slamPos = (0,0)
self.ranges = 0
self.thetas = 0
self.nranges = 0
self.laser_times = 0
self.intensities = 0
self.map_count = 0
self.auto_flag = False
self.planner = loadMap.MotionPlanner()
self.wypt_indx = 1
self.currWypt = (0,0)
self.path = []
self.count = 0
self.cmd = maebot_diff_drive_t()
self.fileWritted = 0
self.time_counter = 0
self.vel_accum = (0,0,0)
self.initialized_map = False
self.replan_time = 0
# LCM Subscribe
self.lc = lcm.LCM()
# NEEDS TO SUBSCRIBE TO OTHER LCM CHANNELS LATER!!!
lcmOdoVelSub = self.lc.subscribe("BOT_ODO_VEL", self.OdoVelocityHandler)
lcmOdoPosSub = self.lc.subscribe("BOT_ODO_POSE", self.OdoPosHandler)
rpLidarSub = self.lc.subscribe("RPLIDAR_LASER", self.LidarHandler)
# Prepare Figure for Lidar
self.fig = plt.figure(figsize=[3, 3], # Inches
dpi=100) # 100 dots per inch,
self.fig.patch.set_facecolor('white')
self.fig.add_axes([0,0,1,1],projection='polar')
self.ax = self.fig.gca()
self.fig2 = plt.figure(figsize=[3, 3], # Inches
dpi=100) # 100 dots per inch,
self.fig2.patch.set_facecolor('white')
self.fig2.add_axes([0,0,1,1])
self.ax2 = self.fig2.gca()
# Prepare Figures for control
path = os.path.realpath(__file__)
path = path[:-17] + "maebotGUI/"
self.arrowup = pygame.image.load(path + 'fwd_button.png')
self.arrowdown = pygame.image.load(path + 'rev_button.png')
self.arrowleft = pygame.image.load(path + 'left_button.png')
self.arrowright = pygame.image.load(path + 'right_button.png')
self.resetbut = pygame.image.load(path + 'reset_button.png')
self.arrows = [0,0,0,0]
# PID Initialization - Change for your Gains!
command = pid_init_t()
command.kp = 0.0
command.ki = 0.0
command.kd = 0.0
command.iSat = 0.0 # Saturation of Integral Term.
# If Zero shoudl reset the Integral Term
self.lc.publish("GS_PID_INIT",command.encode())
# Declare Laser, datamatrix and slam
self.laser = RPLidar(DIST_MIN, DIST_MAX) # lidar
self.datamatrix = DataMatrix(**KWARGS) # handle map data
self.slam = Slam(self.laser, **KWARGS) # do slam processing
