class ServoCalibration:
__kit = None
__looper = True
__keyboardInput = None
__steering = None
__suspension = None
def __init__(self, servoKit):
self.__kit = servoKit
self.__keyboardInput = KeyboardInput("Steering Calibration")
config_file = servoStatusFile = os.path.abspath(
os.path.join(os.path.dirname(__file__),
"..")) + "/config/servo_status.json"
self.__steering = Steering(self.__kit, config_file)
self.__suspension = Suspension(self.__kit, config_file)
def menu(self):
self.__keyboardInput.clear()
print("J2 Controller Steering Calibration Menu:")
print()
print("c: Setup Wheel ports (On Adafruit PWM Board)")
print("n: Setup Suspension ports (On Adafruit PWM Board)")
print("w: Front left Wheel")
print("e: Front right Wheel")
print("s: Rear left Wheel")
print("d: Rear right Wheel")
print("o: Front Suspension")
print("k: Rear Suspension")
print("r: Save current status")
print("t: Reset all to *_start angle in sttering_status.json")
print("a: Set Actuation Angle")
print("--------------------")
print("q: Back")
print("")
self.waitForInput()
def waitForInput(self):
while self.__looper:
keyp = self.__keyboardInput.readkey()
if (keyp == 'q'):
print("Saving...")
self.__steering.save_servo_status()
print("Quit")
self.__looper = False
elif (keyp == 'c'):
print("Enter Port on which front_left steering motor is: ")
self.__steering.set_steering_port(Steering.FRONT_LEFT_POS,
input())
print("Enter Port on which front_right steering motor is: ")
self.__steering.set_steering_port(Steering.FRONT_RIGHT_POS,
input())
print("Enter Port on which rear_left steering motor is: ")
self.__steering.set_steering_port(Steering.REAR_LEFT_POS,
input())
print("Enter Port on which rear_right steering motor is: ")
self.__steering.set_steering_port(Steering.REAR_RIGHT_POS,
input())
self.__steering.save_servo_status()
elif (keyp == 'w'):
self.calibrate_steering(1)
elif (keyp == 'e'):
self.calibrate_steering(2)
elif (keyp == 's'):
self.calibrate_steering(3)
elif (keyp == 'd'):
self.calibrate_steering(4)
elif (keyp == 'r'):
self.__steering.save_servo_status()
print("Saved to file", end='\r', flush=True)
elif (keyp == 't'):
self.__steering.move_servo_to(
Steering.FRONT_LEFT_POS,
self.__steering.servo_status.front_left_start)
self.__steering.move_servo_to(
Steering.FRONT_RIGHT_POS,
self.__steering.servo_status.front_right_start)
self.__steering.move_servo_to(
Steering.REAR_LEFT_POS,
self.__steering.servo_status.rear_left_start)
self.__steering.move_servo_to(
Steering.REAR_RIGHT_POS,
self.__steering.servo_status.rear_right_start)
print("\r\n Servos updated")
print("\r\n Servo status")
self.__steering.print_servo_stats()
elif (keyp == 'a'):
print("Set actuation degrees [180-270]: ")
self.__steering.set_actuation_degrees(int(input()))
time.sleep(0.01)
def calibrate_steering(self, index):
waitForWheel = True
keyboardInput = KeyboardInput("Calibrate Steering:")
print(
"Press Up/Down to test or w/z to adjust wheel delta, 0 to reset, q when done"
)
while waitForWheel:
key = keyboardInput.readkey()
if (key == 'w'):
self.__steering.increment_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 16):
self.__steering.move_servo_by(index, 1)
elif (key == 'z'):
self.__steering.decrement_position(index, 1)
self.__steering.update_servos()
elif (ord(key) == 17):
self.__steering.move_servo_by(index, -1)
elif (key == 'q'):
self.__steering.save_servo_status()
waitForWheel = False
time.sleep(0.01)
class J2controller():
"""The J2 Controller Main processing loop"""
__looper = True
__kit = None
__bt_client = None
__terminalMenu = TerminalMenu()
__piconzero_drive = None # PiconzeroDrive()
__gpiozero_drive = GpiozeroDrive()
__space_invaders = None
__steering = None
__suspension = None
__debug = False
def __init__(self, arg):
self.args = arg
isDebug = False
if (len(self.args) > 1):
print("\n".join(self.args[1:]))
if (self.args[1] == "debug"):
self.__debug = True
atexit.register(self.cleanup)
self.__connect_to_cruncher()
try:
self.__kit = ServoKit(channels=16)
self.__space_invaders = SpaceInvaders(self.__kit)
except:
print("Servokit not initialised, Servo Calibration won't work")
try:
self.__steering = Steering(self.__kit,
steeringStatusFile=os.path.abspath(
"./config/steering_status.json"))
self.__suspension = Suspension(self.__kit)
except:
type, value, traceback = sys.exc_info()
print("Steering status failed to load")
print('Error Details %s: %s %s' % (type, value, traceback))
def init(self):
while self.__looper:
if (self.__bt_client == None):
self.__connect_to_cruncher()
elif (self.__bt_client.connected == False):
try:
print("Trying to connect")
self.__bt_client.connect()
except KeyboardInterrupt:
self.__looper = False
self.cleanup()
except:
type, value, traceback = sys.exc_info()
print('Error Details %s: %s %s' % (type, value, traceback))
time.sleep(1 / 60)
def __connect_to_cruncher(self):
try:
self.__bt_client = BluetoothClient("B8:27:EB:55:22:18",
self.data_received,
auto_connect=True)
if (self.__debug):
print(
"Bluetooth client initialised, ready for Cruncher comms:")
except:
type, value, traceback = sys.exc_info()
if (self.__debug):
print(
"Bluetooth client not initialised, Cruncher comms won't work"
)
print('Error Details %s: %s %s' % (type, value, traceback))
time.sleep(1)
def data_received(self, data_string):
#print("BT Recieved:" + data_string)
try:
lines = data_string.splitlines()
for line in lines:
request = BtRequest(json_def=line)
if (request.cmd == "calibrate"):
if (request.action == "getStatus"):
self.bt_request.send(
json.dumps(self.__steering.steering()))
elif (request.action == "setStatus"):
# self.bt_request.s
pass
if (request.cmd == "suspension"):
if (request.action == "raise"):
if (request.data.which == "front"):
self.__suspension.raise_front_by(
request.data.position)
elif (request.data.which == "rear"):
self.__suspension.raise_rear_by(
request.data.position)
elif (request.data.which == "both"):
self.__suspension.raise_both_by(
request.data.position)
if (request.action == "lower"):
if (request.data.which == "front"):
self.__suspension.lower_front_by(
request.data.position)
elif (request.data.which == "rear"):
self.__suspension.lower_rear_by(
request.data.position)
elif (request.data.which == "both"):
self.__suspension.lower_both_by(
request.data.position)
if (request.cmd == "steering"):
if (request.action == "move"):
self.__gpiozero_drive.move(
int(request.data.directionLeft),
int(request.data.directionRight),
float(request.data.speedLeft),
float(request.data.speedRight))
if (request.cmd == "wheels"):
if (request.action == "strafe"):
# if(self.__debug):
# print("Strafing")
self.__steering.move_servo_to(
Steering.FRONT_LEFT_POS,
int(request.data.frontLeftAngle))
self.__steering.move_servo_to(
Steering.FRONT_RIGHT_POS,
int(request.data.frontRightAngle))
self.__steering.move_servo_to(
Steering.REAR_LEFT_POS,
int(request.data.rearLeftAngle))
self.__steering.move_servo_to(
Steering.REAR_RIGHT_POS,
int(request.data.rearRightAngle))
if (request.cmd == "cannon"):
print("BT Recieved:" + data_string)
if (request.action == "aim"):
self.__space_invaders.aim(int(request.data.position))
# Turn laser on
elif (request.action == "launch"):
# Fire appropriate cannon
self.__space_invaders.launch(int(
request.data.position))
except:
type, value, traceback = sys.exc_info()
if (self.__debug):
print('Error Deserialising %s: %s %s' %
(data_string, type, value))
def cleanup(self):
if (self.__piconzero_drive != None):
self.__piconzero_drive.cleanup()