-
Notifications
You must be signed in to change notification settings - Fork 0
/
swerve_modifier.py
52 lines (40 loc) · 1.52 KB
/
swerve_modifier.py
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
import copy
class SwerveModifier:
def __init__(self, original, wheelbase_width, wheelbase_depth):
self.original = original
self.wheelbase_width = wheelbase_width
self.wheelbase_depth = wheelbase_depth
self.modify()
def modify(self):
self.front_left = []
self.front_right = []
self.back_left = []
self.back_right = []
for i in range(len(self.original)):
seg = self.original[i]
fl = copy.deepcopy(seg)
fr = copy.deepcopy(seg)
bl = copy.deepcopy(seg)
br = copy.deepcopy(seg)
fl.x = seg.x - self.wheelbase_width / 2
fl.y = seg.y + self.wheelbase_depth / 2
fr.x = seg.x + self.wheelbase_width / 2
fr.y = seg.y + self.wheelbase_depth / 2
bl.x = seg.x - self.wheelbase_width / 2
bl.y = seg.y - self.wheelbase_depth / 2
br.x = seg.x + self.wheelbase_width / 2
br.y = seg.y - self.wheelbase_depth / 2
self.front_left.append(fl)
self.front_right.append(fr)
self.back_left.append(bl)
self.back_right.append(br)
def get_original_trajectory(self):
return self.original
def get_front_left_trajectory(self):
return self.front_left
def get_front_right_trajectory(self):
return self.front_right
def get_back_left_trajectory(self):
return self.back_left
def get_back_right_trajectory(self):
return self.back_right