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 sensorCommunicationThread(): while True: #sensorHandlerLock.acquire() inboundSensorMessage = sensorSerialHandler.getMessage() sensorHandler.updateSensors(inboundSensorMessage) outboundSensorMessage = sensorHandler.getServoStateMessage() LOGGER.Debug(outboundSensorMessage) sensorSerialHandler.sendMessage(outboundSensorMessage)
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 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 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 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 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")
sensorSerialHandler.sendMessage(outboundServoMessage) sensorHandlerLock.release() def ceaseAllMotorFunctions(): #Stop all motors testMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) poopMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) # #initialize handlers motorHandler = MotorHandler() sensorHandler = SensorHandler() if CONSTANTS.USING_SENSOR_BOARD: LOGGER.Debug("Initializing sensor serial handler...") sensorSerialHandler = SerialHandler(CONSTANTS.SENSOR_BOARD_PORT) sensorSerialHandler.initSerial() val = "" val = sensorHandler.getSensorValues() LOGGER.Debug(val) if CONSTANTS.USING_MOTOR_BOARD: LOGGER.Debug("Initializing motor serial handler...") motorSerialHandler = SerialHandler(CONSTANTS.MOTOR_BOARD_PORT) motorSerialHandler.initSerial() if CONSTANTS.USING_NETWORK_COMM: networkClient = NetworkClient(CONSTANTS.CONTROL_STATION_IP, CONSTANTS.CONTROL_STATION_PORT) inboundMessageQueue = MessageQueue() networkClient.setInboundMessageQueue(inboundMessageQueue)
# sensorHandlerLock.release() def ceaseAllMotorFunctions(): #Stop all motors leftMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) rightMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) # #initialize handlers motorHandler = MotorHandler() sensorHandler = SensorHandler() if CONSTANTS.USING_SENSOR_BOARD: LOGGER.Debug("Initializing sensor serial handler...") sensorSerialHandler = SerialHandler(CONSTANTS.SENSOR_BOARD_PORT) sensorSerialHandler.initSerial() val = "" val = sensorHandler.getSensorValues() LOGGER.Debug(val) if CONSTANTS.USING_MOTOR_BOARD: LOGGER.Debug("Initializing motor serial handler...") motorSerialHandler = SerialHandler(CONSTANTS.MOTOR_BOARD_PORT) motorSerialHandler.initSerial() if CONSTANTS.USING_NETWORK_COMM: networkClient = NetworkClient(CONSTANTS.CONTROL_STATION_IP, CONSTANTS.CONTROL_STATION_PORT) inboundMessageQueue = MessageQueue()
LOGGER.Debug(outboundSensorMessage) sensorSerialHandler.sendMessage(outboundSensorMessage) #sensorHandlerLock.release() def ceaseAllMotorFunctions(): #Stop all motors leftDriveMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) rightDriveMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) collectorDepthMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) collectorScoopsMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) winchMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) #initialize handlers LOGGER.Debug("Initializing handlers...") motorHandler = MotorHandler() sensorHandler = SensorHandler() if CONSTANTS.USING_SENSOR_BOARD: LOGGER.Debug("Initializing sensor serial handler...") sensorSerialHandler = SerialHandler(CONSTANTS.SENSOR_BOARD_PORT) sensorSerialHandler.initSerial() if CONSTANTS.USING_MOTOR_BOARD: LOGGER.Debug("Initializing motor serial handler...") motorSerialHandler = SerialHandler(CONSTANTS.MOTOR_BOARD_PORT) motorSerialHandler.initSerial() #initialize network comms & server thread if CONSTANTS.USING_NETWORK_COMM:
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
# sensorHandlerLock.release() def ceaseAllMotorFunctions(): #Stop all motors leftDriveMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) rightDriveMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) #collectorDepthMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) #collectorScoopsMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) #winchMotor.setSetpoint(MOTOR_MODES.K_PERCENT_VBUS, 0.0) #initialize handlers LOGGER.Debug("Initializing handlers...") motorHandler = MotorHandler() sensorHandler = SensorHandler() if CONSTANTS.USING_SENSOR_BOARD: LOGGER.Debug("Initializing sensor serial handler...") sensorSerialHandler = SerialHandler(CONSTANTS.SENSOR_BOARD_PORT) sensorSerialHandler.initSerial() if CONSTANTS.USING_MOTOR_BOARD: LOGGER.Debug("Initializing motor serial handler...") motorSerialHandler = SerialHandler(CONSTANTS.MOTOR_BOARD_PORT) motorSerialHandler.initSerial() #initialize network comms & server thread if CONSTANTS.USING_NETWORK_COMM: