Ejemplo n.º 1
0
class Blink(Sketch):
    def setup(self):
        self.led = DigitalOutput(self.tamp, 13)
        self.led_timer = Timer()
        self.led_state = False

    def loop(self):
        if self.led_timer.millis() > 200:
            self.led_timer.reset()
            self.led_state = not self.led_state
            self.led.write(self.led_state)
Ejemplo n.º 2
0
class Blink(Sketch):
    def setup(self):
        self.led = DigitalOutput(self.tamp, 13)
        self.led_timer = Timer()
        self.led_state = False

    def loop(self):
        if self.led_timer.millis() > 200:
            self.led_timer.reset()
            self.led_state = not self.led_state
            self.led.write(self.led_state)
Ejemplo n.º 3
0
class GyroAndEncoderRead(SyncedSketch):
    ss_pin = 10

    def setup(self):
        self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True)
        self.timer = Timer()
        self.gyro_balancer = Timer()

    def loop(self):
        if self.timer.millis() > 100:
            self.timer.reset()
            print "Gyro stuff: " + str(-self.gyro.val - self.gyro_balancer.millis()/2500) + " " + str(self.gyro.status)
Ejemplo n.º 4
0
class MotorWrite(Sketch):
    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 loop(self):
        if self.timer.millis() > 100:
            self.timer.reset()
            self.motor.write(True, abs(50))
            self.motor2.write(True, abs(50))
Ejemplo n.º 5
0
class MotorWrite(Sketch):

    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 loop(self):
        if self.timer.millis() > 100:
            self.timer.reset()
            self.motor.write(True, abs(50))
            self.motor2.write(True, abs(50))
Ejemplo n.º 6
0
class EncoderRead(SyncedSketch):
    ss_pin = 10

    def setup(self):
        self.encoder_left = Encoder(self.tamp, 29,30, continuous=False)
        self.encoder_right = Encoder(self.tamp, 19, 20, continuous=False)
        self.timer = Timer()

    def loop(self):
        if self.timer.millis() > 200:
            self.timer.reset()
            self.encoder_left.update()
            self.encoder_right.update()
            print "Left encoder: " + str(self.encoder_left.val)
            print "Right encoder: " + str(self.encoder_right.val)
Ejemplo n.º 7
0
class EncoderRead(SyncedSketch):
    ss_pin = 10

    def setup(self):
        self.encoder_left = Encoder(self.tamp, 29, 30, continuous=False)
        self.encoder_right = Encoder(self.tamp, 19, 20, continuous=False)
        self.timer = Timer()

    def loop(self):
        if self.timer.millis() > 200:
            self.timer.reset()
            self.encoder_left.update()
            self.encoder_right.update()
            print "Left encoder: " + str(self.encoder_left.val)
            print "Right encoder: " + str(self.encoder_right.val)
Ejemplo n.º 8
0
class IRBot(SyncedSketch):
    def setup(self):
        # =============== 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.timer = Timer()

    def loop(self):
        if self.timer.millis() > 300:
            self.timer.reset()
            lf = self.convertToInches(self.left_front.val)
            ls = self.convertToInches(self.left_side.val)
            rf = self.convertToInches(self.right_front.val)
            rs = self.convertToInches(self.right_side.val)
            print ls, lf, rf, rs

    def convertToInches(self, value):
        inches = 0 if value <= 0 else -5.07243 * ln(0.0000185668 * value)
        return inches
Ejemplo n.º 9
0
class IRBot(SyncedSketch):
    def setup(self):
        # =============== 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.timer = Timer()
        

    def loop(self):
        if self.timer.millis() > 300:
            self.timer.reset()
            lf = self.convertToInches(self.left_front.val)
            ls = self.convertToInches(self.left_side.val)
            rf = self.convertToInches(self.right_front.val)
            rs = self.convertToInches(self.right_side.val)
            print ls, lf, rf, rs



    def convertToInches(self, value):
        inches = 0 if value <= 0 else -5.07243 * ln(0.0000185668 * value)
        return inches
