more commonsensical names in position
Haldean Brown
4 years ago

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] |