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