ZYM/cumcm2024-code
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

write task2

Browse Source
This commit is contained in:
ZhuangYumin
2024-09-06 11:06:46 +08:00
parent feca0ae705
commit a645929964
5 changed files with 248 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
A/2/position_watcher.py
View File

@@ -36,6 +36,7 @@ def GenerateFollowNodeTheta(cur_node_theta, expected_distance):
return actual_distance - expected_distance
return mp.findroot(f, cur_node_theta + 0.1, solver='secant')
kPotingPoints=50
def CalcMoveList(time_point):
first_node_theta = GenerateFirstNodeTheta(time_point)
first_node_dot = Theta2Dot(first_node_theta)
@@ -43,13 +44,13 @@ def CalcMoveList(time_point):
node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}]
for i in range(1, 224):
for i in range(1, kPotingPoints):
expected_distance = kSegLength1 if i == 1 else kSegLength2
cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance)
cur_node_dot = Theta2Dot(cur_node_theta)
cur_node_C = Theta2C(cur_node_theta)
node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C})
for i in range(223):
for i in range(kPotingPoints-1):
AA = kSegLength1 if i == 0 else kSegLength2
theta_i = node_list[i]["theta"]
theta_ip1 = node_list[i+1]["theta"]