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."