Sam Calisch / bounding

embedded code for bounding robot

Dependencies:   QEI mbed

Fork of bounding by Sam Calisch

main.cpp@1:e549754ca234, 2013-11-23 (annotated)

Committer:
calisch
Date:
Sat Nov 23 21:16:12 2013 +0000
Revision:
1:e549754ca234
Parent:
0:fc382eeb78ad
Child:
2:17379e2a6f7d
finished trajectory from array, current integral limiting

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 1:e549754ca234 8 // 500 x 3 array of degree values
calisch 1:e549754ca234 9 const float trajectory[500][3] = {
calisch 1:e549754ca234 10 1,1,0,
calisch 1:e549754ca234 11 1,1,0,
calisch 1:e549754ca234 12 1,1,0,
calisch 1:e549754ca234 13 1,1,0,
calisch 1:e549754ca234 14 1,1,0,
calisch 1:e549754ca234 15 1,1,0,
calisch 1:e549754ca234 16 2,2,0,
calisch 1:e549754ca234 17 2,2,0,
calisch 1:e549754ca234 18 3,3,0,
calisch 1:e549754ca234 19 3,3,0,
calisch 1:e549754ca234 20 3,3,0,
calisch 1:e549754ca234 21 4,4,0,
calisch 1:e549754ca234 22 4,4,0,
calisch 1:e549754ca234 23 5,5,0,
calisch 1:e549754ca234 24 5,5,0,
calisch 1:e549754ca234 25 6,6,0,
calisch 1:e549754ca234 26 6,6,0,
calisch 1:e549754ca234 27 7,7,0,
calisch 1:e549754ca234 28 7,7,0,
calisch 1:e549754ca234 29 7,7,0,
calisch 1:e549754ca234 30 8,8,0,
calisch 1:e549754ca234 31 8,8,0,
calisch 1:e549754ca234 32 9,9,0,
calisch 1:e549754ca234 33 9,9,0,
calisch 1:e549754ca234 34 10,10,0,
calisch 1:e549754ca234 35 10,10,0,
calisch 1:e549754ca234 36 11,11,0,
calisch 1:e549754ca234 37 11,11,0,
calisch 1:e549754ca234 38 11,11,0,
calisch 1:e549754ca234 39 12,12,0,
calisch 1:e549754ca234 40 12,12,0,
calisch 1:e549754ca234 41 13,13,0,
calisch 1:e549754ca234 42 13,13,0,
calisch 1:e549754ca234 43 14,14,0,
calisch 1:e549754ca234 44 14,14,0,
calisch 1:e549754ca234 45 14,14,0,
calisch 1:e549754ca234 46 15,15,0,
calisch 1:e549754ca234 47 15,15,0,
calisch 1:e549754ca234 48 16,16,0,
calisch 1:e549754ca234 49 16,16,0,
calisch 1:e549754ca234 50 17,17,0,
calisch 1:e549754ca234 51 17,17,0,
calisch 1:e549754ca234 52 18,18,0,
calisch 1:e549754ca234 53 18,18,0,
calisch 1:e549754ca234 54 18,18,0,
calisch 1:e549754ca234 55 19,19,0,
calisch 1:e549754ca234 56 19,19,0,
calisch 1:e549754ca234 57 20,20,0,
calisch 1:e549754ca234 58 20,20,0,
calisch 1:e549754ca234 59 21,21,0,
calisch 1:e549754ca234 60 21,21,0,
calisch 1:e549754ca234 61 22,22,0,
calisch 1:e549754ca234 62 22,22,0,
calisch 1:e549754ca234 63 22,22,0,
calisch 1:e549754ca234 64 23,23,0,
calisch 1:e549754ca234 65 23,23,0,
calisch 1:e549754ca234 66 24,24,0,
calisch 1:e549754ca234 67 24,24,0,
calisch 1:e549754ca234 68 25,25,0,
calisch 1:e549754ca234 69 25,25,0,
calisch 1:e549754ca234 70 25,25,0,
calisch 1:e549754ca234 71 26,26,0,
calisch 1:e549754ca234 72 26,26,0,
calisch 1:e549754ca234 73 27,27,0,
calisch 1:e549754ca234 74 27,27,0,
calisch 1:e549754ca234 75 28,28,0,
calisch 1:e549754ca234 76 28,28,0,
calisch 1:e549754ca234 77 29,29,0,
calisch 1:e549754ca234 78 29,29,0,
calisch 1:e549754ca234 79 29,29,0,
calisch 1:e549754ca234 80 30,30,0,
calisch 1:e549754ca234 81 30,30,0,
calisch 1:e549754ca234 82 31,31,0,
calisch 1:e549754ca234 83 31,31,0,
calisch 1:e549754ca234 84 32,32,0,
calisch 1:e549754ca234 85 32,32,0,
calisch 1:e549754ca234 86 33,33,0,
calisch 1:e549754ca234 87 33,33,0,
calisch 1:e549754ca234 88 33,33,0,
calisch 1:e549754ca234 89 34,34,0,
calisch 1:e549754ca234 90 34,34,0,
calisch 1:e549754ca234 91 35,35,0,
calisch 1:e549754ca234 92 35,35,0,
calisch 1:e549754ca234 93 36,36,0,
calisch 1:e549754ca234 94 36,36,0,
calisch 1:e549754ca234 95 36,36,0,
calisch 1:e549754ca234 96 37,37,0,
calisch 1:e549754ca234 97 37,37,0,
calisch 1:e549754ca234 98 38,38,0,
calisch 1:e549754ca234 99 38,38,0,
calisch 1:e549754ca234 100 39,39,0,
calisch 1:e549754ca234 101 39,39,0,
calisch 1:e549754ca234 102 40,40,0,
calisch 1:e549754ca234 103 40,40,0,
calisch 1:e549754ca234 104 40,40,0,
calisch 1:e549754ca234 105 41,41,0,
calisch 1:e549754ca234 106 41,41,0,
calisch 1:e549754ca234 107 42,42,0,
calisch 1:e549754ca234 108 42,42,0,
calisch 1:e549754ca234 109 43,43,0,
calisch 1:e549754ca234 110 43,43,0,
calisch 1:e549754ca234 111 44,44,0,
calisch 1:e549754ca234 112 44,44,0,
calisch 1:e549754ca234 113 44,44,0,
calisch 1:e549754ca234 114 45,45,0,
calisch 1:e549754ca234 115 45,45,0,
calisch 1:e549754ca234 116 46,46,0,
calisch 1:e549754ca234 117 46,46,0,
calisch 1:e549754ca234 118 47,47,0,
calisch 1:e549754ca234 119 47,47,0,
calisch 1:e549754ca234 120 47,47,0,
calisch 1:e549754ca234 121 48,48,0,
calisch 1:e549754ca234 122 48,48,0,
calisch 1:e549754ca234 123 49,49,0,
calisch 1:e549754ca234 124 49,49,0,
calisch 1:e549754ca234 125 50,50,0,
calisch 1:e549754ca234 126 50,50,0,
calisch 1:e549754ca234 127 51,51,0,
calisch 1:e549754ca234 128 51,51,0,
calisch 1:e549754ca234 129 51,51,0,
calisch 1:e549754ca234 130 52,52,0,
calisch 1:e549754ca234 131 52,52,0,
calisch 1:e549754ca234 132 53,53,0,
calisch 1:e549754ca234 133 53,53,0,
calisch 1:e549754ca234 134 53,53,0,
calisch 1:e549754ca234 135 54,54,0,
calisch 1:e549754ca234 136 54,54,0,
calisch 1:e549754ca234 137 54,54,0,
calisch 1:e549754ca234 138 54,54,0,
calisch 1:e549754ca234 139 54,54,0,
calisch 1:e549754ca234 140 54,54,0,
calisch 1:e549754ca234 141 54,54,0,
calisch 1:e549754ca234 142 54,54,0,
calisch 1:e549754ca234 143 54,54,0,
calisch 1:e549754ca234 144 54,54,0,
calisch 1:e549754ca234 145 55,55,0,
calisch 1:e549754ca234 146 55,55,0,
calisch 1:e549754ca234 147 55,55,0,
calisch 1:e549754ca234 148 55,55,0,
calisch 1:e549754ca234 149 55,55,0,
calisch 1:e549754ca234 150 55,55,0,
calisch 1:e549754ca234 151 55,55,0,
calisch 1:e549754ca234 152 55,55,0,
calisch 1:e549754ca234 153 55,55,0,
calisch 1:e549754ca234 154 55,55,0,
calisch 1:e549754ca234 155 55,55,0,
calisch 1:e549754ca234 156 55,55,0,
calisch 1:e549754ca234 157 55,55,0,
calisch 1:e549754ca234 158 55,55,0,
calisch 1:e549754ca234 159 55,55,0,
calisch 1:e549754ca234 160 55,55,0,
calisch 1:e549754ca234 161 55,55,0,
calisch 1:e549754ca234 162 55,55,0,
calisch 1:e549754ca234 163 55,55,0,
calisch 1:e549754ca234 164 55,55,0,
calisch 1:e549754ca234 165 55,55,0,
calisch 1:e549754ca234 166 55,55,0,
calisch 1:e549754ca234 167 55,55,0,
calisch 1:e549754ca234 168 55,55,0,
calisch 1:e549754ca234 169 55,55,0,
calisch 1:e549754ca234 170 55,55,0,
calisch 1:e549754ca234 171 55,55,0,
calisch 1:e549754ca234 172 55,55,0,
calisch 1:e549754ca234 173 55,55,0,
calisch 1:e549754ca234 174 55,55,0,
calisch 1:e549754ca234 175 55,55,0,
calisch 1:e549754ca234 176 55,55,0,
calisch 1:e549754ca234 177 55,55,0,
calisch 1:e549754ca234 178 55,55,0,
calisch 1:e549754ca234 179 55,55,0,
calisch 1:e549754ca234 180 55,55,0,
calisch 1:e549754ca234 181 55,55,0,
calisch 1:e549754ca234 182 55,55,0,
calisch 1:e549754ca234 183 55,55,0,
calisch 1:e549754ca234 184 55,55,0,
calisch 1:e549754ca234 185 55,55,0,
calisch 1:e549754ca234 186 55,55,0,
calisch 1:e549754ca234 187 55,55,0,
calisch 1:e549754ca234 188 55,55,0,
calisch 1:e549754ca234 189 55,55,0,
calisch 1:e549754ca234 190 55,55,0,
calisch 1:e549754ca234 191 55,55,0,
calisch 1:e549754ca234 192 55,55,0,
calisch 1:e549754ca234 193 55,55,0,
calisch 1:e549754ca234 194 55,55,0,
calisch 1:e549754ca234 195 55,55,0,
calisch 1:e549754ca234 196 55,55,0,
calisch 1:e549754ca234 197 55,55,0,
calisch 1:e549754ca234 198 55,55,0,
calisch 1:e549754ca234 199 55,55,0,
calisch 1:e549754ca234 200 55,55,0,
calisch 1:e549754ca234 201 55,55,0,
calisch 1:e549754ca234 202 55,55,0,
calisch 1:e549754ca234 203 55,55,0,
calisch 1:e549754ca234 204 55,55,0,
calisch 1:e549754ca234 205 55,55,0,
calisch 1:e549754ca234 206 55,55,0,
calisch 1:e549754ca234 207 55,55,0,
calisch 1:e549754ca234 208 55,55,0,
calisch 1:e549754ca234 209 55,55,0,
calisch 1:e549754ca234 210 55,55,0,
calisch 1:e549754ca234 211 55,55,0,
calisch 1:e549754ca234 212 55,55,0,
calisch 1:e549754ca234 213 55,55,0,
calisch 1:e549754ca234 214 55,55,0,
calisch 1:e549754ca234 215 55,55,0,
calisch 1:e549754ca234 216 55,55,0,
calisch 1:e549754ca234 217 55,55,0,
calisch 1:e549754ca234 218 55,55,0,
calisch 1:e549754ca234 219 55,55,0,
calisch 1:e549754ca234 220 55,55,0,
calisch 1:e549754ca234 221 55,55,0,
calisch 1:e549754ca234 222 55,55,0,
calisch 1:e549754ca234 223 55,55,0,
calisch 1:e549754ca234 224 55,55,0,
calisch 1:e549754ca234 225 55,55,0,
calisch 1:e549754ca234 226 55,55,0,
calisch 1:e549754ca234 227 55,55,0,
calisch 1:e549754ca234 228 55,55,0,
calisch 1:e549754ca234 229 55,55,0,
calisch 1:e549754ca234 230 55,55,0,
calisch 1:e549754ca234 231 55,55,0,
calisch 1:e549754ca234 232 55,55,0,
calisch 1:e549754ca234 233 55,55,0,
calisch 1:e549754ca234 234 55,55,0,
calisch 1:e549754ca234 235 55,55,0,
calisch 1:e549754ca234 236 55,55,0,
calisch 1:e549754ca234 237 55,55,0,
calisch 1:e549754ca234 238 55,55,0,
calisch 1:e549754ca234 239 55,55,0,
calisch 1:e549754ca234 240 55,55,0,
calisch 1:e549754ca234 241 55,55,0,
calisch 1:e549754ca234 242 55,55,0,
calisch 1:e549754ca234 243 55,55,0,
calisch 1:e549754ca234 244 55,55,0,
calisch 1:e549754ca234 245 55,55,0,
calisch 1:e549754ca234 246 55,55,0,
calisch 1:e549754ca234 247 55,55,0,
calisch 1:e549754ca234 248 55,55,0,
calisch 1:e549754ca234 249 55,55,0,
calisch 1:e549754ca234 250 55,55,0,
calisch 1:e549754ca234 251 55,55,0,
calisch 1:e549754ca234 252 55,55,0,
calisch 1:e549754ca234 253 55,55,0,
calisch 1:e549754ca234 254 55,55,0,
calisch 1:e549754ca234 255 55,55,0,
calisch 1:e549754ca234 256 55,55,0,
calisch 1:e549754ca234 257 55,55,0,
calisch 1:e549754ca234 258 55,55,0,
calisch 1:e549754ca234 259 55,55,0,
calisch 1:e549754ca234 260 55,55,0,
calisch 1:e549754ca234 261 55,55,0,
calisch 1:e549754ca234 262 55,55,0,
calisch 1:e549754ca234 263 55,55,0,
calisch 1:e549754ca234 264 55,55,0,
calisch 1:e549754ca234 265 55,55,0,
calisch 1:e549754ca234 266 55,55,0,
calisch 1:e549754ca234 267 55,55,0,
calisch 1:e549754ca234 268 55,55,0,
calisch 1:e549754ca234 269 55,55,0,
calisch 1:e549754ca234 270 55,55,0,
calisch 1:e549754ca234 271 55,55,0,
calisch 1:e549754ca234 272 55,55,0,
calisch 1:e549754ca234 273 55,55,0,
calisch 1:e549754ca234 274 55,55,0,
calisch 1:e549754ca234 275 55,55,0,
calisch 1:e549754ca234 276 55,55,0,
calisch 1:e549754ca234 277 55,55,0,
calisch 1:e549754ca234 278 55,55,0,
calisch 1:e549754ca234 279 55,55,0,
calisch 1:e549754ca234 280 55,55,0,
calisch 1:e549754ca234 281 55,55,0,
calisch 1:e549754ca234 282 55,55,0,
calisch 1:e549754ca234 283 55,55,0,
calisch 1:e549754ca234 284 55,55,0,
calisch 1:e549754ca234 285 55,55,0,
calisch 1:e549754ca234 286 55,55,0,
calisch 1:e549754ca234 287 55,55,0,
calisch 1:e549754ca234 288 55,55,0,
calisch 1:e549754ca234 289 55,55,0,
calisch 1:e549754ca234 290 55,55,0,
calisch 1:e549754ca234 291 55,55,0,
calisch 1:e549754ca234 292 55,55,0,
calisch 1:e549754ca234 293 55,55,0,
calisch 1:e549754ca234 294 55,55,0,
calisch 1:e549754ca234 295 55,55,0,
calisch 1:e549754ca234 296 55,55,0,
calisch 1:e549754ca234 297 55,55,0,
calisch 1:e549754ca234 298 55,55,0,
calisch 1:e549754ca234 299 55,55,0,
calisch 1:e549754ca234 300 55,55,0,
calisch 1:e549754ca234 301 55,55,0,
calisch 1:e549754ca234 302 55,55,0,
calisch 1:e549754ca234 303 55,55,0,
calisch 1:e549754ca234 304 55,55,0,
calisch 1:e549754ca234 305 55,55,0,
calisch 1:e549754ca234 306 55,55,0,
calisch 1:e549754ca234 307 55,55,0,
calisch 1:e549754ca234 308 55,55,0,
calisch 1:e549754ca234 309 55,55,0,
calisch 1:e549754ca234 310 55,55,0,
calisch 1:e549754ca234 311 55,55,0,
calisch 1:e549754ca234 312 55,55,0,
calisch 1:e549754ca234 313 55,55,0,
calisch 1:e549754ca234 314 55,55,0,
calisch 1:e549754ca234 315 55,55,0,
calisch 1:e549754ca234 316 55,55,0,
calisch 1:e549754ca234 317 55,55,0,
calisch 1:e549754ca234 318 55,55,0,
calisch 1:e549754ca234 319 55,55,0,
calisch 1:e549754ca234 320 55,55,0,
calisch 1:e549754ca234 321 55,55,0,
calisch 1:e549754ca234 322 55,55,0,
calisch 1:e549754ca234 323 55,55,0,
calisch 1:e549754ca234 324 55,55,0,
calisch 1:e549754ca234 325 55,55,0,
calisch 1:e549754ca234 326 55,55,0,
calisch 1:e549754ca234 327 55,55,0,
calisch 1:e549754ca234 328 55,55,0,
calisch 1:e549754ca234 329 55,55,0,
calisch 1:e549754ca234 330 55,55,0,
calisch 1:e549754ca234 331 55,55,0,
calisch 1:e549754ca234 332 55,55,0,
calisch 1:e549754ca234 333 55,55,0,
calisch 1:e549754ca234 334 55,55,0,
calisch 1:e549754ca234 335 55,55,0,
calisch 1:e549754ca234 336 55,55,0,
calisch 1:e549754ca234 337 55,55,0,
calisch 1:e549754ca234 338 55,55,0,
calisch 1:e549754ca234 339 55,55,0,
calisch 1:e549754ca234 340 55,55,0,
calisch 1:e549754ca234 341 55,55,0,
calisch 1:e549754ca234 342 55,55,0,
calisch 1:e549754ca234 343 55,55,0,
calisch 1:e549754ca234 344 55,55,0,
calisch 1:e549754ca234 345 55,55,0,
calisch 1:e549754ca234 346 55,55,0,
calisch 1:e549754ca234 347 55,55,0,
calisch 1:e549754ca234 348 55,55,0,
calisch 1:e549754ca234 349 55,55,0,
calisch 1:e549754ca234 350 55,55,0,
calisch 1:e549754ca234 351 55,55,0,
calisch 1:e549754ca234 352 55,55,0,
calisch 1:e549754ca234 353 55,55,0,
calisch 1:e549754ca234 354 55,55,0,
calisch 1:e549754ca234 355 55,55,0,
calisch 1:e549754ca234 356 55,55,0,
calisch 1:e549754ca234 357 55,55,0,
calisch 1:e549754ca234 358 55,55,0,
calisch 1:e549754ca234 359 55,55,0,
calisch 1:e549754ca234 360 55,55,0,
calisch 1:e549754ca234 361 55,55,0,
calisch 1:e549754ca234 362 55,55,0,
calisch 1:e549754ca234 363 55,55,0,
calisch 1:e549754ca234 364 55,55,0,
calisch 1:e549754ca234 365 55,55,0,
calisch 1:e549754ca234 366 55,55,0,
calisch 1:e549754ca234 367 55,55,0,
calisch 1:e549754ca234 368 55,55,0,
calisch 1:e549754ca234 369 55,55,0,
calisch 1:e549754ca234 370 55,55,0,
calisch 1:e549754ca234 371 55,55,0,
calisch 1:e549754ca234 372 55,55,0,
calisch 1:e549754ca234 373 55,55,0,
calisch 1:e549754ca234 374 55,55,0,
calisch 1:e549754ca234 375 55,55,0,
calisch 1:e549754ca234 376 55,55,0,
calisch 1:e549754ca234 377 55,55,0,
calisch 1:e549754ca234 378 54,54,0,
calisch 1:e549754ca234 379 54,54,0,
calisch 1:e549754ca234 380 54,54,0,
calisch 1:e549754ca234 381 54,54,0,
calisch 1:e549754ca234 382 54,54,0,
calisch 1:e549754ca234 383 54,54,0,
calisch 1:e549754ca234 384 54,54,0,
calisch 1:e549754ca234 385 54,54,0,
calisch 1:e549754ca234 386 54,54,0,
calisch 1:e549754ca234 387 54,54,0,
calisch 1:e549754ca234 388 53,53,0,
calisch 1:e549754ca234 389 53,53,0,
calisch 1:e549754ca234 390 53,53,0,
calisch 1:e549754ca234 391 52,52,0,
calisch 1:e549754ca234 392 52,52,0,
calisch 1:e549754ca234 393 51,51,0,
calisch 1:e549754ca234 394 51,51,0,
calisch 1:e549754ca234 395 51,51,0,
calisch 1:e549754ca234 396 50,50,0,
calisch 1:e549754ca234 397 50,50,0,
calisch 1:e549754ca234 398 49,49,0,
calisch 1:e549754ca234 399 49,49,0,
calisch 1:e549754ca234 400 48,48,0,
calisch 1:e549754ca234 401 48,48,0,
calisch 1:e549754ca234 402 47,47,0,
calisch 1:e549754ca234 403 47,47,0,
calisch 1:e549754ca234 404 47,47,0,
calisch 1:e549754ca234 405 46,46,0,
calisch 1:e549754ca234 406 46,46,0,
calisch 1:e549754ca234 407 45,45,0,
calisch 1:e549754ca234 408 45,45,0,
calisch 1:e549754ca234 409 44,44,0,
calisch 1:e549754ca234 410 44,44,0,
calisch 1:e549754ca234 411 43,43,0,
calisch 1:e549754ca234 412 43,43,0,
calisch 1:e549754ca234 413 43,43,0,
calisch 1:e549754ca234 414 42,42,0,
calisch 1:e549754ca234 415 42,42,0,
calisch 1:e549754ca234 416 41,41,0,
calisch 1:e549754ca234 417 41,41,0,
calisch 1:e549754ca234 418 40,40,0,
calisch 1:e549754ca234 419 40,40,0,
calisch 1:e549754ca234 420 40,40,0,
calisch 1:e549754ca234 421 39,39,0,
calisch 1:e549754ca234 422 39,39,0,
calisch 1:e549754ca234 423 38,38,0,
calisch 1:e549754ca234 424 38,38,0,
calisch 1:e549754ca234 425 37,37,0,
calisch 1:e549754ca234 426 37,37,0,
calisch 1:e549754ca234 427 36,36,0,
calisch 1:e549754ca234 428 36,36,0,
calisch 1:e549754ca234 429 36,36,0,
calisch 1:e549754ca234 430 35,35,0,
calisch 1:e549754ca234 431 35,35,0,
calisch 1:e549754ca234 432 34,34,0,
calisch 1:e549754ca234 433 34,34,0,
calisch 1:e549754ca234 434 33,33,0,
calisch 1:e549754ca234 435 33,33,0,
calisch 1:e549754ca234 436 33,33,0,
calisch 1:e549754ca234 437 32,32,0,
calisch 1:e549754ca234 438 32,32,0,
calisch 1:e549754ca234 439 31,31,0,
calisch 1:e549754ca234 440 31,31,0,
calisch 1:e549754ca234 441 30,30,0,
calisch 1:e549754ca234 442 30,30,0,
calisch 1:e549754ca234 443 29,29,0,
calisch 1:e549754ca234 444 29,29,0,
calisch 1:e549754ca234 445 29,29,0,
calisch 1:e549754ca234 446 28,28,0,
calisch 1:e549754ca234 447 28,28,0,
calisch 1:e549754ca234 448 27,27,0,
calisch 1:e549754ca234 449 27,27,0,
calisch 1:e549754ca234 450 26,26,0,
calisch 1:e549754ca234 451 26,26,0,
calisch 1:e549754ca234 452 25,25,0,
calisch 1:e549754ca234 453 25,25,0,
calisch 1:e549754ca234 454 25,25,0,
calisch 1:e549754ca234 455 24,24,0,
calisch 1:e549754ca234 456 24,24,0,
calisch 1:e549754ca234 457 23,23,0,
calisch 1:e549754ca234 458 23,23,0,
calisch 1:e549754ca234 459 22,22,0,
calisch 1:e549754ca234 460 22,22,0,
calisch 1:e549754ca234 461 21,21,0,
calisch 1:e549754ca234 462 21,21,0,
calisch 1:e549754ca234 463 21,21,0,
calisch 1:e549754ca234 464 20,20,0,
calisch 1:e549754ca234 465 20,20,0,
calisch 1:e549754ca234 466 19,19,0,
calisch 1:e549754ca234 467 19,19,0,
calisch 1:e549754ca234 468 18,18,0,
calisch 1:e549754ca234 469 18,18,0,
calisch 1:e549754ca234 470 18,18,0,
calisch 1:e549754ca234 471 17,17,0,
calisch 1:e549754ca234 472 17,17,0,
calisch 1:e549754ca234 473 16,16,0,
calisch 1:e549754ca234 474 16,16,0,
calisch 1:e549754ca234 475 15,15,0,
calisch 1:e549754ca234 476 15,15,0,
calisch 1:e549754ca234 477 14,14,0,
calisch 1:e549754ca234 478 14,14,0,
calisch 1:e549754ca234 479 14,14,0,
calisch 1:e549754ca234 480 13,13,0,
calisch 1:e549754ca234 481 13,13,0,
calisch 1:e549754ca234 482 12,12,0,
calisch 1:e549754ca234 483 12,12,0,
calisch 1:e549754ca234 484 11,11,0,
calisch 1:e549754ca234 485 11,11,0,
calisch 1:e549754ca234 486 10,10,0,
calisch 1:e549754ca234 487 10,10,0,
calisch 1:e549754ca234 488 10,10,0,
calisch 1:e549754ca234 489 9,9,0,
calisch 1:e549754ca234 490 9,9,0,
calisch 1:e549754ca234 491 8,8,0,
calisch 1:e549754ca234 492 8,8,0,
calisch 1:e549754ca234 493 7,7,0,
calisch 1:e549754ca234 494 7,7,0,
calisch 1:e549754ca234 495 7,7,0,
calisch 1:e549754ca234 496 6,6,0,
calisch 1:e549754ca234 497 6,6,0,
calisch 1:e549754ca234 498 5,5,0,
calisch 1:e549754ca234 499 5,5,0,
calisch 1:e549754ca234 500 4,4,0,
calisch 1:e549754ca234 501 4,4,0,
calisch 1:e549754ca234 502 3,3,0,
calisch 1:e549754ca234 503 3,3,0,
calisch 1:e549754ca234 504 3,3,0,
calisch 1:e549754ca234 505 2,2,0,
calisch 1:e549754ca234 506 2,2,0,
calisch 1:e549754ca234 507 1,1,0,
calisch 1:e549754ca234 508 1,1,0,
calisch 1:e549754ca234 509 1,1,0
calisch 1:e549754ca234 510 };
calisch 1:e549754ca234 511
langfordw 0:fc382eeb78ad 512 Ticker tick;
langfordw 0:fc382eeb78ad 513 Ticker tock;
langfordw 0:fc382eeb78ad 514 Timer t;
langfordw 0:fc382eeb78ad 515
langfordw 0:fc382eeb78ad 516 Serial pc(USBTX, USBRX); // tx, rx
langfordw 0:fc382eeb78ad 517 LocalFileSystem local("data"); // Create the local filesystem under the name "local"
langfordw 0:fc382eeb78ad 518
langfordw 0:fc382eeb78ad 519 // Declare Three Encoders
langfordw 0:fc382eeb78ad 520 QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING); // rear leg
langfordw 0:fc382eeb78ad 521 QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING); // front leg
langfordw 0:fc382eeb78ad 522 QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING); // spine
langfordw 0:fc382eeb78ad 523
langfordw 0:fc382eeb78ad 524 // Specify pinout
langfordw 0:fc382eeb78ad 525 DigitalOut rear_motorA(p15);
langfordw 0:fc382eeb78ad 526 DigitalOut rear_motorB(p16);
langfordw 0:fc382eeb78ad 527 PwmOut rear_motorPWM(p24);
langfordw 0:fc382eeb78ad 528 AnalogIn rear_cs(p20);
langfordw 0:fc382eeb78ad 529
langfordw 0:fc382eeb78ad 530 DigitalOut front_motorA(p7);
langfordw 0:fc382eeb78ad 531 DigitalOut front_motorB(p8);
langfordw 0:fc382eeb78ad 532 PwmOut front_motorPWM(p25);
langfordw 0:fc382eeb78ad 533 AnalogIn front_cs(p19);
langfordw 0:fc382eeb78ad 534
langfordw 0:fc382eeb78ad 535 DigitalOut spine_motorA(p11);
langfordw 0:fc382eeb78ad 536 DigitalOut spine_motorB(p12);
langfordw 0:fc382eeb78ad 537 PwmOut spine_motorPWM(p26);
langfordw 0:fc382eeb78ad 538 AnalogIn spine_cs(p18);
langfordw 0:fc382eeb78ad 539
langfordw 0:fc382eeb78ad 540 //number domains for abstraction
calisch 1:e549754ca234 541 const int rear = 0;
calisch 1:e549754ca234 542 const int front = 1;
calisch 1:e549754ca234 543 const int spine = 2;
langfordw 0:fc382eeb78ad 544
langfordw 0:fc382eeb78ad 545 // Sensing Variables
langfordw 0:fc382eeb78ad 546 volatile int i = 0;
langfordw 0:fc382eeb78ad 547 volatile float w = 0;
langfordw 0:fc382eeb78ad 548 volatile int id = 4000;
langfordw 0:fc382eeb78ad 549 volatile int sign = 0;
langfordw 0:fc382eeb78ad 550
calisch 1:e549754ca234 551 volatile int n[3] = {0,0,0}; //encoder values
calisch 1:e549754ca234 552 volatile int last_n[3] = {0,0,0}; //previous encoder values
calisch 1:e549754ca234 553
calisch 1:e549754ca234 554 volatile float avg_current[3] = {0,0,0}; //integration of current in time
langfordw 0:fc382eeb78ad 555
langfordw 0:fc382eeb78ad 556 // Output Variables
langfordw 0:fc382eeb78ad 557 volatile float pwm = 0;
langfordw 0:fc382eeb78ad 558
langfordw 0:fc382eeb78ad 559 // Control Constants
langfordw 0:fc382eeb78ad 560 const float R = 2.3/1000.0; // [kohm]
langfordw 0:fc382eeb78ad 561 const float Kv = 0.1682;
langfordw 0:fc382eeb78ad 562 const int Vs = 18; // [V]
langfordw 0:fc382eeb78ad 563 const float n2d = 3.3333;
langfordw 0:fc382eeb78ad 564
calisch 1:e549754ca234 565 const float integ_alpha = .5; //peristence of current integration. 0->all past, 1->all present
calisch 1:e549754ca234 566 const float stall_current = 5; //Amps
calisch 1:e549754ca234 567
langfordw 0:fc382eeb78ad 568 // Control Parameters
langfordw 0:fc382eeb78ad 569 float rear_Kp = 0.001;
langfordw 0:fc382eeb78ad 570 float rear_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 571 float rear_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 572
langfordw 0:fc382eeb78ad 573 float front_Kp = 0.001;
langfordw 0:fc382eeb78ad 574 float front_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 575 float front_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 576
langfordw 0:fc382eeb78ad 577 float spine_Kp = 0.001;
langfordw 0:fc382eeb78ad 578 float spine_Ks_p = 200.0;
langfordw 0:fc382eeb78ad 579 float spine_Ks_d = 20.0;
langfordw 0:fc382eeb78ad 580
langfordw 0:fc382eeb78ad 581 float rear_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 582 float front_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 583 float spine_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 584 float rear_w_d = 0;
langfordw 0:fc382eeb78ad 585 float front_w_d = 0;
langfordw 0:fc382eeb78ad 586 float spine_w_d = 0;
langfordw 0:fc382eeb78ad 587
calisch 1:e549754ca234 588 //control flow
calisch 1:e549754ca234 589 volatile int current_sample = 0;
calisch 1:e549754ca234 590 volatile int current_loop = 0;
calisch 1:e549754ca234 591 const int n_samples = 500;
calisch 1:e549754ca234 592 const int n_loops = 1;
calisch 1:e549754ca234 593
langfordw 0:fc382eeb78ad 594 FILE *fp = fopen("/data/out.txt", "w"); // Open "out.txt" on the local file system for writing
langfordw 0:fc382eeb78ad 595
langfordw 0:fc382eeb78ad 596 int read_current(int which_domain) {
langfordw 0:fc382eeb78ad 597 int current = 0;
calisch 1:e549754ca234 598 if (which_domain == rear) { // rear
langfordw 0:fc382eeb78ad 599 current = rear_cs.read()*23570;
calisch 1:e549754ca234 600 } else if (which_domain == front) { // front
langfordw 0:fc382eeb78ad 601 current = front_cs.read()*23570;
calisch 1:e549754ca234 602 } else if (which_domain == spine){ // spine
langfordw 0:fc382eeb78ad 603 current = spine_cs.read()*23570;
langfordw 0:fc382eeb78ad 604 }
calisch 1:e549754ca234 605 avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current; //integrate
langfordw 0:fc382eeb78ad 606 return current; //mA
langfordw 0:fc382eeb78ad 607 }
langfordw 0:fc382eeb78ad 608
langfordw 0:fc382eeb78ad 609 void updateMotor(int which_motor, float power) {
langfordw 0:fc382eeb78ad 610 int dir = 0;
langfordw 0:fc382eeb78ad 611
langfordw 0:fc382eeb78ad 612 if (power < 0) {
langfordw 0:fc382eeb78ad 613 power = -power;
langfordw 0:fc382eeb78ad 614 dir = 0;
langfordw 0:fc382eeb78ad 615 } else {
langfordw 0:fc382eeb78ad 616 dir = 1;
langfordw 0:fc382eeb78ad 617 }
langfordw 0:fc382eeb78ad 618 if (power > 1) { power = 1; }
langfordw 0:fc382eeb78ad 619 if (power < 0) { power = 0; }
langfordw 0:fc382eeb78ad 620
langfordw 0:fc382eeb78ad 621 if (which_motor == 1) { // rear
langfordw 0:fc382eeb78ad 622 if (dir == 1) {
langfordw 0:fc382eeb78ad 623 rear_motorA = 0;
langfordw 0:fc382eeb78ad 624 rear_motorB = 1;
langfordw 0:fc382eeb78ad 625 } else {
langfordw 0:fc382eeb78ad 626 rear_motorA = 1;
langfordw 0:fc382eeb78ad 627 rear_motorB = 0;
langfordw 0:fc382eeb78ad 628 }
langfordw 0:fc382eeb78ad 629 rear_motorPWM.write(power);
langfordw 0:fc382eeb78ad 630 } else if (which_motor == 2) { // front
langfordw 0:fc382eeb78ad 631 if (dir == 1) {
langfordw 0:fc382eeb78ad 632 front_motorA = 0;
langfordw 0:fc382eeb78ad 633 front_motorB = 1;
langfordw 0:fc382eeb78ad 634 } else {
langfordw 0:fc382eeb78ad 635 front_motorA = 1;
langfordw 0:fc382eeb78ad 636 front_motorB = 0;
langfordw 0:fc382eeb78ad 637 }
langfordw 0:fc382eeb78ad 638 front_motorPWM.write(power);
langfordw 0:fc382eeb78ad 639 } else if (which_motor == 3) { // spine
langfordw 0:fc382eeb78ad 640 if (dir == 1) {
langfordw 0:fc382eeb78ad 641 spine_motorA = 0;
langfordw 0:fc382eeb78ad 642 spine_motorB = 1;
langfordw 0:fc382eeb78ad 643 } else {
langfordw 0:fc382eeb78ad 644 spine_motorA = 1;
langfordw 0:fc382eeb78ad 645 spine_motorB = 0;
langfordw 0:fc382eeb78ad 646 }
langfordw 0:fc382eeb78ad 647 spine_motorPWM.write(power);
langfordw 0:fc382eeb78ad 648 }
langfordw 0:fc382eeb78ad 649 }
langfordw 0:fc382eeb78ad 650
calisch 1:e549754ca234 651 float updateEncoder(int which_encoder) {
calisch 1:e549754ca234 652 last_n[which_encoder] = n[which_encoder];
calisch 1:e549754ca234 653 if(which_encoder == rear){
calisch 1:e549754ca234 654 n[which_encoder] = rear_encoder.getPulses();
calisch 1:e549754ca234 655 }
calisch 1:e549754ca234 656 else if(which_encoder == front){
calisch 1:e549754ca234 657 n[which_encoder] = front_encoder.getPulses();
calisch 1:e549754ca234 658 }
calisch 1:e549754ca234 659 else if(which_encoder == spine){
calisch 1:e549754ca234 660 n[which_encoder] = spine_encoder.getPulses();
calisch 1:e549754ca234 661 }
calisch 1:e549754ca234 662 w = (n[which_encoder]-last_n[which_encoder])*1.047; //steps/s --> rad/s
calisch 1:e549754ca234 663 return w;
calisch 1:e549754ca234 664 }
langfordw 0:fc382eeb78ad 665
langfordw 0:fc382eeb78ad 666 void control() {
calisch 1:e549754ca234 667 rear_n_d = -trajectory[current_sample][rear]*n2d;
calisch 1:e549754ca234 668 front_n_d = -trajectory[current_sample][front]*n2d;
calisch 1:e549754ca234 669 spine_n_d = -trajectory[current_sample][spine]*n2d;
calisch 1:e549754ca234 670
langfordw 0:fc382eeb78ad 671 // rear
langfordw 0:fc382eeb78ad 672 i = read_current(rear);
calisch 1:e549754ca234 673 w = updateEncoder(rear);
calisch 1:e549754ca234 674 id = rear_Ks_p*(rear_n_d-n[rear]) + rear_Ks_d*(rear_w_d-w);
langfordw 0:fc382eeb78ad 675 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 676 id = abs(id);
langfordw 0:fc382eeb78ad 677 pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
calisch 1:e549754ca234 678 if (avg_current[rear] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 679 updateMotor(rear,pwm);
langfordw 0:fc382eeb78ad 680
langfordw 0:fc382eeb78ad 681 // front
langfordw 0:fc382eeb78ad 682 i = read_current(front);
calisch 1:e549754ca234 683 w = updateEncoder(front);
calisch 1:e549754ca234 684 id = front_Ks_p*(front_n_d-n[front]) + front_Ks_d*(front_w_d-w);
langfordw 0:fc382eeb78ad 685 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 686 id = abs(id);
langfordw 0:fc382eeb78ad 687 pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
calisch 1:e549754ca234 688 if (avg_current[rear] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 689 updateMotor(front,pwm);
langfordw 0:fc382eeb78ad 690
langfordw 0:fc382eeb78ad 691 // spine
langfordw 0:fc382eeb78ad 692 i = read_current(spine);
calisch 1:e549754ca234 693 w = updateEncoder(spine);
calisch 1:e549754ca234 694 id = spine_Ks_p*(spine_n_d-n[spine]) + spine_Ks_d*(spine_w_d-w);
langfordw 0:fc382eeb78ad 695 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 696 id = abs(id);
langfordw 0:fc382eeb78ad 697 pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
calisch 1:e549754ca234 698 if (avg_current[rear] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 699 updateMotor(spine,pwm);
calisch 1:e549754ca234 700
calisch 1:e549754ca234 701 //step to next control point
calisch 1:e549754ca234 702 if (current_sample == n_samples){
calisch 1:e549754ca234 703 if (current_loop == n_loops){ //done
calisch 1:e549754ca234 704 fclose(fp);
calisch 1:e549754ca234 705 pwm = 0;
calisch 1:e549754ca234 706 updateMotor(rear,pwm);
calisch 1:e549754ca234 707 updateMotor(front,pwm);
calisch 1:e549754ca234 708 }
calisch 1:e549754ca234 709 else{ //end of loop, ready for next
calisch 1:e549754ca234 710 current_sample = 0;
calisch 1:e549754ca234 711 current_loop++;
calisch 1:e549754ca234 712 }
calisch 1:e549754ca234 713 }
calisch 1:e549754ca234 714 else{ //middle of loop
calisch 1:e549754ca234 715 current_sample++;
calisch 1:e549754ca234 716 }
calisch 1:e549754ca234 717
calisch 1:e549754ca234 718 }
langfordw 0:fc382eeb78ad 719
langfordw 0:fc382eeb78ad 720 void save() {
calisch 1:e549754ca234 721 fprintf(fp, "%i %i %i %i\n", t.read_ms(), n[rear], n[front], n[spine]);
langfordw 0:fc382eeb78ad 722 }
langfordw 0:fc382eeb78ad 723
langfordw 0:fc382eeb78ad 724 int main() {
langfordw 0:fc382eeb78ad 725 rear_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 726 front_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 727 spine_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 728 tick.attach(&control,CONTROL_PERIOD);
langfordw 0:fc382eeb78ad 729 tock.attach(&save,SAVE_PERIOD);
calisch 1:e549754ca234 730 t.start();
langfordw 0:fc382eeb78ad 731
langfordw 0:fc382eeb78ad 732 while(1) {
langfordw 0:fc382eeb78ad 733 //DEBUG
langfordw 0:fc382eeb78ad 734 // pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
langfordw 0:fc382eeb78ad 735 }
langfordw 0:fc382eeb78ad 736 }