Tube-Upper-Control-Code/arm.py at main · BB-Tube/Tube-Upper-Control-Code · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
from dynamixel import dynamixel
import time
from waiter import waiter

ball_dict = {
        'b' : 188,
        'n' : 180,
        'w' : 172
    }

class arm(object):
    def __init__(self,
                motor : dynamixel):
        self.motor = motor
        self.motor.set_mode("Current-based_Position")
        self.motor.set_enable(True)

        self.check_stall_timer = waiter()
        self.check_stall_timer.wait(.1)
        self.got_stalled = False
        self.last_input = 'n'
        self.last_last_input = 'n'
        self.backed_off = waiter()
        self.backed_off.wait(.1)

    def ball_input(self, ball):
        if not ball == self.last_input:
            goal = self.__goal(ball)
            self.move_to(goal)
        self.last_last_input = self.last_input
        self.last_input = ball

    def move_to(self, degrees):
        self.motor.set_position_current(degrees, self.current_limit)

    def if_there(self, margin = 3):
        if self.last_last_input == self.last_input:
            # print("     did nothing")
            return True
        if not self.backed_off.if_past(): # waiting for back off
            return False
        if self.got_stalled: # trigger restart
            self.got_stalled = False
            self.ball_input(self.last_input)
            return False
        if self.__if_stalled(): # if stalled
            self.got_stalled = True
            print("     Stalled!!!!!!!!!!!!!!!!!!")
            self.__back_off()
            self.backed_off.wait(.5)
            return False
        at = self.motor.get_extended_position()
        return abs(self.__goal(self.last_input) - at) < abs(margin)

    def __back_off(self):
        self.motor.set_position_current(self.__goal('n'), self.current_limit)

    def set_current_limit(self, milliamps):
        self.current_limit = milliamps

    def __if_stalled(self, current_margin = 20):
        if self.check_stall_timer.if_past():
            self.check_stall_timer.wait(.3)
            current_milliamp = self.motor.get_current()
            if (abs(current_milliamp) > (self.current_limit - current_margin)):
                return True
        return False

    def __goal(self, ball):
        return ball_dict[ball]

if __name__ == '__main__':
    motor = dynamixel(ID = 15, op = 4)
    a = arm(motor)
    a.set_current_limit(800)

    iterator = 0
    start = time.monotonic()
    delta = 0

    while True:
        print('b')
        a.ball_input('b')
        while not a.if_there():
            time.sleep(.1)
        time.sleep(1)

        print('w')
        while not a.if_there():
            time.sleep(.1)
        a.ball_input('w')
        time.sleep(1)

    while False:
        if a.if_there(10):
            a.next_slot(10)
            iterator += 1
            delta = round(time.monotonic() - start,3)
            print(iterator, " time is ", delta)
            if iterator == 100:
                break

    print("balls per second = ", round(iterator / delta, 2))