diff --git a/CV.h b/CV.h index 4f8c0d8..a0b5a4d 100644 --- a/CV.h +++ b/CV.h @@ -5,12 +5,12 @@ //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 - 0b00001000, //CV_2 - Minimum speed + 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; - 0b00000010, //CV_3 - Acceleration rate - CV_3*CV_59 = Time for one discrete speed step change in ms - 0b00000010, //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_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 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 0b01111111, //CV_5 - V_max 0b00111111, //CV_6 - V_mid @@ -60,8 +60,8 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = { 0b00000000, //CV_45 - 0b00000000, //CV_46 - // PID - Configuration ////////////////////////////////////////////////////////////////////////////////////////////// - 0b00000010, //CV_47 - PID Control sampling time t_s in ms for speed step < 2 // - 0b00000101, //CV_48 - PID Control sampling time t_s in ms for speed step > 2 // + 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 // @@ -83,8 +83,8 @@ uint8_t CV_ARRAY_DEFAULT [CV_ARRAY_SIZE] = { // 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 - left_side_array_cutoff - Default = 15 // - 0b00001111, //CV_64 - right_side_array_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 - diff --git a/core0.c b/core0.c index 9789d81..dd3f0d5 100644 --- a/core0.c +++ b/core0.c @@ -5,14 +5,13 @@ ////////////////////////// #include "core0.h" -bool target_direction = true; //Forward = true - Reverse = false -uint target_speed_step = 128; //128 = Stop (Forward Direction) const uint8_t *CV_ARRAY_FLASH = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET); -uint64_t last_bits = 0; +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}; + uint16_t two_sigma(const uint16_t arr[], uint8_t length){ //Calculate arithmetic average uint32_t sum = 0; @@ -41,15 +40,18 @@ uint16_t two_sigma(const uint16_t arr[], uint8_t length){ //Calculate and return "new" arithmetic average from "filtered" array return sum / 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 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); - uint16_t rev = measure(false); - offsets[i] = (fwd+rev)/2; + //uint16_t fwd = measure(true); //TODO: function needs to be rewritten without measure() + //uint16_t rev = measure(false); + //offsets[i] = (fwd+rev)/2; } uint8_t offsets_fwd_avg = two_sigma(offsets, length); uint8_t CV_ARRAY_TEMP[CV_ARRAY_SIZE]; @@ -58,6 +60,10 @@ void adc_offset_adjustment(uint8_t length){ flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_TEMP, FLASH_PAGE_SIZE * 2); } + + +// Confirmation after writing or verifying a CV +// Works by creating a spike in current consumption, this is detected by the command station void acknowledge() { uint16_t max_lvl = _125M/(CV_ARRAY_FLASH[8]*100+10000); pwm_set_gpio_level(MOTOR_FWD_PIN, max_lvl); @@ -67,6 +73,9 @@ void acknowledge() { busy_wait_ms(3); pwm_set_gpio_level(MOTOR_REV_PIN, 0); } + + +// When Bit matches sent byte -> acknowledge() 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; @@ -74,9 +83,16 @@ void verify_cv_bit(uint16_t cv_address,bool bit_val, uint8_t bit_pos) { acknowledge(); } } + + +// When Byte matches sent byte -> acknowledge() void verify_cv_byte(uint16_t cv_address, uint8_t cv_data){ if (CV_ARRAY_FLASH[cv_address] == cv_data) 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) @@ -98,18 +114,25 @@ void write_cv_byte(uint16_t cv_index, uint8_t cv_data){ acknowledge(); } } + + +// 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[]){ - //Check for valid programming command ("address" 112-127) + //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; uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1]; 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); + + // Before accessing flash, timers and interrupts need to be disabled and core1 needs to be shut down. 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(); + + // Verify CV bit instruction if (instruction_type == 0b000001000) { uint8_t bit_pos_mask = 0b00000111; uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3]; @@ -118,19 +141,30 @@ void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ bool bit_val = bit_val_uint; verify_cv_bit(cv_address, bit_val, bit_pos); } + + // Verify CV byte instruction else if (instruction_type == 0b000000100) { uint8_t cv_data = byte_array[number_of_bytes - 3]; verify_cv_byte(cv_address, cv_data); } + + // 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); } + + // Restore interrupts and launch start core1 restore_interrupts(saved_interrupts); multicore_launch_core1(core1_entry); - multicore_fifo_pop_blocking(); //wait for core1 launch before proceeding + + //wait for core1 launch before proceeding + multicore_fifo_pop_blocking(); } } + + +// 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]; @@ -138,11 +172,17 @@ uint32_t get_32bit_CV (uint16_t CV_start_index){ 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) @@ -150,7 +190,7 @@ void set_outputs(uint32_t functions_to_set_bitmask) { 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 * target_direction); + outputs_to_set |= get_32bit_CV(i * 8 + 260 - 4 * get_direction(0)); } temp_mask<<=1; } @@ -171,6 +211,9 @@ void set_outputs(uint32_t functions_to_set_bitmask) { temp_mask<<=1; } } + + +// Update functions F0 - F31 when function command is received or in case of direction change void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, bool direction_change) { static uint32_t active_functions; static uint32_t prev_active_func; @@ -186,27 +229,35 @@ void update_active_functions(uint32_t new_function_bitmask, uint8_t clr_bit_ind, prev_active_func = active_functions; } } + + +// Bitwise XOR for all Bytes -> Successful result is: "0000 0000" bool error_detection(int8_t number_of_bytes, const uint8_t byte_array[]) { - //Bitwise XOR for all Bytes -> Successful result is: "0000 0000" uint8_t xor_byte = 0; for (int i = 0; i < number_of_bytes; i++) { xor_byte = xor_byte ^ byte_array[i]; } return (0 == xor_byte); } -// Returns true for long address + + +// Check for long address. Function returns true for long address. bool is_long_address(uint8_t number_of_bytes, const uint8_t 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 if (byte_array[number_of_bytes - 1] == 255) { return false; } - uint16_t read_address; + //Long Address Package + 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 @@ -215,8 +266,9 @@ bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) { 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); + return (long_address == read_address); } + //Short Address Package else { @@ -225,10 +277,14 @@ bool address_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) { return (CV_ARRAY_FLASH[0] == read_address); } } + + +// Evaluate the type of instruction void instruction_evaluation(uint8_t number_of_bytes,const uint8_t 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; uint8_t command_byte_start_index; - // start of transmission -> ... -> command_byte_n -> ... -> command_byte_0 -> ... -> end of transmission if (is_long_address(number_of_bytes,byte_array)) { command_byte_start_index = number_of_bytes - 3; } @@ -236,18 +292,16 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) command_byte_start_index = number_of_bytes - 2; } command_byte_n = byte_array[command_byte_start_index]; + //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) { - target_direction = true; //Change direction variable to forward (true) - update_active_functions(0,0,1); //Some functions depend on direction - } - else if (target_speed_step<128 && target_direction){ - target_direction = false; //Change direction variable to reverse (false) - update_active_functions(0,0,1); //Some functions depend on direction - } + bool prev_dir = get_direction(0); + 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); } + // 10XX-XXXX (Function Group Instruction) else if (command_byte_n >> 6 == 0b00000010){ switch (command_byte_n >> 4) { @@ -262,6 +316,7 @@ void instruction_evaluation(uint8_t number_of_bytes,const uint8_t byte_array[]) break; } } + //Feature Expansion Instruction 110X-XXXX else if (command_byte_n >> 5 == 0b00000110) { switch (command_byte_n) { @@ -279,41 +334,49 @@ 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[]){ if (byte_array[number_of_bytes-1] == 0b00000000 && byte_array[number_of_bytes-2] == 0b00000000){ - target_speed_step = 1; - //update_active_functions(0,0,37); + uint32_t speed_step = 1; + multicore_fifo_push_blocking(speed_step); + update_active_functions(0,0,0); //TODO: is this correct? return true; } else { return false; } } + + +// Checks for valid package - looking for correct preamble 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; + uint64_t package3Masked = input_bit_buffer & PACKAGE_MASK_3_BYTES; if (package3Masked == PACKAGE_3_BYTES) { return 3; } - uint64_t package4Masked = last_bits & PACKAGE_MASK_4_BYTES; + uint64_t package4Masked = input_bit_buffer & PACKAGE_MASK_4_BYTES; if (package4Masked == PACKAGE_4_BYTES) { return 4; } - uint64_t package5Masked = last_bits & PACKAGE_MASK_5_BYTES; + uint64_t package5Masked = input_bit_buffer & PACKAGE_MASK_5_BYTES; if (package5Masked == PACKAGE_5_BYTES) { return 5; } return -1; } -void writeLastBit(bool bit) { - last_bits <<= 1; - last_bits |= bit; -} + + //start of transmission -> byte_n(address byte) -> ... -> byte_0(error detection byte) -> end of transmission void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) { for (uint8_t i = 0; i < number_of_bytes; i++) { - byte_array[i] = last_bits >> (i * 9 + 1); + byte_array[i] = input_bit_buffer >> (i * 9 + 1); } } + + +// Sequence of bits gets evaluated void evaluation(){ int8_t number_of_bytes = check_for_package(); if (number_of_bytes != -1) { @@ -335,27 +398,42 @@ void evaluation(){ } } } + + +// DCC-Signal (GPIO 21) rising edge interrupt void track_signal_rise(unsigned int gpio, long unsigned int events) { + // Save current timer value rising_edge_time = get_absolute_time(); + // Disable rising edge interrupt and enable falling edge interrupt gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, false, &track_signal_rise); gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_FALL, true, &track_signal_fall); } + + +// DCC-Signal (GPIO 21) falling edge interrupt void track_signal_fall(unsigned int gpio, long unsigned int events) { + // Save current timer value falling_edge_time = get_absolute_time(); + // Time difference is equal to the time the signal was in a logical high state int64_t time_logical_high = absolute_time_diff_us(rising_edge_time,falling_edge_time); - if(time_logical_high > 87) { - writeLastBit(0); - } - else { - writeLastBit(1); - } + // When logical high was longer than 87us write 0 bit into buffer otherwise 1 + bool bit; + if(time_logical_high > 87) bit = 0; + else bit = 1; + input_bit_buffer <<= 1; + input_bit_buffer |= bit; + // evaluate sequence of bits saved in buffer evaluation(); + // Disable falling edge interrupt and enable rising edge interrupt gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_FALL, false, &track_signal_fall); gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &track_signal_rise); } + + +// Output initialization function - configures pwm frequency and clock divider according to CV_ARRAY_FLASH[] void init_outputs(){ gpio_init_mask(GPIO_ALLOWED_OUTPUTS); - gpio_set_dir_out_masked(GPIO_ALLOWED_OUTPUTS); //note: This also disables UART on GPIO0 & GPIO1 + 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 slice, channel; uint16_t wrap, level; @@ -376,9 +454,13 @@ void init_outputs(){ temp_mask<<=1; } } + + +// 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. - multicore_fifo_pop_blocking(); //wait for core1 launch before proceeding 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) @@ -389,10 +471,27 @@ void cv_setup_check(){ program_mode(4,arr); //ADC offset adjustment (write CV_7 = 7) } } + + +// This function ensures that the direction pointer can only be written once. +// After first initialization this function can be called with arbitrary input to get the current direction +bool get_direction(bool *direction_ptr){ + static bool set_flag; + static bool *dir_ptr; + if(!set_flag) { + dir_ptr = direction_ptr; + set_flag = true; + } + return *dir_ptr; +} + + 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()); cv_setup_check(); init_outputs(); gpio_init(DCC_INPUT_PIN); diff --git a/core0.h b/core0.h index b4de3f9..c9bd411 100644 --- a/core0.h +++ b/core0.h @@ -33,17 +33,17 @@ 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 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[]) ; -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 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[]); +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[]); bool reset_package_check(uint8_t number_of_bytes,const uint8_t byte_array[]); int8_t check_for_package(); -void writeLastBit(bool bit) ; -void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) ; +void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]); 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(); -void cv_setup_check(); \ No newline at end of file +void cv_setup_check(); +bool get_direction(bool *direction_ptr); \ No newline at end of file diff --git a/core1.c b/core1.c index 6de7a54..b400441 100644 --- a/core1.c +++ b/core1.c @@ -5,87 +5,61 @@ ////////////////////////// #include "core1.h" - -uint32_t latest_target = 0; -uint8_t latest_target_index = 0; -uint16_t speed_table[127] = {0}; struct repeating_timer pid_control_timer,speed_helper_timer; -typedef struct pid_params{ - int32_t k_p; - int32_t k_i; - int32_t k_d; - int32_t t_s; - int32_t sum_limit_max; - int32_t sum_limit_min; - int32_t output; - int32_t e; - int32_t e_prev; - int32_t e_sum; - int32_t offset; - int32_t offset_base; - uint16_t max_output; - bool direction_prev; -}pid_params; -pid_params pid; -typedef struct measure_params{ - uint32_t val; - uint8_t delay_in_us; - uint8_t total_iterations; - uint8_t left_side_array_cutoff; - uint8_t right_side_array_cutoff; - uint8_t adc_offset; -}measure_params; -measure_params msr; - - -uint16_t calc_end_target(){ - //Forward - if(target_direction){ - if (target_speed_step == 128) return 0; - else return (target_speed_step-129); +// 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; + // Forward + if(speed_step_target > 127) { + pid->direction = true; + if (pid->speed_step_target == 128) pid->end_target_index = 0; + else pid->end_target_index = (pid->speed_step_target-129); } - //Reverse + // Reverse else { - if (target_speed_step == 0) return 0; - else return (target_speed_step-1); + pid->direction = false; + if (pid->speed_step_target == 0) pid->end_target_index = 0; + else pid->end_target_index = (pid->speed_step_target-1); } } -bool speed_helper() { + +bool speed_helper(struct repeating_timer *t) { + pid_params *pid = (pid_params *)(t->user_data); uint8_t accel_rate = CV_ARRAY_FLASH[2]; uint8_t decel_rate = CV_ARRAY_FLASH[3]; - uint16_t end_target_index = calc_end_target(); static uint8_t speed_helper_counter; - //latest_target only gets adjusted when the counter equals the acc/dec rate + 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 - if (target_speed_step == 129 || target_speed_step==1) { - latest_target_index = 0; + if (pid->speed_step_target == 129 || pid->speed_step_target==1) { + speed_table_index = 0; speed_helper_counter = 0; - latest_target = speed_table[latest_target_index]; + pid->setpoint = pid->speed_table[speed_table_index]; } //Acceleration - else if (end_target_index > latest_target_index && speed_helper_counter == accel_rate) { - if(!accel_rate) latest_target_index = end_target_index; // directly accelerate to end target speed - else latest_target_index++; - latest_target = speed_table[latest_target_index]; + 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 - else if (end_target_index < latest_target_index && speed_helper_counter == decel_rate) { - if(!decel_rate) latest_target_index = end_target_index; // directly decelerate to end target speed - else latest_target_index--; - latest_target = speed_table[latest_target_index]; + 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" - else if (end_target_index != latest_target_index) speed_helper_counter++; + else if (pid->end_target_index != speed_table_index) speed_helper_counter++; //Target reached -> Reset Counter else speed_helper_counter = 0; return true; } -uint32_t measure(bool direction){ +float measure(pid_params *pid, bool direction){ uint32_t sum = 0; uint16_t adc_val[100 /*msr.total_iterations*/]; int32_t j = 0; @@ -93,9 +67,9 @@ uint32_t measure(bool direction){ pwm_set_gpio_level(MOTOR_FWD_PIN, 0); pwm_set_gpio_level(MOTOR_REV_PIN, 0); adc_select_input((!direction)+2); - busy_wait_us(msr.delay_in_us); + busy_wait_us(pid->msr_delay_in_us); adc_run(true); - for (int32_t i = 0; i < msr.total_iterations; ++i) { + 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]; @@ -107,16 +81,17 @@ uint32_t measure(bool direction){ adc_val[j + 1] = key; } adc_run(false); - //discard x entries beginning from the lowest entry of the array (counting up); x = msr.left_side_array_cutoff - //discard y entries beginning from the highest index of the array (counting down); y = msr.right_side_array_cutoff - for (int32_t i = msr.left_side_array_cutoff; i < msr.total_iterations - msr.right_side_array_cutoff ; ++i) { + //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]; } - uint32_t res = sum / ( msr.total_iterations - (msr.left_side_array_cutoff+msr.right_side_array_cutoff) ); + 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; } -void adjust_pwm_level(uint16_t level) { - if (target_direction) { +void adjust_pwm_level(uint16_t level, pid_params *pid) { + if (pid->direction) { pwm_set_gpio_level(MOTOR_REV_PIN, 0); pwm_set_gpio_level(MOTOR_FWD_PIN, level); } @@ -126,73 +101,157 @@ void adjust_pwm_level(uint16_t level) { pwm_set_gpio_level(MOTOR_REV_PIN, level); } } -bool pid_control(struct repeating_timer *t){ - if(target_direction != pid.direction_prev || !latest_target) { - pid.e_sum = 0; - pid.offset = pid.offset_base; - } - msr.val = measure(target_direction); - pid.e = (int32_t)latest_target - (int32_t)msr.val + msr.adc_offset; - 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*pid.e + pid.k_i*pid.t_s*pid.e_sum + ( pid.k_d*(pid.e-pid.e_prev) / pid.t_s ) ) / 1000; - pid.output += pid.offset; - if (latest_target_index < 5){ - pid.output += 20*pid.e; //"compensate" friction - pid.offset_base = pid.offset; //"baseline" pwm offset level - } - if (pid.output < 0 || !latest_target) pid.output = 0; - else if (pid.output > pid.max_output) pid.output = pid.max_output; - adjust_pwm_level(pid.output); - adc_fifo_drain(); - pid.direction_prev = target_direction; - pid.e_sum += pid.e; - pid.e_prev = pid.e; - if (pid.e_sum < pid.sum_limit_min) pid.e_sum = pid.sum_limit_min; - else if (pid.e_sum > pid.sum_limit_max) pid.e_sum = pid.sum_limit_max; - // Adjust offset when the absolute value of e_sum is half of max - if (pid.e_sum > pid.sum_limit_max/2 || pid.e_sum < pid.sum_limit_min/2){ - pid.offset += 3*(pid.e_sum/pid.sum_limit_max); - } - if (pid.offset > 5000) pid.offset = 5000; - else if (pid.offset < 1) pid.offset = 1; - // Increase sampling time for higher speed steps - Decrease for lower speed steps - pid_control_timer.delay_us = (8000*latest_target_index)/126 + 2000; - pid.t_s = ((8000*latest_target_index)/126 + 2000)/1000; - printf("%d\n",pid.e_sum); - return true; - //printf("off:%d\n",pid.offset); +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); } -void init_speed_table(){ +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); + } + // Reverse + else{ + pid->y_2_rev += i*pid->k_ff; + printf("%f\n",pid->y_2_rev); + } + update_m(pid); +} +float get_ff_val(pid_params *pid){ + float val; + // Forward + if(pid->direction){ + val = pid->m_fwd*((float)pid->setpoint-pid->x_1_fwd)+pid->y_1_fwd; + } + // Reverse + else{ + val = pid->m_rev*((float)pid->setpoint-pid->x_1_rev)+pid->y_1_rev; + } + return val; +} +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; + } + else{ + k_p = m_2*(sp-x_1)+y_2; + } + return k_p; +} +bool pid_control(struct repeating_timer *t){ + pid_params *pid = (pid_params *)(t->user_data); + // Acceleration from standstill or change in direction -> previous derivative and integral parts are reset + if((!pid->setpoint_prev && !pid->setpoint) || (pid->direction != pid->direction_prev)){ + pid->derivative_prev = 0.0f; + pid->integral_prev = 0.0f; + } + 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 + float e = (float)pid->setpoint - (pid->measurement - pid->adc_offset); + // Proportional + float p = pid->k_p * e; + // Integral + float i = pid->ci_0*(e + pid->e_prev) + pid->integral_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) + 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 + 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(); + // 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 + pid->e_prev = e; + pid->integral_prev = i; + pid->derivative_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; +} + +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; +} +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]; + // 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]; double v_mid = (double)CV_ARRAY_FLASH[5]*16; double v_max = (double)CV_ARRAY_FLASH[4]*16; double m_1 = (v_mid-v_min)/62; double m_2 = (v_max-v_mid)/63; + pid->speed_table[0] = 0; for (uint8_t i = 1; i < 63; ++i) { - speed_table[i] = (uint16_t)lround(m_1*(i-1)+v_min); + pid->speed_table[i] = (uint16_t)lround(m_1*(i-1)+v_min); } for (uint8_t i = 63; i < 127; ++i) { - speed_table[i] = (uint16_t)lround(m_2*(i-63)+v_mid); + pid->speed_table[i] = (uint16_t)lround(m_2*(i-63)+v_mid); } -} -void init_pid(){ - pid.k_p = 2000; - pid.k_i = 50; - pid.k_d = 1000; - pid.t_s = CV_ARRAY_FLASH[46]; // Default: (CV_ARRAY_FLASH[46]) = 2 - pid.offset = 2500; - pid.sum_limit_max = 1000; /*(float)(CV_ARRAY_FLASH[51] );*/ - pid.sum_limit_min = -1000; /*(float)(CV_ARRAY_FLASH[52] * (-1));*/ - pid.direction_prev = target_direction; - pid.max_output = _125M/(CV_ARRAY_FLASH[8]*100+10000); -} -void init_msr(){ - msr.total_iterations = 100;/*CV_ARRAY_FLASH[60];*/ - msr.delay_in_us = 100; /*CV_ARRAY_FLASH[61];*/ - msr.left_side_array_cutoff = 15;/*CV_ARRAY_FLASH[62];*/ - msr.right_side_array_cutoff = 15;/*CV_ARRAY_FLASH[63];*/ - msr.adc_offset = CV_ARRAY_FLASH[171]; + // 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; + 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->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; + update_m(pid); } void init_motor_pwm(uint8_t gpio) { uint16_t wrap_counter = _125M/(CV_ARRAY_FLASH[8]*100+10000); // 125MHz / f_pwm @@ -202,6 +261,8 @@ void init_motor_pwm(uint8_t gpio) { 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){ @@ -214,20 +275,27 @@ void core1_entry() { init_motor_pwm(MOTOR_FWD_PIN); init_motor_pwm(MOTOR_REV_PIN); } - init_speed_table(); - init_msr(); - init_pid(); + 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], pid_control, - NULL, + 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], speed_helper, - NULL, + pid, &speed_helper_timer); - multicore_fifo_push_blocking(20); //send some data to let core0 know that core1 is done - while (1); + // Send direction pointer for access and to let core0 know that core1 is done with initialization, + // 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)); + printf("core1 done\n"); + while (1){ + // 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()); } + } } \ No newline at end of file diff --git a/core1.h b/core1.h index e1c1fc9..cf6a755 100644 --- a/core1.h +++ b/core1.h @@ -7,11 +7,60 @@ #pragma once #include "shared.h" #include -uint16_t calc_end_target(); -bool speed_helper(); -void adjust_pwm_level(uint16_t level); +typedef struct pid_params{ + // Controller variables - won't change after init + float k_i; // Integral gain + float k_d; // Derivative gain + float t; // Sampling time + float tau; // Low-pass-filter time constant + float k_ff; // Feed-Forward gain + float ci_0; // (k_i*t)/2 + float cd_0; // (k_d*2)/(2*tau+t) + float cd_1; // (2*tau-t)/(2*tau+t) + float int_lim_max; // max limit for integrator + float int_lim_min; // min limit for integrator + float max_output; // max possible pwm output value + // 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 measurement_prev; // Previous Measurement + float x_1_fwd; // Base Speed; forward direction + float x_2_fwd; // High Speed; forward direction + float y_1_fwd; // Base PWM; forward direction + float y_2_fwd; // High PWM; forward direction + float x_1_rev; // Base Speed; reverse direction + float x_2_rev; // High Speed; reverse direction + float y_1_rev; // Base PWM; reverse direction + float y_2_rev; // High PWM; reverse direction + float m_fwd; // Slope forward direction + float m_rev; // Slope reverse direction + uint16_t speed_table[127]; // Array with setpoint values corresponding to every speed step + uint32_t speed_step_target; // End target speed step + uint16_t end_target_index; // Index for speed table array corresponding to end target speed step + uint32_t setpoint; // Current setpoint + uint32_t setpoint_prev; // Previous setpoint + bool direction; // Current Direction + bool direction_prev; // Previous Direction + // Measurement Variables + float measurement; // Newest Measurement value + float adc_offset; // ADC offset value + uint8_t msr_delay_in_us; // Delay before VEMF Measurement + uint8_t msr_total_iterations; // Amount of samples + uint8_t l_side_arr_cutoff; // Discarded samples (left side) + uint8_t r_side_arr_cutoff; // Discarded samples (right side) +}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); -void init_speed_table(); -void init_pid(); -void init_msr(); +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 diff --git a/shared.h b/shared.h index d6984c6..b59877b 100644 --- a/shared.h +++ b/shared.h @@ -32,8 +32,5 @@ #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 bool target_direction; -extern uint target_speed_step; extern const uint8_t *CV_ARRAY_FLASH; -void core1_entry(); -uint32_t measure(bool direction); \ No newline at end of file +void core1_entry(); \ No newline at end of file