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:
