class Robot(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
# Arduino and sensor properties
self.ard = arduino.Arduino()
self.pid = arduino.PID(self.ard)
self.motors = Motors(self.ard)
self.bumpers = Bumpers(self.ard)
self.ir = IR(self.ard)
self.vision = Vision()
self.vision.color = Color.Red
self.vision.features = Feature.Ball
self.time = Timer(self)
self.servoBridge = arduino.Servo(self.ard, 6)
self.servoGate = arduino.Servo(self.ard, 3)
# self.bridgeBump = arduino.DigitalInput(self.ard, 27)
# Properties for match
self.ready = False
self.scoring = False
self.buttoned = False # Button has been pushed
self.deployed = False # Bridge has been deployed
self.map = {}
self.tower = []
self.wall = []
self.repeatedBarcodes = False
def run(self):
self.ard.start()
while self.ard.portOpened == False:
time.sleep(0.05)
# self.motors.start()
self.servoGate.setAngle(45)
self.servoBridge.setAngle(5)
self.bumpers.start()
self.ir.start()
self.vision.start()
self.ready = True
def stop(self):
self.pid.stop()
# self.motors.stop()
self.servoGate.setAngle(45)
self.servoBridge.setAngle(5)
self.bumpers.stop()
self.ir.stop()
self.vision.stop()
time.sleep(1)
self.ard.stop()
self.ready = False
def reverse(self, bumped):
speed = 80
# Reverse for 2 seconds
self.motors.right.setSpeed(-speed)
self.motors.left.setSpeed(-speed)
time.sleep(2)
# Rotate for 1 second
if bumped[0] == True:
# Left bump sensor hit
self.motors.right.setSpeed(-speed)
self.motors.left.setSpeed(speed)
elif bumped[1] == True:
# Right bump sensor hit
self.motors.right.setSpeed(speed)
self.motors.left.setSpeed(-speed)
time.sleep(1)
# Move forward for 1 second
self.motors.right.setSpeed(speed)
self.motors.left.setSpeed(speed)
time.sleep(1)
self.motors.right.setSpeed(0)
self.motors.left.setSpeed(0)
def getFarthestPoint(self):
startAngle = ard.getHeading()
firData = []
self.ir.running = True
self.motors.left.setSpeed(50)
self.motors.right.setSpeed(-50)
while self.time.elapsed() > 1 and ard.getHeading() - startAngle < 0.2:
firData.append((self.ir.firRight.getValues(),
self.ir.firLeft.getValues(),
ard.getHeading()))