def move(self):
     """Sends the current speed as a command to the excavator"""
     bucket_speed = self.speed
     leftSpeed = self.speed
     rightSpeed = self.speed
     #Account for direction
     leftSpeed = leftSpeed + self.direction
     rightSpeed = rightSpeed - self.direction
     #Account for going over 1.0 or under -1.0
     if leftSpeed < -1.0:
         leftSpeed = -1.0
     if leftSpeed > 1.0:
         leftSpeed = 1.0
     if rightSpeed < -1.0:
         rightSpeed = -1.0
     if rightSpeed > 1.0:
         rightSpeed = 1.0
     #Send the commands
     leftSpeed *= 64
     rightSpeed *= 64
     
     self.left = leftSpeed
     self.right = rightSpeed
     
     try:
         if self.comm_port:
             self.comm_port.write(" m 2 "+str(int(leftSpeed))+"\r")
             self.comm_port.write(" m 1 "+str(int(rightSpeed))+"\r")
     except:
         logError(sys.exc_info(), self.desktop.logerr, "Exception when sending commands to Excavator, if the comm_port \nisn't connected maybe a powerloss on the lantrix board occured: ")
Ejemplo n.º 2
0
 def pollIRSensors(self):
     """Polls the IR Sensors on a regular period
         returns a list of six IR readings in millimeters
     """
     if not self.started:
         print "Start the Proteus and try again."
         return
     else:
         self.ir_timer = Timer(self.ir_poll_rate, self.pollIRSensors) # Kick off the next timer
         self.ir_timer.start()
     ir_data = [0,0,0,0,0,0]
     data = None
     try:          
         # Request for the IR data
         self.write(CMD_START+OP_SENSOR+SENSOR_IR+CMD_STOP)
         # Wait for the proper response
         data = self.read(12)
         # Parse the data
         if data != None and len(data) == 12:
             # Encode the data as HEX for processing
             data = data.encode("HEX")
             for ir in range(0,6):
                 ir_data[ir] = int(data[(ir*4):((ir+1)*4)], 16)
             # Here is where you can convert the data back to ADC and then into other IR sensor specific distances
             pass
     except Exception as err:
         logError(sys.exc_info(), logerr, "Exception while polling IR Sensors:")
     finally:
         # Pass the data along to the handler
         if self.onIRSensorData:
             self.onIRSensorData(ir_data)
Ejemplo n.º 3
0
 def move(self, speed=0.0, direction=0.0):
     """Adjusts the motors based on the speed and direction you specify.
         
     Speed and Direction should be values between -1.0 and 1.0, inclusively.
     """
     #Validate the parameters
     if speed < -1.0 or speed > 1.0:
         logError(sys.exc_info(), rospy.logerr, "Speed given to the move() function must be between -1.0 and 1.0 inclusively.")
         return
     if direction < -1.0 or direction > 1.0:
         logError(sys.exc_info(), rospy.logerr, "Direction given to the move() function must be between -1.0 and 1.0 inclusively.")
         return
     #First calculate the speed of each motor then send the commands
     #self.setSpeeds2(speed, direction)
     #return
     #Account for speed
     left_speed = speed
     right_speed = speed
     #Account for direction
     left_speed = right_speed + direction
     right_speed = right_speed - direction
     #Account for going over 1.0 or under -1.0
     if left_speed < -1.0:
         left_speed = -1.0
     if left_speed > 1.0:
         left_speed = 1.0
     if right_speed < -1.0:
         right_speed = -1.0
     if right_speed > 1.0:
         right_speed = 1.0
     #Send the commands
     self.setSpeeds(left=left_speed, right=right_speed)
