- added shared.c file containing functions that can be accessed from both cores
- revised CV programming especially writing - Core 1 is only started when valid package with correct address is received - Extended addresses are now correctly verified and implemented - Base PWM measurement is now done on core0 and checked for automatically in cv_setup_check() - Moved Motor PWM, GPIO, ADC to core0 - Implemented inverted direction mode (CV_29) according to DCC-Standards - Limited feed-forward parameters y_2 to max_output/y_1 - k_p gain slopes are now calculated on initialization - k_p gain parameters are also in pid parameter struct now - Revised init_pid() - Implemented Packet timeout check (CV_11) - 16Bit and 32Bit CVs now always have the highest bytes as the lowest array index to accommodate get_XXbit_CV() functions
This commit is contained in:
1 parent
f09c4ff136
commit
fd017ea685
8 files changed
+820
-601
No files matched your search
@@ -24,6 +24,7 @@ add_executable(
|
||||
core0.h
|
||||
core1.c
|
||||
core1.h
|
||||
shared.c
|
||||
shared.h)
|
||||
|
||||
# Multiplier to lengthen XOSC startup delay to accommodate slow-starting oscillator
|
||||
|
||||
@@ -1,16 +1,16 @@
|
||||
#pragma once
|
||||
#define CV_ARRAY_SIZE 512
|
||||
//All CVs are 8-bit numbers (range 0 - 255)
|
||||
//Note : CV_1 ≙ index 0, CV_2 ≙ index 1, CV_3 ≙ index 2, ...
|
||||
//TODO:
|
||||
// - Packet Timeout
|
||||
// All CVs are 8-bit numbers (range 0 - 255)
|
||||
// Note : CV_1 ≙ index 0, CV_2 ≙ index 1, CV_3 ≙ index 2, ...
|
||||
// TODO: - Packet Timeout
|
||||
|
||||
uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = {
|
||||
0b00000011, //CV_1 - Basic address CV_1 = 0 is not allowed and used for initiating ADC offset adjustment //TODO: verify
|
||||
0b00001000, //CV_2 - V_min
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// 0 == Fastest dec/acc rate; 255 == slowest;
|
||||
0b00000000, //CV_3 - Acceleration rate - CV_3*CV_59 = Time for one discrete speed step change in ms
|
||||
0b00000000, //CV_4 - Deceleration rate - CV_4*CV_59 = Time for one discrete speed step change in ms
|
||||
0b00000000, //CV_3 - Acceleration rate - CV_3*CV_175 = Time for one discrete speed step change in ms
|
||||
0b00000000, //CV_4 - Deceleration rate - CV_4*CV_175 = Time for one discrete speed step change in ms
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b01111111, //CV_5 - V_max
|
||||
0b00111111, //CV_6 - V_mid
|
||||
@@ -59,32 +59,30 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = {
|
||||
0b00000000, //CV_44 -
|
||||
0b00000000, //CV_45 -
|
||||
0b00000000, //CV_46 -
|
||||
// PID - Configuration //////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b00001010, //CV_47 - PID Control low pass filter time constant (tau) //
|
||||
0b00000101, //CV_48 - PID Control sampling time t in ms for speed step > 2 //
|
||||
0b01110011, //CV_49 - PID Control P_Factor ≙ CV_49/256 Default = 115 -> 0.4492 //
|
||||
0b01100110, //CV_50 - PID Control I_Factor ≙ CV_50/64 Default = 102 -> 1.594 (1/sec)//
|
||||
0b01111011, //CV_51 - PID Control D_Factor ≙ CV_51/4096 Default = 123 -> 0.03002 sec //
|
||||
0b00110000, //CV_52 - PID Integral Limit positive ≙ CV_52*10 Default = 48 -> +480 //
|
||||
0b00110000, //CV_53 - PID Integral Limit negative ≙ CV_53*(-10) Default = 48 -> -480 //
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b00000001, //CV_54 - --------------------------------------------------------------- //
|
||||
0b00000000, //CV_55 - --------------------------------------------------------------- //
|
||||
0b00000000, //CV_56 - --------------------------------------------------------------- //
|
||||
0b00000000, //CV_57 - --------------------------------------------------------------- //
|
||||
0b00000101, //CV_58 - --------------------------------------------------------------- //
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b00000111, //CV_59 - speed_helper timer delay - can be used to adjust accel/decel rate //
|
||||
0b00010100, //CV_60 - ..................................................................... //
|
||||
// Speed controller - Configuration ////////////////////////////////////////////////////////////////////////////////
|
||||
0b00110010, //CV_47 - Feed-forward gain k_ff ≙ CV_47/10000 Default = 50 -> 0.005 //
|
||||
0b00001010, //CV_48 - PID Control low pass filter time constant (tau) in ms //
|
||||
0b00000101, //CV_49 - PID Control sampling time t in ms //
|
||||
0b00011001, //CV_50 - PID Control I_Factor ≙ CV_50/10 Default = 25 -> 2.5 //
|
||||
0b00110010, //CV_51 - PID Control D_Factor ≙ CV_51/10000 Default = 50 -> 0.005 //
|
||||
0b01100100, //CV_52 - PID Integral Limit positive ≙ CV_52*10 Default = 100 -> +1000 //
|
||||
0b01100100, //CV_53 - PID Integral Limit negative ≙ CV_53*(-10) Default = 100 -> -1000 //
|
||||
0b00000011, //CV_54 - k_p @ x0 - High Byte (Bits 8-15) divided by 100 //
|
||||
0b11101000, //CV_55 - k_p @ x0 - Low Byte (Bits 0-7) Default = 1000 -> 10 //
|
||||
0b00000000, //CV_56 - k_p @ x1 - High Byte (Bits 8-15) divided by 100 //
|
||||
0b11111010, //CV_57 - k_p @ x1 - Low Byte (Bits 0-7) Default = 250 -> 2.5 //
|
||||
0b00000000, //CV_58 - k_p @ x2 - High Byte (Bits 8-15) divided by 100 //
|
||||
0b10010110, //CV_59 - k_p @ x2 - Low Byte (Bits 0-7) Default = 150 -> 1.5 //
|
||||
0b00001101, //CV_60 - x_1 shift in % ≙ CV_60/255 //
|
||||
// V_EMF Measurement Configuration //////////////////////////////////////////////////////////////////////////////////
|
||||
0b01000110, //CV_61 - V_EMF Total amount of Measurement Iterations - Default = 100 iterations //
|
||||
0b01100100, //CV_61 - V_EMF Total amount of Measurement Iterations - Default = 100 iterations //
|
||||
0b01100100, //CV_62 - V_EMF Delay before Measuring - Default = 100us //
|
||||
// CV_63 and CV_64 are used to determine how many elements of the sorted measurement array will be discarded //
|
||||
// For Example: CV_63 = x and CV_64 = y //
|
||||
// -> The x smallest values will be discarded. //
|
||||
// -> The y largest values will be discarded. //
|
||||
0b00001111, //CV_63 - l_side_arr_cutoff - Default = 15 //
|
||||
0b00001111, //CV_64 - r_side_arr_cutoff - Default = 15 // //
|
||||
0b00001111, //CV_63 - l_side_arr_cutoff - Default = 15 //
|
||||
0b00001111, //CV_64 - r_side_arr_cutoff - Default = 15 //
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b00000000, //CV_65 - Values > 0 indicate that flash is still in factory condition
|
||||
0b00000000, //CV_66 -
|
||||
@@ -135,74 +133,74 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = {
|
||||
0b00000000, //CV_111 -
|
||||
///// PWM - Configuration //////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// This Config can cause conflicts when GPIOs controlled by the same slice are enabled at the same time.
|
||||
0b00000000, //CV_112 - GPIO 0-7 enable PWM
|
||||
0b00111100, //CV_113 - GPIO 8-15 enable PWM
|
||||
0b00000000, //CV_114 - GPIO 16-23 enable PWM
|
||||
0b00000000, //CV_115 - GPIO 24-31 enable PWM
|
||||
0b00000000, //CV_112 - GPIO 24-31 enable PWM
|
||||
0b00000000, //CV_113 - GPIO 16-23 enable PWM
|
||||
0b00111100, //CV_114 - GPIO 8-15 enable PWM
|
||||
0b00000000, //CV_115 - GPIO 0-7 enable PWM
|
||||
//Slice 0
|
||||
0b00000000, //CV_116 - Wrap Bits 0-7
|
||||
0b00000000, //CV_117 - Wrap Bits 8-15
|
||||
0b00000001, //CV_118 - Clock Divider
|
||||
0b00000000, //CV_119 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_120 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_121 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_122 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_116 - Wrap Bits 8-15
|
||||
0b00000000, //CV_117 - Wrap Bits 0-7
|
||||
0b00000000, //CV_118 - Clock Divider
|
||||
0b00000000, //CV_119 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_120 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_121 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_122 - Channel B Level Bits 0-7
|
||||
//Slice 1
|
||||
0b00000000, //CV_123 - Wrap Bits 0-7
|
||||
0b00000000, //CV_124 - Wrap Bits 8-15
|
||||
0b00000001, //CV_125 - Clock Divider
|
||||
0b00000000, //CV_126 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_127 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_128 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_129 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_123 - Wrap Bits 8-15
|
||||
0b00000000, //CV_124 - Wrap Bits 0-7
|
||||
0b00000000, //CV_125 - Clock Divider
|
||||
0b00000000, //CV_126 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_127 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_128 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_129 - Channel B Level Bits 0-7
|
||||
//Slice 2
|
||||
0b00000000, //CV_130 - Wrap Bits 0-7
|
||||
0b00000000, //CV_131 - Wrap Bits 8-15
|
||||
0b00000001, //CV_132 - Clock Divider
|
||||
0b00000000, //CV_133 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_134 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_135 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_136 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_130 - Wrap Bits 8-15
|
||||
0b00000000, //CV_131 - Wrap Bits 0-7
|
||||
0b00000000, //CV_132 - Clock Divider
|
||||
0b00000000, //CV_133 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_134 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_135 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_136 - Channel B Level Bits 0-7
|
||||
//Slice 3
|
||||
0b00000000, //CV_137 - Wrap Bits 0-7
|
||||
0b00000000, //CV_138 - Wrap Bits 8-15
|
||||
0b00000001, //CV_139 - Clock Divider
|
||||
0b00000000, //CV_140 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_141 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_142 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_143 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_137 - Wrap Bits 8-15
|
||||
0b00000000, //CV_138 - Wrap Bits 0-7
|
||||
0b00000000, //CV_139 - Clock Divider
|
||||
0b00000000, //CV_140 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_141 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_142 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_143 - Channel B Level Bits 0-7
|
||||
//Slice 4
|
||||
0b00000000, //CV_144 - Wrap Bits 0-7
|
||||
0b00000000, //CV_145 - Wrap Bits 8-15
|
||||
0b00000001, //CV_146 - Clock Divider
|
||||
0b00000000, //CV_147 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_148 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_149 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_150 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_144 - Wrap Bits 8-15
|
||||
0b00000000, //CV_145 - Wrap Bits 0-7
|
||||
0b00000000, //CV_146 - Clock Divider
|
||||
0b00000000, //CV_147 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_148 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_149 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_150 - Channel B Level Bits 0-7
|
||||
//Slice 5
|
||||
0b10001000, //CV_151 - Wrap Bits 0-7
|
||||
0b00010011, //CV_152 - Wrap Bits 8-15
|
||||
0b00000001, //CV_153 - Clock Divider
|
||||
0b11000100, //CV_154 - Channel A Level Bits 0-7
|
||||
0b00001001, //CV_155 - Channel A Level Bits 8-15
|
||||
0b11101000, //CV_156 - Channel B Level Bits 0-7
|
||||
0b00000011, //CV_157 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_151 - Wrap Bits 8-15
|
||||
0b00000000, //CV_152 - Wrap Bits 0-7
|
||||
0b00000000, //CV_153 - Clock Divider
|
||||
0b00000000, //CV_154 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_155 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_156 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_157 - Channel B Level Bits 0-7
|
||||
//Slice 6
|
||||
0b11000100, //CV_158 - Wrap Bits 0-7
|
||||
0b00001001, //CV_159 - Wrap Bits 8-15
|
||||
0b00000001, //CV_160 - Clock Divider
|
||||
0b11100010, //CV_161 - Channel A Level Bits 0-7
|
||||
0b00000100, //CV_162 - Channel A Level Bits 8-15
|
||||
0b11100010, //CV_163 - Channel B Level Bits 0-7
|
||||
0b00000100, //CV_164 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_158 - Wrap Bits 8-15
|
||||
0b00000000, //CV_159 - Wrap Bits 0-7
|
||||
0b00000000, //CV_160 - Clock Divider
|
||||
0b00000000, //CV_161 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_162 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_163 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_164 - Channel B Level Bits 0-7
|
||||
//Slice 7
|
||||
0b00000000, //CV_165 - Wrap Bits 0-7
|
||||
0b00000000, //CV_166 - Wrap Bits 8-15
|
||||
0b00000001, //CV_167 - Clock Divider
|
||||
0b00000000, //CV_168 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_169 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_170 - Channel B Level Bits 0-7
|
||||
0b00000000, //CV_171 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_165 - Wrap Bits 8-15
|
||||
0b00000000, //CV_166 - Wrap Bits 0-7
|
||||
0b00000000, //CV_167 - Clock Divider
|
||||
0b00000000, //CV_168 - Channel A Level Bits 8-15
|
||||
0b00000000, //CV_169 - Channel A Level Bits 0-7
|
||||
0b00000000, //CV_170 - Channel B Level Bits 8-15
|
||||
0b00000000, //CV_171 - Channel B Level Bits 0-7
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//Offset adjustment for ADC //
|
||||
//If CV_172 = 0xFF = 255 there will be a measurement procedure on startup //
|
||||
@@ -211,11 +209,11 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = {
|
||||
0b01111111, //CV_173 - ADC offset measurement cycles Default: 127 //
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
0b00000001, //CV_174 - Additional motor-PWM clock divider.
|
||||
0b00000000, //CV_175 -
|
||||
0b00000000, //CV_176 -
|
||||
0b00000000, //CV_177 -
|
||||
0b00000000, //CV_178 -
|
||||
0b00000000, //CV_179 -
|
||||
0b00000111, //CV_175 - speed_helper timer delay - can be used to adjust accel/decel rate
|
||||
0b00000000, //CV_176 - Base-PWM forward bits 8-15
|
||||
0b00000000, //CV_177 - Base-PWM forward bits 0-7
|
||||
0b00000000, //CV_178 - Base-PWM reverse bits 15-8
|
||||
0b00000000, //CV_179 - Base-PWM reverse bits 0-7
|
||||
0b00000000, //CV_180 -
|
||||
0b00000000, //CV_181 -
|
||||
0b00000000, //CV_182 -
|
||||
@@ -298,323 +296,323 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = {
|
||||
// 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
|
||||
0b00000000, //byte 1 - CV_258
|
||||
0b00000000, //byte 2 - CV_259
|
||||
0b00000000, //byte 3 - CV_260
|
||||
0b00000000, //CV_257 - byte 3
|
||||
0b00000000, //CV_258 - byte 2
|
||||
0b00000000, //CV_259 - byte 1
|
||||
0b00000000, //CV_260 - byte 0
|
||||
//F0 reverse
|
||||
0b00000000, //byte 0 - CV_261
|
||||
0b00000000, //byte 1 - CV_262
|
||||
0b00000000, //byte 2 - CV_263
|
||||
0b00000000, //byte 3 - CV_264
|
||||
0b00000000, //CV_261 - byte 3
|
||||
0b00000000, //CV_262 - byte 2
|
||||
0b00000000, //CV_263 - byte 1
|
||||
0b00000000, //CV_264 - byte 0
|
||||
//F1 forward
|
||||
0b00000000, //byte 0 - CV_265
|
||||
0b00000000, //byte 1 - CV_266
|
||||
0b00000000, //byte 2 - CV_267
|
||||
0b00000000, //byte 3 - CV_268
|
||||
0b00000000, //CV_265 - byte 3
|
||||
0b00000000, //CV_266 - byte 2
|
||||
0b00000000, //CV_267 - byte 1
|
||||
0b00000000, //CV_268 - byte 0
|
||||
//F1 reverse
|
||||
0b00000000, //byte 0 - CV_269
|
||||
0b00000000, //byte 1 - CV_270
|
||||
0b00000000, //byte 2 - CV_271
|
||||
0b00000000, //byte 3 - CV_272
|
||||
0b00000000, //CV_269 - byte 3
|
||||
0b00000000, //CV_270 - byte 2
|
||||
0b00000000, //CV_271 - byte 1
|
||||
0b00000000, //CV_272 - byte 0
|
||||
//F2 forward
|
||||
0b00000000, //byte 0 - CV_273
|
||||
0b00000000, //byte 1 - CV_274
|
||||
0b00000000, //byte 2 - CV_275
|
||||
0b00000000, //byte 3 - CV_276
|
||||
0b00000000, //CV_273 - byte 3
|
||||
0b00000000, //CV_274 - byte 2
|
||||
0b00000000, //CV_275 - byte 1
|
||||
0b00000000, //CV_276 - byte 0
|
||||
//F2 reverse
|
||||
0b00000000, //byte 0 - CV_277
|
||||
0b00000000, //byte 1 - CV_278
|
||||
0b00000000, //byte 2 - CV_279
|
||||
0b00000000, //byte 3 - CV_280
|
||||
0b00000000, //CV_277 - byte 3
|
||||
0b00000000, //CV_278 - byte 2
|
||||
0b00000000, //CV_279 - byte 1
|
||||
0b00000000, //CV_280 - byte 0
|
||||
//F3 forward
|
||||
0b00000000, //byte 0 - CV_281
|
||||
0b00000000, //byte 1 - CV_282
|
||||
0b00000000, //byte 2 - CV_283
|
||||
0b00000000, //byte 3 - CV_284
|
||||
0b00000000, //CV_281 - byte 3
|
||||
0b00000000, //CV_282 - byte 2
|
||||
0b00000000, //CV_283 - byte 1
|
||||
0b00000000, //CV_284 - byte 0
|
||||
//F3 reverse
|
||||
0b00000000, //byte 0 - CV_285
|
||||
0b00000000, //byte 1 - CV_286
|
||||
0b00000000, //byte 2 - CV_287
|
||||
0b00000000, //byte 3 - CV_288
|
||||
0b00000000, //CV_285 - byte 3
|
||||
0b00000000, //CV_286 - byte 2
|
||||
0b00000000, //CV_287 - byte 1
|
||||
0b00000000, //CV_288 - byte 0
|
||||
//F4 forward
|
||||
0b00000000, //byte 0 - CV_289
|
||||
0b00000000, //byte 1 - CV_290
|
||||
0b00000000, //byte 2 - CV_291
|
||||
0b00000000, //byte 3 - CV_292
|
||||
0b00000000, //CV_289 - byte 3
|
||||
0b00000000, //CV_290 - byte 2
|
||||
0b00000000, //CV_291 - byte 1
|
||||
0b00000000, //CV_292 - byte 0
|
||||
//F4 reverse
|
||||
0b00000000, //byte 0 - CV_293
|
||||
0b00000000, //byte 1 - CV_294
|
||||
0b00000000, //byte 2 - CV_295
|
||||
0b00000000, //byte 3 - CV_296
|
||||
0b00000000, //CV_293 - byte 3
|
||||
0b00000000, //CV_294 - byte 2
|
||||
0b00000000, //CV_295 - byte 1
|
||||
0b00000000, //CV_296 - byte 0
|
||||
//F5 forward
|
||||
0b00000000, //byte 0 - CV_297
|
||||
0b00000000, //byte 1 - CV_298
|
||||
0b00000000, //byte 2 - CV_299
|
||||
0b00000000, //byte 3 - CV_300
|
||||
0b00000000, //CV_297 - byte 3
|
||||
0b00000000, //CV_298 - byte 2
|
||||
0b00000000, //CV_299 - byte 1
|
||||
0b00000000, //CV_300 - byte 0
|
||||
//F5 reverse
|
||||
0b00000000, //byte 0 - CV_301
|
||||
0b00000000, //byte 1 - CV_302
|
||||
0b00000000, //byte 2 - CV_303
|
||||
0b00000000, //byte 3 - CV_304
|
||||
0b00000000, //CV_301 - byte 3
|
||||
0b00000000, //CV_302 - byte 2
|
||||
0b00000000, //CV_303 - byte 1
|
||||
0b00000000, //CV_304 - byte 0
|
||||
//F6 forward
|
||||
0b00000000, //byte 0 - CV_305
|
||||
0b00000000, //byte 1 - CV_306
|
||||
0b00000000, //byte 2 - CV_307
|
||||
0b00000000, //byte 3 - CV_308
|
||||
0b00000000, //CV_305 - byte 3
|
||||
0b00000000, //CV_306 - byte 2
|
||||
0b00000000, //CV_307 - byte 1
|
||||
0b00000000, //CV_308 - byte 0
|
||||
//F6 reverse
|
||||
0b00000000, //byte 0 - CV_309
|
||||
0b00000000, //byte 1 - CV_310
|
||||
0b00000000, //byte 2 - CV_311
|
||||
0b00000000, //byte 3 - CV_312
|
||||
0b00000000, //CV_309 - byte 3
|
||||
0b00000000, //CV_310 - byte 2
|
||||
0b00000000, //CV_311 - byte 1
|
||||
0b00000000, //CV_312 - byte 0
|
||||
//F7 forward
|
||||
0b00000000, //byte 0 - CV_313
|
||||
0b00000000, //byte 1 - CV_314
|
||||
0b00000000, //byte 2 - CV_315
|
||||
0b00000000, //byte 3 - CV_316
|
||||
0b00000000, //CV_313 - byte 3
|
||||
0b00000000, //CV_314 - byte 2
|
||||
0b00000000, //CV_315 - byte 1
|
||||
0b00000000, //CV_316 - byte 0
|
||||
//F7 reverse
|
||||
0b00000000, //byte 0 - CV_317
|
||||
0b00000000, //byte 1 - CV_318
|
||||
0b00000000, //byte 2 - CV_319
|
||||
0b00000000, //byte 3 - CV_320
|
||||
0b00000000, //CV_317 - byte 3
|
||||
0b00000000, //CV_318 - byte 2
|
||||
0b00000000, //CV_319 - byte 1
|
||||
0b00000000, //CV_320 - byte 0
|
||||
//F8 forward
|
||||
0b00000000, //byte 0 - CV_321
|
||||
0b00000000, //byte 1 - CV_322
|
||||
0b00000000, //byte 2 - CV_323
|
||||
0b00000000, //byte 3 - CV_324
|
||||
0b00000000, //CV_321 - byte 3
|
||||
0b00000000, //CV_322 - byte 2
|
||||
0b00000000, //CV_323 - byte 1
|
||||
0b00000000, //CV_324 - byte 0
|
||||
//F8 reverse
|
||||
0b00000000, //byte 0 - CV_325
|
||||
0b00000000, //byte 1 - CV_326
|
||||
0b00000000, //byte 2 - CV_327
|
||||
0b00000000, //byte 3 - CV_328
|
||||
0b00000000, //CV_325 - byte 3
|
||||
0b00000000, //CV_326 - byte 2
|
||||
0b00000000, //CV_327 - byte 1
|
||||
0b00000000, //CV_328 - byte 0
|
||||
//F9 forward
|
||||
0b00000000, //byte 0 - CV_329
|
||||
0b00000000, //byte 1 - CV_330
|
||||
0b00000000, //byte 2 - CV_331
|
||||
0b00000000, //byte 3 - CV_332
|
||||
0b00000000, //CV_329 - byte 3
|
||||
0b00000000, //CV_330 - byte 2
|
||||
0b00000000, //CV_331 - byte 1
|
||||
0b00000000, //CV_332 - byte 0
|
||||
//F9 reverse
|
||||
0b00000000, //byte 0 - CV_333
|
||||
0b00000000, //byte 1 - CV_334
|
||||
0b00000000, //byte 2 - CV_335
|
||||
0b00000000, //byte 3 - CV_336
|
||||
0b00000000, //CV_333 - byte 3
|
||||
0b00000000, //CV_334 - byte 2
|
||||
0b00000000, //CV_335 - byte 1
|
||||
0b00000000, //CV_336 - byte 0
|
||||
//F10 forward
|
||||
0b00000000, //byte 0 - CV_337
|
||||
0b00000100, //byte 1 - CV_338
|
||||
0b00000000, //byte 2 - CV_339
|
||||
0b00000000, //byte 3 - CV_340
|
||||
0b00000000, //CV_337 - byte 3
|
||||
0b00000000, //CV_338 - byte 2
|
||||
0b00000000, //CV_339 - byte 1
|
||||
0b00000000, //CV_340 - byte 0
|
||||
//F10 reverse
|
||||
0b00000000, //byte 0 - CV_341
|
||||
0b00000100, //byte 1 - CV_342
|
||||
0b00000000, //byte 2 - CV_343
|
||||
0b00000000, //byte 3 - CV_344
|
||||
0b00000000, //CV_341 - byte 3
|
||||
0b00000000, //CV_342 - byte 2
|
||||
0b00000000, //CV_343 - byte 1
|
||||
0b00000000, //CV_344 - byte 0
|
||||
//F11 forward
|
||||
0b00000000, //byte 0 - CV_345
|
||||
0b00111100, //byte 1 - CV_346
|
||||
0b00000000, //byte 2 - CV_347
|
||||
0b00000000, //byte 3 - CV_348
|
||||
0b00000000, //CV_345 - byte 3
|
||||
0b00000000, //CV_346 - byte 2
|
||||
0b00000000, //CV_347 - byte 1
|
||||
0b00000000, //CV_348 - byte 0
|
||||
//F11 reverse
|
||||
0b00000000, //byte 0 - CV_349
|
||||
0b00000000, //byte 1 - CV_350
|
||||
0b00000000, //byte 2 - CV_351
|
||||
0b00000000, //byte 3 - CV_352
|
||||
0b00000000, //CV_349 - byte 3
|
||||
0b00000000, //CV_350 - byte 2
|
||||
0b00000000, //CV_351 - byte 1
|
||||
0b00000000, //CV_352 - byte 0
|
||||
//F12 forward
|
||||
0b00000000, //byte 0 - CV_353
|
||||
0b00000000, //byte 1 - CV_354
|
||||
0b00000000, //byte 2 - CV_355
|
||||
0b00000000, //byte 3 - CV_356
|
||||
0b00000000, //CV_353 - byte 3
|
||||
0b00000000, //CV_354 - byte 2
|
||||
0b00000000, //CV_355 - byte 1
|
||||
0b00000000, //CV_356 - byte 0
|
||||
//F12 reverse
|
||||
0b00000000, //byte 0 - CV_357
|
||||
0b00000000, //byte 1 - CV_358
|
||||
0b00000000, //byte 2 - CV_359
|
||||
0b00000000, //byte 3 - CV_360
|
||||
0b00000000, //CV_357 - byte 3
|
||||
0b00000000, //CV_358 - byte 2
|
||||
0b00000000, //CV_359 - byte 1
|
||||
0b00000000, //CV_360 - byte 0
|
||||
//F13 forward
|
||||
0b00000000, //byte 0 - CV_361
|
||||
0b00000000, //byte 1 - CV_362
|
||||
0b00000000, //byte 2 - CV_363
|
||||
0b00000000, //byte 3 - CV_364
|
||||
0b00000000, //CV_361 - byte 3
|
||||
0b00000000, //CV_362 - byte 2
|
||||
0b00000000, //CV_363 - byte 1
|
||||
0b00000000, //CV_364 - byte 0
|
||||
//F13 reverse
|
||||
0b00000000, //byte 0 - CV_365
|
||||
0b00000000, //byte 1 - CV_366
|
||||
0b00000000, //byte 2 - CV_367
|
||||
0b00000000, //byte 3 - CV_368
|
||||
0b00000000, //CV_365 - byte 3
|
||||
0b00000000, //CV_366 - byte 2
|
||||
0b00000000, //CV_367 - byte 1
|
||||
0b00000000, //CV_368 - byte 0
|
||||
//F14 forward
|
||||
0b00000000, //byte 0 - CV_369
|
||||
0b00000000, //byte 1 - CV_370
|
||||
0b00000000, //byte 2 - CV_371
|
||||
0b00000000, //byte 3 - CV_372
|
||||
0b00000000, //CV_369 - byte 3
|
||||
0b00000000, //CV_370 - byte 2
|
||||
0b00000000, //CV_371 - byte 1
|
||||
0b00000000, //CV_372 - byte 0
|
||||
//F14 reverse
|
||||
0b00000000, //byte 0 - CV_373
|
||||
0b00000000, //byte 1 - CV_374
|
||||
0b00000000, //byte 2 - CV_375
|
||||
0b00000000, //byte 3 - CV_376
|
||||
0b00000000, //CV_373 - byte 3
|
||||
0b00000000, //CV_374 - byte 2
|
||||
0b00000000, //CV_375 - byte 1
|
||||
0b00000000, //CV_376 - byte 0
|
||||
//F15 forward
|
||||
0b00000000, //byte 0 - CV_377
|
||||
0b00000000, //byte 1 - CV_378
|
||||
0b00000000, //byte 2 - CV_379
|
||||
0b00000000, //byte 3 - CV_380
|
||||
0b00000000, //CV_377 - byte 3
|
||||
0b00000000, //CV_378 - byte 2
|
||||
0b00000000, //CV_379 - byte 1
|
||||
0b00000000, //CV_380 - byte 0
|
||||
//F15 reverse
|
||||
0b00000000, //byte 0 - CV_381
|
||||
0b00000000, //byte 1 - CV_382
|
||||
0b00000000, //byte 2 - CV_383
|
||||
0b00000000, //byte 3 - CV_384
|
||||
0b00000000, //CV_381 - byte 3
|
||||
0b00000000, //CV_382 - byte 2
|
||||
0b00000000, //CV_383 - byte 1
|
||||
0b00000000, //CV_384 - byte 0
|
||||
//F16 forward
|
||||
0b00000000, //byte 0 - CV_385
|
||||
0b00000000, //byte 1 - CV_386
|
||||
0b00000000, //byte 2 - CV_387
|
||||
0b00000000, //byte 3 - CV_388
|
||||
0b00000000, //CV_385 - byte 3
|
||||
0b00000000, //CV_386 - byte 2
|
||||
0b00000000, //CV_387 - byte 1
|
||||
0b00000000, //CV_388 - byte 0
|
||||
//F16 reverse
|
||||
0b00000000, //byte 0 - CV_389
|
||||
0b00000000, //byte 1 - CV_390
|
||||
0b00000000, //byte 2 - CV_391
|
||||
0b00000000, //byte 3 - CV_392
|
||||
0b00000000, //CV_389 - byte 3
|
||||
0b00000000, //CV_390 - byte 2
|
||||
0b00000000, //CV_391 - byte 1
|
||||
0b00000000, //CV_392 - byte 0
|
||||
//F17 forward
|
||||
0b00000000, //byte 0 - CV_393
|
||||
0b00000000, //byte 1 - CV_394
|
||||
0b00000000, //byte 2 - CV_395
|
||||
0b00000000, //byte 3 - CV_396
|
||||
0b00000000, //CV_393 - byte 3
|
||||
0b00000000, //CV_394 - byte 2
|
||||
0b00000000, //CV_395 - byte 1
|
||||
0b00000000, //CV_396 - byte 0
|
||||
//F17 reverse
|
||||
0b00000000, //byte 0 - CV_397
|
||||
0b00000000, //byte 1 - CV_398
|
||||
0b00000000, //byte 2 - CV_399
|
||||
0b00000000, //byte 3 - CV_400
|
||||
0b00000000, //CV_397 - byte 3
|
||||
0b00000000, //CV_398 - byte 2
|
||||
0b00000000, //CV_399 - byte 1
|
||||
0b00000000, //CV_400 - byte 0
|
||||
//F18 forward
|
||||
0b00000000, //byte 0 - CV_401
|
||||
0b00000000, //byte 1 - CV_402
|
||||
0b00000000, //byte 2 - CV_403
|
||||
0b00000000, //byte 3 - CV_404
|
||||
0b00000000, //CV_401 - byte 3
|
||||
0b00000000, //CV_402 - byte 2
|
||||
0b00000000, //CV_403 - byte 1
|
||||
0b00000000, //CV_404 - byte 0
|
||||
//F18 reverse
|
||||
0b00000000, //byte 0 - CV_405
|
||||
0b00000000, //byte 1 - CV_406
|
||||
0b00000000, //byte 2 - CV_407
|
||||
0b00000000, //byte 3 - CV_408
|
||||
0b00000000, //CV_405 - byte 3
|
||||
0b00000000, //CV_406 - byte 2
|
||||
0b00000000, //CV_407 - byte 1
|
||||
0b00000000, //CV_408 - byte 0
|
||||
//F19 forward
|
||||
0b00000000, //byte 0 - CV_409
|
||||
0b00000000, //byte 1 - CV_410
|
||||
0b00000000, //byte 2 - CV_411
|
||||
0b00000000, //byte 3 - CV_412
|
||||
0b00000000, //CV_409 - byte 3
|
||||
0b00000000, //CV_410 - byte 2
|
||||
0b00000000, //CV_411 - byte 1
|
||||
0b00000000, //CV_412 - byte 0
|
||||
//F19 reverse
|
||||
0b00000000, //byte 0 - CV_413
|
||||
0b00000000, //byte 1 - CV_414
|
||||
0b00000000, //byte 2 - CV_415
|
||||
0b00000000, //byte 3 - CV_416
|
||||
0b00000000, //CV_413 - byte 3
|
||||
0b00000000, //CV_414 - byte 2
|
||||
0b00000000, //CV_415 - byte 1
|
||||
0b00000000, //CV_416 - byte 0
|
||||
//F20 forward
|
||||
0b00000000, //byte 0 - CV_417
|
||||
0b00000000, //byte 1 - CV_418
|
||||
0b00000000, //byte 2 - CV_419
|
||||
0b00000000, //byte 3 - CV_420
|
||||
0b00000000, //CV_417 - byte 3
|
||||
0b00000000, //CV_418 - byte 2
|
||||
0b00000000, //CV_419 - byte 1
|
||||
0b00000000, //CV_420 - byte 0
|
||||
//F20 reverse
|
||||
0b00000000, //byte 0 - CV_421
|
||||
0b00000000, //byte 1 - CV_422
|
||||
0b00000000, //byte 2 - CV_423
|
||||
0b00000000, //byte 3 - CV_424
|
||||
0b00000000, //CV_421 - byte 3
|
||||
0b00000000, //CV_422 - byte 2
|
||||
0b00000000, //CV_423 - byte 1
|
||||
0b00000000, //CV_424 - byte 0
|
||||
//F21 forward
|
||||
0b00000000, //byte 0 - CV_425
|
||||
0b00000000, //byte 1 - CV_426
|
||||
0b00000000, //byte 2 - CV_427
|
||||
0b00000000, //byte 3 - CV_428
|
||||
0b00000000, //CV_425 - byte 3
|
||||
0b00000000, //CV_426 - byte 2
|
||||
0b00000000, //CV_427 - byte 1
|
||||
0b00000000, //CV_428 - byte 0
|
||||
//F21 reverse
|
||||
0b00000000, //byte 0 - CV_429
|
||||
0b00000000, //byte 1 - CV_430
|
||||
0b00000000, //byte 2 - CV_431
|
||||
0b00000000, //byte 3 - CV_432
|
||||
0b00000000, //CV_429 - byte 3
|
||||
0b00000000, //CV_430 - byte 2
|
||||
0b00000000, //CV_431 - byte 1
|
||||
0b00000000, //CV_432 - byte 0
|
||||
//F22 forward
|
||||
0b00000000, //byte 0 - CV_433
|
||||
0b00000000, //byte 1 - CV_434
|
||||
0b00000000, //byte 2 - CV_435
|
||||
0b00000000, //byte 3 - CV_436
|
||||
0b00000000, //CV_433 - byte 3
|
||||
0b00000000, //CV_434 - byte 2
|
||||
0b00000000, //CV_435 - byte 1
|
||||
0b00000000, //CV_436 - byte 0
|
||||
//F22 reverse
|
||||
0b00000000, //byte 0 - CV_437
|
||||
0b00000000, //byte 1 - CV_438
|
||||
0b00000000, //byte 2 - CV_439
|
||||
0b00000000, //byte 3 - CV_440
|
||||
0b00000000, //CV_437 - byte 3
|
||||
0b00000000, //CV_438 - byte 2
|
||||
0b00000000, //CV_439 - byte 1
|
||||
0b00000000, //CV_440 - byte 0
|
||||
//F23 forward
|
||||
0b00000000, //byte 0 - CV_441
|
||||
0b00000000, //byte 1 - CV_442
|
||||
0b00000000, //byte 2 - CV_443
|
||||
0b00000000, //byte 3 - CV_444
|
||||
0b00000000, //CV_441 - byte 3
|
||||
0b00000000, //CV_442 - byte 2
|
||||
0b00000000, //CV_443 - byte 1
|
||||
0b00000000, //CV_444 - byte 0
|
||||
//F23 reverse
|
||||
0b00000000, //byte 0 - CV_445
|
||||
0b00000000, //byte 1 - CV_446
|
||||
0b00000000, //byte 2 - CV_447
|
||||
0b00000000, //byte 3 - CV_448
|
||||
0b00000000, //CV_445 - byte 3
|
||||
0b00000000, //CV_446 - byte 2
|
||||
0b00000000, //CV_447 - byte 1
|
||||
0b00000000, //CV_448 - byte 0
|
||||
//F24 forward
|
||||
0b00000000, //byte 0 - CV_449
|
||||
0b00000000, //byte 1 - CV_450
|
||||
0b00000000, //byte 2 - CV_451
|
||||
0b00000000, //byte 3 - CV_452
|
||||
0b00000000, //CV_449 - byte 3
|
||||
0b00000000, //CV_450 - byte 2
|
||||
0b00000000, //CV_451 - byte 1
|
||||
0b00000000, //CV_452 - byte 0
|
||||
//F24 reverse
|
||||
0b00000000, //byte 0 - CV_453
|
||||
0b00000000, //byte 1 - CV_454
|
||||
0b00000000, //byte 2 - CV_455
|
||||
0b00000000, //byte 3 - CV_456
|
||||
0b00000000, //CV_453 - byte 3
|
||||
0b00000000, //CV_454 - byte 2
|
||||
0b00000000, //CV_455 - byte 1
|
||||
0b00000000, //CV_456 - byte 0
|
||||
//F25 forward
|
||||
0b00000000, //byte 0 - CV_457
|
||||
0b00000000, //byte 1 - CV_458
|
||||
0b00000000, //byte 2 - CV_459
|
||||
0b00000000, //byte 3 - CV_460
|
||||
0b00000000, //CV_457 - byte 3
|
||||
0b00000000, //CV_458 - byte 2
|
||||
0b00000000, //CV_459 - byte 1
|
||||
0b00000000, //CV_460 - byte 0
|
||||
//F25 reverse
|
||||
0b00000000, //byte 0 - CV_461
|
||||
0b00000000, //byte 1 - CV_462
|
||||
0b00000000, //byte 2 - CV_463
|
||||
0b00000000, //byte 3 - CV_464
|
||||
0b00000000, //CV_461 - byte 3
|
||||
0b00000000, //CV_462 - byte 2
|
||||
0b00000000, //CV_463 - byte 1
|
||||
0b00000000, //CV_464 - byte 0
|
||||
//F26 forward
|
||||
0b00000000, //byte 0 - CV_465
|
||||
0b00000000, //byte 1 - CV_466
|
||||
0b00000000, //byte 2 - CV_467
|
||||
0b00000000, //byte 3 - CV_468
|
||||
0b00000000, //CV_465 - byte 3
|
||||
0b00000000, //CV_466 - byte 2
|
||||
0b00000000, //CV_467 - byte 1
|
||||
0b00000000, //CV_468 - byte 0
|
||||
//F26 reverse
|
||||
0b00000000, //byte 0 - CV_469
|
||||
0b00000000, //byte 1 - CV_470
|
||||
0b00000000, //byte 2 - CV_471
|
||||
0b00000000, //byte 3 - CV_472
|
||||
0b00000000, //CV_469 - byte 3
|
||||
0b00000000, //CV_470 - byte 2
|
||||
0b00000000, //CV_471 - byte 1
|
||||
0b00000000, //CV_472 - byte 0
|
||||
//F27 forward
|
||||
0b00000000, //byte 0 - CV_473
|
||||
0b00000000, //byte 1 - CV_474
|
||||
0b00000000, //byte 2 - CV_475
|
||||
0b00000000, //byte 3 - CV_476
|
||||
0b00000000, //CV_473 - byte 3
|
||||
0b00000000, //CV_474 - byte 2
|
||||
0b00000000, //CV_475 - byte 1
|
||||
0b00000000, //CV_476 - byte 0
|
||||
//F27 reverse
|
||||
0b00000000, //byte 0 - CV_477
|
||||
0b00000000, //byte 1 - CV_478
|
||||
0b00000000, //byte 2 - CV_479
|
||||
0b00000000, //byte 3 - CV_480
|
||||
0b00000000, //CV_477 - byte 3
|
||||
0b00000000, //CV_478 - byte 2
|
||||
0b00000000, //CV_479 - byte 1
|
||||
0b00000000, //CV_480 - byte 0
|
||||
//F28 forward
|
||||
0b00000000, //byte 0 - CV_481
|
||||
0b00000000, //byte 1 - CV_482
|
||||
0b00000000, //byte 2 - CV_483
|
||||
0b00000000, //byte 3 - CV_484
|
||||
0b00000000, //CV_481 - byte 3
|
||||
0b00000000, //CV_482 - byte 2
|
||||
0b00000000, //CV_483 - byte 1
|
||||
0b00000000, //CV_484 - byte 0
|
||||
//F28 reverse
|
||||
0b00000000, //byte 0 - CV_485
|
||||
0b00000000, //byte 1 - CV_486
|
||||
0b00000000, //byte 2 - CV_487
|
||||
0b00000000, //byte 3 - CV_488
|
||||
0b00000000, //CV_485 - byte 3
|
||||
0b00000000, //CV_486 - byte 2
|
||||
0b00000000, //CV_487 - byte 1
|
||||
0b00000000, //CV_488 - byte 0
|
||||
//F29 forward
|
||||
0b00000000, //byte 0 - CV_489
|
||||
0b00000000, //byte 1 - CV_490
|
||||
0b00000000, //byte 2 - CV_491
|
||||
0b00000000, //byte 3 - CV_492
|
||||
0b00000000, //CV_489 - byte 3
|
||||
0b00000000, //CV_490 - byte 2
|
||||
0b00000000, //CV_491 - byte 1
|
||||
0b00000000, //CV_492 - byte 0
|
||||
//F29 reverse
|
||||
0b00000000, //byte 0 - CV_493
|
||||
0b00000000, //byte 1 - CV_494
|
||||
0b00000000, //byte 2 - CV_495
|
||||
0b00000000, //byte 3 - CV_496
|
||||
0b00000000, //CV_493 - byte 3
|
||||
0b00000000, //CV_494 - byte 2
|
||||
0b00000000, //CV_495 - byte 1
|
||||
0b00000000, //CV_496 - byte 0
|
||||
//F30 forward
|
||||
0b00000000, //byte 0 - CV_497
|
||||
0b00000000, //byte 1 - CV_498
|
||||
0b00000000, //byte 2 - CV_499
|
||||
0b00000000, //byte 3 - CV_500
|
||||
0b00000000, //CV_497 - byte 3
|
||||
0b00000000, //CV_498 - byte 2
|
||||
0b00000000, //CV_499 - byte 1
|
||||
0b00000000, //CV_500 - byte 0
|
||||
//F30 reverse
|
||||
0b00000000, //byte 0 - CV_501
|
||||
0b00000000, //byte 1 - CV_502
|
||||
0b00000000, //byte 2 - CV_503
|
||||
0b00000000, //byte 3 - CV_504
|
||||
0b00000000, //CV_501 - byte 3
|
||||
0b00000000, //CV_502 - byte 2
|
||||
0b00000000, //CV_503 - byte 1
|
||||
0b00000000, //CV_504 - byte 0
|
||||
//F31 forward
|
||||
0b00000000, //byte 0 - CV_505
|
||||
0b00000000, //byte 1 - CV_506
|
||||
0b00000000, //byte 2 - CV_507
|
||||
0b00000000, //byte 3 - CV_508
|
||||
0b00000000, //CV_505 - byte 3
|
||||
0b00000000, //CV_506 - byte 2
|
||||
0b00000000, //CV_507 - byte 1
|
||||
0b00000000, //CV_508 - byte 0
|
||||
//F31 reverse
|
||||
0b00000000, //byte 0 - CV_509
|
||||
0b00000000, //byte 1 - CV_510
|
||||
0b00000000, //byte 2 - CV_511
|
||||
0b00000000, //byte 3 - CV_512
|
||||
0b00000000, //CV_509 - byte 3
|
||||
0b00000000, //CV_510 - byte 2
|
||||
0b00000000, //CV_511 - byte 1
|
||||
0b00000000, //CV_512 - byte 0
|
||||
};
|
||||
@@ -7,56 +7,79 @@
|
||||
#include "core0.h"
|
||||
const uint8_t *CV_ARRAY_FLASH = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET);
|
||||
uint64_t input_bit_buffer = 0;
|
||||
bool reset_package_flag = false;
|
||||
absolute_time_t falling_edge_time,rising_edge_time;
|
||||
uint16_t level_table[32] = {0};
|
||||
absolute_time_t falling_edge_time,rising_edge_time;
|
||||
|
||||
|
||||
uint16_t two_sigma(const uint16_t arr[], uint8_t length){
|
||||
//Calculate arithmetic average
|
||||
uint32_t sum = 0;
|
||||
for (uint i = 0; i < length; ++i) {
|
||||
// Function returns average of values deviating less than twice the standard deviation from original average
|
||||
float two_std_dev(const float arr[], uint32_t length){
|
||||
// Calculate arithmetic average
|
||||
float sum = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
sum += arr[i];
|
||||
}
|
||||
uint16_t x_avg = sum / length;
|
||||
//Calculate variance
|
||||
float x_avg = sum / (float)length;
|
||||
|
||||
// Calculate sample variance and sampled standard deviation
|
||||
sum = 0;
|
||||
for (uint8_t i = 0; i < length; ++i) {
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
sum += (arr[i]-x_avg)*(arr[i]-x_avg);
|
||||
}
|
||||
float variance = (float) sum/ (float)(length-1);
|
||||
//Calculate standard deviation
|
||||
uint16_t std_dev = (uint16_t)sqrtf(variance);
|
||||
//"Filter" every array element that deviates more than 2*(std_dev)
|
||||
float variance = sum / (float)(length-1);
|
||||
float std_dev = sqrtf(variance);
|
||||
|
||||
// Sum every element that is within two times the sampled standard deviation and compute new average
|
||||
sum = 0;
|
||||
uint8_t counter = 0;
|
||||
for (uint8_t i = 0; i < length; ++i) {
|
||||
uint16_t val = arr[i]-x_avg;
|
||||
if ( val < 2*std_dev || val > (0-2*std_dev) ){
|
||||
uint32_t counter = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
float diff = fabsf(arr[i]-x_avg);
|
||||
if ( diff < std_dev*2.0f ){
|
||||
sum += arr[i];
|
||||
counter++;
|
||||
}
|
||||
}
|
||||
//Calculate and return "new" arithmetic average from "filtered" array
|
||||
return sum / counter;
|
||||
return sum / (float)counter;
|
||||
}
|
||||
|
||||
|
||||
// Measures constant adc offset and programs the offset into flash
|
||||
void adc_offset_adjustment(uint8_t length){
|
||||
uint16_t offsets[length];
|
||||
//Set PWM level to 0 just for good measure
|
||||
void adc_offset_adjustment(uint32_t n){
|
||||
float offsets_fwd[n];
|
||||
float offsets_rev[n];
|
||||
// Set PWM level to 0 just to be sure
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
for (uint8_t i = 0; i < length; i++) {
|
||||
//uint16_t fwd = measure(true); //TODO: function needs to be rewritten without measure()
|
||||
//uint16_t rev = measure(false);
|
||||
//offsets[i] = (fwd+rev)/2;
|
||||
busy_wait_ms(1000);
|
||||
|
||||
// Measure n times in "reverse direction" (GPIO 28)
|
||||
adc_select_input(2);
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_rev[i] = (float)adc_fifo_get_blocking();
|
||||
}
|
||||
uint8_t offsets_fwd_avg = two_sigma(offsets, length);
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
busy_wait_ms(1000);
|
||||
|
||||
// Measure n times in "forward direction" (GPIO 29)
|
||||
adc_select_input(3);
|
||||
adc_run(true);
|
||||
for (uint32_t i = 0; i < n; i++) {
|
||||
offsets_fwd[i] = (float)adc_fifo_get_blocking();
|
||||
}
|
||||
adc_run(false);
|
||||
adc_fifo_drain();
|
||||
|
||||
// Find overall average discarding outliers in measurement
|
||||
float offset_avg_fwd = two_std_dev(offsets_fwd, n);
|
||||
float offset_avg_rev = two_std_dev(offsets_rev, n);
|
||||
float overall_avg = (offset_avg_fwd+offset_avg_rev)/2;
|
||||
uint8_t offset = (uint8_t)roundf(overall_avg);
|
||||
|
||||
// Create temporary array -> change CV 172 in temp array -> erase flash -> write temp array to flash
|
||||
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[171] = offsets_fwd_avg;
|
||||
CV_ARRAY_TEMP[171] = offset;
|
||||
flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE);
|
||||
flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_TEMP, FLASH_PAGE_SIZE * 2);
|
||||
}
|
||||
@@ -90,34 +113,60 @@ void verify_cv_byte(uint16_t cv_address, uint8_t cv_data){
|
||||
if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge();
|
||||
}
|
||||
|
||||
void regular_cv_write(uint16_t cv_index, uint8_t cv_data){
|
||||
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[cv_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);
|
||||
acknowledge();
|
||||
}
|
||||
|
||||
|
||||
// CV writing function first erases block in flashes and then rewrites to flash
|
||||
// Writing to read-only functions also gets handled here (e.g. complete reset to default)
|
||||
void write_cv_byte(uint16_t cv_index, uint8_t cv_data){
|
||||
//CV_7 & CV_8 are read-only
|
||||
//Reset all CVs to Default (CV_8; Value = 8)
|
||||
if(cv_index == 7 && 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);
|
||||
}
|
||||
//ADC offset Adjustment (CV_7; Value = 7);
|
||||
else if(cv_index == 6 && cv_data == 7){
|
||||
adc_offset_adjustment(CV_ARRAY_FLASH[173]);
|
||||
}
|
||||
else if( !cv_index && !cv_data );
|
||||
else{
|
||||
uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE];
|
||||
memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP));
|
||||
CV_ARRAY_TEMP[cv_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);
|
||||
acknowledge();
|
||||
void write_cv_handler(uint16_t cv_index, uint8_t cv_data){
|
||||
switch (cv_index) {
|
||||
// CV_1 => Value = 0 is not allowed
|
||||
case 0:
|
||||
if (cv_data == 0) break;
|
||||
// CV_7 & CV_8 are read-only
|
||||
// ADC offset Adjustment (CV_7; Value = 7);
|
||||
case 6:
|
||||
if (cv_data == 7) adc_offset_adjustment(8192);
|
||||
break;
|
||||
// Reset all CVs to Default (CV_8; Value = 8)
|
||||
case 7:
|
||||
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);
|
||||
}
|
||||
break;
|
||||
|
||||
// CV_17 must have a value between 11000000->(192dec) and 11100111->(231dec) inclusive
|
||||
// CV_17 == 192 && CV_18 == 0 -> Address = 0 is also not valid and therefore shall not be set
|
||||
case 16:
|
||||
if ( (cv_data < 192 || cv_data > 231) || (CV_ARRAY_FLASH[17] == 0 && cv_data == 192) ) break;
|
||||
else regular_cv_write(cv_index,cv_data);
|
||||
break;
|
||||
case 17:
|
||||
if(CV_ARRAY_FLASH[16] == 192 && cv_data == 0) break;
|
||||
else regular_cv_write(cv_index,cv_data);
|
||||
break;
|
||||
// CV_31 & CV_32 Index high byte isn't implemented and shall not be set
|
||||
case 30:
|
||||
case 31:
|
||||
break;
|
||||
|
||||
// Regular CV write is carried out
|
||||
default:
|
||||
regular_cv_write(cv_index,cv_data);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CV Programming function resets core1 and evaluates the type of programming instruction (e.g. write byte)
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t * const byte_array, bool restart_core1){
|
||||
//First check for valid programming command ("address" 112-127)
|
||||
if (byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111){
|
||||
uint8_t instruction_type_mask = 0b00001100;
|
||||
@@ -151,46 +200,28 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){
|
||||
// Write CV byte instruction
|
||||
else if (instruction_type == 0b000001100) {
|
||||
uint8_t cv_data = byte_array[number_of_bytes - 3];
|
||||
write_cv_byte(cv_address, cv_data);
|
||||
write_cv_handler(cv_address, cv_data);
|
||||
}
|
||||
|
||||
// Restore interrupts and launch start core1
|
||||
// Restore interrupts
|
||||
restore_interrupts(saved_interrupts);
|
||||
multicore_launch_core1(core1_entry);
|
||||
|
||||
//wait for core1 launch before proceeding
|
||||
multicore_fifo_pop_blocking();
|
||||
busy_wait_ms(5);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// retrieve 4Bytes from CV_ARRAY_FLASH at once and return 32-Bit value
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
uint8_t byte_2 = CV_ARRAY_FLASH[CV_start_index+2];
|
||||
uint8_t byte_3 = CV_ARRAY_FLASH[CV_start_index+3];
|
||||
return (byte_0) + (byte_1<<8) + (byte_2<<16) + (byte_3<<24);
|
||||
}
|
||||
|
||||
|
||||
// retrieve 2Bytes from CV_ARRAY_FLASH at once and return 16-Bit value
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
return (byte_0) + (byte_1<<8);
|
||||
}
|
||||
|
||||
|
||||
// Set outputs according to configuration (PWM on/off, PWM duty cycle/level, ...)
|
||||
void set_outputs(uint32_t functions_to_set_bitmask) {
|
||||
uint32_t outputs_to_set = 0;
|
||||
uint32_t outputs_with_PWM_enabled = get_32bit_CV(111); //get outputs with pwm enabled (bitmask)
|
||||
uint32_t outputs_with_PWM_enabled = get_32bit_CV(111); //get outputs with pwm enabled (bitmask) // TODO: Verify - should be fine...
|
||||
uint32_t outputs_to_set_PWM = outputs_with_PWM_enabled;
|
||||
uint32_t temp_mask = 1;
|
||||
for (uint8_t i = 0; i < 32; i++) {
|
||||
if (temp_mask & functions_to_set_bitmask ){
|
||||
outputs_to_set |= get_32bit_CV(i * 8 + 260 - 4 * get_direction(0));
|
||||
//outputs_to_set |= get_32bit_CV(i * 8 + 260 - 4 * get_direction(0)); //TODO: verify
|
||||
}
|
||||
temp_mask<<=1;
|
||||
}
|
||||
@@ -232,7 +263,7 @@ void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind,
|
||||
|
||||
|
||||
// Bitwise XOR for all Bytes -> Successful result is: "0000 0000"
|
||||
bool error_detection(int8_t number_of_bytes, const uint8_t byte_array[]) {
|
||||
bool error_detection(int8_t number_of_bytes, const uint8_t * const byte_array) {
|
||||
uint8_t xor_byte = 0;
|
||||
for (int i = 0; i < number_of_bytes; i++) {
|
||||
xor_byte = xor_byte ^ byte_array[i];
|
||||
@@ -242,37 +273,45 @@ bool error_detection(int8_t number_of_bytes, const uint8_t byte_array[]) {
|
||||
|
||||
|
||||
// Check for long address. Function returns true for long address.
|
||||
bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]) {
|
||||
bool is_long_address(uint8_t number_of_bytes, const uint8_t * const byte_array) {
|
||||
if ((byte_array[number_of_bytes - 1]>>6) == 0b00000011) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// First checks for idle package and then evaluates address included in byte_array[]
|
||||
bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) {
|
||||
//Check for Idle Package
|
||||
bool address_evaluation(uint8_t number_of_bytes,const uint8_t * const byte_array) {
|
||||
// Check for Idle Package
|
||||
if (byte_array[number_of_bytes - 1] == 255)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
//Long Address Package
|
||||
// Long/extended address decoder configuration (CV_29 Bit5)
|
||||
bool CV_29_bit5 = (CV_ARRAY_FLASH[28]&0b00100000)>>5;
|
||||
uint16_t read_address;
|
||||
if (is_long_address(number_of_bytes, byte_array))
|
||||
{
|
||||
//start of transmission -> address_byte_1 -> address_byte_0 -> ... -> end of transmission
|
||||
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;
|
||||
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);
|
||||
// Decoder configured to receive long/extended address
|
||||
if (CV_29_bit5){
|
||||
// Package received does contain a long address -> Check for accordance
|
||||
if (is_long_address(number_of_bytes, byte_array))
|
||||
{
|
||||
// start of transmission -> address_byte_1 -> address_byte_0 -> ... -> end of transmission
|
||||
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;
|
||||
uint16_t long_address = get_16bit_CV(16) & 0x3FFF; // &Mask to remove long address identifier bits
|
||||
return (long_address == read_address);
|
||||
}
|
||||
// Package received does not contain a long address
|
||||
else return false;
|
||||
}
|
||||
|
||||
//Short Address Package
|
||||
|
||||
// Short address decoder configuration (CV_29 Bit5)
|
||||
else
|
||||
{
|
||||
//start of transmission -> address_byte_0 -> ... -> end of transmission
|
||||
// Short Address Package
|
||||
// start of transmission -> address_byte_0 -> ... -> end of transmission
|
||||
read_address = (byte_array[number_of_bytes - 1]);
|
||||
return (CV_ARRAY_FLASH[0] == read_address);
|
||||
}
|
||||
@@ -280,7 +319,7 @@ bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) {
|
||||
|
||||
|
||||
// Evaluate the type of instruction
|
||||
void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) {
|
||||
void instruction_evaluation(uint8_t number_of_bytes,const uint8_t * const byte_array) {
|
||||
// start of transmission -> ... -> command_byte_n -> ... -> command_byte_0 -> ... -> end of transmission
|
||||
// The position of command bytes depend on whether the address is long or not
|
||||
uint8_t command_byte_n;
|
||||
@@ -299,7 +338,7 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[])
|
||||
uint32_t speed_step = byte_array[command_byte_start_index - 1];
|
||||
multicore_fifo_push_blocking(speed_step);
|
||||
// In case of a direction change, functions need to be updated because functions might depend on direction state
|
||||
if (get_direction(0) != prev_dir) update_active_functions(0,0,1);
|
||||
if (get_direction(0) != prev_dir) update_active_functions(0,0,true);
|
||||
}
|
||||
|
||||
// 10XX-XXXX (Function Group Instruction)
|
||||
@@ -337,7 +376,7 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[])
|
||||
|
||||
|
||||
// Check for reset package - When reset package is found stop the motor and disable all functions
|
||||
bool reset_package_check(uint8_t number_of_bytes,const uint8_t byte_array[]){
|
||||
bool reset_package_check(uint8_t number_of_bytes,const uint8_t * const byte_array){
|
||||
if (byte_array[number_of_bytes-1] == 0b00000000 && byte_array[number_of_bytes-2] == 0b00000000){
|
||||
uint32_t speed_step = 1;
|
||||
multicore_fifo_push_blocking(speed_step);
|
||||
@@ -378,20 +417,32 @@ void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) {
|
||||
|
||||
// Sequence of bits gets evaluated
|
||||
void evaluation(){
|
||||
static bool core1_launched = false;
|
||||
static bool reset_package_flag;
|
||||
int8_t number_of_bytes = check_for_package();
|
||||
// number of bytes is != -1 when the package is valid
|
||||
if (number_of_bytes != -1) {
|
||||
// Split data into 8Bit array
|
||||
uint8_t byte_array[SIZE_BYTE_ARRAY] = {0};
|
||||
bits_to_byte_array(number_of_bytes,byte_array);
|
||||
if(byte_array[number_of_bytes-3]==239 && byte_array[number_of_bytes-4]==151){
|
||||
}
|
||||
// Check for errors
|
||||
if (error_detection(number_of_bytes,byte_array)) {
|
||||
// Check for matching address
|
||||
if (address_evaluation(number_of_bytes,byte_array)) {
|
||||
reset_package_flag = false;
|
||||
if (!core1_launched){
|
||||
multicore_launch_core1(core1_entry);
|
||||
// Wait for pid.direction pointer via multicore fifo also signalizing that core1 initialization is finished.
|
||||
get_direction((bool *) multicore_fifo_pop_blocking());
|
||||
core1_launched = true;
|
||||
}
|
||||
instruction_evaluation(number_of_bytes,byte_array);
|
||||
}
|
||||
// When previous packet was a reset package -> enter program mode
|
||||
else if (reset_package_flag) {
|
||||
program_mode(number_of_bytes,byte_array);
|
||||
program_mode(number_of_bytes,byte_array,true);
|
||||
}
|
||||
// Check for reset package and set flag when reset package is received
|
||||
else {
|
||||
reset_package_flag = reset_package_check(number_of_bytes,byte_array);
|
||||
}
|
||||
@@ -434,7 +485,7 @@ void track_signal_fall(unsigned int gpio, long unsigned int events) {
|
||||
void init_outputs(){
|
||||
gpio_init_mask(GPIO_ALLOWED_OUTPUTS);
|
||||
gpio_set_dir_out_masked(GPIO_ALLOWED_OUTPUTS); //note: This might also disable UART on GPIO0 & GPIO1
|
||||
uint32_t Outputs_with_PWM_enabled = get_32bit_CV(111);
|
||||
uint32_t Outputs_with_PWM_enabled = get_32bit_CV(111); // TODO: Verify - should be fine...
|
||||
uint32_t slice, channel;
|
||||
uint16_t wrap, level;
|
||||
uint32_t temp_mask = 1;
|
||||
@@ -442,8 +493,8 @@ void init_outputs(){
|
||||
if (Outputs_with_PWM_enabled&temp_mask){
|
||||
slice = pwm_gpio_to_slice_num(i);
|
||||
channel = pwm_gpio_to_channel(i); //0 for Channel A, 1 for Channel B
|
||||
wrap = get_16bit_CV(115 + slice*7);
|
||||
level = get_16bit_CV(118 + 7*slice+ 2*channel);
|
||||
wrap = get_16bit_CV(115 + slice*7); // TODO: Verify
|
||||
level = get_16bit_CV(118 + 7*slice+ 2*channel); // TODO: Verify
|
||||
gpio_set_function(i,GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice,wrap);
|
||||
pwm_set_gpio_level(i,0);
|
||||
@@ -456,19 +507,72 @@ void init_outputs(){
|
||||
}
|
||||
|
||||
|
||||
// Measures "base" pwm duty cycle needed to overcome friction
|
||||
uint16_t measure_base_pwm(bool direction, uint8_t iterations){
|
||||
uint gpio;
|
||||
if (direction) gpio = MOTOR_FWD_PIN;
|
||||
else gpio = MOTOR_REV_PIN;
|
||||
float level_arr[iterations];
|
||||
for (int i = 0; i < iterations; ++i) {
|
||||
uint16_t level = 0;
|
||||
float measurement;
|
||||
do {
|
||||
pwm_set_gpio_level(gpio, level);
|
||||
busy_wait_ms(15);
|
||||
level += 10;
|
||||
measurement = measure(CV_ARRAY_FLASH[60],
|
||||
CV_ARRAY_FLASH[61],
|
||||
CV_ARRAY_FLASH[62],
|
||||
CV_ARRAY_FLASH[63],
|
||||
direction);
|
||||
}
|
||||
// (Measurement - ADC_Offset) < 5.0f
|
||||
while((measurement - (float)CV_ARRAY_FLASH[171]) < 5.0f);
|
||||
level_arr[i] = (float)level;
|
||||
busy_wait_ms(500);
|
||||
}
|
||||
// Find and return overall average discarding outliers in measurement
|
||||
return (uint16_t)two_std_dev(level_arr,5);
|
||||
}
|
||||
|
||||
|
||||
// Check for flash memory factory condition and adc offset setup
|
||||
// When factory condition is found the CV_ARRAY_DEFAULT will be written into flash
|
||||
// When ADC Setup is not configured run adc offset adjustment function and write adc offset into flash
|
||||
void cv_setup_check(){
|
||||
//Check for flash factory setting and set CV_FLASH_ARRAY to default values when factory condition is found.
|
||||
if (CV_ARRAY_FLASH[64]){
|
||||
|
||||
// Check for flash factory setting and set CV_FLASH_ARRAY to default values when factory condition is found.
|
||||
if ( CV_ARRAY_FLASH[64] ){
|
||||
uint8_t arr[4] = {125,8,7,124};
|
||||
program_mode(4,arr); //reset to CV_ARRAY_DEFAULT (write CV_8 = 8)
|
||||
program_mode(4,arr,false); //reset to CV_ARRAY_DEFAULT (write CV_8 = 8)
|
||||
}
|
||||
//Check for adc offset setup
|
||||
|
||||
// Check for adc offset setup
|
||||
if ( CV_ARRAY_FLASH[171] == 0xFF ){
|
||||
uint8_t arr[4] = {125,7,6,124};
|
||||
program_mode(4,arr); //ADC offset adjustment (write CV_7 = 7)
|
||||
program_mode(4,arr,false); //ADC offset adjustment (write CV_7 = 7)
|
||||
}
|
||||
|
||||
// Check for base PWM configuration - used for feed-forward
|
||||
// Forward Direction
|
||||
if (get_16bit_CV(175) == 0){
|
||||
uint16_t base_pwm_fwd = measure_base_pwm(true,10);
|
||||
uint8_t base_pwm_fwd_high_byte = base_pwm_fwd>>8;
|
||||
uint8_t base_pwm_fwd_low_byte = base_pwm_fwd&0x00FF;
|
||||
uint8_t arr0[4] = {125,base_pwm_fwd_high_byte,175,124};
|
||||
program_mode(4,arr0,false);
|
||||
uint8_t arr1[4] = {125,base_pwm_fwd_low_byte,176,124};
|
||||
program_mode(4,arr1,false);
|
||||
}
|
||||
// Reverse Direction
|
||||
if (get_16bit_CV(177) == 0){
|
||||
uint16_t base_pwm_rev = measure_base_pwm(false,10);
|
||||
uint8_t base_pwm_rev_high_byte = base_pwm_rev>>8;
|
||||
uint8_t base_pwm_rev_low_byte = base_pwm_rev&0x00FF;
|
||||
uint8_t arr0[4] = {125,base_pwm_rev_high_byte,177,124};
|
||||
program_mode(4,arr0,false);
|
||||
uint8_t arr1[4] = {125,base_pwm_rev_low_byte,178,124};
|
||||
program_mode(4,arr1,false);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -486,12 +590,36 @@ bool get_direction(bool *direction_ptr){
|
||||
}
|
||||
|
||||
|
||||
// Motor PWM initialization
|
||||
void init_motor_pwm(uint8_t gpio) {
|
||||
uint16_t wrap_counter = _125M/(CV_ARRAY_FLASH[8]*100+10000); // 125MHz / f_pwm
|
||||
uint32_t slice_num = pwm_gpio_to_slice_num(gpio);
|
||||
pwm_set_clkdiv_int_frac(slice_num,CV_ARRAY_FLASH[173],0);
|
||||
pwm_set_wrap(slice_num, wrap_counter);
|
||||
pwm_set_gpio_level(gpio,0);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
}
|
||||
|
||||
|
||||
// Main initialization - GPIO Config / ADC Config
|
||||
void init_main(){
|
||||
if(gpio_get_function(MOTOR_FWD_PIN) != 4){
|
||||
gpio_set_function(MOTOR_FWD_PIN, GPIO_FUNC_PWM);
|
||||
gpio_set_function(MOTOR_REV_PIN, GPIO_FUNC_PWM);
|
||||
adc_init();
|
||||
adc_gpio_init(FWD_V_EMF_ADC_PIN);
|
||||
adc_gpio_init(REV_V_EMF_ADC_PIN);
|
||||
adc_fifo_setup(true,false,0,false,false);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
stdio_init_all();
|
||||
printf("core0 init...\n");
|
||||
multicore_launch_core1(core1_entry);
|
||||
// Wait for pid.direction pointer via multicore fifo also signalizing that core1 initialization is finished.
|
||||
get_direction((bool *) multicore_fifo_pop_blocking());
|
||||
init_main();
|
||||
init_motor_pwm(MOTOR_FWD_PIN);
|
||||
init_motor_pwm(MOTOR_REV_PIN);
|
||||
cv_setup_check();
|
||||
init_outputs();
|
||||
gpio_init(DCC_INPUT_PIN);
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
#pragma once
|
||||
#include "shared.h"
|
||||
#include "CV.h"
|
||||
#include "pico/float.h"
|
||||
#define SIZE_BYTE_ARRAY 5
|
||||
#define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001
|
||||
#define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001
|
||||
@@ -23,15 +22,15 @@ const uint32_t clr_bit_arr[6] = {
|
||||
0b11100000000111111111111111111111, // Clear Bits 21-28 (F21-F28)
|
||||
0b00011111111111111111111111111111, // Clear Bits 29-31 (F29-F31)
|
||||
};
|
||||
uint16_t two_sigma(const uint16_t arr[], uint8_t length);
|
||||
void adc_offset_adjustment(uint8_t length);
|
||||
|
||||
float two_std_dev(const float arr[], uint32_t length);
|
||||
void adc_offset_adjustment(uint32_t length);
|
||||
void acknowledge();
|
||||
void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos);
|
||||
void verify_cv_byte(uint16_t cv_address, uint8_t cv_data);
|
||||
void write_cv_byte(uint16_t cv_index, uint8_t cv_data);
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index);
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index);
|
||||
void regular_cv_write(uint16_t cv_index, uint8_t cv_data);
|
||||
void write_cv_handler(uint16_t cv_index, uint8_t cv_data);
|
||||
void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[], bool restart_core1);
|
||||
void set_outputs(uint32_t functions_to_set_bitmask);
|
||||
void update_active_functions(uint32_t byte, uint8_t clr_bit_ind,bool direction_change);
|
||||
bool error_detection(int8_t number_of_bytes, const uint8_t byte_array[]);
|
||||
@@ -45,5 +44,8 @@ void evaluation();
|
||||
void track_signal_rise(unsigned int gpio, long unsigned int events);
|
||||
void track_signal_fall(unsigned int gpio, long unsigned int events);
|
||||
void init_outputs();
|
||||
uint16_t measure_base_pwm(bool direction, uint8_t iterations);
|
||||
void cv_setup_check();
|
||||
bool get_direction(bool *direction_ptr);
|
||||
bool get_direction(bool *direction_ptr);
|
||||
void init_motor_pwm(uint8_t gpio);
|
||||
void init_main();
|
||||
@@ -11,85 +11,69 @@ struct repeating_timer pid_control_timer,speed_helper_timer;
|
||||
// Update speed_step_target, end_target_index and direction on receiving data from core0
|
||||
void update_speed_dir(pid_params *pid, uint32_t speed_step_target){
|
||||
pid->speed_step_target = speed_step_target;
|
||||
// When CV_29 Bit 0 is set direction is reversed
|
||||
bool reverse_dir = CV_ARRAY_FLASH[28] & 0b00000001;
|
||||
// Forward
|
||||
if(speed_step_target > 127) {
|
||||
pid->direction = true;
|
||||
pid->direction = true^reverse_dir;
|
||||
if (pid->speed_step_target == 128) pid->end_target_index = 0;
|
||||
else pid->end_target_index = (pid->speed_step_target-129);
|
||||
}
|
||||
// Reverse
|
||||
else {
|
||||
pid->direction = false;
|
||||
pid->direction = false^reverse_dir;
|
||||
if (pid->speed_step_target == 0) pid->end_target_index = 0;
|
||||
else pid->end_target_index = (pid->speed_step_target-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// This function gets called every x milliseconds where x is CV_175.
|
||||
// The purpose of this function is to implement a time delay in acceleration or deceleration
|
||||
bool speed_helper(struct repeating_timer *t) {
|
||||
// pid->setpoint only gets adjusted when the speed_helper_counter is equal to the accel_rate/decel_rate
|
||||
// -> Time for 1 Speed Step := (speed_helper timer delay)*(accel_rate) or CV_175*CV_3 or CV_175*CV_4
|
||||
pid_params *pid = (pid_params *)(t->user_data);
|
||||
uint8_t accel_rate = CV_ARRAY_FLASH[2];
|
||||
uint8_t decel_rate = CV_ARRAY_FLASH[3];
|
||||
static uint8_t speed_helper_counter;
|
||||
static uint8_t speed_table_index;
|
||||
//latest_setpoint only gets adjusted when the counter equals the acc/dec rate
|
||||
//-> Time for 1 Speed Step := (speed_helper timer delay)*(accel_rate)
|
||||
//Emergency Stop
|
||||
|
||||
// Emergency Stop
|
||||
if (pid->speed_step_target == 129 || pid->speed_step_target==1) {
|
||||
speed_table_index = 0;
|
||||
speed_helper_counter = 0;
|
||||
pid->setpoint = pid->speed_table[speed_table_index];
|
||||
}
|
||||
//Acceleration
|
||||
|
||||
// Acceleration
|
||||
else if (pid->end_target_index > speed_table_index && speed_helper_counter == accel_rate) {
|
||||
if(!accel_rate) speed_table_index = pid->end_target_index; // directly accelerate to end target speed
|
||||
else speed_table_index++;
|
||||
pid->setpoint = pid->speed_table[speed_table_index];
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
//Deceleration
|
||||
|
||||
// Deceleration
|
||||
else if (pid->end_target_index < speed_table_index && speed_helper_counter == decel_rate) {
|
||||
if(!decel_rate) speed_table_index = pid->end_target_index; // directly decelerate to end target speed
|
||||
else speed_table_index--;
|
||||
pid->setpoint = pid->speed_table[speed_table_index];
|
||||
speed_helper_counter = 0;
|
||||
}
|
||||
//Acceleration/Deceleration "scheduled"
|
||||
|
||||
// Acceleration/Deceleration "scheduled"
|
||||
else if (pid->end_target_index != speed_table_index) speed_helper_counter++;
|
||||
//Target reached -> Reset Counter
|
||||
|
||||
// Target reached -> Reset Counter
|
||||
else speed_helper_counter = 0;
|
||||
|
||||
// return true to schedule the repeating timer again
|
||||
return true;
|
||||
}
|
||||
float measure(pid_params *pid, bool direction){
|
||||
uint32_t sum = 0;
|
||||
uint16_t adc_val[100 /*msr.total_iterations*/];
|
||||
int32_t j = 0;
|
||||
uint16_t key;
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
adc_select_input((!direction)+2);
|
||||
busy_wait_us(pid->msr_delay_in_us);
|
||||
adc_run(true);
|
||||
for (int32_t i = 0; i < pid->msr_total_iterations; ++i) {
|
||||
//Take adc value out of fifo when available, then do one step of insertion sort.
|
||||
adc_val[i] = adc_fifo_get_blocking();
|
||||
key = adc_val[i];
|
||||
j = i - 1;
|
||||
while (j >= 0 && adc_val[j] > key) {
|
||||
adc_val[j + 1] = adc_val[j];
|
||||
j--;
|
||||
}
|
||||
adc_val[j + 1] = key;
|
||||
}
|
||||
adc_run(false);
|
||||
//discard x entries beginning from the lowest entry of the array (counting up); x = msr.l_side_arr_cutoff
|
||||
//discard y entries beginning from the highest index of the array (counting down); y = msr.r_side_arr_cutoff
|
||||
for (int32_t i = pid->l_side_arr_cutoff; i < pid->msr_total_iterations - pid->r_side_arr_cutoff ; ++i) {
|
||||
sum += adc_val[i];
|
||||
}
|
||||
float res = (float)sum / (float)( pid->msr_total_iterations - (pid->l_side_arr_cutoff + pid->r_side_arr_cutoff) );
|
||||
// return true to keep the repeating timer active
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
// Helper function to adjust pwm level/duty cycle.
|
||||
void adjust_pwm_level(uint16_t level, pid_params *pid) {
|
||||
if (pid->direction) {
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
@@ -101,23 +85,37 @@ void adjust_pwm_level(uint16_t level, pid_params *pid) {
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, level);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Update feed-forward slope/gradient for both directions
|
||||
void update_m(pid_params *pid){
|
||||
pid->m_fwd = (pid->y_2_fwd-pid->y_1_fwd)/(pid->x_2_fwd-pid->x_1_fwd);
|
||||
pid->m_rev = (pid->y_2_rev-pid->y_1_rev)/(pid->x_2_rev-pid->x_1_rev);
|
||||
}
|
||||
|
||||
|
||||
// Update feedforward function value y_2 in both directions
|
||||
void update_y(pid_params *pid,float i){
|
||||
// Forward
|
||||
if(pid->direction) {
|
||||
pid->y_2_fwd += i*pid->k_ff;
|
||||
printf("%f\n",pid->y_2_fwd);
|
||||
// Limit y_2_fwd
|
||||
if(pid->y_2_fwd > pid->max_output) pid->y_2_fwd = pid->max_output;
|
||||
else if(pid->y_2_fwd < pid->y_1_fwd) pid->y_2_fwd = pid->y_1_fwd;
|
||||
}
|
||||
// Reverse
|
||||
else{
|
||||
pid->y_2_rev += i*pid->k_ff;
|
||||
printf("%f\n",pid->y_2_rev);
|
||||
// Limit y_2_rev
|
||||
if(pid->y_2_rev > pid->max_output) pid->y_2_rev = pid->max_output;
|
||||
else if(pid->y_2_rev < pid->y_1_rev) pid->y_2_rev = pid->y_1_rev;
|
||||
}
|
||||
|
||||
update_m(pid);
|
||||
}
|
||||
|
||||
|
||||
// Returns feedforward value corresponding to current setpoint
|
||||
float get_ff_val(pid_params *pid){
|
||||
float val;
|
||||
// Forward
|
||||
@@ -130,89 +128,93 @@ float get_ff_val(pid_params *pid){
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
|
||||
// Returns proportional gain value corresponding to current setpoint
|
||||
float get_kp(pid_params *pid){
|
||||
float sp = (float)pid->setpoint;
|
||||
float x_1 = (float)pid->speed_table[126]*0.05f;
|
||||
float x_2 = (float)pid->speed_table[126]*0.95f;
|
||||
float y_1 = 60.0f;
|
||||
float y_2 = 4.0f;
|
||||
float y_3 = 1.5f;
|
||||
float m_1 = (y_2-y_1)/x_1;
|
||||
float m_2 = (y_3-y_2)/x_2;
|
||||
float k_p;
|
||||
if (sp < x_1){
|
||||
k_p = m_1*sp+y_1;
|
||||
if (sp < pid->k_p_x_1){
|
||||
return pid->k_p_m_1 * sp + pid->k_p_y_0;
|
||||
}
|
||||
else{
|
||||
k_p = m_2*(sp-x_1)+y_2;
|
||||
return pid->k_p_m_2 * (sp - pid->k_p_x_1) + pid->k_p_y_1;
|
||||
}
|
||||
return k_p;
|
||||
}
|
||||
|
||||
|
||||
// PID controller function gets called every x milliseconds where x is CV_49 aka sampling time (pid->t)
|
||||
bool pid_control(struct repeating_timer *t){
|
||||
// Cast (void *) to (pid_params *)
|
||||
pid_params *pid = (pid_params *)(t->user_data);
|
||||
// Acceleration from standstill or change in direction -> previous derivative and integral parts are reset
|
||||
|
||||
// Acceleration from standstill or change in direction -> reset previous derivative, error and integral parts
|
||||
if((!pid->setpoint_prev && !pid->setpoint) || (pid->direction != pid->direction_prev)){
|
||||
pid->derivative_prev = 0.0f;
|
||||
pid->integral_prev = 0.0f;
|
||||
pid->d_prev = 0.0f;
|
||||
pid->i_prev = 0.0f;
|
||||
pid->e_prev = 0.0f;
|
||||
}
|
||||
|
||||
// Get feed-forward and proportional gain values
|
||||
float feed_fwd = get_ff_val(pid);
|
||||
pid->k_p = get_kp(pid);
|
||||
// Measure BEMF voltage - proportional to the rotational speed of the motor
|
||||
//absolute_time_t from = get_absolute_time();
|
||||
pid->measurement = measure(pid,pid->direction);
|
||||
// Calculate error
|
||||
|
||||
// Measure BEMF voltage and compute error
|
||||
pid->measurement = measure(pid->msr_total_iterations,
|
||||
pid->msr_total_iterations,
|
||||
pid->l_side_arr_cutoff,
|
||||
pid->r_side_arr_cutoff,
|
||||
pid->direction);
|
||||
float e = (float)pid->setpoint - (pid->measurement - pid->adc_offset);
|
||||
// Proportional
|
||||
|
||||
// Proportional part, integral part and derivative part (including digital low-pass-filter with time constant tau)
|
||||
float p = pid->k_p * e;
|
||||
// Integral
|
||||
float i = pid->ci_0*(e + pid->e_prev) + pid->integral_prev;
|
||||
float i = pid->ci_0*(e + pid->e_prev) + pid->i_prev;
|
||||
float d = pid->cd_0*(pid->measurement-pid->measurement_prev)+pid->cd_1*pid->d_prev;
|
||||
|
||||
// Check for limits on the integral part
|
||||
if (i > pid->int_lim_max)i = pid->int_lim_max;
|
||||
else if (i < pid->int_lim_min)i = pid->int_lim_min;
|
||||
// Derivative and digital low-pass-filter with time constant tau
|
||||
float d = pid->cd_0*(pid->measurement-pid->measurement_prev)+pid->cd_1*pid->derivative_prev;
|
||||
// Sum feed forward + all controller terms (p+i+d)
|
||||
|
||||
// Sum feed forward + all controller terms (p+i+d). Limit Output from 0% to 100% duty cycle
|
||||
float output = feed_fwd+p+i+d;
|
||||
// Limit Output from 0% to 100% duty cycle
|
||||
if (output < 0 || !pid->setpoint) output = 0.0f;
|
||||
else if (output > pid->max_output) output = pid->max_output;
|
||||
// Set PWM Level
|
||||
|
||||
// Set PWM Level and discard results in adc fifo
|
||||
adjust_pwm_level((uint16_t)roundf(output),pid);
|
||||
//absolute_time_t to = get_absolute_time();
|
||||
//int64_t bla = absolute_time_diff_us(from,to);
|
||||
// printf("%f\n",i);
|
||||
// Discard results in adc fifo
|
||||
adc_fifo_drain();
|
||||
if(pid->setpoint > 2) {
|
||||
printf("p:%f\n", p);
|
||||
printf("i:%f\n", i);
|
||||
printf("d:%f\n", d);
|
||||
printf("o:%f\n", output);
|
||||
}
|
||||
// Check for High or Low Integrator values -> Update x,y values of feedforward function
|
||||
if( (output < 5000 && output > 2500) && ((i > pid->int_lim_max/2) || (i < pid->int_lim_min/2)) ) update_y(pid,i);
|
||||
// Save previous error, integrator, differentiator and direction
|
||||
if( (output < pid->max_output && output > pid->max_output/2) && ((i > pid->int_lim_max/2) || (i < pid->int_lim_min/2)) ) update_y(pid,i);
|
||||
|
||||
|
||||
// Save previous error, integrator, differentiator, direction, setpoint and measured value also sum error to e_sum
|
||||
pid->e_prev = e;
|
||||
pid->integral_prev = i;
|
||||
pid->derivative_prev = d;
|
||||
pid->i_prev = i;
|
||||
pid->d_prev = d;
|
||||
pid->direction_prev = pid->direction;
|
||||
pid->setpoint_prev = pid->setpoint;
|
||||
pid->measurement_prev = pid->measurement;
|
||||
// return true to keep the repeating timer active
|
||||
|
||||
// return true to schedule the repeating timer again
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t measure_base_pwm(pid_params *pid,bool direction){
|
||||
pid->direction = direction;
|
||||
uint16_t level = 750;
|
||||
while(measure(pid,direction)<20.0f){
|
||||
adjust_pwm_level(level,pid);
|
||||
busy_wait_ms(50);
|
||||
level += 25;
|
||||
}
|
||||
return level;
|
||||
}
|
||||
|
||||
// PID controller and measurement parameter, and speed_table initialization
|
||||
void init_pid(pid_params *pid){
|
||||
// Measurement variables initialization
|
||||
pid->adc_offset = 13.0f; //(float)CV_ARRAY_FLASH[171];
|
||||
pid->msr_total_iterations = 100; //CV_ARRAY_FLASH[60];
|
||||
pid->msr_delay_in_us = 100; //CV_ARRAY_FLASH[61];
|
||||
pid->l_side_arr_cutoff = 15; //CV_ARRAY_FLASH[62];
|
||||
pid->r_side_arr_cutoff = 15; //CV_ARRAY_FLASH[63];
|
||||
pid->adc_offset = (float)CV_ARRAY_FLASH[171];
|
||||
pid->msr_total_iterations = CV_ARRAY_FLASH[60];
|
||||
pid->msr_delay_in_us = CV_ARRAY_FLASH[61];
|
||||
pid->l_side_arr_cutoff = CV_ARRAY_FLASH[62];
|
||||
pid->r_side_arr_cutoff = CV_ARRAY_FLASH[63];
|
||||
|
||||
// Speed table initialization
|
||||
// Calculate speed setpoint table according to V_min, V_max and V_mid CVs
|
||||
double v_min = (double)CV_ARRAY_FLASH[1];
|
||||
@@ -227,65 +229,66 @@ void init_pid(pid_params *pid){
|
||||
for (uint8_t i = 63; i < 127; ++i) {
|
||||
pid->speed_table[i] = (uint16_t)lround(m_2*(i-63)+v_mid);
|
||||
}
|
||||
// Control variables initialization
|
||||
pid->k_p = 5.0f;
|
||||
pid->k_i = 2.0f;
|
||||
pid->k_d = 0.005f;
|
||||
pid->tau = (float)CV_ARRAY_FLASH[46]/1000;
|
||||
pid->t = (float)CV_ARRAY_FLASH[47]/1000;
|
||||
|
||||
// Controller constants initialization
|
||||
pid->k_i = (float)CV_ARRAY_FLASH[49]/10;
|
||||
pid->k_d = (float)CV_ARRAY_FLASH[50]/10000;
|
||||
pid->tau = (float)CV_ARRAY_FLASH[47]/1000;
|
||||
pid->t = (float)CV_ARRAY_FLASH[48]/1000;
|
||||
pid->ci_0 = (pid->k_i*pid->t)/2;
|
||||
pid->cd_0 = -(2*pid->k_d)/(2*pid->tau+pid->t);
|
||||
pid->cd_1 = (2*pid->tau-pid->t)/(2*pid->tau+pid->t);
|
||||
pid->int_lim_max = 1000;
|
||||
pid->int_lim_min = -1000;
|
||||
pid->direction = true;
|
||||
pid->direction_prev = true;
|
||||
uint16_t max_output = _125M/(CV_ARRAY_FLASH[8]*100+10000);
|
||||
pid->max_output = (float)max_output;
|
||||
pid->y_1_fwd = (float)measure_base_pwm(pid,1);
|
||||
pid->y_1_rev = (float)measure_base_pwm(pid,0);
|
||||
pid->y_2_fwd = pid->max_output*0.75f;
|
||||
pid->y_2_rev = pid->max_output*0.75f;
|
||||
pid->int_lim_max = 10*(float)CV_ARRAY_FLASH[51];
|
||||
pid->int_lim_min = -10*(float)CV_ARRAY_FLASH[52];
|
||||
pid->k_ff = (float)CV_ARRAY_FLASH[46]/10000; // 0.005f;
|
||||
pid->max_output = (float)(_125M/(CV_ARRAY_FLASH[8]*100+10000));
|
||||
pid->x_1_rev = 0.0f;
|
||||
pid->x_1_fwd = 0.0f;
|
||||
pid->x_2_rev = (float)pid->speed_table[94];
|
||||
pid->x_2_fwd = (float)pid->speed_table[94];
|
||||
pid->k_ff = 0.005f;
|
||||
pid->y_1_fwd = (float)get_16bit_CV(175); //(float)measure_base_pwm(pid,1);
|
||||
pid->y_1_rev = (float)get_16bit_CV(177); //(float)measure_base_pwm(pid,0);
|
||||
pid->x_2_rev = (float)pid->speed_table[126];
|
||||
pid->x_2_fwd = (float)pid->speed_table[126];
|
||||
|
||||
// Controller variables initialization
|
||||
pid->y_2_fwd = pid->max_output;
|
||||
pid->y_2_rev = pid->max_output;
|
||||
pid->measurement = 0.0f;
|
||||
pid->measurement_prev = 0.0f;
|
||||
pid->e_prev = 0.0f;
|
||||
pid->i_prev = 0.0f;
|
||||
pid->d_prev = 0.0f;
|
||||
pid->direction = false;
|
||||
pid->direction_prev = false;
|
||||
pid->speed_step_target = 0;
|
||||
pid->setpoint = 0;
|
||||
|
||||
// Gain scheduling initialization
|
||||
pid->k_p_x_1_shift = (float)CV_ARRAY_FLASH[59] / 255.0f;
|
||||
pid->k_p_x_1 = (float)pid->speed_table[126] * pid->k_p_x_1_shift;
|
||||
pid->k_p_x_2 = (float)pid->speed_table[126] * (1.0f - pid->k_p_x_1_shift);
|
||||
pid->k_p_y_0 = (float)get_16bit_CV(53) / 100.0f;
|
||||
pid->k_p_y_1 = (float)get_16bit_CV(55) / 100.0f;
|
||||
pid->k_p_y_2 = (float)get_16bit_CV(57) / 100.0f;
|
||||
pid->k_p_m_1 = (pid->k_p_y_1 - pid->k_p_y_0) / pid->k_p_x_1;
|
||||
pid->k_p_m_2 = (pid->k_p_y_2 - pid->k_p_y_1) / pid->k_p_x_2;
|
||||
|
||||
update_m(pid);
|
||||
}
|
||||
void init_motor_pwm(uint8_t gpio) {
|
||||
uint16_t wrap_counter = _125M/(CV_ARRAY_FLASH[8]*100+10000); // 125MHz / f_pwm
|
||||
uint32_t slice_num = pwm_gpio_to_slice_num(gpio);
|
||||
pwm_set_clkdiv_int_frac(slice_num,CV_ARRAY_FLASH[173],0);
|
||||
pwm_set_wrap(slice_num, wrap_counter);
|
||||
pwm_set_gpio_level(gpio,0);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
}
|
||||
|
||||
|
||||
void core1_entry() {
|
||||
printf("core1 init...\n");
|
||||
if(gpio_get_function(MOTOR_FWD_PIN) != 4){
|
||||
gpio_set_function(MOTOR_FWD_PIN, GPIO_FUNC_PWM);
|
||||
gpio_set_function(MOTOR_REV_PIN, GPIO_FUNC_PWM);
|
||||
adc_init();
|
||||
adc_gpio_init(FWD_V_EMF_ADC_PIN);
|
||||
adc_gpio_init(REV_V_EMF_ADC_PIN);
|
||||
adc_fifo_setup(true,false,0,false,false);
|
||||
init_motor_pwm(MOTOR_FWD_PIN);
|
||||
init_motor_pwm(MOTOR_REV_PIN);
|
||||
}
|
||||
pid_params pid_parameters;
|
||||
pid_params *pid = &pid_parameters;
|
||||
init_pid(pid);
|
||||
alarm_pool_add_repeating_timer_ms(alarm_pool_create(0,1),
|
||||
CV_ARRAY_FLASH[47],
|
||||
CV_ARRAY_FLASH[48],
|
||||
pid_control,
|
||||
pid,
|
||||
&pid_control_timer);
|
||||
irq_set_priority(0,0x00); // 0x00 = highest priority
|
||||
alarm_pool_add_repeating_timer_ms(alarm_pool_create(1,1),
|
||||
CV_ARRAY_FLASH[58],
|
||||
CV_ARRAY_FLASH[174],
|
||||
speed_helper,
|
||||
pid,
|
||||
&speed_helper_timer);
|
||||
@@ -293,9 +296,19 @@ void core1_entry() {
|
||||
// Note: Pointer will be cast into uint32_t and then back into bool * on core0 (Both 32-Bit Datatype).
|
||||
// This is done only to accommodate the correct data type associated with multicore_fifo_push_blocking().
|
||||
multicore_fifo_push_blocking((uint32_t) &(pid->direction));
|
||||
uint64_t packet_timeout_threshold_in_us = CV_ARRAY_FLASH[10]*1000000;
|
||||
absolute_time_t time_last_update = get_absolute_time();
|
||||
printf("core1 done\n");
|
||||
while (1){
|
||||
// Check for time difference between last packet and current time (Packet Timeout)
|
||||
absolute_time_t current_time = get_absolute_time();
|
||||
if (absolute_time_diff_us(time_last_update,current_time) > packet_timeout_threshold_in_us){
|
||||
update_speed_dir(pid,0);
|
||||
}
|
||||
// If core0 sends new speed data to core1 speed_step_target and direction variables get updated
|
||||
if(multicore_fifo_rvalid()) { update_speed_dir(pid, multicore_fifo_pop_blocking()); }
|
||||
if(multicore_fifo_rvalid()) {
|
||||
update_speed_dir(pid, multicore_fifo_pop_blocking());
|
||||
time_last_update = get_absolute_time();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#pragma once
|
||||
#include "shared.h"
|
||||
#include <math.h>
|
||||
|
||||
typedef struct pid_params{
|
||||
// Controller variables - won't change after init
|
||||
float k_i; // Integral gain
|
||||
@@ -20,11 +20,19 @@ typedef struct pid_params{
|
||||
float int_lim_max; // max limit for integrator
|
||||
float int_lim_min; // min limit for integrator
|
||||
float max_output; // max possible pwm output value
|
||||
float k_p_x_1_shift;
|
||||
float k_p_x_1;
|
||||
float k_p_x_2;
|
||||
float k_p_y_0;
|
||||
float k_p_y_1;
|
||||
float k_p_y_2;
|
||||
float k_p_m_1;
|
||||
float k_p_m_2;
|
||||
// Controller variables - might change on runtime
|
||||
float k_p; // Proportional gain
|
||||
float e_prev; // Previous Error
|
||||
float integral_prev; // Previous Integral Value
|
||||
float derivative_prev; // Previous Derivative Value
|
||||
float i_prev; // Previous Integral Value
|
||||
float d_prev; // Previous Derivative Value
|
||||
float measurement_prev; // Previous Measurement
|
||||
float x_1_fwd; // Base Speed; forward direction
|
||||
float x_2_fwd; // High Speed; forward direction
|
||||
@@ -54,13 +62,10 @@ typedef struct pid_params{
|
||||
|
||||
void update_speed_dir(pid_params *pid, uint32_t speed_step_target);
|
||||
bool speed_helper(struct repeating_timer *t);
|
||||
float measure(pid_params *pid,bool direction);
|
||||
void adjust_pwm_level(uint16_t level, pid_params *pid);
|
||||
void update_m(pid_params *pid);
|
||||
void update_y(pid_params *pid,float i);
|
||||
float get_ff_val(pid_params *pid);
|
||||
float get_kp(pid_params *pid);
|
||||
bool pid_control(struct repeating_timer *t);
|
||||
uint16_t measure_base_pwm(pid_params *pid,bool direction);
|
||||
void init_pid(pid_params *pid);
|
||||
void init_motor_pwm(uint8_t gpio);
|
||||
void init_pid(pid_params *pid);
|
||||
@@ -0,0 +1,63 @@
|
||||
//////////////////////////
|
||||
// RP2040-Decoder //
|
||||
// Gabriel Koppenstein //
|
||||
// shared.c //
|
||||
//////////////////////////
|
||||
|
||||
#include "shared.h"
|
||||
|
||||
// Functions in shared.c can be accessed by both cores
|
||||
|
||||
// Measures Back-EMF voltage (proportional to the rotational speed of the motor) on GPIO 28 and GPIO 29 respectively (depending on direction)
|
||||
float measure(uint8_t total_iterations,
|
||||
uint8_t measurement_delay_us,
|
||||
uint8_t l_side_arr_cutoff,
|
||||
uint8_t r_side_arr_cutoff,
|
||||
bool direction){
|
||||
uint32_t sum = 0;
|
||||
uint16_t adc_val[total_iterations];
|
||||
int32_t j = 0;
|
||||
uint16_t key;
|
||||
pwm_set_gpio_level(MOTOR_FWD_PIN, 0);
|
||||
pwm_set_gpio_level(MOTOR_REV_PIN, 0);
|
||||
adc_select_input((!direction)+2);
|
||||
busy_wait_us(measurement_delay_us);
|
||||
adc_run(true);
|
||||
for (int32_t i = 0; i < total_iterations; ++i) {
|
||||
//Take adc value out of fifo when available, then do one step of insertion sort.
|
||||
adc_val[i] = adc_fifo_get_blocking();
|
||||
key = adc_val[i];
|
||||
j = i - 1;
|
||||
while (j >= 0 && adc_val[j] > key) {
|
||||
adc_val[j + 1] = adc_val[j];
|
||||
j--;
|
||||
}
|
||||
adc_val[j + 1] = key;
|
||||
}
|
||||
adc_run(false);
|
||||
//discard x entries beginning from the lowest entry of the array (counting up); x = msr.l_side_arr_cutoff
|
||||
//discard y entries beginning from the highest index of the array (counting down); y = msr.r_side_arr_cutoff
|
||||
for (int32_t i = l_side_arr_cutoff; i < total_iterations - r_side_arr_cutoff ; ++i) {
|
||||
sum += adc_val[i];
|
||||
}
|
||||
float res = (float)sum / (float)( total_iterations - (l_side_arr_cutoff + r_side_arr_cutoff) );
|
||||
// return true to keep the repeating timer active
|
||||
return res;
|
||||
}
|
||||
|
||||
// retrieve 4Bytes from CV_ARRAY_FLASH at once and return 32-Bit value
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_3 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_2 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index+2];
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+3];
|
||||
return (byte_0) + (byte_1<<8) + (byte_2<<16) + (byte_3<<24);
|
||||
}
|
||||
|
||||
|
||||
// retrieve 2Bytes from CV_ARRAY_FLASH at once and return 16-Bit value
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index){
|
||||
uint8_t byte_1 = CV_ARRAY_FLASH[CV_start_index];
|
||||
uint8_t byte_0 = CV_ARRAY_FLASH[CV_start_index+1];
|
||||
return (byte_0) + (byte_1<<8);
|
||||
}
|
||||
@@ -5,10 +5,11 @@
|
||||
//////////////////////////
|
||||
|
||||
#pragma once
|
||||
#include <stdio.h>
|
||||
#include "string.h"
|
||||
#include "pico/float.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "pico/multicore.h"
|
||||
#include "pico/printf.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include "hardware/adc.h"
|
||||
#include "hardware/flash.h"
|
||||
@@ -30,7 +31,15 @@
|
||||
//GPIO pin mask with allowed outputs (AUX & GPIO (configured as outputs))
|
||||
#define GPIO_ALLOWED_OUTPUTS (GPIO_OUTPUT_MASK|AUX_PIN_MASK) & ~(GPIO_ILLEGAL_MASK)
|
||||
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE) //This Offset should be far enough away from program data
|
||||
|
||||
extern repeating_timer_t pid_control_timer;
|
||||
extern repeating_timer_t speed_helper_timer;
|
||||
extern const uint8_t *CV_ARRAY_FLASH;
|
||||
void core1_entry();
|
||||
void core1_entry();
|
||||
float measure(uint8_t total_iterations,
|
||||
uint8_t measurement_delay_us,
|
||||
uint8_t l_side_arr_cutoff,
|
||||
uint8_t r_side_arr_cutoff,
|
||||
bool direction);
|
||||
uint32_t get_32bit_CV (uint16_t CV_start_index);
|
||||
uint16_t get_16bit_CV (uint16_t CV_start_index);
|
||||
Reference in new issue
Block a user