From f81cf7fb0ba8865b0fb7f636f8c390d93fe4efa8 Mon Sep 17 00:00:00 2001 From: GabrielKoppenstein Date: Thu, 6 Jan 2022 20:34:44 +0100 Subject: [PATCH] - renamed multicore.c to decoder.c and header files respectively - several changes to pid_control and speed_helper - measurements are now being "filtered" (x smallest and y biggest values will be discarded to counteract noisy measurements) - irq priority for core1 adjusted (pid_control irq now has highest priority) --- CMakeLists.txt | 4 +- CV.h | 42 +++--- multicore.c => decoder.c | 268 +++++++++++++++++++++------------------ multicore.h => decoder.h | 45 +++---- 4 files changed, 194 insertions(+), 165 deletions(-) rename multicore.c => decoder.c (75%) rename multicore.h => decoder.h (65%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 92e6524..b67de1d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,16 +20,14 @@ add_compile_options(-Wall add_executable( hello_multicore - multicore.c + decoder.c ) -pico_generate_pio_header(hello_multicore ${CMAKE_CURRENT_LIST_DIR}/pwm.pio) # Add pico_multicore which is required for multicore functionality target_link_libraries( hello_multicore pico_stdlib - hardware_pio hardware_pwm hardware_adc hardware_flash diff --git a/CV.h b/CV.h index 3877ca3..38e89ec 100644 --- a/CV.h +++ b/CV.h @@ -1,4 +1,6 @@ +#include "inttypes.h" //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 0b00000000, //CV_2 - Minimum Speed @@ -55,31 +57,35 @@ uint8_t CV_ARRAY_DEFAULT [512] = { 0b00000000, //CV_42 - 0b00000000, //CV_43 - 0b00000000, //CV_44 - + 0b00000000, //CV_45 - + 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 // + 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 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // This value will indirectly change the acceleration/deceleration rates - 0b00101101, //CV_45 - Speed Helper Function Period in ms - ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //(CV_46*2us)+(CV_47)+(CV_48*1000) is the Total duration of one PID-cycle i.e. the sampling time t_s - 0b00110010, //CV_46 - V_EMF Measurement Iterations - 0b01100100, //CV_47 - V_EMF Measurement Delay in us - 0b10001010, //CV_48 - V_EMF Measurement Period duration in ms / equals 1 PID-Cycle - ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - 0b01011100, //CV_49 - PID Control P_Factor = CV_49/1024 i.e. Default = 92 -> 0.08984375 - 0b01001101, //CV_50 - PID Control I_Factor = CV_50/1024 i.e. Default = 77 -> 0.0751953125 - 0b10000011, //CV_51 - PID Control D_Factor = CV_51/16384 i.e. Default = 131 -> 0.00799560547 - 0b00110000, //CV_52 - PID Integral Limit positive = CV_52*10 i.e. Default = 100 -> +1000 - 0b00110000, //CV_53 - PID Integral Limit negative = CV_53*(-10) i.e. Default = 100 -> -1000 0b00000000, //CV_54 - 0b00000000, //CV_55 - 0b00000000, //CV_56 - 0b00000000, //CV_57 - 0b00000000, //CV_58 - 0b00000000, //CV_59 - - 0b00000000, //CV_60 - - 0b00000000, //CV_61 - - 0b00000000, //CV_62 - - 0b00000000, //CV_63 - - 0b00000000, //CV_64 - + 0b00010100, //CV_60 - PWM scaling factor (target EMF Voltage) : CV_60 = 16 -> 126*16 = 2016 = end_target + // V_EMF Measurement Configuration ////////////////////////////////////////////////////////////////////////////////// + 0b00110010, //CV_61 - V_EMF Total amount of Measurement Iterations // + 0b01100100, //CV_62 - V_EMF Delay before Measuring // + // 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 // + // -> 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 // + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b00000000, //CV_65 - 0b00000000, //CV_66 - 0b00000000, //CV_67 - diff --git a/multicore.c b/decoder.c similarity index 75% rename from multicore.c rename to decoder.c index 0da4f87..1f3978f 100644 --- a/multicore.c +++ b/decoder.c @@ -1,8 +1,10 @@ -//////////////////////////////////// -// RP2040-Decoder // -// created by Gabriel Koppenstein // -//////////////////////////////////// +////////////////////////// +// RP2040-Decoder // +// Gabriel Koppenstein // +////////////////////////// +#include "CV.h" +#include "decoder.h" #include #include "string.h" #include "pico/stdlib.h" @@ -11,32 +13,37 @@ #include "hardware/adc.h" #include "hardware/flash.h" #include "hardware/irq.h" -#include "CV.h" -#include "multicore.h" -#include "inttypes.h" +#include "stdint.h" 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_v_emf_target = 300; -uint measurement = 0; +uint current_target = 0; bool reset_package_flag = false; typedef struct pid_params{ float k_p; - float k_i; - float k_d; 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_config; -uint measurement_delay_in_us; -uint measurement_iterations; -struct repeating_timer speed_helper_timer; -struct repeating_timer pid_control_timer; +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() //function 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) { @@ -69,7 +76,18 @@ void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) { } } - +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{ + pwm_set_gpio_level(MOTOR_PWM_FORWARD,0); + pwm_set_gpio_level(MOTOR_PWM_REVERSE,level); + } +} void acknowledge(){ target_direction = false; adjust_pwm_level(5000); @@ -106,7 +124,6 @@ void write_cv_byte(uint16_t cv_address, uint8_t cv_data){ flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_TEMP, FLASH_PAGE_SIZE * 2); acknowledge(); } - } } @@ -119,6 +136,7 @@ bool reset_package_check(uint8_t number_of_bytes,const uint8_t byte_array[]){ return false; } } + void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ //Check for valid programming command ("address" 112-127) if (byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111){ @@ -128,6 +146,7 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ 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); multicore_reset_core1(); uint32_t saved_interrupts = save_and_disable_interrupts(); if (instruction_type == 0b000001000) { @@ -151,7 +170,7 @@ 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; @@ -176,7 +195,7 @@ void set_outputs() { // 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); } - } + }*/ } void update_active_functions(uint8_t function_number, uint8_t input_byte, uint8_t count) { @@ -244,7 +263,6 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) target_speed_step = byte_array[command_byte_start_index - 1]; if(target_speed_step>127) target_direction = true; else target_direction = false; - init_speed_helper(); } // 10XX-XXXX (Function Group Instruction) if (command_byte_n >> 6 == 0b00000010){ @@ -287,13 +305,14 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) 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); } + void evaluation(){ int8_t number_of_bytes = check_for_package(); if (number_of_bytes != -1) { @@ -316,67 +335,116 @@ void evaluation(){ } } -void adjust_pwm_level(uint16_t level) -{ - //absolute_time_t from = get_absolute_time(); - //Forward - if(target_direction){ - pwm_set_gpio_level(MOTOR_PWM_REVERSE,0); - pwm_set_gpio_level(MOTOR_PWM_FORWARD,level); +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(measurement_delay_in_us); - uint sum = 0; - for (int i = 0; i < measurement_iterations; ++i) { - sum = sum+adc_read(); + 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(); } - measurement = (int)sum / measurement_iterations; + // 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){ - static float output; - static int error; - static int e_old; - static int e_sum; - measure(); - error = (int)current_v_emf_target - (int)measurement; - e_sum = e_sum + error; - if (e_sum > pid_config.sum_limit_max) e_sum = pid_config.sum_limit_max; - if (e_sum < pid_config.sum_limit_min) e_sum = pid_config.sum_limit_min; - output += (pid_config.k_p * (float)error) + - (pid_config.k_i * pid_config.t_s * (float)e_sum) + - (pid_config.k_d/pid_config.t_s) * (float)(error - e_old); - e_old = error; - if (output > 5000) output = 5000; - if (output < 1400) output = 1400; - if (current_v_emf_target == 0) output = 0; - adjust_pwm_level((uint16_t )output); + 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_config.k_d = (float)CV_ARRAY_FLASH[48]/1024; - pid_config.k_i = (float)CV_ARRAY_FLASH[49]/1024; - pid_config.k_d = (float)CV_ARRAY_FLASH[50]/16384; - pid_config.t_s = (float)CV_ARRAY_FLASH[47]/1000; - pid_config.sum_limit_max = CV_ARRAY_FLASH[51]*10; - pid_config.sum_limit_min = CV_ARRAY_FLASH[52]*(-10); - measurement_delay_in_us = CV_ARRAY_FLASH[46]; - measurement_iterations = CV_ARRAY_FLASH[45]; - //printf("PID variables:\nk_p = %f;\nk_i = %f;\nk_d = %f;\nt_s = %f;\n",pid_config.k_p,pid_config.k_i,pid_config.k_d,pid_config.t_s); + 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) { @@ -386,52 +454,8 @@ void init_pwm(uint gpio) { pwm_set_gpio_level(gpio,0); pwm_set_clkdiv(slice_num,1); pwm_set_enabled(slice_num, true); - //printf("PWM on GPIO %u was initialized.\n",gpio); } -uint calc_end_v_emf_target(){ - //Forward - if(target_direction){ - if (target_speed_step == 128) return 0; - else return (target_speed_step - 129) * 16; - } - //Reverse - else { - if (target_speed_step == 0) return 0; - else return (target_speed_step-1) * 16; - } -} -bool speed_helper(struct repeating_timer *t) { - //Emergency Stop - if (target_speed_step == 129 || target_speed_step==1) { current_v_emf_target = 0; return false; } - else{ - uint end_v_emf_target = calc_end_v_emf_target(); - if (end_v_emf_target > current_v_emf_target) current_v_emf_target += 16; - else if(end_v_emf_target < current_v_emf_target) current_v_emf_target -= 16; - else return false; - } - return true; -} - -void init_speed_helper(){ - uint end_v_emf_target = calc_end_v_emf_target(); - //Acceleration - if(end_v_emf_target > current_v_emf_target){ - int64_t acceleration_rate = -CV_ARRAY_FLASH[2]*7111; - //Timer already running - if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = acceleration_rate; - //Timer not running - else alarm_pool_add_repeating_timer_us(alarm_pool_create(1,1),acceleration_rate, speed_helper, NULL, &speed_helper_timer); - } - //Deceleration - if(end_v_emf_target < current_v_emf_target){ - int64_t deceleration_rate = -CV_ARRAY_FLASH[3]*7111; - //Timer already running - if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = deceleration_rate; - //Timer not running - else alarm_pool_add_repeating_timer_us(alarm_pool_create(1,1),deceleration_rate, speed_helper, NULL, &speed_helper_timer); - } -} 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]); @@ -452,23 +476,27 @@ void core1_entry() { init_pwm(MOTOR_PWM_FORWARD); init_pwm(MOTOR_PWM_REVERSE); init_pid(); - init_speed_helper(); } alarm_pool_add_repeating_timer_ms(alarm_pool_create(0,1), - -CV_ARRAY_FLASH[47], + //-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() { - sleep_ms(1000); + busy_wait_ms(1000); stdio_init_all(); gpio_init(DCC_INPUT_PIN); - gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(DCC_INPUT_PIN, GPIO_IN); - gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); 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"); diff --git a/multicore.h b/decoder.h similarity index 65% rename from multicore.h rename to decoder.h index 09ac521..2afdde9 100644 --- a/multicore.h +++ b/decoder.h @@ -1,25 +1,22 @@ -//////////////////////////////////// -// RP2040-Decoder // -// created by Gabriel Koppenstein // -//////////////////////////////////// - -#pragma once -#define SIZE_BYTE_ARRAY 5 -#define SIZE_ACTIVE_FUNCTIONS 69 -#define DCC_INPUT_PIN 11 -#define MOTOR_PWM_FORWARD 12 -#define MOTOR_PWM_REVERSE 13 -#define V_EMF_ADC_PIN_FORWARD 27 -#define V_EMF_ADC_PIN_REVERSE 26 -#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 init_speed_helper(); -void adjust_pwm_level(uint16_t level); -void gpio_callback_rise(unsigned int gpio, long unsigned int events); +////////////////////////// +// 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