Ejemplo n.º 4
0
    def run(self):
        """Overrides Thread's run method"""
        try:
            serial = self.serial_port
            # Open the serial port if it isn't
            while self._running:
                try:
                    if not serial.isOpen():
                        serial.open()
                except Exception as error:
                    error.args = ("Error opening Serial Port, did you pass a serial port object and not a string?",)
                    raise error
                while True:
                    token = ""
                    message = ""
                    # Make sure we are still supposed to be listening
                    self._listening_lock.acquire()
                    temp_listening = self._listening
                    self._listening_lock.release()
                    if not temp_listening or not self._running:
                        break
                    # Read until you get a delimiter
                    while token not in self.delimiters:
                        if serial.isOpen():
                            token = serial.read()
                        else:
                            print "wtf, seriously.... who closed the serial port???"
                            return
                        message += token
                        self._listening_lock.acquire()
                        temp_listening = self._listening
                        self._listening_lock.release()
                        if not temp_listening or not self._running:
                            break
                    if not temp_listening or not self._running:
                        break
                    # We have gotten a delimiter now we need to
                    # determine if there is a callback for this message
                    for comparator, callback in self.handlers:
                        callback_event = None
                        try:
                            if (
                                (inspect.isfunction(comparator) or inspect.ismethod(comparator)) and comparator(message)
                            ) or (isinstance(comparator, bool) and comparator):
                                callback_event = callback(message)
                            else:
                                self.unhandledMessage(message)
                        except Exception as err:
                            logError(sys.exc_info(), rospy.logerr, "Exception handling serial message:")

                # Close everything after exiting the loop
                serial.close()
                self.latest_unhandled_message = None
                self.received_unhandled_message.set()
                # Wait for either __del__ or listen to be called again
                self._listening_event.wait()
                self._listening_event.clear()
        except Exception as err:
            logError(sys.exc_info(), rospy.logerr, "Exception in Serial Listener:")
 def sendBucket(self):
     """Sends the current bucket as a command to the excavator"""
     bucket_speed = self.bucket
     if bucket_speed < -1.0:
         bucket_speed = -1.0
     if bucket_speed > 1.0:
         bucket_speed = 1.0
     bucket_speed *= 64
     try:
         if self.comm_port:
             self.comm_port.write(" a 2 "+str(int(bucket_speed))+"\r")
     except:
         logError(sys.exc_info(), self.desktop.logerr, "Exception when sending commands to Excavator, if the comm_port \nisn't connected maybe a powerloss on the lantrix board occured: ")
 def sendRemoteStop(self, button=None):
     """Sends the remote stop command to the excavator"""
     # Send magic packet
     try:
         udp_socket = socket(AF_INET, SOCK_DGRAM)
         msg = '\x1B'+'\x01\x00\x00\x00'+'\x00\x00\x00\x00'
         host, _ = self.telnet_ip.text.split(':')
         udp_socket.sendto(msg, (host,0x77f0))
         udp_socket.close()
     except:
         logError(sys.exc_info(), self.desktop.logerr, "Exception Sending Stop Command: ")
     # Reconfigure button
     self.remote_start_button.text = "Remote Start"
     self.remote_start_button.onClick = self.sendRemoteStart
 def connect(self, button):
     """Called when connect is clicked"""
     # Try to connect
     try:
         host, port = self.telnet_ip.text.split(":")
         self.telnet = telnetlib.Telnet(host, int(port), 30)
     except:
         logError(sys.exc_info(), self.desktop.logerr, "Exception connecting via Telnet, %s:%s: " % (type(host), type(port)))
     # Disable the connector
     self.desktop.telnet_serial_selector.disable()
     # Change the connect button to disconnect
     self.connect_button.text = "Disconnect"
     self.connect_button.onClick = self.disconnect
     # Nofity the desktop
     self.desktop.connected()
Ejemplo n.º 8
0
 def keepAlive(self):
     """This functions sends the latest motor speed to prevent the dead man 
         system from stopping the motors.
     """
     try:
         # Lock the speed lock
         self.speed_lock.acquire()
         # Resend the current motor speeds
         self.__setSpeed(self.left_speed, self.right_speed)
         # Release the speed lock
         self.speed_lock.release()
     except Exception as err:
         logError(sys.exc_info(), rospy.logerr, "Exception in keepAlive function: ")
     if self.running:
         self.keep_alive_timer = Timer(0.4, self.keepAlive)
         self.keep_alive_timer.start()
