git.haldean.org
more commonsensical names in position Haldean Brown 4 years ago
1 changed file(s) with 24 addition(s) and 6 deletion(s).
 68 68 let ln = lln + rln + 1 69 69 let pn = ln + 1 70 70 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 71 78 # L[i] is the length of the i'th link. Because we might have fractional chain 72 79 # links let out, not all links that represent chain pivot-to-pivot have the 73 80 # same length 101 108 x[i] = ld * L[i - 1] + x[i - 1] 102 109 103 110 # The next link is the one between the robot pivots 104 L[lln] = robotPivotDist 111 L[rli] = robotPivotDist 105 112 # ...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 108 115 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): 111 118 L[i] = chainLinkDist 112 for i in (lpn + 3)..(pn - 2): 119 for i in (rpri + 1)..(pn - 2): 113 120 x[i] = -rd * L[i - 1] + x[i - 1] 114 121 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 115 133 setlen(wp.pivots, pn) 116 134 for i in 0..(pn - 1): 117 135 wp.pivots[i] = x[i]