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