Пример #1
0
class Commands:
    def __init__(self):
        self.protocol = None
        self.vision_process = None
        self.world = None
        self.strategy = None
        self.highstrategy = None
        self.game = None
        print("! Remember to call:")
        print("! init <room: 0/1> <team_color: blue/yellow> <our_single_spot_color: green/pink>")
        print("! vision")
        print("! connect <device_no>")
        self.init()
        self.vision()
        #self.connect()
        #self.highstrategy.main()

    def init(self, room_num=1, team_color='yellow', our_color='green', computer_goal=False):
        print("init: Room: %s, team color: %s, our single spot color: %s, computer goal: %s" %
              (room_num, team_color, our_color, computer_goal))
        self.world = World(int(room_num), team_color, our_color, computer_goal)
        self.strategy = SimpleStrategy(self.world, self)
        self.game = Game(self.world, self)
        self.highstrategy = StrategyTools(self.world, self, self.game, self.strategy)

    def connect(self, device_no='0'):
        print("connect: Connecting to RF stick")
        self.protocol = RobotProtocol('/dev/ttyACM' + device_no)

    def vision(self):
        if not self.vision_process:
            print("vision: Starting vision")
            self.vision_process = multiprocessing.Process(target=Vision, args=(self.world,))
            self.vision_process.start()

    def query_ball(self):
        a = self.protocol.query_ball()
        print("We have the ball: %s" % a)
        return a

    def start_game(self, case_no):
        #1 is when venus kicks off the game
        if int(case_no) == 1:
            self.highstrategy.kick_off_us()
        else:
            self.highstrategy.kick_off_them()
        self.runstrategy()

    def penalty_attack(self, clockno):
        self.highstrategy.penalty_attack(clockno)

    def penalty_defend(self):
        self.highstrategy.penalty_defend()

    def runstrategy(self):
        self.highstrategy.main()

    def attackwithball(self):
        self.highstrategy.attackwithball()

    def ballwithenemy(self, no):
        self.highstrategy.ballwithenemy(int(no))

    def ballindefensearea(self):
        a= self.highstrategy.ballindefensearea()
        print(a)

    def ballwithfriend(self):
        self.highstrategy.ballwithfriend()

    def grab_ball(self):
        self.strategy.grab_ball()

    def goal(self):
        self.strategy.goal()

    def grab_goal(self):
        self.strategy.grab_goal()

    def pass_ball(self):
        self.strategy.pass_ball()

    def catch_ball(self):
        self.strategy.catch_ball()

    def intercept(self):
        self.strategy.intercept()

    def catch_pass(self):
        self.strategy.catch_pass()

    def pw(self):
        print(self.world)

    def pot(self):
        self.game.mid("FREE_BALL_YOURS", True)

    def pot1(self):
        self.game.mid("FREE_BALL_2_GOALSIDE", True)

    def positionTest(self):
        print self.world.our_goalX, self.world.our_goalmeanY


    def flush(self):
        self.protocol.flush()

    def test1(self):
        self.game.local_potential = np.array([[666, 100, 666, 100, 666],
                                              [666, 100, 0, 0, 666],
                                              [666, 100, 1, 100, 666],
                                              [666, 100, 666, 100, 666],
                                              [666, 100, 666, 100, 666]], dtype=np.float64)
        self.game.points = np.arange(50).reshape((5, 5, 2))
        print self.game.move()

    def test2(self):
        # for x in range(0, 10):
        #     self.forward_right()
        self.forward_left()
        self.forward_right()
        self.pause()
        self.sharp_left()
        self.forward_left()
        self.pause()
        self.sharp_right()
        self.forward_right()

    def test3(self):
        self.forward()
        self.pause()
        self.sharp_left()

    def test4(self):
        """Test of swerving"""
        self.forward()
        self.swerve_left(200)
        self.swerve_right(200)
        self.swerve_left(200)
        self.swerve_right(200)
        self.forward()

    def hol(self):
        self.protocol.move(10, [(FRONT_LEFT, 100), (BACK_LEFT, 100), (FRONT_RIGHT, -100), (BACK_RIGHT, -100), ], wait=True)
        time.sleep(20)
        self.s()

    def stopped(self):
        self.protocol.block_until_stop()
        print("Now it has stopped")

    def turn(self, x, grab=None):
        """Turn clockwise, negative means counter-clockwise"""
        print("  Turn %d deg" % int(x))

        x = int(x)
        s = sign(x)
        x = abs(x)

        # Last calibration: 14 March

        if s > 0:  # Clockwise
            if x > 90:
                x = 1.8055555556 * x - 85.0
            else:
                x = 0.0067224213 * (x ** 2) + 0.2676702509 * x

        else:  # Counter-clockwise
            if x > 90:
                x = 1.7477777778 * x - 93.5
            else:
                x = 0.0063082437 * (x ** 2) + 0.1935483871 * x
        x = int(x)
        if x > 0:
            self.protocol.schedule(x, MOTOR_TURN, [(MOTOR_LEFT, -100 * s),
                                                   (MOTOR_RIGHT, 100 * s),
                                                   (MOTOR_TURN, 100 * s)], self.grab_time(grab))

    def forward_forever(self):
        """Move forward forever"""
        self.protocol.move_forever([(MOTOR_LEFT, -100),
                                    (MOTOR_RIGHT, -100)])

    def backward_forever(self):
        """Move backward forever"""
        self.protocol.move_forever([(MOTOR_LEFT, 100),
                                    (MOTOR_RIGHT, 100)])

    def swerve_right(self, x):
        """Swerve right whilst moving forwards"""
        self.protocol.schedule(x, MOTOR_TURN, [(MOTOR_TURN, 100),
                                               (MOTOR_LEFT, -100),
                                               (MOTOR_RIGHT, -100)])

    def swerve_left(self, x):
        """Swerve left whilst moving forwards"""
        self.protocol.schedule(x, MOTOR_TURN, [(MOTOR_TURN, -100),
                                               (MOTOR_LEFT, -100),
                                               (MOTOR_RIGHT, -100)])

    def pause(self):
        """Pause between motions that immediately change motor direction"""
        #self.protocol.schedule_pause(400)

    def grab_time(self, grab):
        if grab == 'T':
            grab = True
        elif grab == 'F':
            grab = False

        if grab is None:
            return 0
        elif grab:
            return -300
        else:
            return 400

    def forward(self, grab=None):
        """Move a cell forward"""
        print("  Forward")
        self.protocol.move(80, [(MOTOR_LEFT, -100),
                                (MOTOR_RIGHT, -100)], wait=True)
        # self.protocol.schedule(90, MOTOR_LEFT, [(MOTOR_LEFT, -100),
        #                                          (MOTOR_RIGHT, -100)], self.grab_time(grab))

    def backward(self, grab=None):
        """Move a cell backward"""
        print("  Backward")
        self.protocol.move(90, [(MOTOR_LEFT, 100),
                                                 (MOTOR_RIGHT, 100)], wait=True)
        # self.protocol.schedule(70, MOTOR_LEFT, [(MOTOR_LEFT, 100),
        #                                          (MOTOR_RIGHT, 100)], self.grab_time(grab))

    def forward_left(self, grab=None):
        """Move forward and to left"""
        print("  Forward left")
        self.protocol.move(150, [(MOTOR_TURN, -100),
                                                 (MOTOR_LEFT, -80),
                                                 (MOTOR_RIGHT, -100)], wait=True)
        #self.protocol.move(50, [(MOTOR_LEFT, -100),
        #                                        (MOTOR_RIGHT, -100)], wait=True)

        # self.protocol.schedule(170, MOTOR_TURN, [(MOTOR_TURN, -100),
        #                                          (MOTOR_LEFT, -80),
        #                                          (MOTOR_RIGHT, -100)], self.grab_time(grab))
        # self.protocol.schedule(50, MOTOR_LEFT, [(MOTOR_LEFT, -100),
        #                                         (MOTOR_RIGHT, -100)])

    def forward_right(self, grab=None):
        """Move forward and to right"""
        print("  Forward right")
        self.protocol.move(240, [(MOTOR_TURN, 100),
                                                 (MOTOR_LEFT, -100),
                                                 (MOTOR_RIGHT, -80)], wait=True)

        #self.protocol.move(50, [(MOTOR_LEFT, -100),
        #                                        (MOTOR_RIGHT, -100)], wait=True)

        # self.protocol.schedule(170, MOTOR_TURN, [(MOTOR_TURN, 100),
        #                                          (MOTOR_LEFT, -100),
        #                                          (MOTOR_RIGHT, -60)], self.grab_time(grab))
        # self.protocol.schedule(50, MOTOR_RIGHT, [(MOTOR_LEFT, -100),
        #                                          (MOTOR_RIGHT, -100)])

    def backward_left(self, grab=None):
        """Move backward and to left"""
        print("  Backward left")
        #self.protocol.move(20, [(MOTOR_LEFT, 100),
        #                                      (MOTOR_RIGHT, 100)], wait=True)
        self.protocol.move(190, [(MOTOR_TURN, 100),
                                                 (MOTOR_LEFT, 80),
                                                 (MOTOR_RIGHT, 100)], wait=True)
        # self.protocol.schedule(3, MOTOR_LEFT, [(MOTOR_LEFT, 100),
        #                                         (MOTOR_RIGHT, 100)], self.grab_time(grab))
        # self.protocol.schedule(190, MOTOR_TURN, [(MOTOR_TURN, 100),
        #                                          (MOTOR_LEFT, 80),
        #                                          (MOTOR_RIGHT, 100)])

    def backward_right(self, grab=None):
        """Move backward and to right"""
        print("  Backward right")
        #self.protocol.schedule(20, MOTOR_RIGHT, [(MOTOR_LEFT, 100),
        #                                       (MOTOR_RIGHT, 100)], self.grab_time(grab))

        self.protocol.move(250, [(MOTOR_TURN, -100),
                                                 (MOTOR_LEFT, 100),
                                                 (MOTOR_RIGHT, 80)], wait=True)

        #this was commented out before changing to move:


        # self.protocol.schedule(230, MOTOR_TURN, [(MOTOR_TURN, -100),
        #                                          (MOTOR_LEFT, 100),
        #                                          (MOTOR_RIGHT, 80)])

    def sharp_left(self, grab=None):
        """Move to a cell left"""
        print("  Sharp left")
        self.c(-90)
        # self.protocol.move(12, [(MOTOR_TURN, -100),
        #                                         (MOTOR_LEFT, 100),
        #                                         (MOTOR_RIGHT, -100)], wait=True)
        self.protocol.move(80, [(MOTOR_LEFT, -100),
                                                (MOTOR_RIGHT, -100)], wait=True)
        # self.protocol.schedule(12, MOTOR_TURN, [(MOTOR_TURN, -100),
        #                                         (MOTOR_LEFT, 100),
        #                                         (MOTOR_RIGHT, -100)], self.grab_time(grab))
        # self.protocol.schedule(50, MOTOR_LEFT, [(MOTOR_LEFT, -100),
        #                                         (MOTOR_RIGHT, -100)])

    def sharp_right(self, grab=None):
        """Move to a cell right"""
        print("  Sharp right")
        self.c(90)
        # self.protocol.move(18, [(MOTOR_TURN, 100),
        #                                         (MOTOR_LEFT, -100),
        #                                         (MOTOR_RIGHT, 100)], wait=True)
        self.protocol.move(80, [(MOTOR_LEFT, -100),
                                                 (MOTOR_RIGHT, -100)], wait=True)
        # self.protocol.schedule(18, MOTOR_TURN, [(MOTOR_TURN, 100),
        #                                         (MOTOR_LEFT, -100),
        #                                         (MOTOR_RIGHT, 100)], self.grab_time(grab))
        # self.protocol.schedule(50, MOTOR_RIGHT, [(MOTOR_LEFT, -100),
        #                                          (MOTOR_RIGHT, -100)])

    def f(self, x):
        """Move forward, negative x means backward"""
        x = int(x)
        s = sign(x)
        if x > 0:
            if x > 17:
                x = 7.5145299398 * x - 80.3832872418
            else:
                x = 0.0432484778 * (x ** 2) + 2.1632771388 * x
        else:
            x = 13.964509832 * -x - 75.2448595458
        x = int(x)
        if x > 0:
            self.protocol.move(x, [(MOTOR_LEFT, -100 * s),
                                   (MOTOR_RIGHT, -100 * s)], wait=True)

    def c(self, x):
        """Rotate clockwise, negative x means counter-clockwise"""
        x = int(x)
        s = sign(x)
        x = abs(x)

        # Last calibration: 12 March

        if x > 90:
            x = 0.9933333333 * x - 43
        else:
            x = 0.0018797292 * (x ** 2) + 0.420501791 * x
        x = int(x)

        if x > 0:
            self.protocol.move(x, [(MOTOR_LEFT, -100 * s),
                                   (MOTOR_RIGHT, 100 * s),
                                   (MOTOR_TURN, 100 * s)], wait=True)

    def k(self, x):
        """Kick"""
        x = int(x)
        # Not using rotary encoders, granularity too low
        self.protocol.move(x, [(2, 100)], time=True)

    def g(self, x=-300):
        """Grab, positive x means release"""
        x = int(x)
        # This motor does not have rotary encoders
        self.world.grabber_open = x > 0
        self.protocol.move(abs(x), [(MOTOR_GRAB, 100 * sign(x))], time=True)

    def open_wide(self):
        self.g(400)

    def open_narrow(self):
        self.g(350)

    def x(self, x):
        """Kick and release"""
        x = int(x)
        self.k(x)
        self.open_narrow()

    def s(self):
        self.protocol.stop()

    def move(self, x, y, deg):
        """Milestone 1: Move"""
        x = int(x)
        y = int(y)
        deg = int(deg)

        print("First turn, clockwise")
        self.c(90)
        time.sleep(1)

        print("Go x")
        self.f(x)
        time.sleep(1)

        print("Second turn, counterclockwise")
        self.c(-85)

        print("Go y")
        time.sleep(1)
        self.f(y)

        print("Do final turn")
        time.sleep(1)
        self.c(deg)

    def kick(self, distance):
        """Milestone 1: Kick"""
        distance = int(distance)

        # Last calibration: 12 March

        time_value = 2.3530438329 * distance + 38.5507807011
        print("Time for kicking motor: " + str(time_value))
        self.x(time_value)
        '''
        if distance == 50:
            # time = 260
            time = 242
        elif distance == 100:
            # time = 290
            time = 275  # 295?
        elif distance == 150:
            # time = 310
            time = 303
        else:
            time = 0
        self.k(time)
        '''

    def transfer(self, filename, freq_hz):
        """Milestone 1: Communications and Timing"""
        freq_hz = float(freq_hz)
        seconds = 1.0 / freq_hz
        print("Transferring file '%s' at freq %fHz, %fs" %
              (filename, freq_hz, seconds))
        with open(filename, "rb") as f:
            byte = f.read(1)
            while byte != "":
                self.protocol.transfer(byte)
                time.sleep(seconds)
                byte = f.read(1)
                # self.p.read_all()

    def reset_input(self):
        self.protocol.reset_input()

    def block_goal(self, enemy_num):
        self.strategy.block_goal(int(enemy_num))
