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])
