def setup(self):
# initialize sensors, settings, start timers, etc.
self.gameTimer = Timer()
# Pins (7,22); (3,23); (0,21) work.
# Problem was with the Double Motor controller.
self.motorGripper = Motor(self.tamp, 23, 3)
self.motorLeft = Motor(self.tamp, 7, 22)
self.motorRight = Motor(self.tamp, 0, 21) # good
self.motorval = 0
self.motorLeft.write(1, 0)
self.motorRight.write(1, 0)
self.motorGripper.write(1, 0)
self.currentGripperLevel = 2
print "Motors connected."
self.servoDoor = Servo(self.tamp, 20)
self.servovalDoor = self.DOOR_CLOSE_POS
self.servoDoor.write(self.DOOR_CLOSE_POS)
self.timerDoor = Timer()
self.servoGripper = Servo(self.tamp, 10)
self.servovalGripper = self.GRIPPER_CLOSE_POS
self.servoGripper.write(self.servovalGripper)
self.timerGripper = Timer()
print "Servos connected."
left_pins = 6, 5
right_pins = 3, 4
# Encoder doesn't work when after gyro
self.encoderLeft = Encoder(self.tamp, 6, 5, continuous=False)
self.encoderRight = Encoder(self.tamp, 3, 4, continuous=True)
print "Encoders connected."
# TODO: set encoder to 0
self.timer = Timer()
self.gyro = Gyro(self.tamp, 9)
print "Gyro connected."
self.theta = self.gyro.val
self.dT = .01
self.cam = cv2.VideoCapture(0)
print "Camera connected."
# self.color = Color(self.tamp,
# integrationTime=Color.INTEGRATION_TIME_101MS,
# gain=Color.GAIN_1X)
self.encoderLeft.start_continuous()
self.encoderRight.start_continuous()
frontLeftIR_pin = 14
self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin)
frontRightIR_pin = 15
self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin)
leftIR_pin = 16
self.leftIR = AnalogInput(self.tamp, leftIR_pin)
rightIR_pin = 17
self.rightIR = AnalogInput(self.tamp, rightIR_pin)
# Initialize PID Values
self.P = 10
self.I = 5
self.D = 5
self.last_diff = 0
self.integral = 0
self.desiredAngle = self.theta
self.finishedCollectingBlock = False
self.finishedDiscardingBlock = False
self.timer = Timer()
self.state = ExploreState()
# self.state = CollectBlockState()
self.blocksCollected = 0
self.leftIRVals = deque([])
self.rightIRVals = deque([])
self.frontRightIRVals = deque([])
self.frontLeftIRVals = deque([])
self.goIntoTimeoutState = False
self.timeoutCounter = 0
# Starts the robot
print "Robot setup complete."
