git.haldean.org wallbot / 3894537
differentiate between media and control position Haldean Brown 4 years ago
2 changed file(s) with 31 addition(s) and 6 deletion(s). Raw diff Collapse all Expand all
166166 rlabel(wp.rl, 0.5 * (wp.pl + wp.rpos))
167167 rlabel(wp.rr, 0.5 * (wp.pr + wp.rpos))
168168
169 proc circle(pos: Vector2d, color: Color, labelColor: Color) =
169 proc circle(pos: Vector2d, color: Color, labelColor: Color, showLabel = true) =
170170 let pc = newCircleShape(10)
171171 pc.origin = vec2(10, 10)
172172 pc.position = pixel(pos)
175175 pc.fillColor = Transparent
176176 window.draw(pc)
177177
178 let label = newText("{0:+04.2f}\n{1:+04.2f}".fmt(pos.x, pos.y), font, 18)
179 label.position = pc.position + vec2(10, 0)
180 label.color = labelColor
181 window.draw(label)
178 if showLabel:
179 let label = newText("{0:+04.2f}\n{1:+04.2f}".fmt(pos.x, pos.y), font, 18)
180 label.position = pc.position + vec2(10, 0)
181 label.color = labelColor
182 window.draw(label)
182183
183184 circle(wp.pl, c3, c2)
184185 circle(wp.pr, c3, c2)
185 circle(wp.rpos, c4, c1)
186 circle(wp.rpos, c4, c1, false)
187 circle(wp.mpos, c4, c1)
186188
187189 let ccc = newCircleShape(2)
188190 ccc.origin = vec2(1, 1)
00 import basic2d
1 import basic3d
12 import deques
23 import machine
34 import math
910 # The distance between the left and right gear centers. TODO: model the way the
1011 # chain wraps around the sprockets
1112 const robotPivotDist = 0.15 # m
13 const mediaTDist = 0.20 # m
1214
1315 type
1416 Vec = Vector2d
1719 rr*: float
1820 rl_zero*: float
1921 rr_zero*: float
22 # Position of the robot for control purposes: the midpoint between the two
23 # robot pivots
2024 rpos*: Vec
25 # The position of the media, as calculated based on rpos and the robot's
26 # orientation
27 mpos*: Vec
2128 # Positions of the two anchor points
2229 pl*: Vec
2330 pr*: Vec
8087 ld.normalize()
8188 rd.normalize()
8289
90 # Endpoints are fixed
8391 x[0] = wp.pl
8492 x[pn - 1] = wp.pr
8593
94 # First lln - 1 links are all a fixed length
8695 for i in 0..(lln - 2):
8796 L[i] = chainLinkDist
97 # The last link is however long is needed to make up the distance from the robot to
98 # the last chain link that we placed
8899 L[lln - 1] = wp.rl - float(lln) * chainLinkDist
89100 for i in 1..lpn:
90101 x[i] = ld * L[i - 1] + x[i - 1]
91102
103 # The next link is the one between the robot pivots
92104 L[lln] = robotPivotDist
105 # ...whose initial guess is to the left and right of the previous robot position
93106 x[lpn + 1] = guessRobotL
94107 x[lpn + 2] = guessRobotR
95108
102115 setlen(wp.pivots, pn)
103116 for i in 0..(pn - 1):
104117 wp.pivots[i] = x[i]
118
119 let robotL = x[lpn + 1]
120 let robotR = x[lpn + 2]
121 var edge = robotR - robotL
122 edge.normalize()
123 wp.rpos = robotL + 0.5 * robotPivotDist * edge
124
125 let edge3d = vector3d(edge.x, edge.y, 0)
126 let medge = edge3d.cross(vector3d(0, 0, 1))
127 wp.mpos = wp.rpos + mediaTDist * vector2d(medge.x, medge.y)
105128
106129 proc evalRpos(wp: WallPosition): tuple[f: float64, df: Vec, ddf: float64] =
107130 let lv = wp.pl - wp.rpos