From ebdc57e90aa5099b228e0aabac096ddef63b91d9 Mon Sep 17 00:00:00 2001 From: 1nct Date: Sun, 9 Jan 2022 11:30:32 +0100 Subject: [PATCH] - divided decoder.c in core0.c & core1.c - improvements on set_outputs() --- CMakeLists.txt | 16 +-- CV.h | 64 ++++----- decoder.c => core0.c | 305 ++++++++----------------------------------- core0.h | 49 +++++++ core1.c | 197 ++++++++++++++++++++++++++++ core1.h | 29 ++++ decoder.h | 22 ---- shared.h | 19 +++ 8 files changed, 389 insertions(+), 312 deletions(-) rename decoder.c => core0.c (54%) create mode 100644 core0.h create mode 100644 core1.c create mode 100644 core1.h delete mode 100644 decoder.h create mode 100644 shared.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b67de1d..820b682 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.12) # Pull in SDK (must be before project) include(pico_sdk_import.cmake) -project(pico_decoder C CXX ASM) +project(RP2040-Decoder C CXX ASM) set(CMAKE_C_STANDARD 11) set(CMAKE_CXX_STANDARD 17) @@ -19,14 +19,14 @@ add_compile_options(-Wall ) add_executable( - hello_multicore - decoder.c -) + RP2040-Decoder + core0.c + core1.c core1.h shared.h) # Add pico_multicore which is required for multicore functionality target_link_libraries( - hello_multicore + RP2040-Decoder pico_stdlib hardware_pwm hardware_adc @@ -35,8 +35,8 @@ target_link_libraries( ) # enable usb output, disable uart output -pico_enable_stdio_usb(hello_multicore 1) -pico_enable_stdio_uart(hello_multicore 1) +pico_enable_stdio_usb(RP2040-Decoder 1) +pico_enable_stdio_uart(RP2040-Decoder 1) # create map/bin/hex file etc. -pico_add_extra_outputs(hello_multicore) +pico_add_extra_outputs(RP2040-Decoder) diff --git a/CV.h b/CV.h index 38e89ec..5cd8943 100644 --- a/CV.h +++ b/CV.h @@ -2,15 +2,15 @@ //All CVs are 8-bit numbers (range 0 - 255) //Note : CV_1 ≙ index 0, CV_2 ≙ index 1, CV_3 ≙ index 2, ... uint8_t CV_ARRAY_DEFAULT [512] = { - 0b00000111, //CV_1 - Basic Address + 0b00000011, //CV_1 - Basic Address 0b00000000, //CV_2 - Minimum Speed ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // 255 == Fastest dec/acc rate; 0 == slowest; // Note that there is an integer division in play here which will get rid of the fractional part // this means that a value of 254 for example, will result in the same behaviour as an 248. // This does not correspond to the norm and might be changed eventually. - 0b00000010, //CV_3 - Acceleration Rate - 0b00000010, //CV_4 - Deceleration Rate + 0b00000101, //CV_3 - Acceleration Rate + 0b00000101, //CV_4 - Deceleration Rate ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b11111111, //CV_5 - V_max 0b01111111, //CV_6 - V_mid @@ -18,8 +18,8 @@ uint8_t CV_ARRAY_DEFAULT [512] = { 0b00001101, //CV_8 - Manufacturer 0b00000000, //CV_9 - PWM-Period 0b00000001, //CV_10 - PWM Clock Divider - 0b11111111, //CV_11 - Packet Timeout in seconds - 0b00000000, //CV_12 - + 0b11111111, //CV_11 - Packet Timeout in seconds TODO: not implemented yet + 0b00000100, //CV_12 - Permitted operating modes 0b00000000, //CV_13 - 0b00000000, //CV_14 - 0b00000000, //CV_15 - @@ -61,10 +61,10 @@ uint8_t CV_ARRAY_DEFAULT [512] = { 0b00000000, //CV_46 - 0b00000000, //CV_47 - // PID - Configuration ////////////////////////////////////////////////////////////////////////////////////////////// - 0b00000101, //CV_48 - PID Control Sampling Time t_s in ms ≙ Duration of 1 PID-Cycle // - 0b00100110, //CV_49 - PID Control P_Factor ≙ CV_49/1024 i.e. Default = 154 -> 0.1504 // - 0b10000101, //CV_50 - PID Control I_Factor ≙ CV_50/256 i.e. Default = 133 -> 0.5195(1/sec)// - 0b10100011, //CV_51 - PID Control D_Factor ≙ CV_51/16384 i.e. Default = 163 -> 0.009949sec // + 0b00001111, //CV_48 - PID Control Sampling Time t_s in ms ≙ Duration of 1 PID-Cycle // + 0b01110011, //CV_49 - PID Control P_Factor ≙ CV_49/256 i.e. Default = 115 -> 0.4492 // + 0b01100110, //CV_50 - PID Control I_Factor ≙ CV_50/64 i.e. Default = 102 -> 1.594 (1/sec)// + 0b01111011, //CV_51 - PID Control D_Factor ≙ CV_51/4096 i.e. Default = 123 -> 0.03002 sec // 0b00110000, //CV_52 - PID Integral Limit positive ≙ CV_52*10 i.e. Default = 48 -> +480 // 0b00110000, //CV_53 - PID Integral Limit negative ≙ CV_53*(-10) i.e. Default = 48 -> -480 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -74,17 +74,17 @@ uint8_t CV_ARRAY_DEFAULT [512] = { 0b00000000, //CV_57 - 0b00000000, //CV_58 - 0b00000000, //CV_59 - - 0b00010100, //CV_60 - PWM scaling factor (target EMF Voltage) : CV_60 = 16 -> 126*16 = 2016 = end_target + 0b00010100, //CV_60 - PWM scaling factor (target EMF Voltage) -> 126*CV_60 = end_target|max // V_EMF Measurement Configuration ////////////////////////////////////////////////////////////////////////////////// - 0b00110010, //CV_61 - V_EMF Total amount of Measurement Iterations // - 0b01100100, //CV_62 - V_EMF Delay before Measuring // + 0b01000110, //CV_61 - V_EMF Total amount of Measurement Iterations - Default = 70 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 // // This is used to filter out data. // - // For Example: CV_63 = x and CV_64 = y - Default: CV_63 = 10; CV_64 = 10 // + // For Example: CV_63 = x and CV_64 = y // // -> The x smallest values will be discarded. // // -> The y largest values will be discarded. // - 0b00001010, //CV_63 - left_side_array_cutoff // - 0b00001010, //CV_64 - right_side_array_cutoff // + 0b00001111, //CV_63 - left_side_array_cutoff - Default = 15 // + 0b00001111, //CV_64 - right_side_array_cutoff - Default = 15 // // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b00000000, //CV_65 - 0b00000000, //CV_66 - @@ -284,36 +284,36 @@ uint8_t CV_ARRAY_DEFAULT [512] = { // 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 - 0b00010010, //byte 2 - CV_259 + 0b00001110, //byte 1 - CV_258 + 0b00000000, //byte 2 - CV_259 0b00000000, //byte 3 - CV_260 //F0 reverse 0b00000000, //byte 0 - CV_261 - 0b00000000, //byte 1 - CV_262 - 0b00001100, //byte 2 - CV_263 + 0b00001110, //byte 1 - CV_262 + 0b00000000, //byte 2 - CV_263 0b00000000, //byte 3 - CV_264 //F1 forward - 0b00000010, //byte 0 - CV_265 - 0b00000000, //byte 1 - CV_266 + 0b00000000, //byte 0 - CV_265 + 0b00000100, //byte 1 - CV_266 0b00000000, //byte 2 - CV_267 0b00000000, //byte 3 - CV_268 //F1 reverse - 0b00000010, //byte 0 - CV_269 - 0b00000000, //byte 1 - CV_270 + 0b00000000, //byte 0 - CV_269 + 0b00000010, //byte 1 - CV_270 0b00000000, //byte 2 - CV_271 0b00000000, //byte 3 - CV_272 //F2 forward - 0b00000100, //byte 0 - CV_273 + 0b00000000, //byte 0 - CV_273 0b00000000, //byte 1 - CV_274 0b00000000, //byte 2 - CV_275 0b00000000, //byte 3 - CV_276 //F2 reverse - 0b00000100, //byte 0 - CV_277 + 0b00000000, //byte 0 - CV_277 0b00000000, //byte 1 - CV_278 0b00000000, //byte 2 - CV_279 0b00000000, //byte 3 - CV_280 //F3 forward - 0b00001000, //byte 0 - CV_281 + 0b00000000, //byte 0 - CV_281 0b00000000, //byte 1 - CV_282 0b00000000, //byte 2 - CV_283 0b00000000, //byte 3 - CV_284 @@ -563,15 +563,15 @@ uint8_t CV_ARRAY_DEFAULT [512] = { 0b00000000, //byte 2 - CV_479 0b00000000, //byte 3 - CV_480 //F28 forward - 0b00000000, //byte 0 - CV_481 - 0b00000000, //byte 1 - CV_482 + 0b11111100, //byte 0 - CV_481 + 0b00001111, //byte 1 - CV_482 0b00000000, //byte 2 - CV_483 - 0b00010000, //byte 3 - CV_484 + 0b00000000, //byte 3 - CV_484 //F28 reverse - 0b00000000, //byte 0 - CV_485 - 0b00000000, //byte 1 - CV_486 + 0b11111100, //byte 0 - CV_485 + 0b00001111, //byte 1 - CV_486 0b00000000, //byte 2 - CV_487 - 0b00010000, //byte 3 - CV_488 + 0b00000000, //byte 3 - CV_488 //F29 forward 0b00000000, //byte 0 - CV_489 0b00000000, //byte 1 - CV_490 diff --git a/decoder.c b/core0.c similarity index 54% rename from decoder.c rename to core0.c index 1f3978f..847c428 100644 --- a/decoder.c +++ b/core0.c @@ -1,50 +1,22 @@ ////////////////////////// // RP2040-Decoder // // Gabriel Koppenstein // +// core0.c // ////////////////////////// #include "CV.h" -#include "decoder.h" -#include -#include "string.h" -#include "pico/stdlib.h" -#include "pico/multicore.h" -#include "hardware/pwm.h" -#include "hardware/adc.h" -#include "hardware/flash.h" -#include "hardware/irq.h" -#include "stdint.h" +#include "core0.h" +#include "shared.h" + +bool target_direction = true; +uint target_speed_step = 128; const uint8_t *CV_ARRAY_FLASH = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET); bool active_functions[SIZE_ACTIVE_FUNCTIONS] = {false}; uint64_t last_bits = 0; -bool target_direction = true; -uint target_speed_step = 128; -uint current_target = 0; bool reset_package_flag = false; -typedef struct pid_params{ - float k_p; - float t_s; - float a_1; - float a_2; - int sum_limit_max; - int sum_limit_min; - float output; - int e; - int e_prev; - int e_sum; -}pid_params; -pid_params pid; -typedef struct measure_params{ - uint8_t delay_in_us; - uint8_t total_iterations; - uint8_t left_side_array_cutoff; - uint8_t right_side_array_cutoff; -}measure_params; -measure_params msr; -struct repeating_timer pid_control_timer,speed_helper_timer; -int8_t check_for_package() //returns number of bytes if valid bit-pattern is found. Otherwise -1 is returned. -{ + +int8_t check_for_package() { //returns number of bytes if valid bit-pattern is found. Otherwise -1 is returned. uint64_t package3Masked = last_bits & PACKAGE_MASK_3_BYTES; if (package3Masked == PACKAGE_3_BYTES) { return 3; @@ -63,7 +35,7 @@ void writeLastBit(bool bit) { last_bits <<= 1; last_bits |= bit; } -int64_t readBit_alarm_callback(alarm_id_t id, void *user_data){ +int64_t readBit_alarm_callback(alarm_id_t id, void *user_data) { writeLastBit(!gpio_get(DCC_INPUT_PIN)); evaluation(); return 0; @@ -75,55 +47,53 @@ void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) { byte_array[i] = last_bits >> (i * 9 + 1); } } - -void adjust_pwm_level(uint16_t level) -{ - if(target_direction){ +void adjust_pwm_level(uint16_t level) { + if (target_direction) { pwm_set_gpio_level(MOTOR_PWM_REVERSE,0); pwm_set_gpio_level(MOTOR_PWM_FORWARD,level); } //Reverse - else{ + else { pwm_set_gpio_level(MOTOR_PWM_FORWARD,0); pwm_set_gpio_level(MOTOR_PWM_REVERSE,level); } } -void acknowledge(){ +void acknowledge() { target_direction = false; adjust_pwm_level(5000); busy_wait_ms(6); adjust_pwm_level(0); } -void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos){ +void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos) { uint8_t mask = 0b00000001; bool res = ( (CV_ARRAY_FLASH[cv_address] >> bit_pos) &mask ) == bit_val; if (res) { acknowledge(); } + } void verify_cv_byte(uint16_t cv_address, uint8_t cv_data){ if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge(); } -void write_cv_byte(uint16_t cv_address, uint8_t cv_data){ - //CV_7 read-only - if (cv_address == 6){ - } - else{ +void write_cv_byte(uint16_t cv_index, uint8_t cv_data){ + //CV_7 & CV_8 are read-only + if (cv_index == 6 || cv_index == 7){ //Reset all CVs to Default (CV_8; Value = 8) - if (cv_address == 7 && cv_data == 8){ + 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); acknowledge(); } - else{ + } + else if(cv_index == 0 && cv_data == 0); //CV_1 = 0 is not permitted + else{ uint8_t CV_ARRAY_TEMP[512] = {0}; memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP)); - CV_ARRAY_TEMP[cv_address] = cv_data; + 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(); - } } } @@ -145,8 +115,8 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ uint8_t cv_address_ms_bits_mask = 0b00000011; uint16_t cv_address_ms_bits = cv_address_ms_bits_mask & byte_array[number_of_bytes - 1]; uint16_t cv_address = byte_array[number_of_bytes - 2] + (cv_address_ms_bits << 8); - alarm_pool_destroy(pid_control_timer.pool); - alarm_pool_destroy(speed_helper_timer.pool); + if(pid_control_timer.pool)alarm_pool_destroy(pid_control_timer.pool); + if(speed_helper_timer.pool)alarm_pool_destroy(speed_helper_timer.pool); multicore_reset_core1(); uint32_t saved_interrupts = save_and_disable_interrupts(); if (instruction_type == 0b000001000) { @@ -170,32 +140,20 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ } } -void set_outputs() {/* +void set_outputs() { uint32_t GPIO_to_be_set = 0; - //ensures that GPIO's that are used for inputs or PWM (motor) cannot be set HIGH - uint32_t filter_forbidden_GPIO = 0b00000000000111111111111111111110; - for (uint8_t i = 0; i < SIZE_ACTIVE_FUNCTIONS; i++) { + for (uint8_t i = 0; i < 32; i++) { if (active_functions[i]) { - printf("F%u == 1\n",i); uint8_t func_cv_0 = CV_ARRAY_FLASH[260 + i * 8 - 4 * target_direction]; uint8_t func_cv_1 = CV_ARRAY_FLASH[261 + i * 8 - 4 * target_direction]; uint8_t func_cv_2 = CV_ARRAY_FLASH[262 + i * 8 - 4 * target_direction]; uint8_t func_cv_3 = CV_ARRAY_FLASH[263 + i * 8 - 4 * target_direction]; uint32_t func_cv = (func_cv_0) + (func_cv_1 << 8) + (func_cv_2 << 16) + (func_cv_3 << 24); - GPIO_to_be_set = (GPIO_to_be_set | func_cv) & filter_forbidden_GPIO; - uint32_t mask = 1; - for (uint8_t j = 0; j < 32; j++) - { - uint32_t bit_value = (GPIO_to_be_set&mask)>>j; - //printf("GPIO: %u, set to: %d\n",j,bit_value); - //gpio_put(j,bit_value); - mask = mask<<1; - } - // printf("func_cv: %u\n",func_cv); - // printf("func: F%u, direction: %u\n",func,direction); - //printf("func_cv_0_index: %d func_cv_1_index: %d func_cv_2_index: %d func_cv_3_index: %d \n",4+257+i*8-4*direction,4+258+i*8-4*direction,4+259+i*8-4*direction,4+260+i*8-4*direction); + GPIO_to_be_set |= func_cv; } - }*/ + } + GPIO_to_be_set &= ALLOWED_GPIO_MASK; + gpio_put_masked(ALLOWED_GPIO_MASK,GPIO_to_be_set); } void update_active_functions(uint8_t function_number, uint8_t input_byte, uint8_t count) { @@ -261,23 +219,25 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) //0011-1111 (128 Speed Step Control) - 2 Byte length if (command_byte_n == 0b00111111){ target_speed_step = byte_array[command_byte_start_index - 1]; - if(target_speed_step>127) target_direction = true; - else target_direction = false; + if(target_speed_step>127) { + target_direction = true; + } + else { + target_direction = false; + } } // 10XX-XXXX (Function Group Instruction) - if (command_byte_n >> 6 == 0b00000010){ + else if (command_byte_n >> 6 == 0b00000010){ if (command_byte_n >> 5 == 0b00000100) // Functions F0-F4 { - update_active_functions(0, command_byte_n >> 4, 1); //F0 - update_active_functions(1, command_byte_n, 4); //F1-F4 + update_active_functions(0, command_byte_n >> 4, 1); //F0 + update_active_functions(1, command_byte_n, 4); //F1-F4 } else { switch (command_byte_n >> 4) { - // Functions F5-F8 - case 0b00001011: + case 0b00001011: // F5-F8 update_active_functions(5, command_byte_n, 4); break; - // Functions F9-F12 - case 0b00001010: + case 0b00001010: // F9-F12 update_active_functions(9, command_byte_n, 4); break; default: @@ -286,28 +246,24 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) } } //Feature Expansion Instruction 110X-XXXX - if (command_byte_n >> 5 == 0b00000110) + else if (command_byte_n >> 5 == 0b00000110) { switch (command_byte_n) { case 0b11011110: // F13-F20 - // printf("Functions F13-F20 Instruction\n"); update_active_functions(13, byte_array[command_byte_start_index - 1], 8); break; case 0b11011111: // F21-F28 - // printf("Functions F21-F28 Instruction\n"); update_active_functions(21, byte_array[command_byte_start_index - 1], 8); break; - case 0b11011000: // F29-F36 - // printf("Functions F29-F36 Instruction\n"); - update_active_functions(29, byte_array[command_byte_start_index - 1], 8); + case 0b11011000: // F29-F31 + update_active_functions(29, byte_array[command_byte_start_index - 1], 3); break; default: break; } } - //set_outputs(); + set_outputs(); } - //Interrupt handler for DCC Logic Signal void gpio_callback_rise(unsigned int gpio, long unsigned int events) { add_alarm_in_us(87, &readBit_alarm_callback, NULL, true); @@ -335,170 +291,19 @@ void evaluation(){ } } -void quicksort(uint16_t number[msr.total_iterations], int first, int last){ - int i, j, pivot, temp; - if(firstnumber[pivot]) - j--; - if(i current_target && speed_helper_counter == accel_rate) { - current_target += CV_ARRAY_FLASH[59]; - speed_helper_counter = 0; - } - //Deceleration - else if (end_target < current_target && speed_helper_counter == decel_rate) { - current_target -= CV_ARRAY_FLASH[59]; - speed_helper_counter = 0; - } - else{ - speed_helper_counter++; - } - return true; -} - -int measure(){ - pwm_set_gpio_level(MOTOR_PWM_FORWARD,0); - pwm_set_gpio_level(MOTOR_PWM_REVERSE,0); - adc_select_input(!target_direction); - busy_wait_us(msr.delay_in_us); - int sum = 0; - // ≈ 270us - uint16_t adc_values[msr.total_iterations]; - for (int i = 0; i < msr.total_iterations; ++i) { - adc_values[i] = adc_read(); - } - // Worst Case ≈ 415us - quicksort(adc_values, 0, msr.total_iterations - 1); - for (uint8_t i = msr.left_side_array_cutoff; i < msr.total_iterations - msr.right_side_array_cutoff ; ++i) { - sum = sum+adc_values[i]; - } - return sum/(msr.total_iterations - 20); -} - -bool pid_control(struct repeating_timer *t){ - pid.e = (int)current_target - measure(); - pid.e_sum = pid.e_sum + pid.e; - if (pid.e_sum > pid.sum_limit_max) pid.e_sum = pid.sum_limit_max; - else if (pid.e_sum < pid.sum_limit_min) pid.e_sum = pid.sum_limit_min; - pid.output += (pid.k_p * (float)pid.e) + - (pid.a_1 * (float)pid.e_sum) + // k_i*t_s*e_sum - (pid.a_2 * (float)(pid.e - pid.e_prev)); // (k_d/t_s)*(e-e_prev) - pid.e_prev = pid.e; - if (current_target == 0) pid.output = 0; - else if (pid.output > 5000) pid.output = 5000; - else if (pid.output < 1000) pid.output = 1000; - adjust_pwm_level((uint16_t )pid.output); - return true; -} - -void init_pid(){ - pid.k_p = ((float)CV_ARRAY_FLASH[48] / 1024); - pid.t_s = ((float)CV_ARRAY_FLASH[47] / 1000); - pid.a_1 = ((float)CV_ARRAY_FLASH[49] / 256) * pid.t_s ; // k_i*t_s - pid.a_2 = ((float)CV_ARRAY_FLASH[50] / 16384) / pid.t_s; // k_d/t_s // k_d/t_s - pid.sum_limit_max = CV_ARRAY_FLASH[51] * 10; - pid.sum_limit_min = CV_ARRAY_FLASH[52] * (-10); - msr.total_iterations = CV_ARRAY_FLASH[60]; - msr.delay_in_us = CV_ARRAY_FLASH[61]; - msr.left_side_array_cutoff = CV_ARRAY_FLASH[62]; - msr.right_side_array_cutoff = CV_ARRAY_FLASH[63]; -} - -void init_pwm(uint gpio) { - uint slice_num = pwm_gpio_to_slice_num(gpio); - uint32_t wrap_counter = 5000; // 5000 Cycles @ 125MHz for 1 Period -> 25kHz - pwm_set_wrap(slice_num, wrap_counter); - pwm_set_gpio_level(gpio,0); - pwm_set_clkdiv(slice_num,1); - pwm_set_enabled(slice_num, true); -} - -void print_cv_array(const uint8_t *buf, size_t len) { - for (size_t i = 0; i < len; ++i) { - printf("CV_%u: %02x",i+1, buf[i]); - if (i % 4 == 3) - printf("\n"); - else - printf(" "); - } -} - -void core1_entry() { - if(gpio_get_function(MOTOR_PWM_FORWARD)!=4){ - gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM); - gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM); - adc_init(); - adc_gpio_init(V_EMF_ADC_PIN_FORWARD); - adc_gpio_init(V_EMF_ADC_PIN_REVERSE); - init_pwm(MOTOR_PWM_FORWARD); - init_pwm(MOTOR_PWM_REVERSE); - init_pid(); - } - alarm_pool_add_repeating_timer_ms(alarm_pool_create(0,1), - //-CV_ARRAY_FLASH[47], - 5, - pid_control, - NULL, - &pid_control_timer); - irq_set_priority(0,0x00); // 0x00 is equivalent to the highest priority! - alarm_pool_add_repeating_timer_ms(alarm_pool_create(1,1), - 7, - speed_helper, - NULL, - &speed_helper_timer); - while (1); -} - int main() { - busy_wait_ms(1000); + busy_wait_ms(2000); stdio_init_all(); + printf("Launched successfully2.\n"); +// gpio_init(10); +// gpio_set_dir(10,GPIO_OUT); +// gpio_put(10,true); gpio_init(DCC_INPUT_PIN); gpio_set_dir(DCC_INPUT_PIN, GPIO_IN); + gpio_init_mask(ALLOWED_GPIO_MASK); + gpio_set_dir_out_masked(ALLOWED_GPIO_MASK); multicore_launch_core1(core1_entry); gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &gpio_callback_rise); - printf("Launched successfully.\n"); - while (1); + printf("Launched successfully.3\n"); + while (1) ; } \ No newline at end of file diff --git a/core0.h b/core0.h new file mode 100644 index 0000000..99c3e40 --- /dev/null +++ b/core0.h @@ -0,0 +1,49 @@ +////////////////////////// +// RP2040-Decoder // +// Gabriel Koppenstein // +// core0.h // +////////////////////////// + + +#pragma once +#include +#include "string.h" +#include "pico/stdlib.h" +#include "pico/multicore.h" +#include "hardware/pwm.h" +#include "hardware/adc.h" +#include "hardware/flash.h" +#include "hardware/irq.h" +#include "stdint.h" +#define SIZE_BYTE_ARRAY 5 +#define SIZE_ACTIVE_FUNCTIONS 32 +#define DCC_INPUT_PIN 11 //Custom: 11 Default: 14 +//Default +//#define ALLOWED_GPIO_MASK 0b00000000000000000000111111111100 //prevents setting GPIO's that are not allowed (ADC, PWM, ...) +//Custom +#define ALLOWED_GPIO_MASK 0b00000000000000000000001111111100 //prevents setting GPIO's that are not allowed (ADC, PWM, ...) +#define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001 +#define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001 +#define PACKAGE_4_BYTES 0b11111111110000000000000000000000000000000000001 +#define PACKAGE_MASK_4_BYTES 0b11111111111000000001000000001000000001000000001 +#define PACKAGE_5_BYTES 0b11111111110000000000000000000000000000000000000000000001 +#define PACKAGE_MASK_5_BYTES 0b11111111111000000001000000001000000001000000001000000001 +#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE) //This Offset should be far enough away from program data +int8_t check_for_package(); +void writeLastBit(bool bit) ; +int64_t readBit_alarm_callback(alarm_id_t id, void *user_data); +void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) ; +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); +bool reset_package_check(uint8_t number_of_bytes,const uint8_t byte_array[]); +void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]); +void set_outputs() ; +void update_active_functions(uint8_t function_number, uint8_t input_byte, uint8_t count) ; +bool error_detection(int8_t number_of_bytes, const uint8_t byte_array[]) ; +bool is_long_address(uint8_t number_of_bytes, const uint8_t byte_array[]) ; +bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) ; +void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]); +void gpio_callback_rise(unsigned int gpio, long unsigned int events) ; +void evaluation(); \ No newline at end of file diff --git a/core1.c b/core1.c new file mode 100644 index 0000000..516a0c1 --- /dev/null +++ b/core1.c @@ -0,0 +1,197 @@ +////////////////////////// +// RP2040-Decoder // +// Gabriel Koppenstein // +// core1.c // +////////////////////////// + +#include "inttypes.h" +#include "core1.h" +#include "shared.h" +uint current_target = 0; +struct repeating_timer pid_control_timer,speed_helper_timer; + +typedef struct pid_params{ + float k_p; + float t_s; + float a_1; + float a_2; + int sum_limit_max; + int sum_limit_min; + float output; + int e; + int e_prev; + int e_sum; +}pid_params; +pid_params pid; +typedef struct measure_params{ + uint8_t delay_in_us; + uint8_t total_iterations; + uint8_t left_side_array_cutoff; + uint8_t right_side_array_cutoff; +}measure_params; +measure_params msr; + +void quicksort(uint16_t number[], int first, int last){ + int i, j, pivot, temp; + if(firstnumber[pivot]) + j--; + if(i current_target && speed_helper_counter == accel_rate) { + current_target += CV_ARRAY_FLASH[59]; + speed_helper_counter = 0; + } + //Deceleration + else if (end_target < current_target && speed_helper_counter == decel_rate) { + current_target -= CV_ARRAY_FLASH[59]; + speed_helper_counter = 0; + } + //Accel/Decel "scheduled" + else if (end_target != current_target){ + speed_helper_counter++; + } + //Target reached -> Reset Counter + else{ + speed_helper_counter = 0; + } + return true; +} + +int measure(){ + pwm_set_gpio_level(MOTOR_PWM_FORWARD,0); + pwm_set_gpio_level(MOTOR_PWM_REVERSE,0); + adc_select_input(!target_direction); + busy_wait_us(msr.delay_in_us); + gpio_put(9,true); + int sum = 0; + uint16_t adc_values[msr.total_iterations]; + for (int i = 0; i < msr.total_iterations; ++i) { + adc_values[i] = adc_read(); + } + gpio_put(9,false); + quicksort(adc_values, 0, msr.total_iterations - 1); + for (uint8_t i = msr.left_side_array_cutoff; i < msr.total_iterations - msr.right_side_array_cutoff ; ++i) { + sum = sum+adc_values[i]; + } + return sum/(msr.total_iterations - 20); +} + +bool pid_control(struct repeating_timer *t){ + pid.e = (int)current_target - measure(); + pid.e_sum = pid.e_sum + pid.e; + if (pid.e_sum > pid.sum_limit_max) pid.e_sum = pid.sum_limit_max; + else if (pid.e_sum < pid.sum_limit_min) pid.e_sum = pid.sum_limit_min; + pid.output += (pid.k_p * (float)pid.e) + + (pid.a_1 * (float)pid.e_sum) + // k_i*t_s*e_sum + (pid.a_2 * (float)(pid.e - pid.e_prev)); // (k_d/t_s)*(e-e_prev) + pid.e_prev = pid.e; + if (current_target == 0) pid.output = 0; + else if (pid.output > 5000) pid.output = 5000; + else if (pid.output < 1000) pid.output = 1000; + adjust_pwm_level((uint16_t )pid.output); + return true; +} + +void init_pid(){ + // pid.k_p = ((float)CV_ARRAY_FLASH[48] / 256); + // pid.t_s = ((float)CV_ARRAY_FLASH[47] / 1000); + // pid.a_1 = ((float)CV_ARRAY_FLASH[49] / 64) * pid.t_s ; // k_i*t_s + // pid.a_2 = ((float)CV_ARRAY_FLASH[50] / 4096) / pid.t_s; // k_d/t_s + pid.k_p = 0.45f; + pid.t_s = 10.0f / 1000; + pid.a_1 = 1.6f * pid.t_s ; // k_i*t_s + pid.a_2 = 0.03f / pid.t_s; // k_d/t_s + pid.sum_limit_max = CV_ARRAY_FLASH[51] * 10; + pid.sum_limit_min = CV_ARRAY_FLASH[52] * (-10); + msr.total_iterations = 70;/*CV_ARRAY_FLASH[60];*/ + msr.delay_in_us = CV_ARRAY_FLASH[61]; + msr.left_side_array_cutoff = 15;/*CV_ARRAY_FLASH[62];*/ + msr.right_side_array_cutoff = 15;/*CV_ARRAY_FLASH[63];*/ +} + +void init_pwm(uint gpio) { + uint slice_num = pwm_gpio_to_slice_num(gpio); + uint32_t wrap_counter = 5000; // 5000 Cycles @ 125MHz for 1 Period -> 25kHz + pwm_set_wrap(slice_num, wrap_counter); + pwm_set_gpio_level(gpio,0); + pwm_set_clkdiv(slice_num,1); + pwm_set_enabled(slice_num, true); +} + +void print_cv_array(const uint8_t *buf, size_t len) { + for (size_t i = 0; i < len; ++i) { + printf("CV_%u: %02x",i+1, buf[i]); + if (i % 4 == 3) + printf("\n"); + else + printf(" "); + } +} +void core1_entry() { + if(gpio_get_function(MOTOR_PWM_FORWARD)!=4){ + gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM); + gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM); + adc_init(); + adc_gpio_init(V_EMF_ADC_PIN_FORWARD); + adc_gpio_init(V_EMF_ADC_PIN_REVERSE); + init_pwm(MOTOR_PWM_FORWARD); + init_pwm(MOTOR_PWM_REVERSE); + init_pid(); + } + alarm_pool_add_repeating_timer_ms(alarm_pool_create(0,1), + 10/*CV_ARRAY_FLASH[47]*/, + pid_control, + NULL, + &pid_control_timer); + irq_set_priority(0,0x00); // 0x00 is equivalent to the highest priority! + alarm_pool_add_repeating_timer_ms(alarm_pool_create(1,1), + 7, + speed_helper, + NULL, + &speed_helper_timer); + while (1); +} \ No newline at end of file diff --git a/core1.h b/core1.h new file mode 100644 index 0000000..5d39383 --- /dev/null +++ b/core1.h @@ -0,0 +1,29 @@ +////////////////////////// +// RP2040-Decoder // +// Gabriel Koppenstein // +// core1.h // +////////////////////////// + + +#ifndef RP2040_DECODER_CORE1_H +#define RP2040_DECODER_CORE1_H + +#endif //RP2040_DECODER_CORE1_H +#define V_EMF_ADC_PIN_FORWARD 26 +#define V_EMF_ADC_PIN_REVERSE 27 +#include +#include "string.h" +#include "pico/stdlib.h" +#include "pico/multicore.h" +#include "hardware/pwm.h" +#include "hardware/adc.h" +#include "hardware/flash.h" +#include "hardware/irq.h" +#include "stdint.h" +void quicksort(uint16_t number[], int first, int last); +uint calc_end_target(); +bool speed_helper(); +int measure(); +void init_pid(); +void init_pwm(uint gpio); +void print_cv_array(const uint8_t *buf, size_t len); diff --git a/decoder.h b/decoder.h deleted file mode 100644 index 2afdde9..0000000 --- a/decoder.h +++ /dev/null @@ -1,22 +0,0 @@ -////////////////////////// -// RP2040-Decoder // -// Gabriel Koppenstein // -////////////////////////// - -#pragma once -#define SIZE_BYTE_ARRAY 5 -#define SIZE_ACTIVE_FUNCTIONS 69 -#define DCC_INPUT_PIN 11 -#define MOTOR_PWM_FORWARD 10 -#define MOTOR_PWM_REVERSE 13 -#define V_EMF_ADC_PIN_FORWARD 26 -#define V_EMF_ADC_PIN_REVERSE 27 -#define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001 -#define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001 -#define PACKAGE_4_BYTES 0b11111111110000000000000000000000000000000000001 -#define PACKAGE_MASK_4_BYTES 0b11111111111000000001000000001000000001000000001 -#define PACKAGE_5_BYTES 0b11111111110000000000000000000000000000000000000000000001 -#define PACKAGE_MASK_5_BYTES 0b11111111111000000001000000001000000001000000001000000001 -#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES-FLASH_SECTOR_SIZE) //This Offset should be far enough away from program data -void core1_entry(); -void evaluation(); \ No newline at end of file diff --git a/shared.h b/shared.h new file mode 100644 index 0000000..c0a00f2 --- /dev/null +++ b/shared.h @@ -0,0 +1,19 @@ +////////////////////////// +// RP2040-Decoder // +// Gabriel Koppenstein // +// shared.h // +////////////////////////// + + +#ifndef RP2040_DECODER_SHARED_VARS_H +#define RP2040_DECODER_SHARED_VARS_H +#endif //RP2040_DECODER_SHARED_VARS_H +#define MOTOR_PWM_FORWARD 10 //Custom: 10 Default: 12 +#define MOTOR_PWM_REVERSE 13 +extern repeating_timer_t pid_control_timer; +extern repeating_timer_t speed_helper_timer; +extern bool target_direction; +extern uint target_speed_step; +extern const uint8_t *CV_ARRAY_FLASH; +void adjust_pwm_level(uint16_t level); +void core1_entry(); \ No newline at end of file