From 5a045b5fcfb8dc1ddd4ef1f4078cf5caf0b3e7df Mon Sep 17 00:00:00 2001 From: GabrielKoppenstein Date: Tue, 28 Sep 2021 00:06:31 +0200 Subject: [PATCH] Implemented Basic Programming features, changed speed control (speed_helper function with timed interrupt) --- CV.h | 556 +++++++++++++++++++++++++++------------------------- multicore.c | 212 +++++++++++++------- multicore.h | 4 +- 3 files changed, 428 insertions(+), 344 deletions(-) diff --git a/CV.h b/CV.h index b6ea54a..a6da5c5 100644 --- a/CV.h +++ b/CV.h @@ -1,9 +1,15 @@ //All CVs are 8-bit numbers (range 0 - 255) -uint8_t CV_ARRAY [512] = { +uint8_t CV_ARRAY_DEFAULT [512] = { 0b00000111, //CV_1 - Basic Address 0b00000000, //CV_2 - Minimum Speed + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // 255 == Fastest dec/acc rate; 0 == slowest; + // Note that there is an integer division in play here which will get rid of the fractional part + // this means that a value of 254 for example, will result in the same behaviour as an 248. + // This does not correspond to the norm and might be changed eventually. 0b00000000, //CV_3 - Acceleration Rate 0b00000000, //CV_4 - Deceleration Rate + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b11111111, //CV_5 - V_max 0b01111111, //CV_6 - V_mid 0b00000001, //CV_7 - Version No. @@ -16,8 +22,8 @@ uint8_t CV_ARRAY [512] = { 0b00000000, //CV_14 - 0b00000000, //CV_15 - 0b00000000, //CV_16 - - 0b00000000, //CV_17 - - 0b00000000, //CV_18 - + 0b11000011, //CV_17 - Extended/Long 14-Bit Address (Bits 8 to 13) + 0b11101000, //CV_18 - Extended/Long 14-Bit Address (Bits 0 to 7) 0b00000000, //CV_19 - 0b00000000, //CV_20 - 0b00000000, //CV_21 - @@ -28,7 +34,12 @@ uint8_t CV_ARRAY [512] = { 0b00000000, //CV_26 - 0b00000000, //CV_27 - 0b00000000, //CV_28 - - 0b00000000, //CV_29 - + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //Bit_1, Bit_2, Bit_3, Bit_4, Bit_6 are currently not in use and therefore irrelevant. + //Bit_0 is used to reverse direction i.e. 0 = normal; 1 = reverse + //Bit_5 switches between basic and extended address i.e. 0 = basic address 1 = extended address + 0b00000000, //CV_29 - Decoder Configuration + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b00000000, //CV_30 - 0b00000000, //CV_31 - 0b00000000, //CV_32 - @@ -44,16 +55,21 @@ uint8_t CV_ARRAY [512] = { 0b00000000, //CV_42 - 0b00000000, //CV_43 - 0b00000000, //CV_44 - - 0b00000000, //CV_45 - - 0b00000000, //CV_46 - - 0b00000000, //CV_47 - V_EMF Measure Period duration - 0b00000000, //CV_48 - V_EMF Measure Delay - 0b00000000, //CV_49 - PID Control P_Factor - 0b00000000, //CV_50 - PID Control I_Factor - 0b00000000, //CV_51 - PID Control D_Factor - 0b00000000, //CV_52 - - 0b00000000, //CV_53 - - 0b00000000, //CV_54 - + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // This value will indirectly change the acceleration/deceleration rates + 0b00101101, //CV_45 - Speed Helper Function Period in ms + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //(CV_46*2us)+(CV_47)+(CV_48*1000) is the Total duration of one PID-cycle i.e. the sampling time t_s + 0b00110010, //CV_46 - V_EMF Measurement Iterations + 0b01100100, //CV_47 - V_EMF Measurement Delay in us + 0b00001010, //CV_48 - V_EMF Measurement Period duration in ms + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + 0b01011100, //CV_49 - PID Control P_Factor = CV_49/1024 i.e. Default = 92 -> 0.08984375 + 0b01001101, //CV_50 - PID Control I_Factor = CV_50/1024 i.e. Default = 77 -> 0.0751953125 + 0b10000011, //CV_51 - PID Control D_Factor = CV_51/16384 i.e. Default = 131 -> 0.00799560547 + 0b00000000, //CV_52 - PID Integral Limit positive = CV_52*10 i.e. Default = 100 -> +1000 + 0b01100100, //CV_53 - PID Integral Limit negative = CV_53*(-10) i.e. Default = 100 -> -1000 + 0b01100100, //CV_54 - 0b00000000, //CV_55 - 0b00000000, //CV_56 - 0b00000000, //CV_57 - @@ -261,323 +277,323 @@ uint8_t CV_ARRAY [512] = { // byte_3 -- byte_2 -- byte_1 -- byte_0 // X X X X - X X X X -- X X X X - X X X X -- X X X X - X X X X -- X X X X - X X X X //F0 forward - 0b00000000, //byte 0 - CV_257 - Array Index: 0 - 0b00000000, //byte 1 - CV_258 - Array Index: 1 - 0b00010010, //byte 2 - CV_259 - Array Index: 2 - 0b00000000, //byte 3 - CV_260 - Array Index: 3 + 0b00000000, //byte 0 - CV_257 + 0b00000000, //byte 1 - CV_258 + 0b00010010, //byte 2 - CV_259 + 0b00000000, //byte 3 - CV_260 //F0 reverse - 0b00000000, //byte 0 - CV_261 - Array Index: 4 - 0b00000000, //byte 1 - CV_262 - Array Index: 5 - 0b00001100, //byte 2 - CV_263 - Array Index: 6 - 0b00000000, //byte 3 - CV_264 - Array Index: 7 + 0b00000000, //byte 0 - CV_261 + 0b00000000, //byte 1 - CV_262 + 0b00001100, //byte 2 - CV_263 + 0b00000000, //byte 3 - CV_264 //F1 forward - 0b00000010, //byte 0 - CV_265 - Array Index: 8 - 0b00000000, //byte 1 - CV_266 - Array Index: 9 - 0b00000000, //byte 2 - CV_267 - Array Index: 10 - 0b00000000, //byte 3 - CV_268 - Array Index: 11 + 0b00000010, //byte 0 - CV_265 + 0b00000000, //byte 1 - CV_266 + 0b00000000, //byte 2 - CV_267 + 0b00000000, //byte 3 - CV_268 //F1 reverse - 0b00000010, //byte 0 - CV_269 - Array Index: 12 - 0b00000000, //byte 1 - CV_270 - Array Index: 13 - 0b00000000, //byte 2 - CV_271 - Array Index: 14 - 0b00000000, //byte 3 - CV_272 - Array Index: 15 + 0b00000010, //byte 0 - CV_269 + 0b00000000, //byte 1 - CV_270 + 0b00000000, //byte 2 - CV_271 + 0b00000000, //byte 3 - CV_272 //F2 forward - 0b00000100, //byte 0 - CV_273 - Array Index: 16 - 0b00000000, //byte 1 - CV_274 - Array Index: 17 - 0b00000000, //byte 2 - CV_275 - Array Index: 18 - 0b00000000, //byte 3 - CV_276 - Array Index: 19 + 0b00000100, //byte 0 - CV_273 + 0b00000000, //byte 1 - CV_274 + 0b00000000, //byte 2 - CV_275 + 0b00000000, //byte 3 - CV_276 //F2 reverse - 0b00000100, //byte 0 - CV_277 - Array Index: 20 - 0b00000000, //byte 1 - CV_278 - Array Index: 21 - 0b00000000, //byte 2 - CV_279 - Array Index: 22 - 0b00000000, //byte 3 - CV_280 - Array Index: 23 + 0b00000100, //byte 0 - CV_277 + 0b00000000, //byte 1 - CV_278 + 0b00000000, //byte 2 - CV_279 + 0b00000000, //byte 3 - CV_280 //F3 forward - 0b00001000, //byte 0 - CV_281 - Array Index: 24 - 0b00000000, //byte 1 - CV_282 - Array Index: 25 - 0b00000000, //byte 2 - CV_283 - Array Index: 26 - 0b00000000, //byte 3 - CV_284 - Array Index: 27 + 0b00001000, //byte 0 - CV_281 + 0b00000000, //byte 1 - CV_282 + 0b00000000, //byte 2 - CV_283 + 0b00000000, //byte 3 - CV_284 //F3 reverse - 0b00001000, //byte 0 - CV_285 - Array Index: 28 - 0b00000000, //byte 1 - CV_286 - Array Index: 29 - 0b00000000, //byte 2 - CV_287 - Array Index: 30 - 0b00000000, //byte 3 - CV_288 - Array Index: 31 + 0b00001000, //byte 0 - CV_285 + 0b00000000, //byte 1 - CV_286 + 0b00000000, //byte 2 - CV_287 + 0b00000000, //byte 3 - CV_288 //F4 forward - 0b00010000, //byte 0 - CV_289 - Array Index: 32 - 0b00000000, //byte 1 - CV_290 - Array Index: 33 - 0b00000000, //byte 2 - CV_291 - Array Index: 34 - 0b00000000, //byte 3 - CV_292 - Array Index: 35 + 0b00010000, //byte 0 - CV_289 + 0b00000000, //byte 1 - CV_290 + 0b00000000, //byte 2 - CV_291 + 0b00000000, //byte 3 - CV_292 //F4 reverse - 0b00010000, //byte 0 - CV_293 - Array Index: 36 - 0b00000000, //byte 1 - CV_294 - Array Index: 37 - 0b00000000, //byte 2 - CV_295 - Array Index: 38 - 0b00000000, //byte 3 - CV_296 - Array Index: 39 + 0b00010000, //byte 0 - CV_293 + 0b00000000, //byte 1 - CV_294 + 0b00000000, //byte 2 - CV_295 + 0b00000000, //byte 3 - CV_296 //F5 forward - 0b00100000, //byte 0 - CV_297 - Array Index: 40 - 0b00000000, //byte 1 - CV_298 - Array Index: 41 - 0b00000000, //byte 2 - CV_299 - Array Index: 42 - 0b00000000, //byte 3 - CV_300 - Array Index: 43 + 0b00100000, //byte 0 - CV_297 + 0b00000000, //byte 1 - CV_298 + 0b00000000, //byte 2 - CV_299 + 0b00000000, //byte 3 - CV_300 //F5 reverse - 0b00100000, //byte 0 - CV_301 - Array Index: 44 - 0b00000000, //byte 1 - CV_302 - Array Index: 45 - 0b00000000, //byte 2 - CV_303 - Array Index: 46 - 0b00000000, //byte 3 - CV_304 - Array Index: 47 + 0b00100000, //byte 0 - CV_301 + 0b00000000, //byte 1 - CV_302 + 0b00000000, //byte 2 - CV_303 + 0b00000000, //byte 3 - CV_304 //F6 forward - 0b01000000, //byte 0 - CV_305 - Array Index: 48 - 0b00000000, //byte 1 - CV_306 - Array Index: 49 - 0b00000000, //byte 2 - CV_307 - Array Index: 50 - 0b00000000, //byte 3 - CV_308 - Array Index: 51 + 0b01000000, //byte 0 - CV_305 + 0b00000000, //byte 1 - CV_306 + 0b00000000, //byte 2 - CV_307 + 0b00000000, //byte 3 - CV_308 //F6 reverse - 0b01000000, //byte 0 - CV_309 - Array Index: 52 - 0b00000000, //byte 1 - CV_310 - Array Index: 53 - 0b00000000, //byte 2 - CV_311 - Array Index: 54 - 0b00000000, //byte 3 - CV_312 - Array Index: 55 + 0b01000000, //byte 0 - CV_309 + 0b00000000, //byte 1 - CV_310 + 0b00000000, //byte 2 - CV_311 + 0b00000000, //byte 3 - CV_312 //F7 forward - 0b10000000, //byte 0 - CV_313 - Array Index: 56 - 0b00000000, //byte 1 - CV_314 - Array Index: 57 - 0b00000000, //byte 2 - CV_315 - Array Index: 58 - 0b00000000, //byte 3 - CV_316 - Array Index: 59 + 0b10000000, //byte 0 - CV_313 + 0b00000000, //byte 1 - CV_314 + 0b00000000, //byte 2 - CV_315 + 0b00000000, //byte 3 - CV_316 //F7 reverse - 0b10000000, //byte 0 - CV_317 - Array Index: 60 - 0b00000000, //byte 1 - CV_318 - Array Index: 61 - 0b00000000, //byte 2 - CV_319 - Array Index: 62 - 0b00000000, //byte 3 - CV_320 - Array Index: 63 + 0b10000000, //byte 0 - CV_317 + 0b00000000, //byte 1 - CV_318 + 0b00000000, //byte 2 - CV_319 + 0b00000000, //byte 3 - CV_320 //F8 forward - 0b00000000, //byte 0 - CV_321 - Array Index: 64 - 0b00000001, //byte 1 - CV_322 - Array Index: 65 - 0b00000000, //byte 2 - CV_323 - Array Index: 66 - 0b00000000, //byte 3 - CV_324 - Array Index: 67 + 0b00000000, //byte 0 - CV_321 + 0b00000001, //byte 1 - CV_322 + 0b00000000, //byte 2 - CV_323 + 0b00000000, //byte 3 - CV_324 //F8 reverse - 0b00000000, //byte 0 - CV_325 - Array Index: 68 - 0b00000001, //byte 1 - CV_326 - Array Index: 69 - 0b00000000, //byte 2 - CV_327 - Array Index: 70 - 0b00000000, //byte 3 - CV_328 - Array Index: 71 + 0b00000000, //byte 0 - CV_325 + 0b00000001, //byte 1 - CV_326 + 0b00000000, //byte 2 - CV_327 + 0b00000000, //byte 3 - CV_328 //F9 forward - 0b00000000, //byte 0 - CV_329 - Array Index: 72 - 0b00000010, //byte 1 - CV_330 - Array Index: 73 - 0b00000000, //byte 2 - CV_331 - Array Index: 74 - 0b00000000, //byte 3 - CV_332 - Array Index: 75 + 0b00000000, //byte 0 - CV_329 + 0b00000010, //byte 1 - CV_330 + 0b00000000, //byte 2 - CV_331 + 0b00000000, //byte 3 - CV_332 //F9 reverse - 0b00000000, //byte 0 - CV_333 - Array Index: 76 - 0b00000010, //byte 1 - CV_334 - Array Index: 77 - 0b00000000, //byte 2 - CV_335 - Array Index: 78 - 0b00000000, //byte 3 - CV_336 - Array Index: 79 + 0b00000000, //byte 0 - CV_333 + 0b00000010, //byte 1 - CV_334 + 0b00000000, //byte 2 - CV_335 + 0b00000000, //byte 3 - CV_336 //F10 forward - 0b00000000, //byte 0 - CV_337 - Array Index: 80 - 0b00000100, //byte 1 - CV_338 - Array Index: 81 - 0b00000000, //byte 2 - CV_339 - Array Index: 82 - 0b00000000, //byte 3 - CV_340 - Array Index: 83 + 0b00000000, //byte 0 - CV_337 + 0b00000100, //byte 1 - CV_338 + 0b00000000, //byte 2 - CV_339 + 0b00000000, //byte 3 - CV_340 //F10 reverse - 0b00000000, //byte 0 - CV_341 - Array Index: 84 - 0b00000100, //byte 1 - CV_342 - Array Index: 85 - 0b00000000, //byte 2 - CV_343 - Array Index: 86 - 0b00000000, //byte 3 - CV_344 - Array Index: 87 + 0b00000000, //byte 0 - CV_341 + 0b00000100, //byte 1 - CV_342 + 0b00000000, //byte 2 - CV_343 + 0b00000000, //byte 3 - CV_344 //F11 forward - 0b00000000, //byte 0 - CV_345 - Array Index: 88 - 0b00001000, //byte 1 - CV_346 - Array Index: 89 - 0b00000000, //byte 2 - CV_347 - Array Index: 90 - 0b00000000, //byte 3 - CV_348 - Array Index: 91 + 0b00000000, //byte 0 - CV_345 + 0b00001000, //byte 1 - CV_346 + 0b00000000, //byte 2 - CV_347 + 0b00000000, //byte 3 - CV_348 //F11 reverse - 0b00000000, //byte 0 - CV_349 - Array Index: 92 - 0b00001000, //byte 1 - CV_350 - Array Index: 93 - 0b00000000, //byte 2 - CV_351 - Array Index: 94 - 0b00000000, //byte 3 - CV_352 - Array Index: 95 + 0b00000000, //byte 0 - CV_349 + 0b00001000, //byte 1 - CV_350 + 0b00000000, //byte 2 - CV_351 + 0b00000000, //byte 3 - CV_352 //F12 forward - 0b00000000, //byte 0 - CV_353 - Array Index: 96 - 0b00010000, //byte 1 - CV_354 - Array Index: 97 - 0b00000000, //byte 2 - CV_355 - Array Index: 98 - 0b00000000, //byte 3 - CV_356 - Array Index: 99 + 0b00000000, //byte 0 - CV_353 + 0b00010000, //byte 1 - CV_354 + 0b00000000, //byte 2 - CV_355 + 0b00000000, //byte 3 - CV_356 //F12 reverse - 0b00000000, //byte 0 - CV_357 - Array Index: 100 - 0b00010000, //byte 1 - CV_358 - Array Index: 101 - 0b00000000, //byte 2 - CV_359 - Array Index: 102 - 0b00000000, //byte 3 - CV_360 - Array Index: 103 + 0b00000000, //byte 0 - CV_357 + 0b00010000, //byte 1 - CV_358 + 0b00000000, //byte 2 - CV_359 + 0b00000000, //byte 3 - CV_360 //F13 forward - 0b00000000, //byte 0 - CV_361 - Array Index: 104 - 0b00100000, //byte 1 - CV_362 - Array Index: 105 - 0b00000000, //byte 2 - CV_363 - Array Index: 106 - 0b00000000, //byte 3 - CV_364 - Array Index: 107 + 0b00000000, //byte 0 - CV_361 + 0b00100000, //byte 1 - CV_362 + 0b00000000, //byte 2 - CV_363 + 0b00000000, //byte 3 - CV_364 //F13 reverse - 0b00000000, //byte 0 - CV_365 - Array Index: 108 - 0b00100000, //byte 1 - CV_366 - Array Index: 109 - 0b00000000, //byte 2 - CV_367 - Array Index: 110 - 0b00000000, //byte 3 - CV_368 - Array Index: 111 + 0b00000000, //byte 0 - CV_365 + 0b00100000, //byte 1 - CV_366 + 0b00000000, //byte 2 - CV_367 + 0b00000000, //byte 3 - CV_368 //F14 forward - 0b00000000, //byte 0 - CV_369 - Array Index: 112 - 0b01000000, //byte 1 - CV_370 - Array Index: 113 - 0b00000000, //byte 2 - CV_371 - Array Index: 114 - 0b00000000, //byte 3 - CV_372 - Array Index: 115 + 0b00000000, //byte 0 - CV_369 + 0b01000000, //byte 1 - CV_370 + 0b00000000, //byte 2 - CV_371 + 0b00000000, //byte 3 - CV_372 //F14 reverse - 0b00000000, //byte 0 - CV_373 - Array Index: 116 - 0b01000000, //byte 1 - CV_374 - Array Index: 117 - 0b00000000, //byte 2 - CV_375 - Array Index: 118 - 0b00000000, //byte 3 - CV_376 - Array Index: 119 + 0b00000000, //byte 0 - CV_373 + 0b01000000, //byte 1 - CV_374 + 0b00000000, //byte 2 - CV_375 + 0b00000000, //byte 3 - CV_376 //F15 forward - 0b00000000, //byte 0 - CV_377 - Array Index: 120 - 0b10000000, //byte 1 - CV_378 - Array Index: 121 - 0b00000000, //byte 2 - CV_379 - Array Index: 122 - 0b00000000, //byte 3 - CV_380 - Array Index: 123 + 0b00000000, //byte 0 - CV_377 + 0b10000000, //byte 1 - CV_378 + 0b00000000, //byte 2 - CV_379 + 0b00000000, //byte 3 - CV_380 //F15 reverse - 0b00000000, //byte 0 - CV_381 - Array Index: 124 - 0b10000000, //byte 1 - CV_382 - Array Index: 125 - 0b00000000, //byte 2 - CV_383 - Array Index: 126 - 0b00000000, //byte 3 - CV_384 - Array Index: 127 + 0b00000000, //byte 0 - CV_381 + 0b10000000, //byte 1 - CV_382 + 0b00000000, //byte 2 - CV_383 + 0b00000000, //byte 3 - CV_384 //F16 forward - 0b00000000, //byte 0 - CV_385 - Array Index: 128 - 0b00000000, //byte 1 - CV_386 - Array Index: 129 - 0b00000001, //byte 2 - CV_387 - Array Index: 130 - 0b00000000, //byte 3 - CV_388 - Array Index: 131 + 0b00000000, //byte 0 - CV_385 + 0b00000000, //byte 1 - CV_386 + 0b00000001, //byte 2 - CV_387 + 0b00000000, //byte 3 - CV_388 //F16 reverse - 0b00000000, //byte 0 - CV_389 - Array Index: 132 - 0b00000000, //byte 1 - CV_390 - Array Index: 133 - 0b00000001, //byte 2 - CV_391 - Array Index: 134 - 0b00000000, //byte 3 - CV_392 - Array Index: 135 + 0b00000000, //byte 0 - CV_389 + 0b00000000, //byte 1 - CV_390 + 0b00000001, //byte 2 - CV_391 + 0b00000000, //byte 3 - CV_392 //F17 forward - 0b00000000, //byte 0 - CV_393 - Array Index: 136 - 0b00000000, //byte 1 - CV_394 - Array Index: 137 - 0b00000000, //byte 2 - CV_395 - Array Index: 138 - 0b00000000, //byte 3 - CV_396 - Array Index: 139 + 0b00000000, //byte 0 - CV_393 + 0b00000000, //byte 1 - CV_394 + 0b00000000, //byte 2 - CV_395 + 0b00000000, //byte 3 - CV_396 //F17 reverse - 0b00000000, //byte 0 - CV_397 - Array Index: 140 - 0b00000000, //byte 1 - CV_398 - Array Index: 141 - 0b00000000, //byte 2 - CV_399 - Array Index: 142 - 0b00000000, //byte 3 - CV_400 - Array Index: 143 + 0b00000000, //byte 0 - CV_397 + 0b00000000, //byte 1 - CV_398 + 0b00000000, //byte 2 - CV_399 + 0b00000000, //byte 3 - CV_400 //F18 forward - 0b00000000, //byte 0 - CV_401 - Array Index: 144 - 0b00000000, //byte 1 - CV_402 - Array Index: 145 - 0b00000000, //byte 2 - CV_403 - Array Index: 146 - 0b00000000, //byte 3 - CV_404 - Array Index: 147 + 0b00000000, //byte 0 - CV_401 + 0b00000000, //byte 1 - CV_402 + 0b00000000, //byte 2 - CV_403 + 0b00000000, //byte 3 - CV_404 //F18 reverse - 0b00000000, //byte 0 - CV_405 - Array Index: 148 - 0b00000000, //byte 1 - CV_406 - Array Index: 149 - 0b00000000, //byte 2 - CV_407 - Array Index: 150 - 0b00000000, //byte 3 - CV_408 - Array Index: 151 + 0b00000000, //byte 0 - CV_405 + 0b00000000, //byte 1 - CV_406 + 0b00000000, //byte 2 - CV_407 + 0b00000000, //byte 3 - CV_408 //F19 forward - 0b00000000, //byte 0 - CV_409 - Array Index: 152 - 0b00000000, //byte 1 - CV_410 - Array Index: 153 - 0b00000000, //byte 2 - CV_411 - Array Index: 154 - 0b00000000, //byte 3 - CV_412 - Array Index: 155 + 0b00000000, //byte 0 - CV_409 + 0b00000000, //byte 1 - CV_410 + 0b00000000, //byte 2 - CV_411 + 0b00000000, //byte 3 - CV_412 //F19 reverse - 0b00000000, //byte 0 - CV_413 - Array Index: 156 - 0b00000000, //byte 1 - CV_414 - Array Index: 157 - 0b00000000, //byte 2 - CV_415 - Array Index: 158 - 0b00000000, //byte 3 - CV_416 - Array Index: 159 + 0b00000000, //byte 0 - CV_413 + 0b00000000, //byte 1 - CV_414 + 0b00000000, //byte 2 - CV_415 + 0b00000000, //byte 3 - CV_416 //F20 forward - 0b00000000, //byte 0 - CV_417 - Array Index: 160 - 0b00000000, //byte 1 - CV_418 - Array Index: 161 - 0b00000000, //byte 2 - CV_419 - Array Index: 162 - 0b00000000, //byte 3 - CV_420 - Array Index: 163 + 0b00000000, //byte 0 - CV_417 + 0b00000000, //byte 1 - CV_418 + 0b00000000, //byte 2 - CV_419 + 0b00000000, //byte 3 - CV_420 //F20 reverse - 0b00000000, //byte 0 - CV_421 - Array Index: 164 - 0b00000000, //byte 1 - CV_422 - Array Index: 165 - 0b00000000, //byte 2 - CV_423 - Array Index: 166 - 0b00000000, //byte 3 - CV_424 - Array Index: 167 + 0b00000000, //byte 0 - CV_421 + 0b00000000, //byte 1 - CV_422 + 0b00000000, //byte 2 - CV_423 + 0b00000000, //byte 3 - CV_424 //F21 forward - 0b00000000, //byte 0 - CV_425 - Array Index: 168 - 0b00000000, //byte 1 - CV_426 - Array Index: 169 - 0b00000000, //byte 2 - CV_427 - Array Index: 170 - 0b00000000, //byte 3 - CV_428 - Array Index: 171 + 0b00000000, //byte 0 - CV_425 + 0b00000000, //byte 1 - CV_426 + 0b00000000, //byte 2 - CV_427 + 0b00000000, //byte 3 - CV_428 //F21 reverse - 0b00000000, //byte 0 - CV_429 - Array Index: 172 - 0b00000000, //byte 1 - CV_430 - Array Index: 173 - 0b00000000, //byte 2 - CV_431 - Array Index: 174 - 0b00000000, //byte 3 - CV_432 - Array Index: 175 + 0b00000000, //byte 0 - CV_429 + 0b00000000, //byte 1 - CV_430 + 0b00000000, //byte 2 - CV_431 + 0b00000000, //byte 3 - CV_432 //F22 forward - 0b00000000, //byte 0 - CV_433 - Array Index: 176 - 0b00000000, //byte 1 - CV_434 - Array Index: 177 - 0b00000000, //byte 2 - CV_435 - Array Index: 178 - 0b00000000, //byte 3 - CV_436 - Array Index: 179 + 0b00000000, //byte 0 - CV_433 + 0b00000000, //byte 1 - CV_434 + 0b00000000, //byte 2 - CV_435 + 0b00000000, //byte 3 - CV_436 //F22 reverse - 0b00000000, //byte 0 - CV_437 - Array Index: 180 - 0b00000000, //byte 1 - CV_438 - Array Index: 181 - 0b00000000, //byte 2 - CV_439 - Array Index: 182 - 0b00000000, //byte 3 - CV_440 - Array Index: 183 + 0b00000000, //byte 0 - CV_437 + 0b00000000, //byte 1 - CV_438 + 0b00000000, //byte 2 - CV_439 + 0b00000000, //byte 3 - CV_440 //F23 forward - 0b00000000, //byte 0 - CV_441 - Array Index: 184 - 0b00000000, //byte 1 - CV_442 - Array Index: 185 - 0b00000000, //byte 2 - CV_443 - Array Index: 186 - 0b00000000, //byte 3 - CV_444 - Array Index: 187 + 0b00000000, //byte 0 - CV_441 + 0b00000000, //byte 1 - CV_442 + 0b00000000, //byte 2 - CV_443 + 0b00000000, //byte 3 - CV_444 //F23 reverse - 0b00000000, //byte 0 - CV_445 - Array Index: 188 - 0b00000000, //byte 1 - CV_446 - Array Index: 189 - 0b00000000, //byte 2 - CV_447 - Array Index: 190 - 0b00000000, //byte 3 - CV_448 - Array Index: 191 + 0b00000000, //byte 0 - CV_445 + 0b00000000, //byte 1 - CV_446 + 0b00000000, //byte 2 - CV_447 + 0b00000000, //byte 3 - CV_448 //F24 forward - 0b00000000, //byte 0 - CV_449 - Array Index: 192 - 0b00000000, //byte 1 - CV_450 - Array Index: 193 - 0b00000000, //byte 2 - CV_451 - Array Index: 194 - 0b00000000, //byte 3 - CV_452 - Array Index: 195 + 0b00000000, //byte 0 - CV_449 + 0b00000000, //byte 1 - CV_450 + 0b00000000, //byte 2 - CV_451 + 0b00000000, //byte 3 - CV_452 //F24 reverse - 0b00000000, //byte 0 - CV_453 - Array Index: 196 - 0b00000000, //byte 1 - CV_454 - Array Index: 197 - 0b00000000, //byte 2 - CV_455 - Array Index: 198 - 0b00000000, //byte 3 - CV_456 - Array Index: 199 + 0b00000000, //byte 0 - CV_453 + 0b00000000, //byte 1 - CV_454 + 0b00000000, //byte 2 - CV_455 + 0b00000000, //byte 3 - CV_456 //F25 forward - 0b00000000, //byte 0 - CV_457 - Array Index: 200 - 0b00000000, //byte 1 - CV_458 - Array Index: 201 - 0b00000000, //byte 2 - CV_459 - Array Index: 202 - 0b00000000, //byte 3 - CV_460 - Array Index: 203 + 0b00000000, //byte 0 - CV_457 + 0b00000000, //byte 1 - CV_458 + 0b00000000, //byte 2 - CV_459 + 0b00000000, //byte 3 - CV_460 //F25 reverse - 0b00000000, //byte 0 - CV_461 - Array Index: 204 - 0b00000000, //byte 1 - CV_462 - Array Index: 205 - 0b00000000, //byte 2 - CV_463 - Array Index: 206 - 0b00000000, //byte 3 - CV_464 - Array Index: 207 + 0b00000000, //byte 0 - CV_461 + 0b00000000, //byte 1 - CV_462 + 0b00000000, //byte 2 - CV_463 + 0b00000000, //byte 3 - CV_464 //F26 forward - 0b00000000, //byte 0 - CV_465 - Array Index: 208 - 0b00000000, //byte 1 - CV_466 - Array Index: 209 - 0b00000000, //byte 2 - CV_467 - Array Index: 210 - 0b00000000, //byte 3 - CV_468 - Array Index: 211 + 0b00000000, //byte 0 - CV_465 + 0b00000000, //byte 1 - CV_466 + 0b00000000, //byte 2 - CV_467 + 0b00000000, //byte 3 - CV_468 //F26 reverse - 0b00000000, //byte 0 - CV_469 - Array Index: 212 - 0b00000000, //byte 1 - CV_470 - Array Index: 213 - 0b00000000, //byte 2 - CV_471 - Array Index: 214 - 0b00000000, //byte 3 - CV_472 - Array Index: 215 + 0b00000000, //byte 0 - CV_469 + 0b00000000, //byte 1 - CV_470 + 0b00000000, //byte 2 - CV_471 + 0b00000000, //byte 3 - CV_472 //F27 forward - 0b00000000, //byte 0 - CV_473 - Array Index: 216 - 0b00000000, //byte 1 - CV_474 - Array Index: 217 - 0b00000000, //byte 2 - CV_475 - Array Index: 218 - 0b00000000, //byte 3 - CV_476 - Array Index: 219 + 0b00000000, //byte 0 - CV_473 + 0b00000000, //byte 1 - CV_474 + 0b00000000, //byte 2 - CV_475 + 0b00000000, //byte 3 - CV_476 //F27 reverse - 0b00000000, //byte 0 - CV_477 - Array Index: 220 - 0b00000000, //byte 1 - CV_478 - Array Index: 221 - 0b00000000, //byte 2 - CV_479 - Array Index: 222 - 0b00000000, //byte 3 - CV_480 - Array Index: 223 + 0b00000000, //byte 0 - CV_477 + 0b00000000, //byte 1 - CV_478 + 0b00000000, //byte 2 - CV_479 + 0b00000000, //byte 3 - CV_480 //F28 forward - 0b00000000, //byte 0 - CV_481 - Array Index: 224 - 0b00000000, //byte 1 - CV_482 - Array Index: 225 - 0b00000000, //byte 2 - CV_483 - Array Index: 226 - 0b00010000, //byte 3 - CV_484 - Array Index: 227 + 0b00000000, //byte 0 - CV_481 + 0b00000000, //byte 1 - CV_482 + 0b00000000, //byte 2 - CV_483 + 0b00010000, //byte 3 - CV_484 //F28 reverse - 0b00000000, //byte 0 - CV_485 - Array Index: 228 - 0b00000000, //byte 1 - CV_486 - Array Index: 229 - 0b00000000, //byte 2 - CV_487 - Array Index: 230 - 0b00010000, //byte 3 - CV_488 - Array Index: 231 + 0b00000000, //byte 0 - CV_485 + 0b00000000, //byte 1 - CV_486 + 0b00000000, //byte 2 - CV_487 + 0b00010000, //byte 3 - CV_488 //F29 forward - 0b00000000, //byte 0 - CV_489 - Array Index: 232 - 0b00000000, //byte 1 - CV_490 - Array Index: 233 - 0b00000000, //byte 2 - CV_491 - Array Index: 234 - 0b00000000, //byte 3 - CV_492 - Array Index: 235 + 0b00000000, //byte 0 - CV_489 + 0b00000000, //byte 1 - CV_490 + 0b00000000, //byte 2 - CV_491 + 0b00000000, //byte 3 - CV_492 //F29 reverse - 0b00000000, //byte 0 - CV_493 - Array Index: 236 - 0b00000000, //byte 1 - CV_494 - Array Index: 237 - 0b00000000, //byte 2 - CV_495 - Array Index: 238 - 0b00000000, //byte 3 - CV_496 - Array Index: 239 + 0b00000000, //byte 0 - CV_493 + 0b00000000, //byte 1 - CV_494 + 0b00000000, //byte 2 - CV_495 + 0b00000000, //byte 3 - CV_496 //F30 forward - 0b00000000, //byte 0 - CV_497 - Array Index: 240 - 0b00000000, //byte 1 - CV_498 - Array Index: 241 - 0b00000000, //byte 2 - CV_499 - Array Index: 242 - 0b00000000, //byte 3 - CV_500 - Array Index: 243 + 0b00000000, //byte 0 - CV_497 + 0b00000000, //byte 1 - CV_498 + 0b00000000, //byte 2 - CV_499 + 0b00000000, //byte 3 - CV_500 //F30 reverse - 0b00000000, //byte 0 - CV_501 - Array Index: 244 - 0b00000000, //byte 1 - CV_502 - Array Index: 245 - 0b00000000, //byte 2 - CV_503 - Array Index: 246 - 0b00000000, //byte 3 - CV_504 - Array Index: 247 + 0b00000000, //byte 0 - CV_501 + 0b00000000, //byte 1 - CV_502 + 0b00000000, //byte 2 - CV_503 + 0b00000000, //byte 3 - CV_504 //F31 forward - 0b00000000, //byte 0 - CV_505 - Array Index: 248 - 0b00000000, //byte 1 - CV_506 - Array Index: 249 - 0b00000000, //byte 2 - CV_507 - Array Index: 250 - 0b00000000, //byte 3 - CV_508 - Array Index: 251 + 0b00000000, //byte 0 - CV_505 + 0b00000000, //byte 1 - CV_506 + 0b00000000, //byte 2 - CV_507 + 0b00000000, //byte 3 - CV_508 //F31 reverse - 0b00000000, //byte 0 - CV_509 - Array Index: 252 - 0b00000000, //byte 1 - CV_510 - Array Index: 253 - 0b00000000, //byte 2 - CV_511 - Array Index: 254 - 0b00000000, //byte 3 - CV_512 - Array Index: 255 + 0b00000000, //byte 0 - CV_509 + 0b00000000, //byte 1 - CV_510 + 0b00000000, //byte 2 - CV_511 + 0b00000000, //byte 3 - CV_512 }; diff --git a/multicore.c b/multicore.c index 124d334..198c788 100644 --- a/multicore.c +++ b/multicore.c @@ -1,4 +1,5 @@ #include +#include "string.h" #include "pico/stdlib.h" #include "pico/multicore.h" #include "hardware/irq.h" @@ -8,23 +9,23 @@ #include "CV.h" #include "multicore.h" const uint8_t *CV_ARRAY_FLASH = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET); -uint64_t last_bits = 0; bool active_functions[SIZE_ACTIVE_FUNCTIONS] = {false}; -uint32_t target_speed_step = 128; +uint64_t last_bits = 0; bool target_direction = true; -int error = 0; -int e_old = 0; -int e_sum = 0; -float t_s = 0.01f; +uint target_speed_step = 128; +uint current_v_emf_target = 0; +uint measurement = 0; +bool reset_package_flag = false; int sum_limit_max = 1000; int sum_limit_min = -1000; float k_p = 0.09f; float k_i = 0.075f; float k_d = 0.008f; -float output = 0; -uint target_v_emf = 0; -uint measurement; -bool program_mode_flag = false; +float t_s = 0.01f; +uint measurement_delay_in_us; +uint measurement_iterations; +uint measurement_period_in_ms; +struct repeating_timer speed_helper_timer; int8_t check_for_package() //function returns number of bytes if valid bit-pattern is found. Otherwise -1 is returned { @@ -60,23 +61,59 @@ void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) { } } -void interp_speed_step(uint8_t speed_byte){ - target_speed_step = speed_byte; - //Forward - if(speed_byte>127){ - target_v_emf = (speed_byte-129)*32; - target_direction = true; +void acknowledge(){ + for (int i = 0; i < 4; ++i) { + gpio_put(MOTOR_PWM_REVERSE,0); + gpio_put(MOTOR_PWM_FORWARD,1); + sleep_ms(1); + gpio_put(MOTOR_PWM_FORWARD,0); + gpio_put(MOTOR_PWM_REVERSE,1); + sleep_ms(1); } - //Reverse - if(speed_byte<128) { - target_v_emf = (speed_byte-1)*32; - target_direction = false; - } - if (speed_byte == 128 || speed_byte==0) target_v_emf = 0; +} +void verify_cv(uint16_t cv_address,uint8_t cv_data){ + if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge(); } -void program_mode(){ +void write_cv(uint16_t cv_address,uint8_t cv_data){ + multicore_reset_core1(); + uint32_t ints = save_and_disable_interrupts(); + // CV_n is at index n-1 of the array e.g.: CV_1 -> index = 0 + uint16_t cv_array_index = cv_address-1; + //CV_7 and CV_8 are read-only + if (cv_address == 7){ + } + if (cv_address == 8){ + //Reset all CVs to Default + if (cv_data == 8){ + flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); + flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_DEFAULT, FLASH_PAGE_SIZE * 2); + restore_interrupts(ints); + acknowledge(); + } + } + else{ + uint8_t CV_ARRAY_TEMP[512] = {0}; + memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP)); + CV_ARRAY_TEMP[cv_array_index] = cv_data; + flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); + flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_TEMP, FLASH_PAGE_SIZE * 2); + restore_interrupts(ints); + acknowledge(); + } + multicore_launch_core1(core1_entry); +} +void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ + //TODO: Program only on second identical Command/Packet... + uint8_t instruction_type_mask = 0b00001100; + uint8_t instruction_type = instruction_type_mask&byte_array[number_of_bytes-1]; + uint8_t cv_address_mask = 0b00000011; + uint8_t cv_address_ms_bits = cv_address_mask&byte_array[number_of_bytes-1]; + uint16_t cv_address = byte_array[number_of_bytes-2]+(cv_address_ms_bits<<8); + uint8_t cv_data = byte_array[number_of_bytes-3]; + if (instruction_type == 0b000000100) verify_cv(cv_address,cv_data); + if (instruction_type == 0b000001100) write_cv(cv_address,cv_data); } void set_outputs() { @@ -86,10 +123,10 @@ void set_outputs() { for (uint8_t i = 0; i < SIZE_ACTIVE_FUNCTIONS; i++) { if (active_functions[i]) { printf("F%u == 1\n",i); - uint8_t func_cv_0 = CV_ARRAY[4 + i * 8 - 4 * target_direction]; - uint8_t func_cv_1 = CV_ARRAY[5 + i * 8 - 4 * target_direction]; - uint8_t func_cv_2 = CV_ARRAY[6 + i * 8 - 4 * target_direction]; - uint8_t func_cv_3 = CV_ARRAY[7 + i * 8 - 4 * target_direction]; + uint8_t func_cv_0 = CV_ARRAY_FLASH[4 + i * 8 - 4 * target_direction]; //TODO: Index needs to be corrected here... + uint8_t func_cv_1 = CV_ARRAY_FLASH[5 + i * 8 - 4 * target_direction]; + uint8_t func_cv_2 = CV_ARRAY_FLASH[6 + i * 8 - 4 * target_direction]; + uint8_t func_cv_3 = CV_ARRAY_FLASH[7 + i * 8 - 4 * target_direction]; uint32_t func_cv = (func_cv_0) + (func_cv_1 << 8) + (func_cv_2 << 16) + (func_cv_3 << 24); GPIO_to_be_set = (GPIO_to_be_set | func_cv) & filter_forbidden_GPIO; uint32_t mask = 1; @@ -132,11 +169,11 @@ bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]) { bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) { uint16_t read_address; - //Enters Programming Mode - if (program_mode_flag && byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111){ - program_mode(); + //Check for address 112-127 and for previous packet == reset package + if (reset_package_flag && byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111){ + program_mode(number_of_bytes,byte_array); + return false; } - program_mode_flag = false; //Check for Idle Package if (byte_array[number_of_bytes - 1] == 255) { @@ -155,16 +192,17 @@ bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) { uint16_t address_byte_1 = (byte_array[number_of_bytes - 1]) - 192; //remove long address identifier bits uint16_t address_byte_0 = (byte_array[number_of_bytes - 2]); read_address = (address_byte_1 << 8) + address_byte_0; - //printf("long address: %u, was read. \n", read_address); + uint8_t long_address_mask = 0b00111111; + uint16_t long_address = ((CV_ARRAY_FLASH[16]&long_address_mask)<<8)+CV_ARRAY_FLASH[17]; + return(long_address==read_address); } //Short Address Package else { //start of transmission -> address_byte_0 -> ... -> end of transmission read_address = (byte_array[number_of_bytes - 1]); - //printf("short address: %u, was read. \n",read_address); + return (CV_ARRAY_FLASH[0] == read_address); } - return (CV_ARRAY[0] == read_address); //returns true if Address Matches } void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) { @@ -181,14 +219,12 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) if (command_byte_n == 0b00000000){ target_speed_step = 1; update_active_functions(0,0,37); - program_mode_flag = true; - } + reset_package_flag = true; + }else reset_package_flag = false; //0011-1111 (128 Speed Step Control) - 2 Byte length if (command_byte_n == 0b00111111) { - //printf("0011-1111 (128 Speed Step Control) Instruction\n"); - uint8_t command_byte_n_minus1 = byte_array[command_byte_start_index - 1]; - if (command_byte_n_minus1 != target_speed_step) interp_speed_step(command_byte_n_minus1); + target_speed_step = byte_array[command_byte_start_index - 1]; } // 10XX-XXXX (Function Group Instruction) if (command_byte_n >> 6 == 0b00000010) @@ -234,13 +270,13 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) } set_outputs(); } + //Interrupt handler for DCC Logic Signal void gpio_callback_rise(unsigned int gpio, long unsigned int events) { writeLastBit(readBit()); int8_t number_of_bytes = check_for_package(); if (number_of_bytes != -1) { uint8_t byte_array[SIZE_BYTE_ARRAY] = {0}; - //uint8_t byte_array[SIZE_BYTE_ARRAY] = {(5^128^63),128,0b00111111,5,0}; bits_to_byte_array(number_of_bytes,byte_array); if (error_detection(number_of_bytes,byte_array)) { //Returns true if the XOR verification was error-free if (address_evaluation(number_of_bytes,byte_array)) { //Returns true if address matches with CV @@ -249,6 +285,7 @@ void gpio_callback_rise(unsigned int gpio, long unsigned int events) { } } } + void adjust_pwm_level(uint16_t level) { if(target_direction){ @@ -264,17 +301,21 @@ void adjust_pwm_level(uint16_t level) void measure(){ pwm_set_gpio_level(MOTOR_PWM_FORWARD,0); pwm_set_gpio_level(MOTOR_PWM_REVERSE,0); - busy_wait_us(100); + busy_wait_us(measurement_delay_in_us); uint sum = 0; - for (int i = 0; i < N_MEASUREMENTS; ++i) { + for (int i = 0; i < measurement_iterations; ++i) { sum = sum+adc_read(); } - measurement = (int)sum / N_MEASUREMENTS; + measurement = (int)sum / measurement_iterations; } void pid_control(){ + static float output; + static int error; + static int e_old; + static int e_sum; measure(); - error = (int)target_v_emf - (int)measurement; + error = (int)current_v_emf_target - (int)measurement; e_sum = e_sum + error; if (e_sum > sum_limit_max) e_sum = sum_limit_max; if (e_sum < sum_limit_min) e_sum = sum_limit_min; @@ -282,9 +323,23 @@ void pid_control(){ e_old = error; if (output > 5000) output = 5000; if (output < 1400) output = 1400; - if (target_v_emf == 0) output = 0; + if (current_v_emf_target == 0) output = 0; adjust_pwm_level((u_int16_t )output); - busy_wait_us(10000); + sleep_ms(measurement_period_in_ms); +} + +void init_pid(){ + k_p = (float)CV_ARRAY_FLASH[48]/1024; + k_i = (float)CV_ARRAY_FLASH[49]/1024; + k_d = (float)CV_ARRAY_FLASH[50]/16384; + int t_s_in_us = CV_ARRAY_FLASH[47]*1000+CV_ARRAY_FLASH[46]+CV_ARRAY_FLASH[45]*3; + t_s = (float)t_s_in_us/1000000; + sum_limit_max = CV_ARRAY_FLASH[51]*10; + sum_limit_min = CV_ARRAY_FLASH[52]*(-10); + measurement_delay_in_us = CV_ARRAY_FLASH[46]; + measurement_iterations = CV_ARRAY_FLASH[45]; + measurement_period_in_ms = CV_ARRAY_FLASH[47]; + printf("PID variables:\nk_p = %f;\nk_i = %f;\nk_d = %f;\nt_s = %f;\n",k_p,k_i,k_d,t_s); } void init_pwm(uint gpio) { @@ -294,14 +349,39 @@ void init_pwm(uint gpio) { pwm_set_gpio_level(gpio,0); pwm_set_clkdiv(slice_num,1); pwm_set_enabled(slice_num, true); - printf("PWM on GPIO %u was initialized.\n",gpio); + printf("PWM on GPIO %u was initialized.\n",gpio); } void init_adc(uint gpio){ adc_init(); adc_gpio_init(gpio); adc_select_input(0); - printf("ADC on GPIO %d was initialized.\n",gpio); + printf("ADC on GPIO %d was initialized.\n",gpio); +} +bool speed_helper(struct repeating_timer *t) { + uint end_v_emf_target; + //Emergency Stop + if (target_speed_step == 129 || target_speed_step==1) current_v_emf_target = 0; + else{ + //First Speed currently equals 2*32 while all the other steps are 1*32 + //Forward + if(target_speed_step>127){ + target_direction = true; + if (target_speed_step == 128) end_v_emf_target = 0; + else end_v_emf_target = (target_speed_step - 129) * 32; + if (end_v_emf_target > current_v_emf_target) current_v_emf_target+=CV_ARRAY_FLASH[2]+1/8; + if (end_v_emf_target < current_v_emf_target) current_v_emf_target-=CV_ARRAY_FLASH[3]+1/8; + } + //Reverse + if(target_speed_step<128) { + target_direction = false; + if (target_speed_step == 0) end_v_emf_target = 0; + else end_v_emf_target = (target_speed_step-1) * 32; + if (end_v_emf_target > current_v_emf_target) current_v_emf_target+=CV_ARRAY_FLASH[2]+1/8; + if (end_v_emf_target < current_v_emf_target) current_v_emf_target-=CV_ARRAY_FLASH[3]+1/8; + } + } + return true; } void core1_entry() { @@ -310,48 +390,36 @@ void core1_entry() { init_adc(V_EMF_ADC_PIN); init_pwm(MOTOR_PWM_FORWARD); init_pwm(MOTOR_PWM_REVERSE); + init_pid(); + add_repeating_timer_ms(-CV_ARRAY_FLASH[44], speed_helper, NULL, &speed_helper_timer); printf("Core 1 was initialized.\n"); while (1) pid_control(); } - -void print_buf(const uint8_t *buf, size_t len) { +void print_cv_array(const uint8_t *buf, size_t len) { for (size_t i = 0; i < len; ++i) { printf("CV_%u: %02x",i+1, buf[i]); - if (i % 8 == 7) + if (i % 4 == 3) printf("\n"); else printf(" "); } } -void write_updated_cvs_to_flash(){ - multicore_reset_core1(); - uint32_t ints = save_and_disable_interrupts(); - printf("Erasing CV's...\n"); - flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE*2); - printf("Erasing Done.\n"); - printf("Programming CV's...\n"); - flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY, FLASH_PAGE_SIZE * 2); - printf("Programming Done. Read back CV's:\n"); - print_buf(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2); - restore_interrupts(ints); - multicore_launch_core1(core1_entry); -} - int main() { sleep_ms(1000); stdio_init_all(); gpio_init(DCC_INPUT_PIN); gpio_set_dir(DCC_INPUT_PIN, GPIO_IN); - multicore_launch_core1(core1_entry); + gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); + gpio_put(PICO_DEFAULT_LED_PIN, true); busy_wait_ms(1000); //This delay is necessary to catch the breakpoint gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &gpio_callback_rise); busy_wait_ms(200); //This delay is necessary to catch the breakpoint - //gpio_callback_rise(0,0); - gpio_init(PICO_DEFAULT_LED_PIN); - gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - gpio_put(PICO_DEFAULT_LED_PIN, true); - while (1); -} + printf("Core 0 was initialized.\n"); + printf("CV_ARRAY read from flash:\n"); + print_cv_array(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2); + multicore_launch_core1(core1_entry); + while (1); +} \ No newline at end of file diff --git a/multicore.h b/multicore.h index 2206428..c8d6376 100644 --- a/multicore.h +++ b/multicore.h @@ -15,7 +15,6 @@ #define MOTOR_PWM_FORWARD 21 #define MOTOR_PWM_REVERSE 22 #define V_EMF_ADC_PIN 26 -#define N_MEASUREMENTS 50 #define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001 #define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001 #define PACKAGE_4_BYTES 0b11111111110000000000000000000000000000000000001 @@ -23,4 +22,5 @@ #define PACKAGE_5_BYTES 0b11111111110000000000000000000000000000000000000000000001 #define PACKAGE_MASK_5_BYTES 0b11111111111000000001000000001000000001000000001000000001 #define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE*2) -void pid_control(); \ No newline at end of file +void pid_control(); +void core1_entry(); \ No newline at end of file