Ejemplo n.º 9
0
 def pollEncoders(self):
     """Polls the encoder on a period"""
     # Kick off the next polling iteration timer
     if self.running:
         self.encoder_timer = Timer(self.encoder_rate, self.pollEncoders)
         self.encoder_timer.start()
     else:
         return
     encoder_1 = None
     encoder_2 = None
     try:
         # Lock the speed lock
         self.speed_lock.acquire()
         # Lock the serial lock
         self.serial_lock.acquire()
         # Query encoder 1
         encoder_1 = self.getHexData("?Q4\r")
         # Query encoder 2
         encoder_2 = self.getHexData("?Q5\r")
         # Release the serial lock
         self.serial_lock.release()
         # Release the speed lock
         self.speed_lock.release()
         # Convert the encoder data to ints
         if encoder_1 != None:
             encoder_1 = self.decodeEncoderValue(encoder_1) * -1
         else:
             encoder_1 = 0
         if encoder_2 != None:
             encoder_2 = self.decodeEncoderValue(encoder_2) * -1
         else:
             encoder_2 = 0
         # Publish the encoder data
         #header = roslib.msg._Header.Header()
         message = Encoder(left=encoder_1, right=encoder_2)
         message.header.stamp = rospy.Time.now()
         message.header.frame_id = "0"
         
         try:
             self.encoders_pub.publish(message)
         except:
             pass
     except ValueError:
         rospy.logerr("Invalid encoder data received, skipping this one.")
     except Exception as err:
         logError(sys.exc_info(), rospy.logerr, "Exception while Querying the Encoders: ")
         self.encoder_timer.cancel()
Ejemplo n.º 10
0
 def connect(self, button):
     """Called when connect is clicked"""
     try:
         # Try to connect
         self.serial = Serial()
         self.serial.port = self.serial_txt.text
         self.serial.baudrate = 19200
         self.serial.open()
         # Disable the connector
         self.desktop.telnet_serial_selector.disable()
         # Change the connect button to disconnect
         self.connect_button.text = "Disconnect"
         self.connect_button.onClick = self.disconnect
         # Nofity the desktop
         self.desktop.connected()
     except Exception as err:
         logError(sys.exc_info(), self.desktop.logerr, "Exception opening the serial port:")
Ejemplo n.º 11
0
 def __init__(self, desktop):
     self.desktop = desktop
     
     errorStyle = gui.defaultLabelStyle.copy()
     errorStyle['border-width'] = 1
     errorStyle['border-color'] = (255,0,0)
     errorStyle['wordwrap'] = True
     errorStyle['autosize'] = False
     
     # Setup Log file
     self.log_file = None
     try:
         self.log_file = open("err.log", "w+")
     except Exception as err:
         logError(sys.exc_info(), self.logerr, "Exception opening log file: ")
     
     # Text box for displaying errors
     Label.__init__(self, position = (30,Y-(Y/3)-30),size = (X/2,Y/3), parent = self.desktop, text = "Error Text goes here", style = errorStyle)
     self.visible = False
     self.onClick = self.hide
