.
This commit is contained in:
1 parent
af105ebc45
commit
66f4eea593
3 files changed
+29
-24
No files matched your search
@@ -7,8 +7,8 @@ uint8_t CV_ARRAY_DEFAULT [512] = {
|
|||||||
// Note that there is an integer division in play here which will get rid of the fractional part
|
// 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 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.
|
// This does not correspond to the norm and might be changed eventually.
|
||||||
0b00000000, //CV_3 - Acceleration Rate
|
0b00000010, //CV_3 - Acceleration Rate
|
||||||
0b00000000, //CV_4 - Deceleration Rate
|
0b00000010, //CV_4 - Deceleration Rate
|
||||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||||
0b11111111, //CV_5 - V_max
|
0b11111111, //CV_5 - V_max
|
||||||
0b01111111, //CV_6 - V_mid
|
0b01111111, //CV_6 - V_mid
|
||||||
@@ -67,9 +67,9 @@ uint8_t CV_ARRAY_DEFAULT [512] = {
|
|||||||
0b01011100, //CV_49 - PID Control P_Factor = CV_49/1024 i.e. Default = 92 -> 0.08984375
|
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
|
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
|
0b10000011, //CV_51 - PID Control D_Factor = CV_51/16384 i.e. Default = 131 -> 0.00799560547
|
||||||
0b00000000, //CV_52 - PID Integral Limit positive = CV_52*10 i.e. Default = 100 -> +1000
|
0b00110000, //CV_52 - PID Integral Limit positive = CV_52*10 i.e. Default = 100 -> +1000
|
||||||
0b01100100, //CV_53 - PID Integral Limit negative = CV_53*(-10) i.e. Default = 100 -> -1000
|
0b00110000, //CV_53 - PID Integral Limit negative = CV_53*(-10) i.e. Default = 100 -> -1000
|
||||||
0b01100100, //CV_54 -
|
0b00000000, //CV_54 -
|
||||||
0b00000000, //CV_55 -
|
0b00000000, //CV_55 -
|
||||||
0b00000000, //CV_56 -
|
0b00000000, //CV_56 -
|
||||||
0b00000000, //CV_57 -
|
0b00000000, //CV_57 -
|
||||||
|
|||||||
+22
-18
@@ -141,7 +141,7 @@ void set_outputs() {
|
|||||||
for (uint8_t j = 0; j < 32; j++)
|
for (uint8_t j = 0; j < 32; j++)
|
||||||
{
|
{
|
||||||
uint32_t bit_value = (GPIO_to_be_set&mask)>>j;
|
uint32_t bit_value = (GPIO_to_be_set&mask)>>j;
|
||||||
printf("GPIO: %u, set to: %d\n",j,bit_value);
|
//printf("GPIO: %u, set to: %d\n",j,bit_value);
|
||||||
//gpio_put(j,bit_value);
|
//gpio_put(j,bit_value);
|
||||||
mask = mask<<1;
|
mask = mask<<1;
|
||||||
}
|
}
|
||||||
@@ -314,6 +314,7 @@ void adjust_pwm_level(uint16_t level)
|
|||||||
void measure(){
|
void measure(){
|
||||||
pwm_set_gpio_level(MOTOR_PWM_FORWARD,0);
|
pwm_set_gpio_level(MOTOR_PWM_FORWARD,0);
|
||||||
pwm_set_gpio_level(MOTOR_PWM_REVERSE,0);
|
pwm_set_gpio_level(MOTOR_PWM_REVERSE,0);
|
||||||
|
adc_select_input(target_direction);
|
||||||
busy_wait_us(measurement_delay_in_us);
|
busy_wait_us(measurement_delay_in_us);
|
||||||
uint sum = 0;
|
uint sum = 0;
|
||||||
for (int i = 0; i < measurement_iterations; ++i) {
|
for (int i = 0; i < measurement_iterations; ++i) {
|
||||||
@@ -332,7 +333,7 @@ bool pid_control(){
|
|||||||
e_sum = e_sum + error;
|
e_sum = e_sum + error;
|
||||||
if (e_sum > sum_limit_max) e_sum = sum_limit_max;
|
if (e_sum > sum_limit_max) e_sum = sum_limit_max;
|
||||||
if (e_sum < sum_limit_min) e_sum = sum_limit_min;
|
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));
|
output = output+ (k_p * (float)error) + (k_i * t_s * (float)e_sum) + (k_d/t_s) * (float)(error - e_old);
|
||||||
e_old = error;
|
e_old = error;
|
||||||
if (output > 5000) output = 5000;
|
if (output > 5000) output = 5000;
|
||||||
if (output < 1400) output = 1400;
|
if (output < 1400) output = 1400;
|
||||||
@@ -366,19 +367,18 @@ void init_pwm(uint gpio) {
|
|||||||
void init_adc(uint gpio){
|
void init_adc(uint gpio){
|
||||||
adc_init();
|
adc_init();
|
||||||
adc_gpio_init(gpio);
|
adc_gpio_init(gpio);
|
||||||
adc_select_input(0);
|
|
||||||
printf("ADC on GPIO %d was initialized.\n",gpio);
|
printf("ADC on GPIO %d was initialized.\n",gpio);
|
||||||
}
|
}
|
||||||
uint calc_end_v_emf_target(){
|
uint calc_end_v_emf_target(){
|
||||||
//Forward
|
//Forward
|
||||||
if(target_direction){
|
if(target_direction){
|
||||||
if (target_speed_step == 128) return 0;
|
if (target_speed_step == 128) return 0;
|
||||||
else return (target_speed_step - 129) * 32;
|
else return (target_speed_step - 129) * 16;
|
||||||
}
|
}
|
||||||
//Reverse
|
//Reverse
|
||||||
else {
|
else {
|
||||||
if (target_speed_step == 0) return 0;
|
if (target_speed_step == 0) return 0;
|
||||||
else return (target_speed_step-1) * 32;
|
else return (target_speed_step-1) * 16;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
bool speed_helper(struct repeating_timer *t) {
|
bool speed_helper(struct repeating_timer *t) {
|
||||||
@@ -386,8 +386,8 @@ bool speed_helper(struct repeating_timer *t) {
|
|||||||
if (target_speed_step == 129 || target_speed_step==1) { current_v_emf_target = 0; return false; }
|
if (target_speed_step == 129 || target_speed_step==1) { current_v_emf_target = 0; return false; }
|
||||||
else{
|
else{
|
||||||
uint end_v_emf_target = calc_end_v_emf_target();
|
uint end_v_emf_target = calc_end_v_emf_target();
|
||||||
if (end_v_emf_target > current_v_emf_target) { current_v_emf_target += 32; }
|
if (end_v_emf_target > current_v_emf_target) current_v_emf_target += 16;
|
||||||
if (end_v_emf_target < current_v_emf_target) { current_v_emf_target -= 32; }
|
else if(end_v_emf_target < current_v_emf_target) current_v_emf_target -= 16;
|
||||||
else return false;
|
else return false;
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
@@ -397,30 +397,33 @@ void init_speed_helper(){
|
|||||||
uint end_v_emf_target = calc_end_v_emf_target();
|
uint end_v_emf_target = calc_end_v_emf_target();
|
||||||
//Acceleration
|
//Acceleration
|
||||||
if(end_v_emf_target > current_v_emf_target){
|
if(end_v_emf_target > current_v_emf_target){
|
||||||
uint32_t acceleration_rate = CV_ARRAY_FLASH[2]*7111;
|
int64_t acceleration_rate = -CV_ARRAY_FLASH[2]*7111;
|
||||||
//Timer already running
|
//Timer already running
|
||||||
if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = acceleration_rate;
|
if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = acceleration_rate;
|
||||||
//Timer not running
|
//Timer not running
|
||||||
else add_repeating_timer_us(-acceleration_rate, speed_helper, NULL, &speed_helper_timer);
|
else add_repeating_timer_us(acceleration_rate, speed_helper, NULL, &speed_helper_timer);
|
||||||
}
|
}
|
||||||
//Deceleration
|
//Deceleration
|
||||||
else{
|
if(end_v_emf_target < current_v_emf_target){
|
||||||
uint32_t deceleration_rate = CV_ARRAY_FLASH[3]*7111;
|
int64_t deceleration_rate = -CV_ARRAY_FLASH[3]*7111;
|
||||||
//Timer already running
|
//Timer already running
|
||||||
if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = deceleration_rate;
|
if (speed_helper_timer.alarm_id != 0) speed_helper_timer.delay_us = deceleration_rate;
|
||||||
//Timer not running
|
//Timer not running
|
||||||
else add_repeating_timer_us(-deceleration_rate, speed_helper, NULL, &speed_helper_timer);
|
else add_repeating_timer_us(deceleration_rate, speed_helper, NULL, &speed_helper_timer);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void core1_entry() {
|
void core1_entry() {
|
||||||
gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM);
|
gpio_set_function(MOTOR_PWM_FORWARD, GPIO_FUNC_PWM);
|
||||||
gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM);
|
gpio_set_function(MOTOR_PWM_REVERSE, GPIO_FUNC_PWM);
|
||||||
init_adc(V_EMF_ADC_PIN);
|
init_adc(V_EMF_ADC_PIN_FORWARD);
|
||||||
|
init_adc(V_EMF_ADC_PIN_REVERSE);
|
||||||
init_pwm(MOTOR_PWM_FORWARD);
|
init_pwm(MOTOR_PWM_FORWARD);
|
||||||
init_pwm(MOTOR_PWM_REVERSE);
|
init_pwm(MOTOR_PWM_REVERSE);
|
||||||
init_pid();
|
init_pid();
|
||||||
add_repeating_timer_ms(-CV_ARRAY_FLASH[47], pid_control, NULL, &pid_control_timer);
|
add_repeating_timer_ms(-CV_ARRAY_FLASH[47], pid_control, NULL, &pid_control_timer);
|
||||||
|
printf("Core 1 was initialized.\n");
|
||||||
while (1);
|
while (1);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -438,15 +441,16 @@ int main() {
|
|||||||
sleep_ms(1000);
|
sleep_ms(1000);
|
||||||
stdio_init_all();
|
stdio_init_all();
|
||||||
gpio_init(DCC_INPUT_PIN);
|
gpio_init(DCC_INPUT_PIN);
|
||||||
|
gpio_init(PICO_DEFAULT_LED_PIN);
|
||||||
gpio_set_dir(DCC_INPUT_PIN, GPIO_IN);
|
gpio_set_dir(DCC_INPUT_PIN, GPIO_IN);
|
||||||
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
||||||
gpio_put(PICO_DEFAULT_LED_PIN, true);
|
|
||||||
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
|
|
||||||
printf("Core 0 was initialized.\n");
|
|
||||||
printf("CV_ARRAY read from flash:\n");
|
printf("CV_ARRAY read from flash:\n");
|
||||||
print_cv_array(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2);
|
print_cv_array(CV_ARRAY_FLASH, FLASH_PAGE_SIZE * 2);
|
||||||
multicore_launch_core1(core1_entry);
|
multicore_launch_core1(core1_entry);
|
||||||
|
busy_wait_ms(2000); //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);
|
while (1);
|
||||||
}
|
}
|
||||||
+2
-1
@@ -14,7 +14,8 @@
|
|||||||
#define DCC_INPUT_PIN 17
|
#define DCC_INPUT_PIN 17
|
||||||
#define MOTOR_PWM_FORWARD 21
|
#define MOTOR_PWM_FORWARD 21
|
||||||
#define MOTOR_PWM_REVERSE 22
|
#define MOTOR_PWM_REVERSE 22
|
||||||
#define V_EMF_ADC_PIN 26
|
#define V_EMF_ADC_PIN_FORWARD 27
|
||||||
|
#define V_EMF_ADC_PIN_REVERSE 26
|
||||||
#define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001
|
#define PACKAGE_3_BYTES 0b11111111110000000000000000000000000001
|
||||||
#define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001
|
#define PACKAGE_MASK_3_BYTES 0b11111111111000000001000000001000000001
|
||||||
#define PACKAGE_4_BYTES 0b11111111110000000000000000000000000000000000001
|
#define PACKAGE_4_BYTES 0b11111111110000000000000000000000000000000000001
|
||||||
|
|||||||
Reference in new issue
Block a user