def sendToArduino():
global cmdBuffer
while True:
try:
if cmdBuffer == []:
continue
else:
outString = cmdBuffer[-1]
print(outString[0])
if outString[1] == ghzLed:
ardSocket.sendto(bytes(outString[0][:-2],'utf-8'), ardConnectData)
elif outString[1] == mhzLed:
#for i in range(2):
ardSocket.sendto(bytes(outString[0], 'utf-8'), ardConnectData)
cmdBuffer = []
re_data = ardSocket.recvfrom(512)
while bytes.decode(re_data[0]) != "r":
re_data = ardSocket.recvfrom(512)
#print("after reading r: ", re_data)
try:
# Write to LED lights bus
writeToBus(int(outString[0][-1]), ghzLed) if int(outString[1]) == ghzLed else writeToBus(drivingMode, mhzLed)
except:
print("LED error")
except:
cmdBuffer = []
#print("Ard fail")
pass
def connectionLost():
global storedPoints, connected, myDriver
'''
while len(storedPoints) > 0 and not connected:
myDriver.goTo(storedPoints.pop())
'''
if not connected:
writeToBus(4, 4)
def sendMotors(self):
'''
Description:
Performs Arduino socket call with appropriate self.__motor1 and self.__motor2 motor values
Args:
None
Returns:
Nothing
'''
try:
outString = str(self.__motor1) + ',' + str(self.__motor2) + '0,0,0,0,0,0,0,4'
self.__client_socket.sendto(bytes(outString,'utf-8'), self.__address)
writeToBus(autoLed, autoLed)
except:
print("Arduino send failed")
Flashes LED pattern for on board visual cue.
Args:
None
Returns:
Nothing
'''
self.__motor1 = self.__motor2 = 0
self.sendMotors()
'''
# Blink lights
leds = [0, 1, 2]
color = toggle = 0
for i in range(12): # Blink red, green, and blue for 3 seconds
try:
writeToBus(color, color)
time.sleep(0.25)
toggle += 1
if toggle >= len(leds):
toggle = 0
color = leds[toggle]
except:
pass
#print("led fail in notifyArrival")
def setMinMaxFwdSpeeds(self, min, max):
'''
Description:
Method overwrites default speedY[min, max] speeds. Confined to MINFORWARDSPEED
and MAXFORWARDSPEED.
Args:
def connectionLost():
global storedPoints, ghzConnection, mhzConnection, myDriver
while True:
while len(storedPoints) > 0 and not ghzConnection and not mhzConnection:
myDriver.goTo(storedPoints.pop())
writeToBus(4, 4)
def main(*argv):
global paused, mobiliyMode, ser
startUp(argv) # Load appropriate controller(s) config file
joystick_count = pygame.joystick.get_count()
for i in range(joystick_count):
pygame.joystick.Joystick(i).init()
while True:
pygame.event.pump() # Keeps pygame in sync with system, performs internal upkeep
joystick_count = pygame.joystick.get_count()
if joystick_count == 0:
stop()
for i in range(joystick_count):
joystick = pygame.joystick.Joystick(i)
checkAxes(joystick)
checkHats(joystick)
checkButtons(joystick)
throttleStep()
checkRotate()
checkPause()
checkModes()
setLed()
#print("Sending Tx2 command")
#re_data = client_socket.recvfrom(512)
#print(bytes.decode(re_data[0])) # Debug
#if bytes.decode(re_data[0]) == "r":
# print("Received packet") # Debug
if paused:
outVals = list(map(getZero, actionList))
else:
outVals = turn(list(map(computeSpeed, actionList))) # Output string determined by actionList[] order
# make a copy of the outVals List because this is what we will package and send over the socket, and ham frequency
# we will also package the values to crunch the bytes down, instead of AF_INET, SOCK_STREAMsending a string
o = outVals
t = round(time(), 3)
ghzBytePack = pack('s 10h d s', 'a'.encode('utf-8'), o[0], o[1], o[2], o[3], o[4], o[5], o[6], o[7], o[8], o[9], t, '#'.encode('utf-8'))
hamBytePack = pack('s 10h d s', 'b'.encode('utf-8'), o[0], o[1], o[2], o[3], o[4], o[5], o[6], o[7], o[8], 3, t, '#'.encode('utf-8'))
#writeToBus(o[9], o[9])
if "mode" in mobilityMode:
if "roverType" in mobilityMode:
if mobilityMode["mode"] == "manual" and mobilityMode["roverType"] == os.environ["roverType"]:
try:
pass #check - What is this pass for?
client_socket.sendto(ghzBytePack, address) # string bytes
writeToBus(1, o[9])
print(outVals)
except:
print("Couldn't send over Ghz")
'''
try:
if os.environ['roverType'] == 'base' and hamPiRelaySocket != None:
hamPiRelaySocket.sendto(hamBytePack, ('192.168.1.5', 9005))
print('SENT HAM DATA')
#ser.write(hamBytePack) # packed bytes
except:
try:
hamPiRelaySocket.close() #check - doesn't this close need a delay before new socket
hamPiRelaySocket = socket(AF_INET, SOCK_STREAM) #check - shouldn't this be UDP
hamPiRelaySocket.connect(('192.168.1.5', 9005))
except:
pass
print("Coudn't send over Ham")
'''
else:
#check -how does this pause the rover???
print("Pausing mobility becuase of deepstream record: " + str(mobilityMode))
else:
print("The key 'roverType' is missing from the deepstream record: mode")
#print(ghzBytePack)
#print(hamBytePack)
print()
else:
print("Not in Manual Mode, MobilityMode: " + ' HAS NOT BEEN SET IN DEEPSTREAM' if mobilityMode == {} else str(mobilityMode))