Ejemplo n.º 12
0
def main():
    pygame.init()
    screen = pygame.display.set_mode((X,Y), RESIZABLE)
    fullscreen = False
    clock = pygame.time.Clock()
    initStyle()
    desktop = ControlCenterDesktop()
    try:
        while desktop.running:
            clock.tick()
            # Update FPS display
            desktop.fps_label.text = str(int(clock.get_fps()))
            # Handle Events
            events = [pygame.event.wait(),]+pygame.event.get()
            for event in events:
                if event.type == QUIT:
                    desktop.running = False
                elif event.type == KEYDOWN and event.key == K_ESCAPE:
                    desktop.running = False
                elif event.type == KEYDOWN and event.key == K_f:
                    if not fullscreen:
                        screen = pygame.display.set_mode((X,Y), FULLSCREEN)
                        fullscreen = True
                    else:
                        screen = pygame.display.set_mode((X,Y), RESIZABLE)
                        fullscreen = False
                elif event.type == VIDEORESIZE:
                    screen = pygame.display.set_mode(event.size, RESIZABLE)
                elif desktop.joy == None:
                    continue
                elif event.type == JOYAXISMOTION:
                    desktop.joy.axis[event.axis] = event.value
                elif event.type == JOYBALLMOTION:
                    desktop.joy.ball[event.ball] = event.rel
                elif event.type == JOYHATMOTION:
                    desktop.joy.hat[event.hat] = event.value
                elif event.type == JOYBUTTONUP:
                    desktop.joy.button[event.button] = 0
                elif event.type == JOYBUTTONDOWN:
                    desktop.joy.button[event.button] = 1
            # End For loop
            # Update the excavator
            desktop.excavator.update(desktop.joy)
            # try:
            #     desktop.logerr(desktop.excavator.comm_port.read_some())
            # except:
            #     pass
            # Pass along the events
            gui.setEvents(events)
            # Update the Desktop
            desktop.update()
            # Begin Rendering
            screen.fill((20,40,50))
            screen.blit(desktop.bg_image, (0,0))
            desktop.draw()
            pygame.display.flip()
    except:
        logError(sys.exc_info(), log, "Exception in main Loop: ")
    finally:
        desktop.close()
    # After exiting the while loop quit
    pygame.quit()
Ejemplo n.º 13
0
 def update(self, joystick):
     """Updates the excavator, if necessary, given a joystick_handler"""
     try:
         if not self.comm_port or not joystick:
             self.debug_window.text = "Excavator Output:\nNot Processing Currently."
             return
         speed_needs_to_be_updated = False
         direction_needs_to_be_updated = False
         lift_needs_to_be_updated = False
         bucket_needs_to_be_updated = False
         # The dead man button is pressed do speed and direction
         if joystick.button[DEAD_MAN_BUTTON] == 1:
             # Check if the speed needs to be updated
             temp_speed = joystick.axis[SPEED_AXIS]
             if INVERT_SPEED:
                 temp_speed *= -1
             if abs(temp_speed) < SPEED_AND_DIRECTION_DEAD_ZONE:
                 self.speed = 0
                 speed_needs_to_be_updated = True
             elif abs(self.speed - temp_speed) >= SPEED_AND_DIRECTION_SENSITIVITY:
                 self.speed = temp_speed*SPEED_SCALAR
                 speed_needs_to_be_updated = True
             # Check if the speed needs to be updated
             temp_direction = joystick.axis[DIRECTION_AXIS]
             if INVERT_DIRECTION:
                 temp_direction *= -1
             if abs(temp_direction) < SPEED_AND_DIRECTION_DEAD_ZONE:
                 self.direction = 0
                 direction_needs_to_be_updated = True
             elif abs(self.direction - temp_direction) >= SPEED_AND_DIRECTION_SENSITIVITY:
                 self.direction = temp_direction*DIRECTION_SCALAR
                 direction_needs_to_be_updated = True
         else: # Dead man switch off, stop the motors
             self.speed = 0
             speed_needs_to_be_updated = True
             self.direction = 0
             direction_needs_to_be_updated = True
         # Check if the lift/bucket needs to be updated
         if joystick.button[LIFT_AND_BUCKET_TOGGLE_BUTTON] == IF_BUCKET: # If the bucket
             self.lift = 0
             lift_needs_to_be_updated = True
             temp_bucket = joystick.axis[LIFT_AND_BUCKET_AXIS]
             if INVERT_BUCKET:
                 temp_bucket *= -1
             if abs(temp_bucket) < BUCKET_DEAD_ZONE:
                 self.bucket = 0
                 bucket_needs_to_be_updated = True
             elif abs(self.bucket - temp_bucket) >= BUCKET_SENSITIVITY:
                 self.bucket = temp_bucket*LIFT_AND_BUCKET_SCALAR
                 bucket_needs_to_be_updated = True
         else: # If the Lift
             self.bucket = 0
             bucket_needs_to_be_updated = True
             temp_lift = joystick.axis[LIFT_AND_BUCKET_AXIS]
             if INVERT_LIFT:
                 temp_lift *= -1
             if abs(temp_lift) < LIFT_DEAD_ZONE:
                 self.lift = 0
                 lift_needs_to_be_updated = True
             if abs(self.lift - temp_lift) >= LIFT_SENSITIVITY:
                 self.lift = temp_lift*LIFT_AND_BUCKET_SCALAR
                 lift_needs_to_be_updated = True
         # Send new commands if necessary
         if speed_needs_to_be_updated or direction_needs_to_be_updated:
             self.move()
         if lift_needs_to_be_updated:
             self.sendLift()
         if bucket_needs_to_be_updated:
             self.sendBucket()
         # Update the display
         self.updateDisplay()
     except:
         logError(sys.exc_info(), self.desktop.logerr, "Exception reading joystick data @ %s: " % str(time.time()))
