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