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: ")
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)
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)
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()
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()
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()
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:")
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
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()
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()))
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: ")
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:')