Ejemplo n.º 14
0
 def publishMessage(self):
     """Publishes the two messages, pos an utm, as one message"""
     if self.onPublishGPS:
         # Parse out the data from POS
         try:
             msg = self.current_pos
             position_type           = \
                 intOrNone(msg[2])           # position type: 0 = autonomous 1 = position differentially corrected
             number_of_sats_pos      = \
                 intOrNone(msg[3])           # Number of satellites used in position computation (no leading zero)
             time_pos                = \
                 floatOrNone(msg[4])         # Current UTC time, (hhmmss), of position computation in hours, minutes and seconds
             latitude                = \
                 floatOrNone(msg[5])         # Latitude component of position in degrees, minutes, and fraction of minutes (ddmm.mmmm)
             latitude_sector         = \
                 msg[6]                      # Latitude sector: N = North, S = South
             longitude               = \
                 floatOrNone(msg[7])         # Longitude component of position in degrees, minutes, and fraction of minutes
             longitude_sector        = \
                 msg[8]                      # Longitude sector: E = East, W = West
             altitude               = \
                 floatOrNone(msg[9])         # Altitude in meters above WGS-84 reference ellipsoid. 
                                             # For 2-D position computation this item contains the altitude held fixed.
                                             # If f1 positive, “+” sign not mentioned
             site_id                 = \
                 msg[10]                     # Site ID
             heading                 = \
                 floatOrNone(msg[11])        # True track/true course over ground in degrees (000.00 to 359.99 degrees)
             speed                   = \
                 floatOrNone(msg[12])        # Speed over ground in knots
             velocity                = \
                 floatOrNone(msg[13])        # Vertical velocity in meters per second
             pdop                    = \
                 floatOrNone(msg[14])        # PDOP—position dilution of precision
             hdop                    = \
                 floatOrNone(msg[15])        # HDOP—horizontal dilution of precision
             vdop                    = \
                 floatOrNone(msg[16])        # VDOP—vertical dilution of precision
             tdop                    = \
                 floatOrNone(msg[17])        # TDOP—time dilution of precision
             firmware_version        = \
                 msg[18]                     # Firmware version ID
             
             # Parse out the data from UTM
             msg = self.current_utm
             time_utm                = \
                 floatOrNone(msg[2])         # UTC of position in hours, minutes, and decimal seconds (hhmmss.ss)
             utm_zone                = \
                 msg[3]                      # Zone number for the coordinates
             utm_easting             = \
                 floatOrNone(msg[4])         # East UTM coordinate (meters)
             utm_northing            = \
                 floatOrNone(msg[5])         # North UTM coordinate (meters)
             fix_mode                = \
                 intOrNone(msg[6])           # Position fix mode indicator. 
                                             # 1—Raw position 
                                             # 2—RTCM differential or CPD float 
                                             # 3—Carrier phase differential (CDP) fixed
             num_sats_used           = \
                 intOrNone(msg[7])           # Number of GPS satellites being used to compute positions
             hdop_utm                = \
                 floatOrNone(msg[8])         # Horizontal dilution of precision (HDOP)
             antenna_height          = \
                 floatOrNone(msg[9])         # Antenna height (meters)
             geoidal_sep             = \
                 floatOrNone(msg[10])        # Geoidal separation in meters
             geoidal_units           = \
                 msg[11]                     # Geoidal separation units (meters)
             age_of_dif_correction   = \
                 intOrNone(msg[12])          # Age of differential corrections
             dif_station_id          = \
                 msg[13]                     # Differential reference station ID
             
             if time_pos != time_utm:
                 self.logerr("Clock skew detected in POS and UTM messages:\nPOS - Time: %f, Message: %s\nUTM - Time: %f, Message: %s\n" % \
                             (time_pos, ','.join(self.current_pos), time_utm, ','.join(self.current_utm)))
                 return
             # Assemble and publish the message
             result = []
             result.append(time_pos)
             if latitude_sector == "S": # If southern hemisphere, it needs to be negative
                 latitude *= -1
             result.append(latitude)
             if longitude_sector == "W": # If West of Prime meridian, it needs to be negative
                 longitude *= -1
             result.append(longitude)
             result.append(altitude)
             result.append(utm_easting)
             result.append(utm_northing)
             result.append(utm_zone)
             result.append(heading)
             result.append(hdop)
             result.append(vdop)
             result.append(0) # err_horizontal
             result.append(0) # err_vertical
             # Fix Quality
             if longitude == None:
                 result.append(0) # No Fix
             elif position_type == 0:
                 result.append(1) # GPS Fix
             elif position_type == 1:
                 result.append(2) # DGPS Fix
             else:
                 result.append(0) # Fallback default
             result.append(number_of_sats_pos)
             self.onPublishGPS(result)
         except Exception:
             logError(sys.exc_info(), self.logerr, "Exception parsing POS or UTM data: ")