Ejemplo n.º 10
0
class MainBot(SyncedSketch):
    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

    def loop(self):
        try:
            if self.overal_runtime.millis() > 180000:
                raise SystemExit("TIME'S UP!")

            if self.sorter_timer.millis() > 300:
                self.sorter_timer.reset()
                if self.servo_val > self.CENTER:
                    self.servo.write(self.CENTER - 8)
                    self.servo_val = self.CENTER - 8
                else:
                    self.servo.write(self.CENTER + 8)
                    self.servo_val = self.CENTER + 8

                detect_red = self.color.r > 1.3*self.color.g
                detect_green = self.color.g > 1.3*self.color.r
                sum_val = self.color.r+self.color.g+self.color.b
                #if detect_red and sum_val > 300:
                if(detect_red):
                    self.current_color = "RED"
                elif(detect_green):
                    self.current_color = "GREEN"



                if(sum_val > 300 and self.current_color == self.previous_color):
                    self.color_counter += 1
                else:
                    self.color_counter = 0
                print self.color_counter
                if(self.color_counter > 5):
                    self.servo.write(self.CENTER)
                    self.color_counter = 0
                    return

                self.previous_color = self.current_color

                if (detect_red and sum_val > 300 and self.color_switch.val) or \
                    (detect_green and sum_val > 300 and not self.color_switch.val):

                    self.servo.write(self.LEFT_TOWER)
                    self.servo_val = self.LEFT_TOWER
                #elif detect_green and sum_val > 300:
                elif (detect_green and sum_val > 300 and self.color_switch.val) or \
                    (detect_red and sum_val > 300 and not self.color_switch.val):
                    self.servo.write(self.RIGHT_TOWER)
                    self.servo_val = self.RIGHT_TOWER
                else:
                    return

            if self.intake_timer.millis() > 200:
                self.motor.write(True,125)
                encoder_val = self.conveyor_encoder.val
                if abs(self.prev_encoder_value - encoder_val) <= 50:
                    self.motor.write(False,150)
                self.prev_encoder_value = encoder_val
                self.intake_timer.reset()

            if self.timer2.millis() > 100:
                self.timer2.reset()
                self.encoder_left.update()
                self.encoder_right.update()
                cur_enc_left = -self.encoder_left.val
                cur_enc_right = self.encoder_right.val
                if abs(cur_enc_left - self.prev_enc_left) <= 40 and abs(cur_enc_right - self.prev_enc_right) <= 40:
                    self.stall_couter += 1
                    if self.stall_couter > 20:
                        self.state_timer.reset()
                        print "Left:", cur_enc_left, self.prev_enc_left
                        print "Right:", cur_enc_right, self.prev_enc_right
                        self.state_timer.reset()
                        self.stall_couter = 0
                        self.state = "TIMEOUT"
                else: 
                    self.stall_couter = 0
                self.prev_enc_left = cur_enc_left
                self.prev_enc_right = cur_enc_right

            if self.timer.millis() > 30:
                self.timer.reset()

                if self.state == "TIMEOUT":
                    print self.state, 
                    if self.state_timer.millis() < 400:
                        self.motor_left.write(False, 30)
                        self.motor_right.write(False, 30)
                        self.desired_angle_timeout = [random.randint(60,120),random.randint(-120,-60)][random.randint(0,1)]+self.gyro.val                        
                    elif self.state_timer.millis() > 400:
                        power = self.PID_controller.power(self.desired_angle_timeout)
                        speed = min(30, abs(power))

                        if abs(speed) < 10:
                            speed = 0

                        if power >= 0:
                            self.motor_left.write(True, speed)
                            self.motor_right.write(False, speed)
                        else:
                            self.motor_left.write(False, speed)
                            self.motor_right.write(True, speed)

                    if abs(self.gyro.val-self.desired_angle_timeout) < 5:
                    #elif self.state_timer.millis() > 1200:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "EXPLORE"


                if self.state == "EXPLORE":
                    lf = self.convertToInches(self.left_front.val)
                    ls = self.convertToInches(self.left_side.val)
                    rf = self.convertToInches(self.right_front.val)
                    rs = self.convertToInches(self.right_side.val)
                    if rf <= .5:
                        rf = 20
                    detections = [ls < 4, lf < 5, rf < 5, rs < 4]
                    print self.state, detections
                    if detections == [0,0,0,0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(True, 50)
                    elif detections == [0,0,1,0] or detections == [0,0,0,1] or detections == [0,0,1,1]:
                        self.motor_left.write(False, 50)
                        self.motor_right.write(True, 50)
                    elif detections == [0,1,0,0] or detections == [1,0,0,0] or detections == [1,1,0,0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(False, 50)
                    elif detections == [0,1,1,0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(False, 50)

                    if self.state_timer.millis() > 5000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"


                if self.state == "SEARCH":
                    self.socket.send(b"get_image")
                    message = self.socket.recv()
                    message = message.split(",")
                    print self.state, message
                    if message[0] == "None":
                        if self.turn_timer.millis() > 3000:
                            self.turn_timer.reset()
                            self.sign *= -1
                        diff = 90 * self.sign + self.gyro.val
                    else:
                        diff = int(message[1]) - 80
                        if abs(diff) < 5:
                            diff = 0

                    if diff == 0:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "APPROACH"

                    power = self.PID_controller.power(diff)
                    speed = min(30, abs(power))

                    if abs(speed) < 10:
                        speed = 0

                    if power >= 0:
                        self.motor_left.write(True, speed)
                        self.motor_right.write(False, speed)
                    else:
                        self.motor_left.write(False, speed)
                        self.motor_right.write(True, speed)

                    if self.state_timer.millis() > 7000:
                        if message[0] != "None":
                            self.motor_left.write(True, 0)
                            self.motor_right.write(True, 0)
                            self.state_timer.reset()
                            self.state = "APPROACH"
                        else:
                            self.motor_left.write(True, 0)
                            self.motor_right.write(True, 0)
                            self.state_timer.reset()
                            self.state = "EXPLORE"

                if self.state == "APPROACH":
                    self.socket.send(b"get_image")
                    message = self.socket.recv()
                    message = message.split(",")
                    print self.state, message

                    if message[0] == "None":
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

                    diff = int(message[1]) - self.desired_center

                    if abs(diff) < 5 and int(message[2]) > 90:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "COLLECT"

                    diff = 0 if abs(diff) < 3 else diff
                    power = self.PID_controller.power(diff)

                    self.motor_left.write(True, min(max(0, 30 + power), 30))
                    self.motor_right.write(True, min(max(0, 30 - power), 30))

                    if self.state_timer.millis() > 9000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"


                if self.state == "COLLECT":
                    print self.state
                    self.motor_left.write(True, 40)
                    self.motor_right.write(True, 40)
                    if self.state_timer.millis() > 2000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

        except (KeyboardInterrupt, SystemExit):
            self.motor_left.write(True, 0)
            self.motor_right.write(True, 0)
            self.motor.write(True,0)
            self.servo_bottom.write(100)
            sleep(1)  
            print "YOOOOOOOOOOOOOOOO!"
            self.stop()
            self.on_exit()

            
    def convertToInches(self, value):
        inches = 0 if value <= 0 else -5.07243 * ln(0.0000185668 * value)
        return inches
Ejemplo n.º 11
0
class MainBot(SyncedSketch):
    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

    def loop(self):
        try:
            if self.overal_runtime.millis() > 180000:
                raise SystemExit("TIME'S UP!")

            if self.sorter_timer.millis() > 300:
                self.sorter_timer.reset()
                if self.servo_val > self.CENTER:
                    self.servo.write(self.CENTER - 8)
                    self.servo_val = self.CENTER - 8
                else:
                    self.servo.write(self.CENTER + 8)
                    self.servo_val = self.CENTER + 8

                detect_red = self.color.r > 1.3 * self.color.g
                detect_green = self.color.g > 1.3 * self.color.r
                sum_val = self.color.r + self.color.g + self.color.b
                #if detect_red and sum_val > 300:
                if (detect_red):
                    self.current_color = "RED"
                elif (detect_green):
                    self.current_color = "GREEN"

                if (sum_val > 300
                        and self.current_color == self.previous_color):
                    self.color_counter += 1
                else:
                    self.color_counter = 0
                print self.color_counter
                if (self.color_counter > 5):
                    self.servo.write(self.CENTER)
                    self.color_counter = 0
                    return

                self.previous_color = self.current_color

                if (detect_red and sum_val > 300 and self.color_switch.val) or \
                    (detect_green and sum_val > 300 and not self.color_switch.val):

                    self.servo.write(self.LEFT_TOWER)
                    self.servo_val = self.LEFT_TOWER
                #elif detect_green and sum_val > 300:
                elif (detect_green and sum_val > 300 and self.color_switch.val) or \
                    (detect_red and sum_val > 300 and not self.color_switch.val):
                    self.servo.write(self.RIGHT_TOWER)
                    self.servo_val = self.RIGHT_TOWER
                else:
                    return

            if self.intake_timer.millis() > 200:
                self.motor.write(True, 125)
                encoder_val = self.conveyor_encoder.val
                if abs(self.prev_encoder_value - encoder_val) <= 50:
                    self.motor.write(False, 150)
                self.prev_encoder_value = encoder_val
                self.intake_timer.reset()

            if self.timer2.millis() > 100:
                self.timer2.reset()
                self.encoder_left.update()
                self.encoder_right.update()
                cur_enc_left = -self.encoder_left.val
                cur_enc_right = self.encoder_right.val
                if abs(cur_enc_left - self.prev_enc_left) <= 40 and abs(
                        cur_enc_right - self.prev_enc_right) <= 40:
                    self.stall_couter += 1
                    if self.stall_couter > 20:
                        self.state_timer.reset()
                        print "Left:", cur_enc_left, self.prev_enc_left
                        print "Right:", cur_enc_right, self.prev_enc_right
                        self.state_timer.reset()
                        self.stall_couter = 0
                        self.state = "TIMEOUT"
                else:
                    self.stall_couter = 0
                self.prev_enc_left = cur_enc_left
                self.prev_enc_right = cur_enc_right

            if self.timer.millis() > 30:
                self.timer.reset()

                if self.state == "TIMEOUT":
                    print self.state,
                    if self.state_timer.millis() < 400:
                        self.motor_left.write(False, 30)
                        self.motor_right.write(False, 30)
                        self.desired_angle_timeout = [
                            random.randint(60, 120),
                            random.randint(-120, -60)
                        ][random.randint(0, 1)] + self.gyro.val
                    elif self.state_timer.millis() > 400:
                        power = self.PID_controller.power(
                            self.desired_angle_timeout)
                        speed = min(30, abs(power))

                        if abs(speed) < 10:
                            speed = 0

                        if power >= 0:
                            self.motor_left.write(True, speed)
                            self.motor_right.write(False, speed)
                        else:
                            self.motor_left.write(False, speed)
                            self.motor_right.write(True, speed)

                    if abs(self.gyro.val - self.desired_angle_timeout) < 5:
                        #elif self.state_timer.millis() > 1200:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "EXPLORE"

                if self.state == "EXPLORE":
                    lf = self.convertToInches(self.left_front.val)
                    ls = self.convertToInches(self.left_side.val)
                    rf = self.convertToInches(self.right_front.val)
                    rs = self.convertToInches(self.right_side.val)
                    if rf <= .5:
                        rf = 20
                    detections = [ls < 4, lf < 5, rf < 5, rs < 4]
                    print self.state, detections
                    if detections == [0, 0, 0, 0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(True, 50)
                    elif detections == [0, 0, 1, 0] or detections == [
                            0, 0, 0, 1
                    ] or detections == [0, 0, 1, 1]:
                        self.motor_left.write(False, 50)
                        self.motor_right.write(True, 50)
                    elif detections == [0, 1, 0, 0] or detections == [
                            1, 0, 0, 0
                    ] or detections == [1, 1, 0, 0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(False, 50)
                    elif detections == [0, 1, 1, 0]:
                        self.motor_left.write(True, 50)
                        self.motor_right.write(False, 50)

                    if self.state_timer.millis() > 5000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

                if self.state == "SEARCH":
                    self.socket.send(b"get_image")
                    message = self.socket.recv()
                    message = message.split(",")
                    print self.state, message
                    if message[0] == "None":
                        if self.turn_timer.millis() > 3000:
                            self.turn_timer.reset()
                            self.sign *= -1
                        diff = 90 * self.sign + self.gyro.val
                    else:
                        diff = int(message[1]) - 80
                        if abs(diff) < 5:
                            diff = 0

                    if diff == 0:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "APPROACH"

                    power = self.PID_controller.power(diff)
                    speed = min(30, abs(power))

                    if abs(speed) < 10:
                        speed = 0

                    if power >= 0:
                        self.motor_left.write(True, speed)
                        self.motor_right.write(False, speed)
                    else:
                        self.motor_left.write(False, speed)
                        self.motor_right.write(True, speed)

                    if self.state_timer.millis() > 7000:
                        if message[0] != "None":
                            self.motor_left.write(True, 0)
                            self.motor_right.write(True, 0)
                            self.state_timer.reset()
                            self.state = "APPROACH"
                        else:
                            self.motor_left.write(True, 0)
                            self.motor_right.write(True, 0)
                            self.state_timer.reset()
                            self.state = "EXPLORE"

                if self.state == "APPROACH":
                    self.socket.send(b"get_image")
                    message = self.socket.recv()
                    message = message.split(",")
                    print self.state, message

                    if message[0] == "None":
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

                    diff = int(message[1]) - self.desired_center

                    if abs(diff) < 5 and int(message[2]) > 90:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "COLLECT"

                    diff = 0 if abs(diff) < 3 else diff
                    power = self.PID_controller.power(diff)

                    self.motor_left.write(True, min(max(0, 30 + power), 30))
                    self.motor_right.write(True, min(max(0, 30 - power), 30))

                    if self.state_timer.millis() > 9000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

                if self.state == "COLLECT":
                    print self.state
                    self.motor_left.write(True, 40)
                    self.motor_right.write(True, 40)
                    if self.state_timer.millis() > 2000:
                        self.motor_left.write(True, 0)
                        self.motor_right.write(True, 0)
                        self.state_timer.reset()
                        self.state = "SEARCH"

        except (KeyboardInterrupt, SystemExit):
            self.motor_left.write(True, 0)
            self.motor_right.write(True, 0)
            self.motor.write(True, 0)
            self.servo_bottom.write(100)
            sleep(1)
            print "YOOOOOOOOOOOOOOOO!"
            self.stop()
            self.on_exit()

    def convertToInches(self, value):
        inches = 0 if value <= 0 else -5.07243 * ln(0.0000185668 * value)
        return inches
Ejemplo n.º 12
0
class RandomBot(SyncedSketch):
    def setup(self):
        # =============== "Run Code" setup ============ #
        self.run_code_input = DigitalInput(self.tamp,15)


        # =============== 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)

        # =============== 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 = 85
        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+5)
        self.servo_val = self.CENTER-5

        self.servo_bottom = Servo(self.tamp,22)
        self.servo_bottom.write(34)
        #self.servo_bottom.write(90)
        self.second_timer = Timer()

        self.color_switch = DigitalInput(self.tamp,14)

        # ============== Intake setup ============ #
        self.motor = Motor(self.tamp, 24, 25)
        #self.motor.write(True, 125)

        self.conveyor_encoder = Encoder(self.tamp, 32,32, continuous=True)
        self.conveyor_encoder.update()
        self.prev_encoder_value = 0
        self.third_timer = Timer()
        
        self.time_out = Timer()
        self.STOP = False

        self.conveyor_counter = 0

        # =============== Match Timeout setup ============= #
        self.time_limit = time() + 3*60

    def loop(self):
        if not self.run_code_input.val:
            try:
                if time() >= self.time_limit:
                    raise SystemExit("TIME'S UP!")
                if self.STOP:
                    self.motor_left.write(True, 0)
                    self.motor_right.write(True, 0)
                    return
                #print self.third_timer.millis()
                if self.third_timer.millis() > 100:# and self.third_timer.millis() < 1400:
                    self.motor.write(True,125)
                    encoder_val = self.conveyor_encoder.val
                    print "Current encoder"
                    print encoder_val
                    print "Previous encoder value"
                    print self.prev_encoder_value
                    if abs(self.prev_encoder_value - encoder_val) <= 50:
                        self.motor.write(False,150)
                    self.prev_encoder_value = self.conveyor_encoder.val
                    self.third_timer.reset()

                if self.second_timer.millis() > 300:
                     self.second_timer.reset()
                     if self.servo_val > self.CENTER:
                         self.servo.write(self.CENTER - 5)
                         self.servo_val = self.CENTER - 5
                     else:
                         self.servo.write(self.CENTER + 5)
                         self.servo_val = self.CENTER + 5


                     detect_red = self.color.r > 1.3*self.color.g
                     detect_green = self.color.g > 1.3*self.color.r
                     sum_val = self.color.r+self.color.g+self.color.b
                     if (detect_red and sum_val > 300 and self.color_switch.val) or \
                        (detect_green and sum_val > 300 and not self.color_switch.val):
                         self.servo.write(self.LEFT_TOWER)
                         self.servo_val = self.LEFT_TOWER
                     elif (detect_green and sum_val > 300 and self.color_switch.val) or \
                        (detect_red and sum_val > 300 and self.color_switch.val):
                         self.servo.write(self.RIGHT_TOWER)
                         self.servo_val = self.RIGHT_TOWER
                     else:
                         return

                if self.timer.millis() > self.INTERVAL:
                    self.timer.reset()

                    self.socket.send(b"need_image")
                    message = self.socket.recv().split(",")

                    # debug
                    print message
                    if message[0] == "None":				# no tower found
                        lf = self.convertToInches(self.left_front.val)
                        ls = self.convertToInches(self.left_side.val)
                        rf = self.convertToInches(self.right_front.val)
                        rs = self.convertToInches(self.right_side.val)

                        print self.gyro.val

                        if self.run_timer.millis() > self.RUN_TIME or lf < 11 or ls < 8.5 or rf < 9.3 or rs < 8.5:
                            print self.time_out.millis()
                            if self.run_timer.millis() < self.RUN_TIME:
                                if self.time_out.millis() > 10000:
                                    self.motor_left.write(True, 30)
                                    self.motor_right.write(False, 30)
                                    return
                                print "COLLISION-DETECTED, adjusting"
                                if lf < 0:
                                    lf = 100
                                if ls < 0:
                                    ls = 100
                                if rf < 0:
                                    rf = 100
                                if rs < 0:
                                    rs = 100
                                detections = [ls < 8.5, lf < 11, rf < 9.3, rs < 8.5]
                                print detections
                                if detections == [True, True, True, True]:  # backup
        							self.motor_left.write(False, 50)
        							self.motor_right.write(False, 50)
                                elif detections == [True, True, True, False]:  # back up a bit and turn right
        							self.motor_left.write(True, 50)
        							self.motor_right.write(False, 50)	
                                elif detections == [False, True, True, True]: # back up a bit and turn left
        							self.motor_left.write(False, 50)
        							self.motor_right.write(True, 50)
                                elif detections == [True, True, False, False]:  # turn right
        							self.motor_left.write(True, 50)
        							self.motor_right.write(False, 50)
                                elif detections == [False, False, True, True]:  # turn left
        							self.motor_left.write(False, 50)
        							self.motor_right.write(True, 50)
                                elif detections == [True, False, False, False]: # turn slightly right
        							self.motor_left.write(True, 45)
        							self.motor_right.write(True, 20)
                                elif detections == [False, False, False, True]:  # turn slightly left
        							self.motor_left.write(True, 20)
        							self.motor_right.write(True, 45)
                                else:  # everything else, move up a bit
        							self.motor_left.write(True, 30)
        							self.motor_right.write(True, 30)
                                self.desired_angle = self.gyro.val
                                self.run_timer.reset()
                                return
                            else:
                                self.time_out.reset()
                                self.run_timer.reset()
                                self.desired_angle = random.randint(-90, 90) + self.gyro.val
                                self.RUNTIME = random.randint(3, 5) * 1000

                            print "DESIRED ANGLE:", self.desired_angle
                            diff = self.desired_center - self.gyro.val
                            power = self.PID_controller.power(diff)
                            self.motor_left.write(True, min(max(0, 50 + power), 50))
                            self.motor_right.write(True, min(max(0, 50 - power), 50))
                    else:									# found a tower / blocks
                        self.time_out.reset()
                        diff = self.desired_center - int(message[1])
                        if abs(diff) < 5 and int(message[2]) > 110:
                            self.STOP = True
                            return
                        diff = 0 if abs(diff) < 2 else -diff
                        power = self.PID_controller.power(diff)

                        self.motor_left.write(True, min(max(0, 30 + power), 30))
                        self.motor_right.write(True, min(max(0, 30 - power), 30))

            except (KeyboardInterrupt,SystemExit):
                self.motor_left.write(True, 0)
                self.motor_right.write(True, 0)
                self.motor.write(True,0)
                self.servo_bottom.write(100)
                sleep(1)  
                print "YOOOOOOOOOOOOOOOO!"
                self.stop()
                self.on_exit()  
        else:
            print "here"
            self.time_limit = time() + 3*60

    def convertToInches(self, value):
        inches = 0 if value <= 0 else -5.07243 * ln(0.0000185668 * value)
        return inches