Пример #2
0
 def connect(self, device_no='0'):
     print("connect: Connecting to RF stick")
     self.protocol = RobotProtocol('/dev/ttyACM' + device_no)
Пример #3
0
class Commands:
    def __init__(self):
        self.protocol = None
        self.vision_process = None
        self.world = None
        self.strategy = None
        self.highstrategy = None
        self.game = None
        print("! Remember to call:")
        print("! init <room: 0/1> <team_color: blue/yellow> <our_single_spot_color: green/pink>")
        print("! vision")
        print("! connect <device_no>")
        self.init()
        self.vision()
        #self.connect()
        #self.highstrategy.main()

    def hs(self):
        self.highstrategy.main()

    def init(self, room_num=0, team_color='yellow', our_color='pink', computer_goal=False):
        print("init: Room: %s, team color: %s, our single spot color: %s, computer goal: %s" %
              (room_num, team_color, our_color, computer_goal))
        self.world = World(int(room_num), team_color, our_color, computer_goal)
        self.strategy = SimpleStrategy(self.world, self)
        self.game = Game(self.world, self)
        self.highstrategy = StrategyTools(self.world, self, self.game, self.strategy)

    def connect(self, device_no='0'):
        print("connect: Connecting to RF stick")
        self.protocol = RobotProtocol('/dev/ttyACM' + device_no)

    def vision(self):
        if not self.vision_process:
            print("vision: Starting vision")
            self.vision_process = multiprocessing.Process(target=Vision, args=(self.world,))
            self.vision_process.start()

    def map(self, state, num):
        self.game.mid(state, True, num)

    def test(self, state):
        while True:
            self.game.mid(state, False)

    def move(self, direction, angle): # direction is between our orientation and where we want to go
        dir = int(direction)
        ang = int(angle)
        if dir < 0:
            d = 45 - dir
        else:
            d = -(dir - 45)

        a = -math.radians(ang)
        dir = math.radians(d)
        idea = np.array([np.cos(dir), np.sin(dir), a])
        rad = 0.1
        m = np.array([[1, 0, rad],
                      [0, -1, rad],
                      [-1, 0, rad],
                      [0, 1, rad]])

        movement = np.dot(m, idea)
        sizes = np.fabs(movement)
        factor = np.amax(sizes)
        movement = np.multiply(100.0/factor, movement)

        movement = movement.round()
        s = np.sign(movement)
        movement = s*((40*abs(movement))/100 + 60)
        self.protocol.move_forever([(0, int(movement[0])), (1, int(movement[1])), (2, int(movement[2])), (3, int(movement[3])), ])
        #self.protocol.move(20, [(0, movement[0]), (1, movement[1]), (2, movement[2]), (3, movement[3]), ], wait=True)

    def penalty_attack(self, clockno):
        self.highstrategy.penalty_attack(clockno)

    def penalty_defend(self):
        self.highstrategy.penalty_defend()

    def runstrategy(self):
        self.highstrategy.main()

    def attackwithball(self):
        self.highstrategy.attackwithball()

    def ballwithenemy(self, no):
        self.highstrategy.ballwithenemy(int(no))

    def ballindefensearea(self):
        a= self.highstrategy.ballindefensearea()
        print(a)

    def ballwithfriend(self):
        self.highstrategy.ballwithfriend()

    def f(self, x):
        """Move forward, negative x means backward"""
        x = int(x)
        s = sign(x)
        x = abs(x)
        # Calibrated for the holonomic robot on 27 March, only forward
        x = 5.3169850194 * x - 14.9575723714
        x = int(x)
        if x > 0:
            self.protocol.move(x, [(0, 100 * s), (1, -100 * s), (2, -100 * s), (3, 100 * s)], wait=True)

    def c(self, x):
        """Rotate clockwise, negative x means counter-clockwise"""
        x = int(x)
        s = sign(x)
        x = abs(x)

        # Calibrated for the holonomic robot on 30 March
        if s > 0:
            if x > 90:
                x = 0.8 * x - 37.0
            else:
                x = 0.0015306535 * (x ** 2) + 0.3025825153 * x
        else:
            if x > 90:
                x = 0.823333333 * x - 33.5
            else:
                x = 0.001206074 * (x ** 2) + 0.3634378289 * x
        x = int(x)

        if x > 0:
            self.protocol.move(x, [(0, -80 * s), (1, -80 * s), (2, -80 * s), (3, -80 * s)], wait=True)

    def s(self):
        self.protocol.stop()

    def k(self):
        # self.protocol.move(500, [(4, -100)], time=True)
        self.protocol.move_forever([(0, -100), (1, 100), (2, 100), (3, -100), ])
        time.sleep(0.5)
        self.protocol.move_forever([(0, 100), (1, -100), (2, -100), (3, 100), ])
        time.sleep(1)
        self.protocol.stop()

    def o(self):
        self.protocol.move(400, [(4, -100)], time=True, wait=True)

    def g(self):
        self.protocol.move(400, [(4, 80)], time=True, wait=True)

    def ss(self, x):
        x = int(x)
        s = sign(x)
        x = abs(x)

        if x < 90:
            return

        if x > 90:
            if s > 0:
                x = 0.0008417761 * (x ** 2) + 0.3865014241 * x - 41.5767918089
            else:
                x = 0.0013813605 * (x ** 2) + 0.1536110506 * x - 25.1058020478
            x = int(x)
            self.protocol.schedule(x, 0, [(0, -100 * s), (1, -100 * s), (2, -100 * s), (3, -100 * s)])

        self.protocol.schedule(200, 0, [(0, -100 * s), (1, -100 * s), (2, -100 * s), (3, -100 * s)], grab=-400)

        #self.protocol.move(400, [(4, -100)], time=True)

    def flush(self):
        self.protocol.flush()

    def ee(self, x):
        x = int(x)
        s = sign(x)
        self.protocol.move(400, [(4, -100)], time=True)
        self.protocol.move(200, [(0, -100 * s), (1, -100 * s), (2, -100 * s), (3, -100 * s)], wait=True)

    def w(self):
        print self.world

    def query_ball(self):
        if self.world.room_num == 0:
            threshold = 180
        else:
            threshold = 192
        a = self.protocol.query_ball(threshold)
        print("We have the ball: %s" % a)
        return a

    def pass_ball(self):
        self.strategy.pass_ball()

    def catch_ball(self):
        self.strategy.catch_ball()

    def goal(self):
        self.strategy.goal()

    def who(self):
        print "Who has the ball?"
        if self.world.venus.hasBallInRange[0]:
            print "0: venus"
        if self.world.friend.hasBallInRange[0]:
            print "1: friend"
        if self.world.enemy1.hasBallInRange[0]:
            print "2: enemy1"
        if self.world.enemy2.hasBallInRange[0]:
            print "3: enemy2"

    def rr(self):
        self.protocol.move_forever([(0, 70), (1, -100), (2, -70), (3, 100), ])

    def stopped(self):
        self.protocol.block_until_stop()
        print("Now it has stopped")

    def v(self):
        while True:
            #print "---"
            #print self.world.ball_velocity[0], self.world.ball_velocity[1]
            print math.sqrt(self.world.ball_velocity[0]**2 + self.world.ball_velocity[1]**2)/6.0

    def j(self):
        angle, motion_length = self.strategy.calculate_angle_length_ball()
        print angle, motion_length
        self.c(angle)

    def b(self):
        last = self.world.ball_moving[0]
        i = 0
        while True:
            this = self.world.ball_moving[0]
            if last == 0 and this == 1:
                print "\a%d Started moving" % i
                i += 1
            elif last == 1 and this == 0:
                print "\a%d Stopped moving" % i
                i += 1
            last = this

    def gg(self):
        while True:
            self.game.mid("FREE_BALL_YOURS", False)