Robomaster-Micropython/robo.py at main · JohnieBraaf/Robomaster-Micropython · 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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
from crc import Crc
from can import CanInterface
from com import Command
import pyb, time

class RoboMaster():
    def __init__(self):

        self.crc = Crc()
        self.can1 = CanInterface(1)
        self.com = Command(self)

        self.linear = Linear()
        #self.angular = Angular()

        self.timcount  = 0

        self.tim2 = pyb.Timer(2)
        self.tim2.init(freq=10)
        #self.tim2.callback(self.cb)

    def cb(self, t):
        self.linear.reset()

    def cb10ms(self):
        self.timcount += 1
        self.linear.reset()

        start = time.ticks_ms()

        self.com.add_10ms()

        if self.timcount % 4 == 0:
            self.com.add_40ms()

        if self.timcount % 9 == 0:
            self.com.add_90ms()

        if self.timcount % 100 == 0:
            self.com.add_1sec()

        #if self.timcount % 1000 == 0:
        #    self.com.add_10sec()

        if self.timcount == 1001:
            self.timecount = 1

        ''''while self.com.buf.any():
                                            pyb.LED(2).on()
                                            while (self.can1.can.info()[5] == 3):
                                                pass # wait till TX buffer is free
                                            self.can1.can.send(self.com.get()[3], 0x201)
                                            pyb.LED(2).off()'''

        end = time.ticks_ms()
        #if end -start > 1:
        #    print('Time taken: ' + str(end - start))
        #print(str(self.timcount) + ' ' + str(self.linear.x))

    @micropython.native
    def process_tcp(self, data):
        '''if len(data) > 4:
            # check header for stick type
            if data[0] == 0x55 and data[1] == len(data) and data[2] == 0x04 and self.crc.check_crc8(data, 4):

                x  = data[6] << 8 | data[7]
                y  = data[8] << 8 | data[9]
                rx = data[10] << 8 | data[11]
                ry = data[12] << 8 | data[13]
                z  = data[14] << 8 | data[15]
                rz = data[16] << 8 | data[17]
                self.linear.set(-(1000 - y) / 1000, (1000 - x) / 1000, 0)
                self.angular.set(-(1000 - ry) / 1000, (1000 - rx) / 1000, 0)
                #print(self.angular.x)
            else:'''
        # try to parse command
        if data:
            com = data.decode()
            #print(com)
            if com[:4] == 'CMD:':
                val = com[4:].split(',')
                self.linear.set(float(val[0]), -float(val[1]), 0)
            else:
                print(com)


class Linear():
    def __init__(self):
        print('Linear init')
        self.t0 = time.ticks_ms()
        self.x = 0.0
        self.y = 0.0
        self.z = 0.0
        self.moving = 0

    def reset(self):
        #print(self.y)
        if self.moving and time.ticks_diff(time.ticks_ms(), self.t0) > 300:
            print('reset')
            self.x = 0.0
            self.y = 0.0
            self.z = 0.0
            self.moving = 0

    def set(self, x, y, z):
        self.t0 = time.ticks_ms()
        self.x = self.check_value(x)
        self.y = self.check_value(y)
        self.z = self.check_value(z)
        self.moving = 1

    def check_value(self, value):
        if value > 1.0 or value < -1.0:
            value = 0.0

        if value == 1.0:
            value = 0.9998
        elif value == -1.0:
            value = -0.9998

        return value