def driveForwardTime(seconds):
initTime = time.time()
LOGGER.Debug("Driving Forward")
while time.time() < initTime + seconds:
testMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, -.6)
poopMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
ceaseAllMotorFunctions()
def centerWithTarget():
#foundTarget = False
#side = ""
#LOGGER.Debug(str(cap.isOpened()))
#while cap.isOpened():
# have_frame, frame = cap.read()
# if have_frame:
# contours = pipeline.process(frame)
# pan()
# LOGGER.Debug("Contours "+ str(len(contours)))
# if len(contours)==2:
# foundTarget = True
# side = center(contours)
# LOGGER.Debug("SIDE " + center(contours))
# break
# if foundTarget:
# break
#cap.release()
#global reachedTarget
#LOGGER.Debug(str(reachedTarget))
#if reachedTarget == False:
LOGGER.Debug(fakeside)
alignWithTarget(fakeside, fakeCameraAngle)
driveForwardTime(3.5)
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)
def sensorCommunicationThread():
while True:
#sensorHandlerLock.acquire()
inboundSensorMessage = sensorSerialHandler.getMessage()
sensorHandler.updateSensors(inboundSensorMessage)
outboundSensorMessage = sensorHandler.getServoStateMessage()
LOGGER.Debug(outboundSensorMessage)
sensorSerialHandler.sendMessage(outboundSensorMessage)
def turnLeftTicks(ticks):
initTicks = poopMotor.position
desPosition = initTicks - ticks
if poopMotor.position > desPosition:
LOGGER.Debug("Desired Position: " + str(desPosition) +
"Encoder Ticks: " + str(poopMotor.position))
testMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
poopMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
def turnLeftTime(seconds):
LOGGER.Debug("TURNING LEFT")
initTime = time.time()
while time.time() < initTime + seconds:
leftMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
rightMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
ceaseAllMotorFunctions()
def turnRightTime(seconds):
initTime = time.time()
LOGGER.Debug("Turning RIGHT")
while time.time() < initTime + seconds:
leftMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, -.6)
rightMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, -.6)
ceaseAllMotorFunctions()
def motorCommunicationThread():
while True:
motorHandlerLock.acquire()
#get the messages of each motor status from the HERO and update our motor values
inboundMotorMessage = motorSerialHandler.getMessage()
#LOGGER.Debug(inboundMotorMessage)
if inboundMotorMessage != "":
LOGGER.Debug("Received update from HERO")
LOGGER.Debug(inboundMotorMessage)
motorHandler.updateMotors(inboundMotorMessage)
#Get our motor state message and send that to the HERO
outboundMotorMessage = motorHandler.getMotorStateMessage()
motorSerialHandler.sendMessage(outboundMotorMessage)
#LOGGER.Debug("Sent")
motorHandlerLock.release()
def driveForwardTime(seconds):
initTime = time.time()
LOGGER.Debug("Driving Forward")
while time.time() < initTime + seconds:
leftMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, -.6)
rightMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, .6)
ceaseAllMotorFunctions()
global reachedTarget
reachedTarget = True
def cameraCommunicationThread():
global frame
global cameraOutput
LOGGER.Debug("Starting Cameraaaaaaaaaaaa")
pipeline = TargetFinder()
while cap.isOpened():
cameraHandlerLock.acquire()
have_frame, frame = cap.read()
ret, jpeg = cv2.imencode(".jpg", frame)
cameraOutput = jpeg.tobytes()
#LOGGER.Debug(str(len(frame)))
cameraHandlerLock.release()
def centerWithTarget():
foundTarget = False
side = ""
while cap.isOpened():
have_frame, frame = cap.read()
if have_frame:
contours = pipeline.process(frame)
#Begin sweeping the camera servo
#for angle in range(0,60):
# cameraServo.setSetpoint(angle)
LOGGER.Debug("Contours " + str(len(contours)))
if len(contours) == 2:
foundTarget = True
side = center(contours)
LOGGER.Debug("SIDE " + center(contours))
break
if foundTarget:
break
cap.release()
LOGGER.Debug(side)
alignWithTarget(side, fakeCameraAngle)
driveForwardTime(2)
def parseMessage(self, msg_str):
try:
msg_str = msg_str.strip("\< \>\n\r")
msg_parts1 = msg_str.split("|")
self.type = RobotState.STATES[int(msg_parts1[0])]
msg_parts2 = msg_parts1[1].split(":")
self.messageNumber = int(msg_parts2[0])
for i in range(1, len(msg_parts2)):
self.messageData.append(float(msg_parts2[i]))
#Some error occurred in parsing the message - probably malformed.
except:
LOGGER.Moderate("Error on message:" + msg_str)
pass
def getMessage(self): #newMsg = "" #eol = b'\r' #leneol = len(eol) if(self.ser.isOpen()): line = self._readline() #for i in range(2): # c = self.ser.read(1) # if c: # self.inbound_buffer += c # if self.inbound_buffer[-leneol:] == eol: # newMsg = str(self.inbound_buffer) # self.inbound_buffer = bytearray() LOGGER.Debug(str(line)) return str(line)
def centerWithTarget():
#foundTarget = False
side = ""
while cap.isOpened():
have_frame, frame = cap.read()
if have_frame:
contours = pipeline.process(frame)
#Begin sweeping the camera servo
#for angle in range(0,60):
# cameraServo.setSetpoint(angle)
LOGGER.Debug("Contours "+ str(len(contours)))
if len(contours)==2:
#foundTarget = True
side = center(contours)
# LOGGER.Debug("SIDE " + center(contours))
break
def motorCommunicationThread(): while True: motorHandlerLock.acquire() #get the messages of each motor status from the HERO and update our motor values inboundMotorMessage = motorSerialHandler.getMessage() LOGGER.Debug(inboundMotorMessage) motorHandler.updateMotors(inboundMotorMessage) #guiMotorMessage= inboundMotorMessage #Gets the listed messages # flaskupdate(inboundMotorMessage) #Get our motor state message and send that to the HERO outboundMotorMessage = motorHandler.getMotorStateMessage() motorSerialHandler.sendMessage(outboundMotorMessage) motorHandlerLock.release()
def alignWithTarget(side, angle):
if side == "Left":
if 0 < angle <= 90:
LOGGER.Debug("Left side facing right")
turnRightTime(2)
elif 90 < angle <= 180:
LOGGER.Debug("Left side facing away")
turnLeftTime(2)
elif 180 < angle <= 270:
LOGGER.Debug("Left side facing left")
turnLeftTime(4)
elif 270 < angle <= 360:
LOGGER.Debug("Left side facing towards the target")
turnRightTime(3.5)
elif side == "Right":
if 0 < angle <= 90:
LOGGER.Debug("Right side facing towards")
turnLeftTime(4)
elif 90 < angle <= 180:
LOGGER.Debug("Right side facing right")
turnRightTime(4)
elif 180 < angle <= 270:
LOGGER.Debug("Right side facing away")
turnRightTime(2)
elif 270 < angle <= 360:
LOGGER.Debug("Right side facing left")
turnLeftTime(2)
else:
LOGGER.Debug("Side is incorrect")
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])
networkClient.setInboundMessageQueue(inboundMessageQueue)
outboundMessageQueue = MessageQueue()
lastReceivedMessageNumber = -1
currentReceivedMessageNumber = -1
stateStartTime = -1
while True:
if CONSTANTS.USING_NETWORK_COMM:
connected = False
while (not connected):
try:
if (outboundMessageQueue.isEmpty()):
networkClient.send("Hiiiii\n\r")
else:
networkClient.send(outboundMessageQueue.getNext())
LOGGER.Debug("Connected to socket")
connected = True
except Exception as e:
# print e
LOGGER.Debug(
"Could not connect to network, attempting to reconnect...")
# print (connected)
if CONSTANTS.USING_NETWORK_COMM and connected:
if (not inboundMessageQueue.isEmpty()):
currentMessage = inboundMessageQueue.getNext()
currentMessage.printMessage()
lastReceivedMessageNumber = currentReceivedMessageNumber
currentReceivedMessageNumber = currentMessage.messageNumber
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 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():