Ejemplo n.º 15
0
 def run(self):
     """Overrides Thread's run method"""
     try:
         serial = self.serial_port
         # Open the serial port if it isn't
         while self._running:
             try:
                 if not serial.isOpen():
                     serial.open()
             except Exception as error:
                 error.args = ("Error opening Serial Port, did you pass a serial port object and not a string?",)
                 raise error
             while True:
                 token = ''
                 message = ''
                 # Make sure we are still supposed to be listening
                 self._listening_lock.acquire()
                 temp_listening = self._listening
                 self._listening_lock.release()
                 if not temp_listening or not self._running:
                     break
                 # Read until you get a delimiter
                 while token not in self.delimiters:
                     if serial.isOpen():
                         token = serial.read()
                     else:
                         print "wtf, seriously.... who closed the serial port???"
                         return
                     message += token
                     self._listening_lock.acquire()
                     temp_listening = self._listening
                     self._listening_lock.release()
                     if not temp_listening or not self._running:
                         break
                 if not temp_listening or not self._running:
                     break
                 # We have gotten a delimiter now we need to 
                 # determine if there is a callback for this message
                 handled = False
                 for comparator, callback in self.handlers:
                     callback_event = None
                     try:
                         if ((inspect.isfunction(comparator) or inspect.ismethod(comparator)) and comparator(message)) or \
                                                                         (isinstance(comparator, bool) and comparator):
                             skipped = callback(message)
                             if not skipped:
                                 handled = True
                                 break
                     except Exception as err:
                         logError(sys.exc_info(), rospy.logerr, 'Exception handling serial message:')
                 if not handled:
                     self.unhandledMessage(message)
             # Close everything after exiting the loop
             serial.close()
             self.latest_unhandled_message = None
             self.received_unhandled_message.set()
             # Wait for either __del__ or listen to be called again
             self._listening_event.wait()
             self._listening_event.clear()
     except Exception as err:
         logError(sys.exc_info(), rospy.logerr, 'Exception in Serial Listener:')