Sam Calisch / bounding

embedded code for bounding robot

Dependencies:   QEI mbed

Fork of bounding by Sam Calisch

main.cpp@3:f68eaa68f4ec, 2013-11-24 (annotated)

Committer:
calisch
Date:
Sun Nov 24 21:57:52 2013 +0000
Revision:
3:f68eaa68f4ec
Parent:
2:17379e2a6f7d
Child:
4:7f9c9bd9da26
added time_out, added standup and sitdown

Who changed what in which revision?

UserRevisionLine numberNew contents of line
langfordw 0:fc382eeb78ad 1 #include "mbed.h"
langfordw 0:fc382eeb78ad 2 #include "QEI.h"
langfordw 0:fc382eeb78ad 3 #define CONTROL_PERIOD 0.002 // 500Hz ***
langfordw 0:fc382eeb78ad 4 #define SAVE_PERIOD 0.005 // 200HZ
langfordw 0:fc382eeb78ad 5
calisch 3:f68eaa68f4ec 6 // 500 x 3 array of degree values
calisch 3:f68eaa68f4ec 7 const float trajectory[1000][3] = {
calisch 3:f68eaa68f4ec 8 49,49,0,
calisch 3:f68eaa68f4ec 9 49,49,0,
calisch 3:f68eaa68f4ec 10 49,49,0,
calisch 3:f68eaa68f4ec 11 49,49,0,
calisch 3:f68eaa68f4ec 12 49,49,0,
calisch 3:f68eaa68f4ec 13 49,49,0,
calisch 3:f68eaa68f4ec 14 49,49,0,
calisch 3:f68eaa68f4ec 15 49,49,0,
calisch 3:f68eaa68f4ec 16 49,49,0,
calisch 3:f68eaa68f4ec 17 49,49,0,
calisch 3:f68eaa68f4ec 18 49,49,0,
calisch 3:f68eaa68f4ec 19 49,49,0,
calisch 3:f68eaa68f4ec 20 49,49,0,
calisch 3:f68eaa68f4ec 21 49,49,0,
calisch 3:f68eaa68f4ec 22 49,49,0,
calisch 3:f68eaa68f4ec 23 49,49,0,
calisch 3:f68eaa68f4ec 24 49,49,0,
calisch 3:f68eaa68f4ec 25 49,49,0,
calisch 3:f68eaa68f4ec 26 49,49,0,
calisch 3:f68eaa68f4ec 27 49,49,0,
calisch 3:f68eaa68f4ec 28 49,49,0,
calisch 3:f68eaa68f4ec 29 49,49,0,
calisch 3:f68eaa68f4ec 30 49,49,0,
calisch 3:f68eaa68f4ec 31 49,49,0,
calisch 3:f68eaa68f4ec 32 49,49,0,
calisch 3:f68eaa68f4ec 33 49,49,0,
calisch 3:f68eaa68f4ec 34 49,49,0,
calisch 3:f68eaa68f4ec 35 49,49,0,
calisch 3:f68eaa68f4ec 36 49,49,0,
calisch 3:f68eaa68f4ec 37 49,49,0,
calisch 3:f68eaa68f4ec 38 49,49,0,
calisch 3:f68eaa68f4ec 39 49,49,0,
calisch 3:f68eaa68f4ec 40 49,49,0,
calisch 3:f68eaa68f4ec 41 49,49,0,
calisch 3:f68eaa68f4ec 42 49,49,0,
calisch 3:f68eaa68f4ec 43 49,49,0,
calisch 3:f68eaa68f4ec 44 49,49,0,
calisch 3:f68eaa68f4ec 45 49,49,0,
calisch 3:f68eaa68f4ec 46 49,49,0,
calisch 3:f68eaa68f4ec 47 49,49,0,
calisch 3:f68eaa68f4ec 48 49,49,0,
calisch 3:f68eaa68f4ec 49 49,49,0,
calisch 3:f68eaa68f4ec 50 49,49,0,
calisch 3:f68eaa68f4ec 51 49,49,0,
calisch 3:f68eaa68f4ec 52 49,49,0,
calisch 3:f68eaa68f4ec 53 49,49,0,
calisch 3:f68eaa68f4ec 54 49,49,0,
calisch 3:f68eaa68f4ec 55 49,49,0,
calisch 3:f68eaa68f4ec 56 49,49,0,
calisch 3:f68eaa68f4ec 57 49,49,0,
calisch 3:f68eaa68f4ec 58 49,49,0,
calisch 3:f68eaa68f4ec 59 49,49,0,
calisch 3:f68eaa68f4ec 60 49,49,0,
calisch 3:f68eaa68f4ec 61 49,49,0,
calisch 3:f68eaa68f4ec 62 49,49,0,
calisch 3:f68eaa68f4ec 63 49,49,0,
calisch 3:f68eaa68f4ec 64 49,49,0,
calisch 3:f68eaa68f4ec 65 49,49,0,
calisch 3:f68eaa68f4ec 66 49,49,0,
calisch 3:f68eaa68f4ec 67 49,49,0,
calisch 3:f68eaa68f4ec 68 49,49,0,
calisch 3:f68eaa68f4ec 69 49,49,0,
calisch 3:f68eaa68f4ec 70 49,49,0,
calisch 3:f68eaa68f4ec 71 49,49,0,
calisch 3:f68eaa68f4ec 72 49,49,0,
calisch 3:f68eaa68f4ec 73 49,49,0,
calisch 3:f68eaa68f4ec 74 49,49,0,
calisch 3:f68eaa68f4ec 75 49,49,0,
calisch 3:f68eaa68f4ec 76 49,49,0,
calisch 3:f68eaa68f4ec 77 49,49,0,
calisch 3:f68eaa68f4ec 78 49,49,0,
calisch 3:f68eaa68f4ec 79 49,49,0,
calisch 3:f68eaa68f4ec 80 49,49,0,
calisch 3:f68eaa68f4ec 81 49,49,0,
calisch 3:f68eaa68f4ec 82 49,49,0,
calisch 3:f68eaa68f4ec 83 49,49,0,
calisch 3:f68eaa68f4ec 84 49,49,0,
calisch 3:f68eaa68f4ec 85 49,49,0,
calisch 3:f68eaa68f4ec 86 49,49,0,
calisch 3:f68eaa68f4ec 87 49,49,0,
calisch 3:f68eaa68f4ec 88 49,49,0,
calisch 3:f68eaa68f4ec 89 49,49,0,
calisch 3:f68eaa68f4ec 90 49,49,0,
calisch 3:f68eaa68f4ec 91 49,49,0,
calisch 3:f68eaa68f4ec 92 49,49,0,
calisch 3:f68eaa68f4ec 93 49,49,0,
calisch 3:f68eaa68f4ec 94 49,49,0,
calisch 3:f68eaa68f4ec 95 49,49,0,
calisch 3:f68eaa68f4ec 96 49,49,0,
calisch 3:f68eaa68f4ec 97 49,49,0,
calisch 3:f68eaa68f4ec 98 49,49,0,
calisch 3:f68eaa68f4ec 99 49,49,0,
calisch 3:f68eaa68f4ec 100 49,49,0,
calisch 3:f68eaa68f4ec 101 49,49,0,
calisch 3:f68eaa68f4ec 102 49,49,0,
calisch 3:f68eaa68f4ec 103 49,49,0,
calisch 3:f68eaa68f4ec 104 49,49,0,
calisch 3:f68eaa68f4ec 105 49,49,0,
calisch 3:f68eaa68f4ec 106 49,49,0,
calisch 3:f68eaa68f4ec 107 49,49,0,
calisch 3:f68eaa68f4ec 108 49,49,0,
calisch 3:f68eaa68f4ec 109 49,49,0,
calisch 3:f68eaa68f4ec 110 49,49,0,
calisch 3:f68eaa68f4ec 111 49,49,0,
calisch 3:f68eaa68f4ec 112 49,49,0,
calisch 3:f68eaa68f4ec 113 49,49,0,
calisch 3:f68eaa68f4ec 114 49,49,0,
calisch 3:f68eaa68f4ec 115 49,49,0,
calisch 3:f68eaa68f4ec 116 49,49,0,
calisch 3:f68eaa68f4ec 117 49,49,0,
calisch 3:f68eaa68f4ec 118 49,49,0,
calisch 3:f68eaa68f4ec 119 49,49,0,
calisch 3:f68eaa68f4ec 120 49,49,0,
calisch 3:f68eaa68f4ec 121 49,49,0,
calisch 3:f68eaa68f4ec 122 49,49,0,
calisch 3:f68eaa68f4ec 123 49,49,0,
calisch 3:f68eaa68f4ec 124 49,49,0,
calisch 3:f68eaa68f4ec 125 49,49,0,
calisch 3:f68eaa68f4ec 126 49,49,0,
calisch 3:f68eaa68f4ec 127 49,49,0,
calisch 3:f68eaa68f4ec 128 49,49,0,
calisch 3:f68eaa68f4ec 129 49,49,0,
calisch 3:f68eaa68f4ec 130 49,49,0,
calisch 3:f68eaa68f4ec 131 49,49,0,
calisch 3:f68eaa68f4ec 132 49,49,0,
calisch 3:f68eaa68f4ec 133 49,49,0,
calisch 3:f68eaa68f4ec 134 49,49,0,
calisch 3:f68eaa68f4ec 135 49,49,0,
calisch 3:f68eaa68f4ec 136 49,49,0,
calisch 3:f68eaa68f4ec 137 49,49,0,
calisch 3:f68eaa68f4ec 138 49,49,0,
calisch 3:f68eaa68f4ec 139 49,49,0,
calisch 3:f68eaa68f4ec 140 49,49,0,
calisch 3:f68eaa68f4ec 141 49,49,0,
calisch 3:f68eaa68f4ec 142 49,49,0,
calisch 3:f68eaa68f4ec 143 49,49,0,
calisch 3:f68eaa68f4ec 144 49,49,0,
calisch 3:f68eaa68f4ec 145 49,49,0,
calisch 3:f68eaa68f4ec 146 49,49,0,
calisch 3:f68eaa68f4ec 147 49,49,0,
calisch 3:f68eaa68f4ec 148 49,49,0,
calisch 3:f68eaa68f4ec 149 49,49,0,
calisch 3:f68eaa68f4ec 150 49,49,0,
calisch 3:f68eaa68f4ec 151 49,49,0,
calisch 3:f68eaa68f4ec 152 49,49,0,
calisch 3:f68eaa68f4ec 153 49,49,0,
calisch 3:f68eaa68f4ec 154 49,49,0,
calisch 3:f68eaa68f4ec 155 49,49,0,
calisch 3:f68eaa68f4ec 156 49,49,0,
calisch 3:f68eaa68f4ec 157 49,49,0,
calisch 3:f68eaa68f4ec 158 49,49,0,
calisch 3:f68eaa68f4ec 159 49,49,0,
calisch 3:f68eaa68f4ec 160 49,49,0,
calisch 3:f68eaa68f4ec 161 49,49,0,
calisch 3:f68eaa68f4ec 162 49,49,0,
calisch 3:f68eaa68f4ec 163 49,49,0,
calisch 3:f68eaa68f4ec 164 49,49,0,
calisch 3:f68eaa68f4ec 165 49,49,0,
calisch 3:f68eaa68f4ec 166 49,49,0,
calisch 3:f68eaa68f4ec 167 49,49,0,
calisch 3:f68eaa68f4ec 168 49,49,0,
calisch 3:f68eaa68f4ec 169 49,49,0,
calisch 3:f68eaa68f4ec 170 49,49,0,
calisch 3:f68eaa68f4ec 171 49,49,0,
calisch 3:f68eaa68f4ec 172 49,49,0,
calisch 3:f68eaa68f4ec 173 49,49,0,
calisch 3:f68eaa68f4ec 174 49,49,0,
calisch 3:f68eaa68f4ec 175 49,49,0,
calisch 3:f68eaa68f4ec 176 49,49,0,
calisch 3:f68eaa68f4ec 177 49,49,0,
calisch 3:f68eaa68f4ec 178 49,49,0,
calisch 3:f68eaa68f4ec 179 49,49,0,
calisch 3:f68eaa68f4ec 180 49,49,0,
calisch 3:f68eaa68f4ec 181 49,49,0,
calisch 3:f68eaa68f4ec 182 49,49,0,
calisch 3:f68eaa68f4ec 183 49,49,0,
calisch 3:f68eaa68f4ec 184 49,49,0,
calisch 3:f68eaa68f4ec 185 49,49,0,
calisch 3:f68eaa68f4ec 186 49,49,0,
calisch 3:f68eaa68f4ec 187 49,49,0,
calisch 3:f68eaa68f4ec 188 49,49,0,
calisch 3:f68eaa68f4ec 189 49,49,0,
calisch 3:f68eaa68f4ec 190 49,49,0,
calisch 3:f68eaa68f4ec 191 49,49,0,
calisch 3:f68eaa68f4ec 192 49,49,0,
calisch 3:f68eaa68f4ec 193 49,49,0,
calisch 3:f68eaa68f4ec 194 49,49,0,
calisch 3:f68eaa68f4ec 195 49,49,0,
calisch 3:f68eaa68f4ec 196 49,49,0,
calisch 3:f68eaa68f4ec 197 49,49,0,
calisch 3:f68eaa68f4ec 198 49,49,0,
calisch 3:f68eaa68f4ec 199 49,49,0,
calisch 3:f68eaa68f4ec 200 49,49,0,
calisch 3:f68eaa68f4ec 201 49,49,0,
calisch 3:f68eaa68f4ec 202 49,49,0,
calisch 3:f68eaa68f4ec 203 49,49,0,
calisch 3:f68eaa68f4ec 204 49,49,0,
calisch 3:f68eaa68f4ec 205 49,49,0,
calisch 3:f68eaa68f4ec 206 49,49,0,
calisch 3:f68eaa68f4ec 207 49,49,0,
calisch 3:f68eaa68f4ec 208 49,49,0,
calisch 3:f68eaa68f4ec 209 49,49,0,
calisch 3:f68eaa68f4ec 210 49,49,0,
calisch 3:f68eaa68f4ec 211 49,49,0,
calisch 3:f68eaa68f4ec 212 49,49,0,
calisch 3:f68eaa68f4ec 213 49,49,0,
calisch 3:f68eaa68f4ec 214 49,49,0,
calisch 3:f68eaa68f4ec 215 49,49,0,
calisch 3:f68eaa68f4ec 216 49,49,0,
calisch 3:f68eaa68f4ec 217 49,49,0,
calisch 3:f68eaa68f4ec 218 49,49,0,
calisch 3:f68eaa68f4ec 219 49,49,0,
calisch 3:f68eaa68f4ec 220 49,49,0,
calisch 3:f68eaa68f4ec 221 49,49,0,
calisch 3:f68eaa68f4ec 222 49,49,0,
calisch 3:f68eaa68f4ec 223 49,49,0,
calisch 3:f68eaa68f4ec 224 49,49,0,
calisch 3:f68eaa68f4ec 225 49,49,0,
calisch 3:f68eaa68f4ec 226 49,49,0,
calisch 3:f68eaa68f4ec 227 49,49,0,
calisch 3:f68eaa68f4ec 228 49,49,0,
calisch 3:f68eaa68f4ec 229 49,49,0,
calisch 3:f68eaa68f4ec 230 49,49,0,
calisch 3:f68eaa68f4ec 231 49,49,0,
calisch 3:f68eaa68f4ec 232 49,49,0,
calisch 3:f68eaa68f4ec 233 49,49,0,
calisch 3:f68eaa68f4ec 234 49,49,0,
calisch 3:f68eaa68f4ec 235 49,49,0,
calisch 3:f68eaa68f4ec 236 49,49,0,
calisch 3:f68eaa68f4ec 237 49,49,0,
calisch 3:f68eaa68f4ec 238 49,49,0,
calisch 3:f68eaa68f4ec 239 49,49,0,
calisch 3:f68eaa68f4ec 240 49,49,0,
calisch 3:f68eaa68f4ec 241 49,49,0,
calisch 3:f68eaa68f4ec 242 49,49,0,
calisch 3:f68eaa68f4ec 243 49,49,0,
calisch 3:f68eaa68f4ec 244 49,49,0,
calisch 3:f68eaa68f4ec 245 49,49,0,
calisch 3:f68eaa68f4ec 246 49,49,0,
calisch 3:f68eaa68f4ec 247 49,49,0,
calisch 3:f68eaa68f4ec 248 49,49,0,
calisch 3:f68eaa68f4ec 249 49,49,0,
calisch 3:f68eaa68f4ec 250 49,49,0,
calisch 3:f68eaa68f4ec 251 49,49,0,
calisch 3:f68eaa68f4ec 252 49,49,0,
calisch 3:f68eaa68f4ec 253 49,49,0,
calisch 3:f68eaa68f4ec 254 49,49,0,
calisch 3:f68eaa68f4ec 255 49,49,0,
calisch 3:f68eaa68f4ec 256 49,49,0,
calisch 3:f68eaa68f4ec 257 49,49,0,
calisch 3:f68eaa68f4ec 258 49,49,0,
calisch 3:f68eaa68f4ec 259 49,49,0,
calisch 3:f68eaa68f4ec 260 49,49,0,
calisch 3:f68eaa68f4ec 261 49,49,0,
calisch 3:f68eaa68f4ec 262 49,49,0,
calisch 3:f68eaa68f4ec 263 49,49,0,
calisch 3:f68eaa68f4ec 264 49,49,0,
calisch 3:f68eaa68f4ec 265 49,49,0,
calisch 3:f68eaa68f4ec 266 49,49,0,
calisch 3:f68eaa68f4ec 267 49,49,0,
calisch 3:f68eaa68f4ec 268 49,49,0,
calisch 3:f68eaa68f4ec 269 49,49,0,
calisch 3:f68eaa68f4ec 270 49,49,0,
calisch 3:f68eaa68f4ec 271 49,49,0,
calisch 3:f68eaa68f4ec 272 49,49,0,
calisch 3:f68eaa68f4ec 273 49,49,0,
calisch 3:f68eaa68f4ec 274 49,49,0,
calisch 3:f68eaa68f4ec 275 49,49,0,
calisch 3:f68eaa68f4ec 276 49,49,0,
calisch 3:f68eaa68f4ec 277 49,49,0,
calisch 3:f68eaa68f4ec 278 49,49,0,
calisch 3:f68eaa68f4ec 279 49,49,0,
calisch 3:f68eaa68f4ec 280 49,49,0,
calisch 3:f68eaa68f4ec 281 49,49,0,
calisch 3:f68eaa68f4ec 282 49,49,0,
calisch 3:f68eaa68f4ec 283 49,49,0,
calisch 3:f68eaa68f4ec 284 49,49,0,
calisch 3:f68eaa68f4ec 285 49,49,0,
calisch 3:f68eaa68f4ec 286 49,49,0,
calisch 3:f68eaa68f4ec 287 49,49,0,
calisch 3:f68eaa68f4ec 288 49,49,0,
calisch 3:f68eaa68f4ec 289 49,49,0,
calisch 3:f68eaa68f4ec 290 49,49,0,
calisch 3:f68eaa68f4ec 291 49,49,0,
calisch 3:f68eaa68f4ec 292 49,49,0,
calisch 3:f68eaa68f4ec 293 49,49,0,
calisch 3:f68eaa68f4ec 294 49,49,0,
calisch 3:f68eaa68f4ec 295 49,49,0,
calisch 3:f68eaa68f4ec 296 49,49,0,
calisch 3:f68eaa68f4ec 297 49,49,0,
calisch 3:f68eaa68f4ec 298 49,49,0,
calisch 3:f68eaa68f4ec 299 49,49,0,
calisch 3:f68eaa68f4ec 300 49,49,0,
calisch 3:f68eaa68f4ec 301 49,49,0,
calisch 3:f68eaa68f4ec 302 49,49,0,
calisch 3:f68eaa68f4ec 303 49,49,0,
calisch 3:f68eaa68f4ec 304 49,49,0,
calisch 3:f68eaa68f4ec 305 49,49,0,
calisch 3:f68eaa68f4ec 306 49,49,0,
calisch 3:f68eaa68f4ec 307 49,49,0,
calisch 3:f68eaa68f4ec 308 49,49,0,
calisch 3:f68eaa68f4ec 309 49,49,0,
calisch 3:f68eaa68f4ec 310 49,49,0,
calisch 3:f68eaa68f4ec 311 49,49,0,
calisch 3:f68eaa68f4ec 312 49,49,0,
calisch 3:f68eaa68f4ec 313 49,49,0,
calisch 3:f68eaa68f4ec 314 49,49,0,
calisch 3:f68eaa68f4ec 315 49,49,0,
calisch 3:f68eaa68f4ec 316 49,49,0,
calisch 3:f68eaa68f4ec 317 49,49,0,
calisch 3:f68eaa68f4ec 318 49,49,0,
calisch 3:f68eaa68f4ec 319 49,49,0,
calisch 3:f68eaa68f4ec 320 49,49,0,
calisch 3:f68eaa68f4ec 321 49,49,0,
calisch 3:f68eaa68f4ec 322 49,49,0,
calisch 3:f68eaa68f4ec 323 49,49,0,
calisch 3:f68eaa68f4ec 324 49,49,0,
calisch 3:f68eaa68f4ec 325 49,49,0,
calisch 3:f68eaa68f4ec 326 49,49,0,
calisch 3:f68eaa68f4ec 327 49,49,0,
calisch 3:f68eaa68f4ec 328 49,49,0,
calisch 3:f68eaa68f4ec 329 49,49,0,
calisch 3:f68eaa68f4ec 330 49,49,0,
calisch 3:f68eaa68f4ec 331 49,49,0,
calisch 3:f68eaa68f4ec 332 49,49,0,
calisch 3:f68eaa68f4ec 333 49,49,0,
calisch 3:f68eaa68f4ec 334 49,49,0,
calisch 3:f68eaa68f4ec 335 49,49,0,
calisch 3:f68eaa68f4ec 336 49,49,0,
calisch 3:f68eaa68f4ec 337 49,49,0,
calisch 3:f68eaa68f4ec 338 49,49,0,
calisch 3:f68eaa68f4ec 339 49,49,0,
calisch 3:f68eaa68f4ec 340 49,49,0,
calisch 3:f68eaa68f4ec 341 49,49,0,
calisch 3:f68eaa68f4ec 342 49,49,0,
calisch 3:f68eaa68f4ec 343 49,49,0,
calisch 3:f68eaa68f4ec 344 49,49,0,
calisch 3:f68eaa68f4ec 345 49,49,0,
calisch 3:f68eaa68f4ec 346 49,49,0,
calisch 3:f68eaa68f4ec 347 49,49,0,
calisch 3:f68eaa68f4ec 348 49,49,0,
calisch 3:f68eaa68f4ec 349 49,49,0,
calisch 3:f68eaa68f4ec 350 49,49,0,
calisch 3:f68eaa68f4ec 351 49,49,0,
calisch 3:f68eaa68f4ec 352 49,49,0,
calisch 3:f68eaa68f4ec 353 49,49,0,
calisch 3:f68eaa68f4ec 354 49,49,0,
calisch 3:f68eaa68f4ec 355 49,49,0,
calisch 3:f68eaa68f4ec 356 49,49,0,
calisch 3:f68eaa68f4ec 357 49,49,0,
calisch 3:f68eaa68f4ec 358 49,49,0,
calisch 3:f68eaa68f4ec 359 49,49,0,
calisch 3:f68eaa68f4ec 360 49,49,0,
calisch 3:f68eaa68f4ec 361 49,49,0,
calisch 3:f68eaa68f4ec 362 49,49,0,
calisch 3:f68eaa68f4ec 363 49,49,0,
calisch 3:f68eaa68f4ec 364 49,49,0,
calisch 3:f68eaa68f4ec 365 49,49,0,
calisch 3:f68eaa68f4ec 366 49,49,0,
calisch 3:f68eaa68f4ec 367 49,49,0,
calisch 3:f68eaa68f4ec 368 49,49,0,
calisch 3:f68eaa68f4ec 369 49,49,0,
calisch 3:f68eaa68f4ec 370 49,49,0,
calisch 3:f68eaa68f4ec 371 49,49,0,
calisch 3:f68eaa68f4ec 372 49,49,0,
calisch 3:f68eaa68f4ec 373 49,49,0,
calisch 3:f68eaa68f4ec 374 49,49,0,
calisch 3:f68eaa68f4ec 375 49,49,0,
calisch 3:f68eaa68f4ec 376 49,49,0,
calisch 3:f68eaa68f4ec 377 49,49,0,
calisch 3:f68eaa68f4ec 378 49,49,0,
calisch 3:f68eaa68f4ec 379 49,49,0,
calisch 3:f68eaa68f4ec 380 49,49,0,
calisch 3:f68eaa68f4ec 381 49,49,0,
calisch 3:f68eaa68f4ec 382 49,49,0,
calisch 3:f68eaa68f4ec 383 49,49,0,
calisch 3:f68eaa68f4ec 384 49,49,0,
calisch 3:f68eaa68f4ec 385 49,49,0,
calisch 3:f68eaa68f4ec 386 49,49,0,
calisch 3:f68eaa68f4ec 387 49,49,0,
calisch 3:f68eaa68f4ec 388 49,49,0,
calisch 3:f68eaa68f4ec 389 49,49,0,
calisch 3:f68eaa68f4ec 390 49,49,0,
calisch 3:f68eaa68f4ec 391 49,49,0,
calisch 3:f68eaa68f4ec 392 49,49,0,
calisch 3:f68eaa68f4ec 393 49,49,0,
calisch 3:f68eaa68f4ec 394 49,49,0,
calisch 3:f68eaa68f4ec 395 49,49,0,
calisch 3:f68eaa68f4ec 396 49,49,0,
calisch 3:f68eaa68f4ec 397 49,49,0,
calisch 3:f68eaa68f4ec 398 49,49,0,
calisch 3:f68eaa68f4ec 399 49,49,0,
calisch 3:f68eaa68f4ec 400 49,49,0,
calisch 3:f68eaa68f4ec 401 49,49,0,
calisch 3:f68eaa68f4ec 402 49,49,0,
calisch 3:f68eaa68f4ec 403 49,49,0,
calisch 3:f68eaa68f4ec 404 49,49,0,
calisch 3:f68eaa68f4ec 405 49,49,0,
calisch 3:f68eaa68f4ec 406 49,49,0,
calisch 3:f68eaa68f4ec 407 49,49,0,
calisch 3:f68eaa68f4ec 408 49,49,0,
calisch 3:f68eaa68f4ec 409 49,49,0,
calisch 3:f68eaa68f4ec 410 49,49,0,
calisch 3:f68eaa68f4ec 411 49,49,0,
calisch 3:f68eaa68f4ec 412 49,49,0,
calisch 3:f68eaa68f4ec 413 49,49,0,
calisch 3:f68eaa68f4ec 414 49,49,0,
calisch 3:f68eaa68f4ec 415 49,49,0,
calisch 3:f68eaa68f4ec 416 49,49,0,
calisch 3:f68eaa68f4ec 417 49,49,0,
calisch 3:f68eaa68f4ec 418 49,49,0,
calisch 3:f68eaa68f4ec 419 49,49,0,
calisch 3:f68eaa68f4ec 420 49,49,0,
calisch 3:f68eaa68f4ec 421 49,49,0,
calisch 3:f68eaa68f4ec 422 49,49,0,
calisch 3:f68eaa68f4ec 423 49,49,0,
calisch 3:f68eaa68f4ec 424 49,49,0,
calisch 3:f68eaa68f4ec 425 49,49,0,
calisch 3:f68eaa68f4ec 426 49,49,0,
calisch 3:f68eaa68f4ec 427 49,49,0,
calisch 3:f68eaa68f4ec 428 49,49,0,
calisch 3:f68eaa68f4ec 429 49,49,0,
calisch 3:f68eaa68f4ec 430 49,49,0,
calisch 3:f68eaa68f4ec 431 49,49,0,
calisch 3:f68eaa68f4ec 432 49,49,0,
calisch 3:f68eaa68f4ec 433 49,49,0,
calisch 3:f68eaa68f4ec 434 49,49,0,
calisch 3:f68eaa68f4ec 435 49,49,0,
calisch 3:f68eaa68f4ec 436 49,49,0,
calisch 3:f68eaa68f4ec 437 49,49,0,
calisch 3:f68eaa68f4ec 438 49,49,0,
calisch 3:f68eaa68f4ec 439 49,49,0,
calisch 3:f68eaa68f4ec 440 49,49,0,
calisch 3:f68eaa68f4ec 441 49,49,0,
calisch 3:f68eaa68f4ec 442 49,49,0,
calisch 3:f68eaa68f4ec 443 49,49,0,
calisch 3:f68eaa68f4ec 444 49,49,0,
calisch 3:f68eaa68f4ec 445 49,49,0,
calisch 3:f68eaa68f4ec 446 49,49,0,
calisch 3:f68eaa68f4ec 447 49,49,0,
calisch 3:f68eaa68f4ec 448 49,49,0,
calisch 3:f68eaa68f4ec 449 49,49,0,
calisch 3:f68eaa68f4ec 450 49,49,0,
calisch 3:f68eaa68f4ec 451 49,49,0,
calisch 3:f68eaa68f4ec 452 49,49,0,
calisch 3:f68eaa68f4ec 453 49,49,0,
calisch 3:f68eaa68f4ec 454 49,49,0,
calisch 3:f68eaa68f4ec 455 49,49,0,
calisch 3:f68eaa68f4ec 456 49,49,0,
calisch 3:f68eaa68f4ec 457 49,49,0,
calisch 3:f68eaa68f4ec 458 49,49,0,
calisch 3:f68eaa68f4ec 459 49,49,0,
calisch 3:f68eaa68f4ec 460 49,49,0,
calisch 3:f68eaa68f4ec 461 49,49,0,
calisch 3:f68eaa68f4ec 462 49,49,0,
calisch 3:f68eaa68f4ec 463 49,49,0,
calisch 3:f68eaa68f4ec 464 49,49,0,
calisch 3:f68eaa68f4ec 465 49,49,0,
calisch 3:f68eaa68f4ec 466 49,49,0,
calisch 3:f68eaa68f4ec 467 49,49,0,
calisch 3:f68eaa68f4ec 468 49,49,0,
calisch 3:f68eaa68f4ec 469 49,49,0,
calisch 3:f68eaa68f4ec 470 49,49,0,
calisch 3:f68eaa68f4ec 471 49,49,0,
calisch 3:f68eaa68f4ec 472 49,49,0,
calisch 3:f68eaa68f4ec 473 49,49,0,
calisch 3:f68eaa68f4ec 474 49,49,0,
calisch 3:f68eaa68f4ec 475 49,49,0,
calisch 3:f68eaa68f4ec 476 49,49,0,
calisch 3:f68eaa68f4ec 477 49,49,0,
calisch 3:f68eaa68f4ec 478 49,49,0,
calisch 3:f68eaa68f4ec 479 49,49,0,
calisch 3:f68eaa68f4ec 480 49,49,0,
calisch 3:f68eaa68f4ec 481 49,49,0,
calisch 3:f68eaa68f4ec 482 49,49,0,
calisch 3:f68eaa68f4ec 483 49,49,0,
calisch 3:f68eaa68f4ec 484 49,49,0,
calisch 3:f68eaa68f4ec 485 49,49,0,
calisch 3:f68eaa68f4ec 486 49,49,0,
calisch 3:f68eaa68f4ec 487 49,49,0,
calisch 3:f68eaa68f4ec 488 49,49,0,
calisch 3:f68eaa68f4ec 489 49,49,0,
calisch 3:f68eaa68f4ec 490 49,49,0,
calisch 3:f68eaa68f4ec 491 49,49,0,
calisch 3:f68eaa68f4ec 492 49,49,0,
calisch 3:f68eaa68f4ec 493 49,49,0,
calisch 3:f68eaa68f4ec 494 49,49,0,
calisch 3:f68eaa68f4ec 495 49,49,0,
calisch 3:f68eaa68f4ec 496 49,49,0,
calisch 3:f68eaa68f4ec 497 49,49,0,
calisch 3:f68eaa68f4ec 498 49,49,0,
calisch 3:f68eaa68f4ec 499 49,49,0,
calisch 3:f68eaa68f4ec 500 49,49,0,
calisch 3:f68eaa68f4ec 501 49,49,0,
calisch 3:f68eaa68f4ec 502 49,49,0,
calisch 3:f68eaa68f4ec 503 49,49,0,
calisch 3:f68eaa68f4ec 504 49,49,0,
calisch 3:f68eaa68f4ec 505 49,49,0,
calisch 3:f68eaa68f4ec 506 49,49,0,
calisch 3:f68eaa68f4ec 507 49,49,0,
calisch 3:f68eaa68f4ec 508 49,49,0,
calisch 3:f68eaa68f4ec 509 49,49,0,
calisch 3:f68eaa68f4ec 510 49,49,0,
calisch 3:f68eaa68f4ec 511 49,49,0,
calisch 3:f68eaa68f4ec 512 49,49,0,
calisch 3:f68eaa68f4ec 513 49,49,0,
calisch 3:f68eaa68f4ec 514 49,49,0,
calisch 3:f68eaa68f4ec 515 49,49,0,
calisch 3:f68eaa68f4ec 516 49,49,0,
calisch 3:f68eaa68f4ec 517 49,49,0,
calisch 3:f68eaa68f4ec 518 49,49,0,
calisch 3:f68eaa68f4ec 519 49,49,0,
calisch 3:f68eaa68f4ec 520 49,49,0,
calisch 3:f68eaa68f4ec 521 49,49,0,
calisch 3:f68eaa68f4ec 522 49,49,0,
calisch 3:f68eaa68f4ec 523 49,49,0,
calisch 3:f68eaa68f4ec 524 49,49,0,
calisch 3:f68eaa68f4ec 525 49,49,0,
calisch 3:f68eaa68f4ec 526 49,49,0,
calisch 3:f68eaa68f4ec 527 49,49,0,
calisch 3:f68eaa68f4ec 528 49,49,0,
calisch 3:f68eaa68f4ec 529 49,49,0,
calisch 3:f68eaa68f4ec 530 49,49,0,
calisch 3:f68eaa68f4ec 531 49,49,0,
calisch 3:f68eaa68f4ec 532 49,49,0,
calisch 3:f68eaa68f4ec 533 49,49,0,
calisch 3:f68eaa68f4ec 534 49,49,0,
calisch 3:f68eaa68f4ec 535 49,49,0,
calisch 3:f68eaa68f4ec 536 49,49,0,
calisch 3:f68eaa68f4ec 537 49,49,0,
calisch 3:f68eaa68f4ec 538 49,49,0,
calisch 3:f68eaa68f4ec 539 49,49,0,
calisch 3:f68eaa68f4ec 540 49,49,0,
calisch 3:f68eaa68f4ec 541 49,49,0,
calisch 3:f68eaa68f4ec 542 49,49,0,
calisch 3:f68eaa68f4ec 543 49,49,0,
calisch 3:f68eaa68f4ec 544 49,49,0,
calisch 3:f68eaa68f4ec 545 49,49,0,
calisch 3:f68eaa68f4ec 546 49,49,0,
calisch 3:f68eaa68f4ec 547 49,49,0,
calisch 3:f68eaa68f4ec 548 49,49,0,
calisch 3:f68eaa68f4ec 549 49,49,0,
calisch 3:f68eaa68f4ec 550 49,49,0,
calisch 3:f68eaa68f4ec 551 49,49,0,
calisch 3:f68eaa68f4ec 552 49,49,0,
calisch 3:f68eaa68f4ec 553 49,49,0,
calisch 3:f68eaa68f4ec 554 49,49,0,
calisch 3:f68eaa68f4ec 555 49,49,0,
calisch 3:f68eaa68f4ec 556 49,49,0,
calisch 3:f68eaa68f4ec 557 49,49,0,
calisch 3:f68eaa68f4ec 558 49,49,0,
calisch 3:f68eaa68f4ec 559 49,49,0,
calisch 3:f68eaa68f4ec 560 49,49,0,
calisch 3:f68eaa68f4ec 561 49,49,0,
calisch 3:f68eaa68f4ec 562 49,49,0,
calisch 3:f68eaa68f4ec 563 49,49,0,
calisch 3:f68eaa68f4ec 564 49,49,0,
calisch 3:f68eaa68f4ec 565 49,49,0,
calisch 3:f68eaa68f4ec 566 49,49,0,
calisch 3:f68eaa68f4ec 567 49,49,0,
calisch 3:f68eaa68f4ec 568 49,49,0,
calisch 3:f68eaa68f4ec 569 49,49,0,
calisch 3:f68eaa68f4ec 570 49,49,0,
calisch 3:f68eaa68f4ec 571 49,49,0,
calisch 3:f68eaa68f4ec 572 49,49,0,
calisch 3:f68eaa68f4ec 573 49,49,0,
calisch 3:f68eaa68f4ec 574 49,49,0,
calisch 3:f68eaa68f4ec 575 49,49,0,
calisch 3:f68eaa68f4ec 576 49,49,0,
calisch 3:f68eaa68f4ec 577 49,49,0,
calisch 3:f68eaa68f4ec 578 49,49,0,
calisch 3:f68eaa68f4ec 579 49,49,0,
calisch 3:f68eaa68f4ec 580 49,49,0,
calisch 3:f68eaa68f4ec 581 49,49,0,
calisch 3:f68eaa68f4ec 582 49,49,0,
calisch 3:f68eaa68f4ec 583 49,49,0,
calisch 3:f68eaa68f4ec 584 49,49,0,
calisch 3:f68eaa68f4ec 585 49,49,0,
calisch 3:f68eaa68f4ec 586 49,49,0,
calisch 3:f68eaa68f4ec 587 49,49,0,
calisch 3:f68eaa68f4ec 588 49,49,0,
calisch 3:f68eaa68f4ec 589 49,49,0,
calisch 3:f68eaa68f4ec 590 49,49,0,
calisch 3:f68eaa68f4ec 591 49,49,0,
calisch 3:f68eaa68f4ec 592 49,49,0,
calisch 3:f68eaa68f4ec 593 49,49,0,
calisch 3:f68eaa68f4ec 594 49,49,0,
calisch 3:f68eaa68f4ec 595 49,49,0,
calisch 3:f68eaa68f4ec 596 49,49,0,
calisch 3:f68eaa68f4ec 597 49,49,0,
calisch 3:f68eaa68f4ec 598 49,49,0,
calisch 3:f68eaa68f4ec 599 49,49,0,
calisch 3:f68eaa68f4ec 600 49,49,0,
calisch 3:f68eaa68f4ec 601 49,49,0,
calisch 3:f68eaa68f4ec 602 49,49,0,
calisch 3:f68eaa68f4ec 603 49,49,0,
calisch 3:f68eaa68f4ec 604 49,49,0,
calisch 3:f68eaa68f4ec 605 49,49,0,
calisch 3:f68eaa68f4ec 606 49,49,0,
calisch 3:f68eaa68f4ec 607 49,49,0,
calisch 3:f68eaa68f4ec 608 49,49,0,
calisch 3:f68eaa68f4ec 609 49,49,0,
calisch 3:f68eaa68f4ec 610 49,49,0,
calisch 3:f68eaa68f4ec 611 49,49,0,
calisch 3:f68eaa68f4ec 612 49,49,0,
calisch 3:f68eaa68f4ec 613 49,49,0,
calisch 3:f68eaa68f4ec 614 49,49,0,
calisch 3:f68eaa68f4ec 615 49,49,0,
calisch 3:f68eaa68f4ec 616 49,49,0,
calisch 3:f68eaa68f4ec 617 49,49,0,
calisch 3:f68eaa68f4ec 618 49,49,0,
calisch 3:f68eaa68f4ec 619 49,49,0,
calisch 3:f68eaa68f4ec 620 49,49,0,
calisch 3:f68eaa68f4ec 621 49,49,0,
calisch 3:f68eaa68f4ec 622 49,49,0,
calisch 3:f68eaa68f4ec 623 49,49,0,
calisch 3:f68eaa68f4ec 624 49,49,0,
calisch 3:f68eaa68f4ec 625 49,49,0,
calisch 3:f68eaa68f4ec 626 49,49,0,
calisch 3:f68eaa68f4ec 627 49,49,0,
calisch 3:f68eaa68f4ec 628 49,49,0,
calisch 3:f68eaa68f4ec 629 49,49,0,
calisch 3:f68eaa68f4ec 630 49,49,0,
calisch 3:f68eaa68f4ec 631 49,49,0,
calisch 3:f68eaa68f4ec 632 49,49,0,
calisch 3:f68eaa68f4ec 633 49,49,0,
calisch 3:f68eaa68f4ec 634 49,49,0,
calisch 3:f68eaa68f4ec 635 49,49,0,
calisch 3:f68eaa68f4ec 636 49,49,0,
calisch 3:f68eaa68f4ec 637 49,49,0,
calisch 3:f68eaa68f4ec 638 49,49,0,
calisch 3:f68eaa68f4ec 639 49,49,0,
calisch 3:f68eaa68f4ec 640 49,49,0,
calisch 3:f68eaa68f4ec 641 49,49,0,
calisch 3:f68eaa68f4ec 642 49,49,0,
calisch 3:f68eaa68f4ec 643 49,49,0,
calisch 3:f68eaa68f4ec 644 49,49,0,
calisch 3:f68eaa68f4ec 645 49,49,0,
calisch 3:f68eaa68f4ec 646 49,49,0,
calisch 3:f68eaa68f4ec 647 49,49,0,
calisch 3:f68eaa68f4ec 648 49,49,0,
calisch 3:f68eaa68f4ec 649 49,49,0,
calisch 3:f68eaa68f4ec 650 49,49,0,
calisch 3:f68eaa68f4ec 651 49,49,0,
calisch 3:f68eaa68f4ec 652 49,49,0,
calisch 3:f68eaa68f4ec 653 49,49,0,
calisch 3:f68eaa68f4ec 654 49,49,0,
calisch 3:f68eaa68f4ec 655 49,49,0,
calisch 3:f68eaa68f4ec 656 49,49,0,
calisch 3:f68eaa68f4ec 657 49,49,0,
calisch 3:f68eaa68f4ec 658 49,49,0,
calisch 3:f68eaa68f4ec 659 49,49,0,
calisch 3:f68eaa68f4ec 660 49,49,0,
calisch 3:f68eaa68f4ec 661 49,49,0,
calisch 3:f68eaa68f4ec 662 49,49,0,
calisch 3:f68eaa68f4ec 663 49,49,0,
calisch 3:f68eaa68f4ec 664 49,49,0,
calisch 3:f68eaa68f4ec 665 49,49,0,
calisch 3:f68eaa68f4ec 666 49,49,0,
calisch 3:f68eaa68f4ec 667 49,49,0,
calisch 3:f68eaa68f4ec 668 49,49,0,
calisch 3:f68eaa68f4ec 669 49,49,0,
calisch 3:f68eaa68f4ec 670 49,49,0,
calisch 3:f68eaa68f4ec 671 49,49,0,
calisch 3:f68eaa68f4ec 672 49,49,0,
calisch 3:f68eaa68f4ec 673 49,49,0,
calisch 3:f68eaa68f4ec 674 49,49,0,
calisch 3:f68eaa68f4ec 675 49,49,0,
calisch 3:f68eaa68f4ec 676 49,49,0,
calisch 3:f68eaa68f4ec 677 49,49,0,
calisch 3:f68eaa68f4ec 678 49,49,0,
calisch 3:f68eaa68f4ec 679 49,49,0,
calisch 3:f68eaa68f4ec 680 49,49,0,
calisch 3:f68eaa68f4ec 681 49,49,0,
calisch 3:f68eaa68f4ec 682 49,49,0,
calisch 3:f68eaa68f4ec 683 49,49,0,
calisch 3:f68eaa68f4ec 684 49,49,0,
calisch 3:f68eaa68f4ec 685 49,49,0,
calisch 3:f68eaa68f4ec 686 49,49,0,
calisch 3:f68eaa68f4ec 687 49,49,0,
calisch 3:f68eaa68f4ec 688 49,49,0,
calisch 3:f68eaa68f4ec 689 49,49,0,
calisch 3:f68eaa68f4ec 690 49,49,0,
calisch 3:f68eaa68f4ec 691 49,49,0,
calisch 3:f68eaa68f4ec 692 49,49,0,
calisch 3:f68eaa68f4ec 693 49,49,0,
calisch 3:f68eaa68f4ec 694 49,49,0,
calisch 3:f68eaa68f4ec 695 49,49,0,
calisch 3:f68eaa68f4ec 696 49,49,0,
calisch 3:f68eaa68f4ec 697 49,49,0,
calisch 3:f68eaa68f4ec 698 49,49,0,
calisch 3:f68eaa68f4ec 699 49,49,0,
calisch 3:f68eaa68f4ec 700 49,49,0,
calisch 3:f68eaa68f4ec 701 49,49,0,
calisch 3:f68eaa68f4ec 702 49,49,0,
calisch 3:f68eaa68f4ec 703 49,49,0,
calisch 3:f68eaa68f4ec 704 49,49,0,
calisch 3:f68eaa68f4ec 705 49,49,0,
calisch 3:f68eaa68f4ec 706 49,49,0,
calisch 3:f68eaa68f4ec 707 49,49,0,
calisch 3:f68eaa68f4ec 708 49,49,0,
calisch 3:f68eaa68f4ec 709 49,49,0,
calisch 3:f68eaa68f4ec 710 49,49,0,
calisch 3:f68eaa68f4ec 711 49,49,0,
calisch 3:f68eaa68f4ec 712 49,49,0,
calisch 3:f68eaa68f4ec 713 49,49,0,
calisch 3:f68eaa68f4ec 714 49,49,0,
calisch 3:f68eaa68f4ec 715 49,49,0,
calisch 3:f68eaa68f4ec 716 49,49,0,
calisch 3:f68eaa68f4ec 717 49,49,0,
calisch 3:f68eaa68f4ec 718 49,49,0,
calisch 3:f68eaa68f4ec 719 49,49,0,
calisch 3:f68eaa68f4ec 720 49,49,0,
calisch 3:f68eaa68f4ec 721 49,49,0,
calisch 3:f68eaa68f4ec 722 49,49,0,
calisch 3:f68eaa68f4ec 723 49,49,0,
calisch 3:f68eaa68f4ec 724 49,49,0,
calisch 3:f68eaa68f4ec 725 49,49,0,
calisch 3:f68eaa68f4ec 726 49,49,0,
calisch 3:f68eaa68f4ec 727 49,49,0,
calisch 3:f68eaa68f4ec 728 49,49,0,
calisch 3:f68eaa68f4ec 729 49,49,0,
calisch 3:f68eaa68f4ec 730 49,49,0,
calisch 3:f68eaa68f4ec 731 49,49,0,
calisch 3:f68eaa68f4ec 732 49,49,0,
calisch 3:f68eaa68f4ec 733 49,49,0,
calisch 3:f68eaa68f4ec 734 49,49,0,
calisch 3:f68eaa68f4ec 735 49,49,0,
calisch 3:f68eaa68f4ec 736 49,49,0,
calisch 3:f68eaa68f4ec 737 49,49,0,
calisch 3:f68eaa68f4ec 738 49,49,0,
calisch 3:f68eaa68f4ec 739 49,49,0,
calisch 3:f68eaa68f4ec 740 49,49,0,
calisch 3:f68eaa68f4ec 741 49,49,0,
calisch 3:f68eaa68f4ec 742 49,49,0,
calisch 3:f68eaa68f4ec 743 49,49,0,
calisch 3:f68eaa68f4ec 744 49,49,0,
calisch 3:f68eaa68f4ec 745 49,49,0,
calisch 3:f68eaa68f4ec 746 49,49,0,
calisch 3:f68eaa68f4ec 747 49,49,0,
calisch 3:f68eaa68f4ec 748 49,49,0,
calisch 3:f68eaa68f4ec 749 49,49,0,
calisch 3:f68eaa68f4ec 750 49,49,0,
calisch 3:f68eaa68f4ec 751 49,49,0,
calisch 3:f68eaa68f4ec 752 49,49,0,
calisch 3:f68eaa68f4ec 753 49,49,0,
calisch 3:f68eaa68f4ec 754 49,49,0,
calisch 3:f68eaa68f4ec 755 49,49,0,
calisch 3:f68eaa68f4ec 756 49,49,0,
calisch 3:f68eaa68f4ec 757 49,49,0,
calisch 3:f68eaa68f4ec 758 49,49,0,
calisch 3:f68eaa68f4ec 759 49,49,0,
calisch 3:f68eaa68f4ec 760 49,49,0,
calisch 3:f68eaa68f4ec 761 49,49,0,
calisch 3:f68eaa68f4ec 762 49,49,0,
calisch 3:f68eaa68f4ec 763 49,49,0,
calisch 3:f68eaa68f4ec 764 49,49,0,
calisch 3:f68eaa68f4ec 765 49,49,0,
calisch 3:f68eaa68f4ec 766 49,49,0,
calisch 3:f68eaa68f4ec 767 49,49,0,
calisch 3:f68eaa68f4ec 768 49,49,0,
calisch 3:f68eaa68f4ec 769 49,49,0,
calisch 3:f68eaa68f4ec 770 49,49,0,
calisch 3:f68eaa68f4ec 771 49,49,0,
calisch 3:f68eaa68f4ec 772 49,49,0,
calisch 3:f68eaa68f4ec 773 49,49,0,
calisch 3:f68eaa68f4ec 774 49,49,0,
calisch 3:f68eaa68f4ec 775 49,49,0,
calisch 3:f68eaa68f4ec 776 49,49,0,
calisch 3:f68eaa68f4ec 777 49,49,0,
calisch 3:f68eaa68f4ec 778 49,49,0,
calisch 3:f68eaa68f4ec 779 49,49,0,
calisch 3:f68eaa68f4ec 780 49,49,0,
calisch 3:f68eaa68f4ec 781 49,49,0,
calisch 3:f68eaa68f4ec 782 49,49,0,
calisch 3:f68eaa68f4ec 783 49,49,0,
calisch 3:f68eaa68f4ec 784 49,49,0,
calisch 3:f68eaa68f4ec 785 49,49,0,
calisch 3:f68eaa68f4ec 786 49,49,0,
calisch 3:f68eaa68f4ec 787 49,49,0,
calisch 3:f68eaa68f4ec 788 49,49,0,
calisch 3:f68eaa68f4ec 789 49,49,0,
calisch 3:f68eaa68f4ec 790 49,49,0,
calisch 3:f68eaa68f4ec 791 49,49,0,
calisch 3:f68eaa68f4ec 792 49,49,0,
calisch 3:f68eaa68f4ec 793 49,49,0,
calisch 3:f68eaa68f4ec 794 49,49,0,
calisch 3:f68eaa68f4ec 795 49,49,0,
calisch 3:f68eaa68f4ec 796 49,49,0,
calisch 3:f68eaa68f4ec 797 49,49,0,
calisch 3:f68eaa68f4ec 798 49,49,0,
calisch 3:f68eaa68f4ec 799 49,49,0,
calisch 3:f68eaa68f4ec 800 49,49,0,
calisch 3:f68eaa68f4ec 801 49,49,0,
calisch 3:f68eaa68f4ec 802 49,49,0,
calisch 3:f68eaa68f4ec 803 49,49,0,
calisch 3:f68eaa68f4ec 804 49,49,0,
calisch 3:f68eaa68f4ec 805 49,49,0,
calisch 3:f68eaa68f4ec 806 49,49,0,
calisch 3:f68eaa68f4ec 807 49,49,0,
calisch 3:f68eaa68f4ec 808 49,49,0,
calisch 3:f68eaa68f4ec 809 49,49,0,
calisch 3:f68eaa68f4ec 810 49,49,0,
calisch 3:f68eaa68f4ec 811 49,49,0,
calisch 3:f68eaa68f4ec 812 49,49,0,
calisch 3:f68eaa68f4ec 813 49,49,0,
calisch 3:f68eaa68f4ec 814 49,49,0,
calisch 3:f68eaa68f4ec 815 49,49,0,
calisch 3:f68eaa68f4ec 816 49,49,0,
calisch 3:f68eaa68f4ec 817 49,49,0,
calisch 3:f68eaa68f4ec 818 49,49,0,
calisch 3:f68eaa68f4ec 819 49,49,0,
calisch 3:f68eaa68f4ec 820 49,49,0,
calisch 3:f68eaa68f4ec 821 49,49,0,
calisch 3:f68eaa68f4ec 822 49,49,0,
calisch 3:f68eaa68f4ec 823 49,49,0,
calisch 3:f68eaa68f4ec 824 49,49,0,
calisch 3:f68eaa68f4ec 825 49,49,0,
calisch 3:f68eaa68f4ec 826 49,49,0,
calisch 3:f68eaa68f4ec 827 49,49,0,
calisch 3:f68eaa68f4ec 828 49,49,0,
calisch 3:f68eaa68f4ec 829 49,49,0,
calisch 3:f68eaa68f4ec 830 49,49,0,
calisch 3:f68eaa68f4ec 831 49,49,0,
calisch 3:f68eaa68f4ec 832 49,49,0,
calisch 3:f68eaa68f4ec 833 49,49,0,
calisch 3:f68eaa68f4ec 834 49,49,0,
calisch 3:f68eaa68f4ec 835 49,49,0,
calisch 3:f68eaa68f4ec 836 49,49,0,
calisch 3:f68eaa68f4ec 837 49,49,0,
calisch 3:f68eaa68f4ec 838 49,49,0,
calisch 3:f68eaa68f4ec 839 49,49,0,
calisch 3:f68eaa68f4ec 840 49,49,0,
calisch 3:f68eaa68f4ec 841 49,49,0,
calisch 3:f68eaa68f4ec 842 49,49,0,
calisch 3:f68eaa68f4ec 843 49,49,0,
calisch 3:f68eaa68f4ec 844 49,49,0,
calisch 3:f68eaa68f4ec 845 49,49,0,
calisch 3:f68eaa68f4ec 846 49,49,0,
calisch 3:f68eaa68f4ec 847 49,49,0,
calisch 3:f68eaa68f4ec 848 49,49,0,
calisch 3:f68eaa68f4ec 849 49,49,0,
calisch 3:f68eaa68f4ec 850 49,49,0,
calisch 3:f68eaa68f4ec 851 49,49,0,
calisch 3:f68eaa68f4ec 852 49,49,0,
calisch 3:f68eaa68f4ec 853 49,49,0,
calisch 3:f68eaa68f4ec 854 49,49,0,
calisch 3:f68eaa68f4ec 855 49,49,0,
calisch 3:f68eaa68f4ec 856 49,49,0,
calisch 3:f68eaa68f4ec 857 49,49,0,
calisch 3:f68eaa68f4ec 858 49,49,0,
calisch 3:f68eaa68f4ec 859 49,49,0,
calisch 3:f68eaa68f4ec 860 49,49,0,
calisch 3:f68eaa68f4ec 861 49,49,0,
calisch 3:f68eaa68f4ec 862 49,49,0,
calisch 3:f68eaa68f4ec 863 49,49,0,
calisch 3:f68eaa68f4ec 864 49,49,0,
calisch 3:f68eaa68f4ec 865 49,49,0,
calisch 3:f68eaa68f4ec 866 49,49,0,
calisch 3:f68eaa68f4ec 867 49,49,0,
calisch 3:f68eaa68f4ec 868 49,49,0,
calisch 3:f68eaa68f4ec 869 49,49,0,
calisch 3:f68eaa68f4ec 870 49,49,0,
calisch 3:f68eaa68f4ec 871 49,49,0,
calisch 3:f68eaa68f4ec 872 49,49,0,
calisch 3:f68eaa68f4ec 873 49,49,0,
calisch 3:f68eaa68f4ec 874 49,49,0,
calisch 3:f68eaa68f4ec 875 49,49,0,
calisch 3:f68eaa68f4ec 876 49,49,0,
calisch 3:f68eaa68f4ec 877 49,49,0,
calisch 3:f68eaa68f4ec 878 49,49,0,
calisch 3:f68eaa68f4ec 879 49,49,0,
calisch 3:f68eaa68f4ec 880 49,49,0,
calisch 3:f68eaa68f4ec 881 49,49,0,
calisch 3:f68eaa68f4ec 882 49,49,0,
calisch 3:f68eaa68f4ec 883 49,49,0,
calisch 3:f68eaa68f4ec 884 49,49,0,
calisch 3:f68eaa68f4ec 885 49,49,0,
calisch 3:f68eaa68f4ec 886 49,49,0,
calisch 3:f68eaa68f4ec 887 49,49,0,
calisch 3:f68eaa68f4ec 888 49,49,0,
calisch 3:f68eaa68f4ec 889 49,49,0,
calisch 3:f68eaa68f4ec 890 49,49,0,
calisch 3:f68eaa68f4ec 891 49,49,0,
calisch 3:f68eaa68f4ec 892 49,49,0,
calisch 3:f68eaa68f4ec 893 49,49,0,
calisch 3:f68eaa68f4ec 894 49,49,0,
calisch 3:f68eaa68f4ec 895 49,49,0,
calisch 3:f68eaa68f4ec 896 49,49,0,
calisch 3:f68eaa68f4ec 897 49,49,0,
calisch 3:f68eaa68f4ec 898 49,49,0,
calisch 3:f68eaa68f4ec 899 49,49,0,
calisch 3:f68eaa68f4ec 900 49,49,0,
calisch 3:f68eaa68f4ec 901 49,49,0,
calisch 3:f68eaa68f4ec 902 49,49,0,
calisch 3:f68eaa68f4ec 903 49,49,0,
calisch 3:f68eaa68f4ec 904 49,49,0,
calisch 3:f68eaa68f4ec 905 49,49,0,
calisch 3:f68eaa68f4ec 906 49,49,0,
calisch 3:f68eaa68f4ec 907 49,49,0,
calisch 3:f68eaa68f4ec 908 49,49,0,
calisch 3:f68eaa68f4ec 909 49,49,0,
calisch 3:f68eaa68f4ec 910 49,49,0,
calisch 3:f68eaa68f4ec 911 49,49,0,
calisch 3:f68eaa68f4ec 912 49,49,0,
calisch 3:f68eaa68f4ec 913 49,49,0,
calisch 3:f68eaa68f4ec 914 49,49,0,
calisch 3:f68eaa68f4ec 915 49,49,0,
calisch 3:f68eaa68f4ec 916 49,49,0,
calisch 3:f68eaa68f4ec 917 49,49,0,
calisch 3:f68eaa68f4ec 918 49,49,0,
calisch 3:f68eaa68f4ec 919 49,49,0,
calisch 3:f68eaa68f4ec 920 49,49,0,
calisch 3:f68eaa68f4ec 921 49,49,0,
calisch 3:f68eaa68f4ec 922 49,49,0,
calisch 3:f68eaa68f4ec 923 49,49,0,
calisch 3:f68eaa68f4ec 924 49,49,0,
calisch 3:f68eaa68f4ec 925 49,49,0,
calisch 3:f68eaa68f4ec 926 49,49,0,
calisch 3:f68eaa68f4ec 927 49,49,0,
calisch 3:f68eaa68f4ec 928 49,49,0,
calisch 3:f68eaa68f4ec 929 49,49,0,
calisch 3:f68eaa68f4ec 930 49,49,0,
calisch 3:f68eaa68f4ec 931 49,49,0,
calisch 3:f68eaa68f4ec 932 49,49,0,
calisch 3:f68eaa68f4ec 933 49,49,0,
calisch 3:f68eaa68f4ec 934 49,49,0,
calisch 3:f68eaa68f4ec 935 49,49,0,
calisch 3:f68eaa68f4ec 936 49,49,0,
calisch 3:f68eaa68f4ec 937 49,49,0,
calisch 3:f68eaa68f4ec 938 49,49,0,
calisch 3:f68eaa68f4ec 939 49,49,0,
calisch 3:f68eaa68f4ec 940 49,49,0,
calisch 3:f68eaa68f4ec 941 49,49,0,
calisch 3:f68eaa68f4ec 942 49,49,0,
calisch 3:f68eaa68f4ec 943 49,49,0,
calisch 3:f68eaa68f4ec 944 49,49,0,
calisch 3:f68eaa68f4ec 945 49,49,0,
calisch 3:f68eaa68f4ec 946 49,49,0,
calisch 3:f68eaa68f4ec 947 49,49,0,
calisch 3:f68eaa68f4ec 948 49,49,0,
calisch 3:f68eaa68f4ec 949 49,49,0,
calisch 3:f68eaa68f4ec 950 49,49,0,
calisch 3:f68eaa68f4ec 951 49,49,0,
calisch 3:f68eaa68f4ec 952 49,49,0,
calisch 3:f68eaa68f4ec 953 49,49,0,
calisch 3:f68eaa68f4ec 954 49,49,0,
calisch 3:f68eaa68f4ec 955 49,49,0,
calisch 3:f68eaa68f4ec 956 49,49,0,
calisch 3:f68eaa68f4ec 957 49,49,0,
calisch 3:f68eaa68f4ec 958 49,49,0,
calisch 3:f68eaa68f4ec 959 49,49,0,
calisch 3:f68eaa68f4ec 960 49,49,0,
calisch 3:f68eaa68f4ec 961 49,49,0,
calisch 3:f68eaa68f4ec 962 49,49,0,
calisch 3:f68eaa68f4ec 963 49,49,0,
calisch 3:f68eaa68f4ec 964 49,49,0,
calisch 3:f68eaa68f4ec 965 49,49,0,
calisch 3:f68eaa68f4ec 966 49,49,0,
calisch 3:f68eaa68f4ec 967 49,49,0,
calisch 3:f68eaa68f4ec 968 49,49,0,
calisch 3:f68eaa68f4ec 969 49,49,0,
calisch 3:f68eaa68f4ec 970 49,49,0,
calisch 3:f68eaa68f4ec 971 49,49,0,
calisch 3:f68eaa68f4ec 972 49,49,0,
calisch 3:f68eaa68f4ec 973 49,49,0,
calisch 3:f68eaa68f4ec 974 49,49,0,
calisch 3:f68eaa68f4ec 975 49,49,0,
calisch 3:f68eaa68f4ec 976 49,49,0,
calisch 3:f68eaa68f4ec 977 49,49,0,
calisch 3:f68eaa68f4ec 978 49,49,0,
calisch 3:f68eaa68f4ec 979 49,49,0,
calisch 3:f68eaa68f4ec 980 49,49,0,
calisch 3:f68eaa68f4ec 981 49,49,0,
calisch 3:f68eaa68f4ec 982 49,49,0,
calisch 3:f68eaa68f4ec 983 49,49,0,
calisch 3:f68eaa68f4ec 984 49,49,0,
calisch 3:f68eaa68f4ec 985 49,49,0,
calisch 3:f68eaa68f4ec 986 49,49,0,
calisch 3:f68eaa68f4ec 987 49,49,0,
calisch 3:f68eaa68f4ec 988 49,49,0,
calisch 3:f68eaa68f4ec 989 49,49,0,
calisch 3:f68eaa68f4ec 990 49,49,0,
calisch 3:f68eaa68f4ec 991 49,49,0,
calisch 3:f68eaa68f4ec 992 49,49,0,
calisch 3:f68eaa68f4ec 993 49,49,0,
calisch 3:f68eaa68f4ec 994 49,49,0,
calisch 3:f68eaa68f4ec 995 49,49,0,
calisch 3:f68eaa68f4ec 996 49,49,0,
calisch 3:f68eaa68f4ec 997 49,49,0,
calisch 3:f68eaa68f4ec 998 49,49,0,
calisch 3:f68eaa68f4ec 999 49,49,0,
calisch 3:f68eaa68f4ec 1000 49,49,0,
calisch 3:f68eaa68f4ec 1001 49,49,0,
calisch 3:f68eaa68f4ec 1002 49,49,0,
calisch 3:f68eaa68f4ec 1003 49,49,0,
calisch 3:f68eaa68f4ec 1004 49,49,0,
calisch 3:f68eaa68f4ec 1005 49,49,0,
calisch 3:f68eaa68f4ec 1006 49,49,0,
calisch 3:f68eaa68f4ec 1007 49,49,0
calisch 3:f68eaa68f4ec 1008 };
calisch 1:e549754ca234 1009
calisch 2:17379e2a6f7d 1010 //control flow
calisch 3:f68eaa68f4ec 1011 volatile int getting_up = 1; //are we currently in the process of standing up
calisch 3:f68eaa68f4ec 1012 volatile int getting_down = 0; //are we currently in the process of sitting down
calisch 3:f68eaa68f4ec 1013
calisch 2:17379e2a6f7d 1014 volatile int current_sample = 0;
calisch 2:17379e2a6f7d 1015 volatile int current_loop = 0;
calisch 2:17379e2a6f7d 1016 const int n_samples = 1000;
calisch 2:17379e2a6f7d 1017 const int n_loops = 2;
calisch 2:17379e2a6f7d 1018
langfordw 0:fc382eeb78ad 1019 Ticker tick;
langfordw 0:fc382eeb78ad 1020 Ticker tock;
langfordw 0:fc382eeb78ad 1021 Timer t;
langfordw 0:fc382eeb78ad 1022
langfordw 0:fc382eeb78ad 1023 Serial pc(USBTX, USBRX); // tx, rx
langfordw 0:fc382eeb78ad 1024 LocalFileSystem local("data"); // Create the local filesystem under the name "local"
langfordw 0:fc382eeb78ad 1025
langfordw 0:fc382eeb78ad 1026 // Declare Three Encoders
langfordw 0:fc382eeb78ad 1027 QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING); // rear leg
langfordw 0:fc382eeb78ad 1028 QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING); // front leg
langfordw 0:fc382eeb78ad 1029 QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING); // spine
langfordw 0:fc382eeb78ad 1030
langfordw 0:fc382eeb78ad 1031 // Specify pinout
langfordw 0:fc382eeb78ad 1032 DigitalOut rear_motorA(p15);
langfordw 0:fc382eeb78ad 1033 DigitalOut rear_motorB(p16);
langfordw 0:fc382eeb78ad 1034 PwmOut rear_motorPWM(p24);
langfordw 0:fc382eeb78ad 1035 AnalogIn rear_cs(p20);
langfordw 0:fc382eeb78ad 1036
langfordw 0:fc382eeb78ad 1037 DigitalOut front_motorA(p7);
langfordw 0:fc382eeb78ad 1038 DigitalOut front_motorB(p8);
langfordw 0:fc382eeb78ad 1039 PwmOut front_motorPWM(p25);
langfordw 0:fc382eeb78ad 1040 AnalogIn front_cs(p19);
langfordw 0:fc382eeb78ad 1041
langfordw 0:fc382eeb78ad 1042 DigitalOut spine_motorA(p11);
langfordw 0:fc382eeb78ad 1043 DigitalOut spine_motorB(p12);
langfordw 0:fc382eeb78ad 1044 PwmOut spine_motorPWM(p26);
langfordw 0:fc382eeb78ad 1045 AnalogIn spine_cs(p18);
langfordw 0:fc382eeb78ad 1046
calisch 2:17379e2a6f7d 1047 //LEDs for current safety
calisch 2:17379e2a6f7d 1048 DigitalOut rear_led(LED1);
calisch 2:17379e2a6f7d 1049 DigitalOut front_led(LED2);
calisch 2:17379e2a6f7d 1050 DigitalOut spine_led(LED3);
calisch 2:17379e2a6f7d 1051
langfordw 0:fc382eeb78ad 1052 //number domains for abstraction
calisch 1:e549754ca234 1053 const int rear = 0;
calisch 1:e549754ca234 1054 const int front = 1;
calisch 1:e549754ca234 1055 const int spine = 2;
langfordw 0:fc382eeb78ad 1056
langfordw 0:fc382eeb78ad 1057 // Sensing Variables
langfordw 0:fc382eeb78ad 1058 volatile int i = 0;
langfordw 0:fc382eeb78ad 1059 volatile float w = 0;
langfordw 0:fc382eeb78ad 1060 volatile int id = 4000;
langfordw 0:fc382eeb78ad 1061 volatile int sign = 0;
langfordw 0:fc382eeb78ad 1062
calisch 1:e549754ca234 1063 volatile int n[3] = {0,0,0}; //encoder values
calisch 1:e549754ca234 1064 volatile int last_n[3] = {0,0,0}; //previous encoder values
calisch 1:e549754ca234 1065
calisch 1:e549754ca234 1066 volatile float avg_current[3] = {0,0,0}; //integration of current in time
langfordw 0:fc382eeb78ad 1067
langfordw 0:fc382eeb78ad 1068 // Output Variables
langfordw 0:fc382eeb78ad 1069 volatile float pwm = 0;
langfordw 0:fc382eeb78ad 1070
langfordw 0:fc382eeb78ad 1071 // Control Constants
langfordw 0:fc382eeb78ad 1072 const float R = 2.3/1000.0; // [kohm]
langfordw 0:fc382eeb78ad 1073 const float Kv = 0.1682;
langfordw 0:fc382eeb78ad 1074 const int Vs = 18; // [V]
langfordw 0:fc382eeb78ad 1075 const float n2d = 3.3333;
langfordw 0:fc382eeb78ad 1076
calisch 2:17379e2a6f7d 1077 const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
calisch 3:f68eaa68f4ec 1078 const float stall_current = 6000; //mA
calisch 3:f68eaa68f4ec 1079
calisch 3:f68eaa68f4ec 1080 const float time_out_current = 4000; //mA if avg_current is above, increment the timeout count
calisch 3:f68eaa68f4ec 1081 const int time_out_steps = 5; //max steps after which kill the test
calisch 3:f68eaa68f4ec 1082 volatile int time_out_count[3] = {0,0,0}; //counter for time out
calisch 3:f68eaa68f4ec 1083 int kill_test = 0; //kill switch
calisch 1:e549754ca234 1084
langfordw 0:fc382eeb78ad 1085 // Control Parameters
langfordw 0:fc382eeb78ad 1086 float rear_Kp = 0.001;
langfordw 0:fc382eeb78ad 1087 float rear_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 1088 float rear_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 1089
langfordw 0:fc382eeb78ad 1090 float front_Kp = 0.001;
langfordw 0:fc382eeb78ad 1091 float front_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 1092 float front_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 1093
langfordw 0:fc382eeb78ad 1094 float spine_Kp = 0.001;
langfordw 0:fc382eeb78ad 1095 float spine_Ks_p = 200.0;
langfordw 0:fc382eeb78ad 1096 float spine_Ks_d = 20.0;
langfordw 0:fc382eeb78ad 1097
langfordw 0:fc382eeb78ad 1098 float rear_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 1099 float front_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 1100 float spine_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 1101 float rear_w_d = 0;
langfordw 0:fc382eeb78ad 1102 float front_w_d = 0;
langfordw 0:fc382eeb78ad 1103 float spine_w_d = 0;
langfordw 0:fc382eeb78ad 1104
calisch 1:e549754ca234 1105
langfordw 0:fc382eeb78ad 1106 FILE *fp = fopen("/data/out.txt", "w"); // Open "out.txt" on the local file system for writing
langfordw 0:fc382eeb78ad 1107
langfordw 0:fc382eeb78ad 1108 int read_current(int which_domain) {
langfordw 0:fc382eeb78ad 1109 int current = 0;
calisch 1:e549754ca234 1110 if (which_domain == rear) { // rear
langfordw 0:fc382eeb78ad 1111 current = rear_cs.read()*23570;
calisch 1:e549754ca234 1112 } else if (which_domain == front) { // front
langfordw 0:fc382eeb78ad 1113 current = front_cs.read()*23570;
calisch 1:e549754ca234 1114 } else if (which_domain == spine){ // spine
langfordw 0:fc382eeb78ad 1115 current = spine_cs.read()*23570;
langfordw 0:fc382eeb78ad 1116 }
calisch 1:e549754ca234 1117 avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current; //integrate
langfordw 0:fc382eeb78ad 1118 return current; //mA
langfordw 0:fc382eeb78ad 1119 }
langfordw 0:fc382eeb78ad 1120
langfordw 0:fc382eeb78ad 1121 void updateMotor(int which_motor, float power) {
langfordw 0:fc382eeb78ad 1122 int dir = 0;
langfordw 0:fc382eeb78ad 1123
langfordw 0:fc382eeb78ad 1124 if (power < 0) {
langfordw 0:fc382eeb78ad 1125 power = -power;
langfordw 0:fc382eeb78ad 1126 dir = 0;
langfordw 0:fc382eeb78ad 1127 } else {
langfordw 0:fc382eeb78ad 1128 dir = 1;
langfordw 0:fc382eeb78ad 1129 }
langfordw 0:fc382eeb78ad 1130 if (power > 1) { power = 1; }
langfordw 0:fc382eeb78ad 1131 if (power < 0) { power = 0; }
langfordw 0:fc382eeb78ad 1132
calisch 2:17379e2a6f7d 1133 if (which_motor == rear) { // rear
langfordw 0:fc382eeb78ad 1134 if (dir == 1) {
langfordw 0:fc382eeb78ad 1135 rear_motorA = 0;
langfordw 0:fc382eeb78ad 1136 rear_motorB = 1;
langfordw 0:fc382eeb78ad 1137 } else {
langfordw 0:fc382eeb78ad 1138 rear_motorA = 1;
langfordw 0:fc382eeb78ad 1139 rear_motorB = 0;
langfordw 0:fc382eeb78ad 1140 }
langfordw 0:fc382eeb78ad 1141 rear_motorPWM.write(power);
calisch 2:17379e2a6f7d 1142 } else if (which_motor == front) { // front
langfordw 0:fc382eeb78ad 1143 if (dir == 1) {
langfordw 0:fc382eeb78ad 1144 front_motorA = 0;
langfordw 0:fc382eeb78ad 1145 front_motorB = 1;
langfordw 0:fc382eeb78ad 1146 } else {
langfordw 0:fc382eeb78ad 1147 front_motorA = 1;
langfordw 0:fc382eeb78ad 1148 front_motorB = 0;
langfordw 0:fc382eeb78ad 1149 }
langfordw 0:fc382eeb78ad 1150 front_motorPWM.write(power);
calisch 2:17379e2a6f7d 1151 } else if (which_motor == spine) { // spine
langfordw 0:fc382eeb78ad 1152 if (dir == 1) {
langfordw 0:fc382eeb78ad 1153 spine_motorA = 0;
langfordw 0:fc382eeb78ad 1154 spine_motorB = 1;
langfordw 0:fc382eeb78ad 1155 } else {
langfordw 0:fc382eeb78ad 1156 spine_motorA = 1;
langfordw 0:fc382eeb78ad 1157 spine_motorB = 0;
langfordw 0:fc382eeb78ad 1158 }
langfordw 0:fc382eeb78ad 1159 spine_motorPWM.write(power);
langfordw 0:fc382eeb78ad 1160 }
langfordw 0:fc382eeb78ad 1161 }
langfordw 0:fc382eeb78ad 1162
calisch 1:e549754ca234 1163 float updateEncoder(int which_encoder) {
calisch 1:e549754ca234 1164 last_n[which_encoder] = n[which_encoder];
calisch 1:e549754ca234 1165 if(which_encoder == rear){
calisch 1:e549754ca234 1166 n[which_encoder] = rear_encoder.getPulses();
calisch 1:e549754ca234 1167 }
calisch 1:e549754ca234 1168 else if(which_encoder == front){
calisch 1:e549754ca234 1169 n[which_encoder] = front_encoder.getPulses();
calisch 1:e549754ca234 1170 }
calisch 1:e549754ca234 1171 else if(which_encoder == spine){
calisch 1:e549754ca234 1172 n[which_encoder] = spine_encoder.getPulses();
calisch 1:e549754ca234 1173 }
calisch 1:e549754ca234 1174 w = (n[which_encoder]-last_n[which_encoder])*1.047; //steps/s --> rad/s
calisch 1:e549754ca234 1175 return w;
calisch 1:e549754ca234 1176 }
langfordw 0:fc382eeb78ad 1177
langfordw 0:fc382eeb78ad 1178 void control() {
calisch 3:f68eaa68f4ec 1179 if(getting_up){
calisch 3:f68eaa68f4ec 1180 rear_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
calisch 3:f68eaa68f4ec 1181 front_n_d = -50*(.001*current_sample)*n2d; //linear ramp up over 500 samples
calisch 3:f68eaa68f4ec 1182 spine_n_d = 0.0;
calisch 3:f68eaa68f4ec 1183 }
calisch 3:f68eaa68f4ec 1184 else if(getting_down){
calisch 3:f68eaa68f4ec 1185 rear_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
calisch 3:f68eaa68f4ec 1186 front_n_d = -50*(1-.001*current_sample)*n2d; //linear ramp up over 500 samples
calisch 3:f68eaa68f4ec 1187 spine_n_d = 0.0;
calisch 3:f68eaa68f4ec 1188 }
calisch 3:f68eaa68f4ec 1189 else{
calisch 3:f68eaa68f4ec 1190 rear_n_d = -trajectory[current_sample][rear]*n2d;
calisch 3:f68eaa68f4ec 1191 front_n_d = -trajectory[current_sample][front]*n2d;
calisch 3:f68eaa68f4ec 1192 spine_n_d = -trajectory[current_sample][spine]*n2d;
calisch 3:f68eaa68f4ec 1193 }
calisch 3:f68eaa68f4ec 1194
langfordw 0:fc382eeb78ad 1195 // rear
langfordw 0:fc382eeb78ad 1196 i = read_current(rear);
calisch 1:e549754ca234 1197 w = updateEncoder(rear);
calisch 1:e549754ca234 1198 id = rear_Ks_p*(rear_n_d-n[rear]) + rear_Ks_d*(rear_w_d-w);
langfordw 0:fc382eeb78ad 1199 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 1200 id = abs(id);
langfordw 0:fc382eeb78ad 1201 pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
calisch 2:17379e2a6f7d 1202 if (avg_current[rear] > stall_current){pwm = 0;rear_led=1;}
langfordw 0:fc382eeb78ad 1203 updateMotor(rear,pwm);
langfordw 0:fc382eeb78ad 1204
langfordw 0:fc382eeb78ad 1205 // front
langfordw 0:fc382eeb78ad 1206 i = read_current(front);
calisch 1:e549754ca234 1207 w = updateEncoder(front);
calisch 1:e549754ca234 1208 id = front_Ks_p*(front_n_d-n[front]) + front_Ks_d*(front_w_d-w);
langfordw 0:fc382eeb78ad 1209 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 1210 id = abs(id);
langfordw 0:fc382eeb78ad 1211 pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
calisch 2:17379e2a6f7d 1212 if (avg_current[front] > stall_current){pwm = 0;front_led=1;}
langfordw 0:fc382eeb78ad 1213 updateMotor(front,pwm);
langfordw 0:fc382eeb78ad 1214
langfordw 0:fc382eeb78ad 1215 // spine
langfordw 0:fc382eeb78ad 1216 i = read_current(spine);
calisch 1:e549754ca234 1217 w = updateEncoder(spine);
calisch 1:e549754ca234 1218 id = spine_Ks_p*(spine_n_d-n[spine]) + spine_Ks_d*(spine_w_d-w);
langfordw 0:fc382eeb78ad 1219 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 1220 id = abs(id);
langfordw 0:fc382eeb78ad 1221 pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
calisch 2:17379e2a6f7d 1222 if (avg_current[spine] > stall_current){pwm = 0;spine_led=1;}
langfordw 0:fc382eeb78ad 1223 updateMotor(spine,pwm);
calisch 1:e549754ca234 1224
calisch 3:f68eaa68f4ec 1225 //timeout for motor safety
calisch 3:f68eaa68f4ec 1226 if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
calisch 3:f68eaa68f4ec 1227 if( avg_current[front] > time_out_current){ time_out_count[front]++;}
calisch 3:f68eaa68f4ec 1228 if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
calisch 3:f68eaa68f4ec 1229 if( time_out_count[rear]>time_out_steps){ fclose(fp);kill_test = 1;}
calisch 3:f68eaa68f4ec 1230 if( time_out_count[front]>time_out_steps){ fclose(fp);kill_test = 1;}
calisch 3:f68eaa68f4ec 1231 if( time_out_count[spine]>time_out_steps){ fclose(fp);kill_test = 1;}
calisch 3:f68eaa68f4ec 1232
calisch 3:f68eaa68f4ec 1233
calisch 3:f68eaa68f4ec 1234
calisch 1:e549754ca234 1235 //step to next control point
calisch 3:f68eaa68f4ec 1236 if (getting_up){
calisch 3:f68eaa68f4ec 1237 if(current_sample == 1000){ getting_up = 0; current_sample = 0;} //we're up
calisch 3:f68eaa68f4ec 1238 else{ current_sample++;} //still getting up
calisch 3:f68eaa68f4ec 1239 }
calisch 3:f68eaa68f4ec 1240 else if(getting_down){
calisch 3:f68eaa68f4ec 1241 if(current_sample == 1000){ //we're down
calisch 2:17379e2a6f7d 1242 tick.detach();
calisch 2:17379e2a6f7d 1243 tock.detach();
calisch 1:e549754ca234 1244 fclose(fp);
calisch 1:e549754ca234 1245 pwm = 0;
calisch 1:e549754ca234 1246 updateMotor(rear,pwm);
calisch 1:e549754ca234 1247 updateMotor(front,pwm);
calisch 2:17379e2a6f7d 1248 updateMotor(spine,pwm);
calisch 1:e549754ca234 1249 }
calisch 3:f68eaa68f4ec 1250 else{current_sample++;} //still getting down
calisch 3:f68eaa68f4ec 1251 }
calisch 3:f68eaa68f4ec 1252 else if (current_sample == n_samples){ //normal operation
calisch 3:f68eaa68f4ec 1253 if (current_loop == n_loops){ getting_down = 1; current_sample=0;} //ready to sit
calisch 1:e549754ca234 1254 else{ //end of loop, ready for next
calisch 1:e549754ca234 1255 current_sample = 0;
calisch 1:e549754ca234 1256 current_loop++;
calisch 1:e549754ca234 1257 }
calisch 1:e549754ca234 1258 }
calisch 3:f68eaa68f4ec 1259 else{ current_sample++;} //middle of running loop
calisch 1:e549754ca234 1260 }
langfordw 0:fc382eeb78ad 1261
langfordw 0:fc382eeb78ad 1262 void save() {
calisch 2:17379e2a6f7d 1263 fprintf(fp, "%i %i %i %i %f %f %f\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2]);
langfordw 0:fc382eeb78ad 1264 }
langfordw 0:fc382eeb78ad 1265
langfordw 0:fc382eeb78ad 1266 int main() {
langfordw 0:fc382eeb78ad 1267 rear_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 1268 front_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 1269 spine_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 1270 tick.attach(&control,CONTROL_PERIOD);
langfordw 0:fc382eeb78ad 1271 tock.attach(&save,SAVE_PERIOD);
calisch 1:e549754ca234 1272 t.start();
langfordw 0:fc382eeb78ad 1273
calisch 3:f68eaa68f4ec 1274 while(~kill_test) {
langfordw 0:fc382eeb78ad 1275 //DEBUG
langfordw 0:fc382eeb78ad 1276 // pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
langfordw 0:fc382eeb78ad 1277 }
langfordw 0:fc382eeb78ad 1278 }