Sam Calisch / bounding

embedded code for bounding robot

Dependencies:   QEI mbed

Fork of bounding by Sam Calisch

main.cpp@2:17379e2a6f7d, 2013-11-23 (annotated)

Committer:
calisch
Date:
Sat Nov 23 22:30:04 2013 +0000
Revision:
2:17379e2a6f7d
Parent:
1:e549754ca234
Child:
3:f68eaa68f4ec
detach tickers, clean up, testing

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