def setup(self):
self.motor = Motor(self.tamp, 2, 3)
self.motor2 = Motor(self.tamp, 8, 9)
self.motor.write(1, 0)
self.timer = Timer()
def setup(self):
# =============== Encoder setup =============== #
self.encoder_left = Encoder(self.tamp, 29, 30, continuous=False)
self.encoder_right = Encoder(self.tamp, 19, 20, continuous=False)
# =============== IR setup ================= #
self.left_front = AnalogInput(self.tamp, 10)
self.left_side = AnalogInput(self.tamp, 11)
self.right_front = AnalogInput(self.tamp, 12)
self.right_side = AnalogInput(self.tamp, 13)
self.run_timer = Timer()
self.RUN_TIME = 2000
# =============== Gyro setup =============== #
self.gyro = Gyro(self.tamp, 10, integrate=True)
# =============== Motor setup =============== #
self.motor_left = Motor(self.tamp, 2, 3)
self.motor_right = Motor(self.tamp, 8, 9)
self.motor_left.write(True, 50)
self.motor_right.write(True, 50)
self.encoder_left.update()
self.encoder_right.update()
# =============== Timer setup =============== #
self.timer = Timer()
self.INTERVAL = 30 # 30ms ~ 33.3 hz
# =============== PID setup =============== #
self.PID_controller = PID(self.INTERVAL / 1000.)
self.desired_center = 80
self.desired_angle = 0
# =============== Vision setup ============= #
self.context = zmq.Context()
self.socket = self.context.socket(zmq.REQ)
self.socket.connect("tcp://localhost:5555")
# ============ Sorting setup ============ #
self.LEFT_TOWER = 87
self.RIGHT_TOWER = 24
self.CENTER = 53
self.color = Color(self.tamp,
integrationTime=Color.INTEGRATION_TIME_101MS,
gain=Color.GAIN_1X)
self.servo = Servo(self.tamp, 23)
self.servo.write(self.CENTER + 10)
self.servo_val = self.CENTER - 10
self.servo_bottom = Servo(self.tamp, 22)
self.servo_bottom.write(34)
self.sorter_timer = Timer()
self.color_switch = DigitalInput(self.tamp, 14)
self.color_counter = 0
self.previous_color = "RED"
self.current_color = "RED"
# ============== Intake setup ============ #
self.motor = Motor(self.tamp, 24, 25)
self.motor.write(True, 0)
self.conveyor_encoder = Encoder(self.tamp, 32, 32, continuous=True)
self.conveyor_encoder.update()
self.prev_encoder_value = 0
self.intake_timer = Timer()
# ============== State Machine setup =========== #
self.state = "SEARCH"
self.state_timer = Timer()
#for search
self.turn_timer = Timer()
self.sign = 1
#for timeout
self.prev_enc_left = 0
self.prev_enc_right = 0
self.overal_runtime = Timer()
self.timer2 = Timer()
self.stall_couter = 0
self.desired_angle_timeout = 0
