def update(self, update_info): # message contains info for all motors, need to parse info specific to this motor #matchList = re.findall(r'<(\d+):(\d+):(\d+):(\d+):(\d+):(\-?\d+):(\d+):(\d+):(\d+):(\d+)>', update_info, re.M|re.I) #matchList = re.findall(r'<(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+):(\-?\d+)>', update_info, re.M|re.I) #LOGGER.Debug(update_info) matchList = re.findall( r'<(\d+):(\-?\d+\.\d+):(\-?\d+\.\d+):(\-?\d+\.\d+):(\-?\d+\.\d+):(\-?\d+\.\d+):(\-?\d+\.\d+):(\d+):([01]):([01])>', update_info, re.M | re.I) if update_info == "": LOGGER.Low("Hero sent no motor data") elif (len(matchList) == 0): LOGGER.Low("No Matches!:" + update_info) # check each match to see if this contains data for this motor controller for match in matchList: # match has enough data elements to contain valid data if (len(match) >= 10): # be ready to throw an error if any data is incorrect try: #try to read all the values before assigning anything msg_deviceID = int(match[0]) msg_current = float(match[1]) msg_temperature = float(match[2]) msg_voltageOutput = float(match[3]) msg_speed = float(match[4]) msg_position = float(match[5]) msg_setpoint = float(match[6]) msg_controlMode = int(match[7]) msg_forward_limit = bool(int(match[8])) msg_reverse_limit = bool(int(match[9])) if (msg_deviceID == self.deviceID): self.current_val = msg_current self.temperature_val = msg_temperature self.voltageOutput = msg_voltageOutput self.speed = msg_speed self.position = msg_position self.mode = msg_controlMode self.actual_val = msg_setpoint self.forward_limit = msg_forward_limit self.reverse_limit = msg_reverse_limit except: print "ERROR: Unable to parse one or more of the message components. Device ID: " + str( self.deviceID)
from SerialHandler import SerialHandler from NetworkHandler import NetworkHandler from MessageQueue import MessageQueue from JoystickReader import JoystickReader from NetworkClient import NetworkClient from NetworkMessage import NetworkMessage from Servo import Servo from scipy.interpolate import interp1d from TargetPipeline import TargetFinder from AngleWithTarget import center from time import gmtime, strftime cameraOutput = "" #from main import inboundMessageQueue LOGGER.Low("Beginning Program Execution") #Threading stuff... i still don't know this motorHandlerLock = threading.Lock() #sensorHandlerLock = threading.Lock() cameraHandlerLock = threading.Lock() #LOGGER.Low("Motor Handler Lock: " + str(motorHandlerLock)) stopped = True joyMap = interp1d([-1.0, 1.0], [-.5, .5]) fakeCameraAngle = 330 fakeside = "Left" numloop = 0 def motorCommunicationThread():
from Sensor import Sensor from SensorHandler import SensorHandler from RobotState import RobotState from SerialHandler import SerialHandler from NetworkHandler import NetworkHandler from MessageQueue import MessageQueue from JoystickReader import JoystickReader from NetworkClient import NetworkClient from NetworkMessage import NetworkMessage from Servo import Servo import BeepCodes as BEEPCODES from time import gmtime, strftime #from main import inboundMessageQueue LOGGER.Low("Beginning Program Execution") #Threading stuff... i still don't know this motorHandlerLock = threading.Lock() #sensorHandlerLock = threading.Lock() #LOGGER.Low("Motor Handler Lock: " + str(motorHandlerLock)) def motorCommunicationThread(): while True: motorHandlerLock.acquire() #get the messages of each motor status from the HERO and update our motor values inboundMotorMessage = motorSerialHandler.getMessage() motorHandler.updateMotors(inboundMotorMessage) #Get our motor state message and send that to the HERO
from RobotState import RobotState from SerialHandler import SerialHandler from NetworkHandler import NetworkHandler from MessageQueue import MessageQueue from JoystickReader import JoystickReader from NetworkClient import NetworkClient from NetworkMessage import NetworkMessage from Servo import Servo from TargetPipeline import TargetFinder from AngleWithTarget import center import BeepCodes as BEEPCODES from scipy.interpolate import interp1d from time import gmtime, strftime #from main import inboundMessageQueue LOGGER.Low("Beginning Program Execution") #*************************CAMERA STUFF************************************* cameraOutput = "" targetAngle = -1.0 side = "" #************************************************************************** #Threading stuff... i still don't know this motorHandlerLock = threading.Lock() #sensorHandlerLock = threading.Lock() #cameraHandlerLock = threading.Lock() #LOGGER.Low("Motor Handler Lock: " + str(motorHandlerLock)) joyMap = interp1d([-1.0, 1.0], [-0.5, 0.5])