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