diff --git a/CMakeLists.txt b/CMakeLists.txt index c4eb086..6774443 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/CV.h b/CV.h index a0b5a4d..cc864d0 100644 --- a/CV.h +++ b/CV.h @@ -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 }; diff --git a/core0.c b/core0.c index dd3f0d5..a63dc5a 100644 --- a/core0.c +++ b/core0.c @@ -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); diff --git a/core0.h b/core0.h index c9bd411..4576a29 100644 --- a/core0.h +++ b/core0.h @@ -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); \ No newline at end of file +bool get_direction(bool *direction_ptr); +void init_motor_pwm(uint8_t gpio); +void init_main(); \ No newline at end of file diff --git a/core1.c b/core1.c index b400441..03cf4f7 100644 --- a/core1.c +++ b/core1.c @@ -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(); + } } } \ No newline at end of file diff --git a/core1.h b/core1.h index cf6a755..9d473df 100644 --- a/core1.h +++ b/core1.h @@ -6,7 +6,7 @@ #pragma once #include "shared.h" -#include + 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); \ No newline at end of file +void init_pid(pid_params *pid); \ No newline at end of file diff --git a/shared.c b/shared.c new file mode 100644 index 0000000..4c0e69e --- /dev/null +++ b/shared.c @@ -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); +} \ No newline at end of file diff --git a/shared.h b/shared.h index b59877b..572c8af 100644 --- a/shared.h +++ b/shared.h @@ -5,10 +5,11 @@ ////////////////////////// #pragma once -#include #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(); \ No newline at end of file +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); \ No newline at end of file