Implemented Basic Programming features, changed speed control (speed_helper function with timed interrupt)
This commit is contained in:
1 parent
df9a1848e4
commit
5a045b5fcf
3 files changed
+428
-344
No files matched your search
@@ -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
|
||||
};
|
||||
+140
-72
@@ -1,4 +1,5 @@
|
||||
#include <stdio.h>
|
||||
#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);
|
||||
}
|
||||
+2
-2
@@ -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();
|
||||
void pid_control();
|
||||
void core1_entry();
|
||||
Reference in new issue
Block a user