def __init__(self):
#Get the motors set up
GyroAndCompassRobot.__init__(self)
with open('config.json') as configFile:
KillableRobot.__init__(self, killCode = json.load(configFile).get('killCode') or 228)
# Camera orientation - numbers need checking
self.cameraMatrix = (
Matrix4.new_translate(0, 0.48, 0) * #0.5m up from the center of the robot
Matrix4.new_rotatex(math.radians(18)) #Tilted forward by 18 degrees
)
# Cache, since .inverse is expensive
self.worldTransform = self.cameraMatrix.inverse()
#Set compass zero offset
self.compass.heading = 0
#get hardware
self.arm = Arm()
self.lifter = Lifter()
self.us = Ultrasonic()
