-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtry_code.py
101 lines (82 loc) · 2.69 KB
/
try_code.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
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
import numpy
import matplotlib.pyplot as plt
import astropy.units as u
def T(current_pos,
target_pos,
current_velocity,
max_velocity=6 * u.deg / u.second,
max_acceleration=1 * u.deg / u.second ** 2):
"""
Parameters
----------
current_pos :
target_pos :
current_velocity :
max_velocity :
max_acceleration :
Returns
-------
"""
delta_pos = target_pos - current_pos
triangle_distance = max_velocity ** 2 / max_acceleration / 2
if current_velocity.value == 0 and abs(delta_pos.value) >= triangle_distance.value:
return sene_1(current_pos,
target_pos,
current_velocity)
def sene_1(current_pos,
target_pos,
current_velocity,
max_velocity=6 * u.deg / u.second,
max_acceleration=1 * u.deg / u.second ** 2):
"""
trapezoid velocity curve, calculate with direction
Parameters
----------
current_pos :
target_pos :
current_velocity :
max_velocity :
max_acceleration :
Returns
-------
"""
delta_pos = target_pos - current_pos
direction = numpy.sign(delta_pos.value)
max_velocity = max_velocity * direction
max_acceleration = max_acceleration * direction
# acceleration phase
t1 = max_velocity / max_acceleration
s1 = max_acceleration * t1 ** 2 / 2
# deceleration phase
t3 = -max_velocity / -max_acceleration
s3 = (max_velocity) * t3 + (-max_acceleration) * t3 ** 2 / 2
# constant velocity phase
s2 = delta_pos - s1 - s3
t2 = s2 / max_velocity
assert t2 >= 0
# total time
t = t1 + t2 + t3
assert t >= 0
# plot velocity curve
tx = numpy.linspace(0, t.value, 100) * t1.unit
vx = numpy.zeros(tx.shape) * max_velocity.unit
vx[tx <= t1] = max_acceleration * tx[tx <= t1]
vx[tx > t1] = max_velocity
vx[tx > t1 + t2] = max_velocity - max_acceleration * (
tx[tx > t1 + t2] - t1 - t2)
# plot
plt.figure()
plt.plot(tx, vx)
# plot position curve
sx = numpy.zeros(tx.shape) * u.deg
sx[tx <= t1] = max_acceleration * tx[tx <= t1] ** 2 / 2
sx[tx > t1] = max_velocity * (tx[tx > t1] - t1) + max_acceleration * (
tx[tx > t1] - t1) ** 2 / 2 + max_acceleration * t1 ** 2 / 2
sx[tx > t1 + t2] = max_velocity * (
tx[tx > t1 + t2] - t1) - max_acceleration * (
tx[tx > t1 + t2] - t1 - t2) ** 2 / 2 + max_velocity * (
t2 - t1) + max_acceleration * (
t2 - t1) ** 2 / 2 + max_acceleration * t1 ** 2 / 2 + max_acceleration * t1 ** 2 / 2
# plt.figure()
plt.plot(tx, sx)
plt.show()