git.haldean.org wallbot / 337585c
more commonsensical names in position Haldean Brown 3 years ago
1 changed file(s) with 24 addition(s) and 6 deletion(s). Raw diff Collapse all Expand all
6868 let ln = lln + rln + 1
6969 let pn = ln + 1
7070
71 # The index of the left robot pivot
72 let rpli = lpn + 1
73 # The index of the right robot pivot
74 let rpri = lpn + 2
75 # The index of the robot link
76 let rli = lln
77
7178 # L[i] is the length of the i'th link. Because we might have fractional chain
7279 # links let out, not all links that represent chain pivot-to-pivot have the
7380 # same length
101108 x[i] = ld * L[i - 1] + x[i - 1]
102109
103110 # The next link is the one between the robot pivots
104 L[lln] = robotPivotDist
111 L[rli] = robotPivotDist
105112 # ...whose initial guess is to the left and right of the previous robot position
106 x[lpn + 1] = guessRobotL
107 x[lpn + 2] = guessRobotR
113 x[rpli] = guessRobotL
114 x[rpri] = guessRobotR
108115
109 L[lln + 1] = wp.rr - float(rln) * chainLinkDist
110 for i in (lln + 2)..(ln - 1):
116 L[rli + 1] = wp.rr - float(rln) * chainLinkDist
117 for i in (rli + 2)..(ln - 1):
111118 L[i] = chainLinkDist
112 for i in (lpn + 3)..(pn - 2):
119 for i in (rpri + 1)..(pn - 2):
113120 x[i] = -rd * L[i - 1] + x[i - 1]
114121
122 # Okay, now we're ready to start simulating. First step on each iteration is
123 # to calculate the force on each link
124 var F = newSeq[Vec](ln)
125 for i in 1..(ln - 1):
126 let m =
127 if i == rli: robotMass
128 else: chainLinkMass
129 F[i].x = 0
130 F[i].y = -m
131
132 # Copy result into the WallPosition struct
115133 setlen(wp.pivots, pn)
116134 for i in 0..(pn - 1):
117135 wp.pivots[i] = x[i]