def __init__(self):
BluetoothServer.__init__(self)
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
GPIO.setup(11, GPIO.OUT)
self.pwm = GPIO.PWM(11, 50)
self.pwm.start(5)
def __init__(self):
BluetoothServer.__init__(self)
def handleRedirect(self):
BluetoothServer.send("D4:3B:04:76:E7:55")
"NAVigate:PICKup": ScpiCommandInline(write=set_pickup, read=query_pickup),
"NAVigate:DROPoff": ScpiCommandInline(write=set_dropoff, read=query_dropoff),
"NAVigate:PASSengers": ScpiCommandInline(write=set_passengers, read=query_passengers),
"SYStem:VELocity": ScpiCommandInline(write=set_vel),
"SYStem:P": ScpiCommandInline(write=set_p),
"SYStem:I": ScpiCommandInline(write=set_i),
"SYStem:D": ScpiCommandInline(write=set_d),
"*IDN": ScpiCommandInline(write=lambda args: None, read=lambda args: "AGV-3 0.1.1")
}
if __name__ == '__main__':
# Init modules
server = ScpiServer(cmds)
bluetooth = BluetoothServer("AGV-3", server)
sensors = AGVSensors()
motors = ScpiClient("/dev/ttyACM0", baudrate=230400)
mode = LineMode.DISABLE
cameras = AGVCamera()
time.sleep(1)
integral = 0
last_i = 0
print("[MAIN] Battery voltage: %f V" % sensors.get_battery())
# Start main loop
dist_integral = 0.0
print("[MAIN] Starting main loop...")
state.start() # initialize start state
try: