diff --git a/CV.h b/CV.h index 6216f0d..3877ca3 100644 --- a/CV.h +++ b/CV.h @@ -62,7 +62,7 @@ uint8_t CV_ARRAY_DEFAULT [512] = { //(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 - 0b00001010, //CV_48 - V_EMF Measurement Period duration in ms / equals 1 PID-Cycle + 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 diff --git a/multicore.c b/multicore.c index 46b4660..63f0c7a 100644 --- a/multicore.c +++ b/multicore.c @@ -5,24 +5,30 @@ #include "hardware/pwm.h" #include "hardware/adc.h" #include "hardware/flash.h" +#include "hardware/irq.h" #include "CV.h" #include "multicore.h" +#include "inttypes.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 = 0; +uint current_v_emf_target = 300; uint measurement = 0; bool reset_package_flag = false; -int sum_limit_max; -int sum_limit_min; -float k_p; -float k_i; -float k_d; -float t_s; +typedef struct pid_params{ + float k_p; + float k_i; + float k_d; + float t_s; + int sum_limit_max; + int sum_limit_min; +}pid_params; +pid_params pid_config; uint measurement_delay_in_us; uint measurement_iterations; +static uint8_t prev_byte_array[SIZE_BYTE_ARRAY] = {0}; struct repeating_timer speed_helper_timer; struct repeating_timer pid_control_timer; @@ -59,68 +65,89 @@ void bits_to_byte_array(int8_t number_of_bytes,uint8_t byte_array[]) { byte_array[i] = last_bits >> (i * 9 + 1); } } +absolute_time_t from; +absolute_time_t to; + void acknowledge(){ - for (int i = 0; i < 4; ++i) { - gpio_put(MOTOR_PWM_REVERSE,0); - gpio_put(MOTOR_PWM_FORWARD,1); - sleep_ms(1); - gpio_put(MOTOR_PWM_FORWARD,0); - gpio_put(MOTOR_PWM_REVERSE,1); - sleep_ms(1); - } -} -void verify_cv(uint16_t cv_address,uint8_t cv_data){ - if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge(); + target_direction = false; + adjust_pwm_level(5000); + busy_wait_ms(6); + adjust_pwm_level(0); } -void write_cv(uint16_t cv_address,uint8_t cv_data){ - multicore_reset_core1(); - uint32_t ints = save_and_disable_interrupts(); - // CV_n is at index n-1 of the array e.g.: CV_1 -> index = 0 - uint16_t cv_array_index = cv_address-1; - //CV_7 and CV_8 are read-only - if (cv_address == 7){ +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(); } - if (cv_address == 8){ +} +void verify_cv_byte(uint16_t cv_address, uint8_t cv_data){ + if (CV_ARRAY_FLASH[cv_address] == cv_data) acknowledge(); +} +//TODO: FIX Acknowledge +void write_cv_byte(uint16_t cv_address, uint8_t cv_data){ + uint32_t saved_interrupts = save_and_disable_interrupts(); + multicore_reset_core1(); + //CV_7 and CV_8 are read-only + if (cv_address == 6){ + } + else if (cv_address == 7){ //Reset all CVs to Default 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); - restore_interrupts(ints); acknowledge(); } } else{ uint8_t CV_ARRAY_TEMP[512] = {0}; memcpy(CV_ARRAY_TEMP, CV_ARRAY_FLASH, sizeof(CV_ARRAY_TEMP)); - CV_ARRAY_TEMP[cv_array_index] = cv_data; + CV_ARRAY_TEMP[cv_address] = cv_data; flash_range_erase(FLASH_TARGET_OFFSET, FLASH_SECTOR_SIZE); flash_range_program(FLASH_TARGET_OFFSET, CV_ARRAY_TEMP, FLASH_PAGE_SIZE * 2); - restore_interrupts(ints); acknowledge(); } + busy_wait_ms(100); + restore_interrupts(saved_interrupts); + busy_wait_ms(100); multicore_launch_core1(core1_entry); } +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); + return true; + }else { + return false; + } +} void program_mode(uint8_t number_of_bytes, const uint8_t byte_array[]){ - static uint8_t prev_byte_array[SIZE_BYTE_ARRAY]; - uint counter = 0; - for (int i = 0; i < number_of_bytes; ++i) { - if (prev_byte_array[i] == byte_array[i]) counter++; - } - for (int i = 0; i < number_of_bytes; ++i) { - prev_byte_array [i] = byte_array[i]; - } - if (number_of_bytes == counter) { - uint8_t instruction_type_mask = 0b00001100; - uint8_t instruction_type = instruction_type_mask & byte_array[number_of_bytes - 1]; - uint8_t cv_address_mask = 0b00000011; - uint8_t cv_address_ms_bits = cv_address_mask & byte_array[number_of_bytes - 1]; - uint16_t cv_address = byte_array[number_of_bytes - 2] + (cv_address_ms_bits << 8); - uint8_t cv_data = byte_array[number_of_bytes - 3]; - if (instruction_type == 0b000000100) verify_cv(cv_address, cv_data); - if (instruction_type == 0b000001100) write_cv(cv_address, cv_data); + //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); + if (instruction_type == 0b000001000) { + uint8_t bit_pos_mask = 0b00000111; + uint8_t bit_pos = bit_pos_mask&byte_array[number_of_bytes - 3]; + uint8_t bit_val_mask = 0b00000001; + uint8_t bit_val_uint = (byte_array[number_of_bytes - 3]>>3) & bit_val_mask; + bool bit_val = bit_val_uint; + verify_cv_bit(cv_address, bit_val, bit_pos); + } + else if (instruction_type == 0b000000100) { + uint8_t cv_data = byte_array[number_of_bytes - 3]; + verify_cv_byte(cv_address, cv_data); + } + else if (instruction_type == 0b000001100) { + uint8_t cv_data = byte_array[number_of_bytes - 3]; + write_cv_byte(cv_address, cv_data); + } } } @@ -176,23 +203,12 @@ 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[]) { - uint16_t read_address; - //Check for address 112-127 and for previous packet == reset package - if (reset_package_flag && byte_array[number_of_bytes - 1]<128 && byte_array[number_of_bytes - 1]>111){ - program_mode(number_of_bytes,byte_array); - return false; - } //Check for Idle Package if (byte_array[number_of_bytes - 1] == 255) { return false; } - //Check for Broadcast Package - if (byte_array[number_of_bytes-1] == 0) - { - printf("Broadcast-Package found!\n"); - return true; - } + uint16_t read_address; //Long Address Package if (is_long_address(number_of_bytes, byte_array)) { @@ -223,23 +239,15 @@ 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]; - //0000-0000 - Reset Package - if (command_byte_n == 0b00000000){ - target_speed_step = 1; - update_active_functions(0,0,37); - reset_package_flag = true; - }else reset_package_flag = false; //0011-1111 (128 Speed Step Control) - 2 Byte length - if (command_byte_n == 0b00111111) - { + 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; init_speed_helper(); } // 10XX-XXXX (Function Group Instruction) - if (command_byte_n >> 6 == 0b00000010) - { + 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 @@ -287,11 +295,20 @@ void gpio_callback_rise(unsigned int gpio, long unsigned int events) { writeLastBit(readBit()); int8_t number_of_bytes = check_for_package(); if (number_of_bytes != -1) { - uint8_t byte_array[SIZE_BYTE_ARRAY] = {0}; - bits_to_byte_array(number_of_bytes,byte_array); - if (error_detection(number_of_bytes,byte_array)) { //Returns true if the XOR verification was error-free - if (address_evaluation(number_of_bytes,byte_array)) { //Returns true if address matches with CV - instruction_evaluation(number_of_bytes,byte_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){ + } + if (error_detection(number_of_bytes,byte_array)) { + if (address_evaluation(number_of_bytes,byte_array)) { + reset_package_flag = false; + instruction_evaluation(number_of_bytes,byte_array); + } + else if (reset_package_flag){ + program_mode(number_of_bytes,byte_array); + } + else{ + reset_package_flag = reset_package_check(number_of_bytes,byte_array); } } } @@ -323,7 +340,7 @@ void measure(){ measurement = (int)sum / measurement_iterations; } -bool pid_control(){ +bool pid_control(struct repeating_timer *t){ static float output; static int error; static int e_old; @@ -331,9 +348,11 @@ bool pid_control(){ measure(); error = (int)current_v_emf_target - (int)measurement; e_sum = e_sum + error; - if (e_sum > sum_limit_max) e_sum = sum_limit_max; - if (e_sum < sum_limit_min) e_sum = sum_limit_min; - output = output+ (k_p * (float)error) + (k_i * t_s * (float)e_sum) + (k_d/t_s) * (float)(error - e_old); + 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; @@ -343,15 +362,15 @@ bool pid_control(){ } void init_pid(){ - k_p = (float)CV_ARRAY_FLASH[48]/1024; - k_i = (float)CV_ARRAY_FLASH[49]/1024; - k_d = (float)CV_ARRAY_FLASH[50]/16384; - t_s = (float)CV_ARRAY_FLASH[47]/1000; - sum_limit_max = CV_ARRAY_FLASH[51]*10; - sum_limit_min = CV_ARRAY_FLASH[52]*(-10); + 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",k_p,k_i,k_d,t_s); + 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); } void init_pwm(uint gpio) { @@ -383,6 +402,8 @@ uint calc_end_v_emf_target(){ } bool speed_helper(struct repeating_timer *t) { //Emergency Stop + uint core_num = get_core_num(); + printf("c_n:%u\n",core_num); 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(); @@ -401,7 +422,7 @@ void init_speed_helper(){ //Timer already running if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = acceleration_rate; //Timer not running - else add_repeating_timer_us(acceleration_rate, speed_helper, NULL, &speed_helper_timer); + else alarm_pool_add_repeating_timer_us(alarm_pool_create(1,2),acceleration_rate, speed_helper, NULL, &speed_helper_timer); } //Deceleration if(end_v_emf_target < current_v_emf_target){ @@ -409,24 +430,9 @@ void init_speed_helper(){ //Timer already running if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = deceleration_rate; //Timer not running - else add_repeating_timer_us(deceleration_rate, speed_helper, NULL, &speed_helper_timer); + else alarm_pool_add_repeating_timer_us(alarm_pool_create(1,2),deceleration_rate, speed_helper, NULL, &speed_helper_timer); } } - - -void core1_entry() { - gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM); - gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM); - init_adc(V_EMF_ADC_PIN_FORWARD); - init_adc(V_EMF_ADC_PIN_REVERSE); - init_pwm(MOTOR_PWM_FORWARD); - init_pwm(MOTOR_PWM_REVERSE); - init_pid(); - add_repeating_timer_ms(-CV_ARRAY_FLASH[47], pid_control, NULL, &pid_control_timer); - printf("Core 1 was initialized.\n"); - while (1); -} - 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]); @@ -437,6 +443,28 @@ void print_cv_array(const uint8_t *buf, size_t len) { } } +void core1_entry() { + gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM); + gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM); + init_adc(V_EMF_ADC_PIN_FORWARD); + init_adc(V_EMF_ADC_PIN_REVERSE); + init_pwm(MOTOR_PWM_FORWARD); + init_pwm(MOTOR_PWM_REVERSE); + init_pid(); + init_speed_helper(); + if (pid_control_timer.alarm_id != 0) { + pid_control_timer.delay_us = /*-CV_ARRAY_FLASH[47]*/ -1*1000; + } + else{ + alarm_pool_add_repeating_timer_ms( alarm_pool_create(0,1),/*-CV_ARRAY_FLASH[47]*/ -1, pid_control, NULL, &pid_control_timer); + } + printf("delay_us: %lld \n",pid_control_timer.delay_us); + //printf("CV_ARRAY read from flash:\n"); + //print_cv_array(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2); + printf("Core 1 was initialized.\n"); + while (1); +} + int main() { sleep_ms(1000); stdio_init_all(); @@ -444,13 +472,10 @@ int main() { gpio_init(PICO_DEFAULT_LED_PIN); gpio_set_dir(DCC_INPUT_PIN, GPIO_IN); gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT); - printf("CV_ARRAY read from flash:\n"); - print_cv_array(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2); + busy_wait_ms(1000); //This delay is necessary to catch the breakpoint multicore_launch_core1(core1_entry); - busy_wait_ms(2000); //This delay is necessary to catch the breakpoint + busy_wait_ms(1000); //This delay is necessary to catch the breakpoint gpio_set_irq_enabled_with_callback(DCC_INPUT_PIN, GPIO_IRQ_EDGE_RISE, true, &gpio_callback_rise); - busy_wait_ms(200); //This delay is necessary to catch the breakpoint - gpio_put(PICO_DEFAULT_LED_PIN, true); printf("Core 0 was initialized.\n"); while (1); } \ No newline at end of file diff --git a/multicore.h b/multicore.h index 1fbc4d4..9a116ac 100644 --- a/multicore.h +++ b/multicore.h @@ -11,9 +11,9 @@ (byte & 0x01 ? '1' : '0') #define SIZE_BYTE_ARRAY 5 #define SIZE_ACTIVE_FUNCTIONS 69 -#define DCC_INPUT_PIN 17 -#define MOTOR_PWM_FORWARD 21 -#define MOTOR_PWM_REVERSE 22 +#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 @@ -22,6 +22,9 @@ #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*2) +//#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES*2-FLASH_SECTOR_SIZE) // <=> PICO_FLASH_SIZE_BYTES*2 = 2MB*2 = 4MB +#define FLASH_TARGET_OFFSET (512 * 1024) // <=> PICO_FLASH_SIZE_BYTES*2 = 2MB*2 = 4MB void core1_entry(); -void init_speed_helper(); \ No newline at end of file +void init_speed_helper(); +void adjust_pwm_level(uint16_t level); +void gpio_callback_rise(unsigned int gpio, long unsigned int events); \ No newline at end of file