Commit e7d6de84 authored by Antoine Boche's avatar Antoine Boche
Browse files

Addition of the PID functionality.

 * Convert raw data from current and voltage sensors to volt and amp values
 * Calculation of duty cycle using PID with defined gain
 * Update of PWM with the new duty cycle
parent cc543072
/*
* Copyright (c) 2021 LAAS-CNRS
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 2.1 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* SPDX-License-Identifier: LGLPV2.1
*/
/**
* @author Antoine Boche <antoine.boche@laas.fr>
*/
/////
// Include
// Zephyr
#include <zephyr.h>
// Stdlib
#include <stdio.h>
// Application
#include "data_acquisition.h"
// Current file header
#include "data_conversion.h"
/////
// Local variables
//ADC convertions variable
//ADC1
static float32_t raw_values_adc1[4];
static float32_t raw_values_offset_removed_adc1[4];
static float32_t converted_values_adc1[4];
static float32_t gain_adc1[4] = {0.0125, 0.0107, 0.0125, 0};
static float32_t offset_adc1[4] = {2030.88, 2029.065, 2017.68, 0};
//ADC2
static float32_t raw_values_adc2[3];
static float32_t raw_values_offset_removed_adc2[3];
static float32_t converted_values_adc2[3];
static float32_t gain_adc2[3] = {0.0355, 0.0355, 0.0672};
static float32_t offset_adc2[3] = {2012.11, 2011.74, 3.022};
static float32_t gain_adc1_triple_linearization[3] = {0.0439, 0.0261, 0.0453};
static float32_t offset_adc2_triple_linearization[3] = {2036.583, 1834.98, 2160.97};
/////
// Public Functions
/**
* This function select the right conversion equation
* depending of the voltage measured to solve the non linear response
* of V1 and V2 measurment component
*/
void triple_linerarization(float32_t value_adc, int8_t adc_channel)
{
if (value_adc < 2350)
{
gain_adc2[adc_channel] = gain_adc1_triple_linearization[0];
offset_adc2[adc_channel] = offset_adc2_triple_linearization[0];
}
else if ((value_adc >= 2350) && (value_adc <= 2609))
{
gain_adc2[adc_channel] = gain_adc1_triple_linearization[1];
offset_adc2[adc_channel] = offset_adc2_triple_linearization[1];
}
else if (value_adc > 2609)
{
gain_adc2[adc_channel] = gain_adc1_triple_linearization[2];
offset_adc2[adc_channel] = offset_adc2_triple_linearization[2];
}
}
/////
// Public Functions
/**
* This function allows to convert the raw data from the adc
* into the proper value it represents
*/
void data_conversion()
{
if( (data_dispatch_get_values_available_in_adc1_channel(0)!=0) &&
(data_dispatch_get_values_available_in_adc1_channel(1)!=0) &&
(data_dispatch_get_values_available_in_adc1_channel(2)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(0)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(1)!=0) &&
(data_dispatch_get_values_available_in_adc2_channel(2)!=0) )
{
//Data gathering from the buffer and conversion to float for PID calculation
raw_values_adc1[0] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(0);
raw_values_adc1[1] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(1);
raw_values_adc1[2] = (float32_t)data_dispatch_get_next_value_from_adc1_channel(2);
raw_values_adc2[0] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(0);
raw_values_adc2[1] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(1);
raw_values_adc2[2] = (float32_t)data_dispatch_get_next_value_from_adc2_channel(2);
//Update of the linearization depending of the value
triple_linerarization(raw_values_adc2[0], 0);
triple_linerarization(raw_values_adc2[1], 1);
//Conversion of the raw ADC value in volt and ampere
arm_sub_f32(raw_values_adc1, offset_adc1, raw_values_offset_removed_adc1, 4);
arm_sub_f32(raw_values_adc2, offset_adc2, raw_values_offset_removed_adc2, 3);
arm_mult_f32(raw_values_offset_removed_adc1, gain_adc1, converted_values_adc1, 4);
arm_mult_f32(raw_values_offset_removed_adc2, gain_adc2, converted_values_adc2, 3);
}
}
/**
* This function allow to get le value converted anywhere in the code
*/
float32_t get_value_converted(uint8_t adc_number, uint8_t adc_channel)
{
if (adc_number == 1)
{
return converted_values_adc1[adc_channel];
}
else if (adc_number == 2)
{
return converted_values_adc2[adc_channel];
}
}
\ No newline at end of file
/*
* Copyright (c) 2021 LAAS-CNRS
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 2.1 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* SPDX-License-Identifier: LGLPV2.1
*/
/**
* @author Antoine Boche <antoine.boche@laas.fr>
*/
#ifndef DATA_CONVERSION_H_
#define DATA_CONVERSION_H_
#include <arm_math.h> // adds all the CMSIS library
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief Convert the raw data from the adc
* into the proper value it represents
*/
void data_conversion();
/**
* @brief Extract le value converted anywhere in the code
*
* @param[in] adc_number number of the adc
* @param[in] adc_channel channel of the adc
*/
float32_t get_value_converted(uint8_t adc_number, uint8_t adc_channel);
#ifdef __cplusplus
}
#endif
#endif // DATA_CONVERSION_H_
/*
* Copyright (c) 2021 LAAS-CNRS
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 2.1 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* SPDX-License-Identifier: LGLPV2.1
*/
/**
* @author Jean Alinei <jean.alinei@laas.fr>
* @author Clément Foucher <clement.foucher@laas.fr>
* @author Antoine Boche <antoine.boche@laas.fr>
*/
/////
// Include
// Zephyr
#include <zephyr.h>
// Stdlib
#include <stdio.h>
// Drivers
#include "leg.h" // PWM management layer by inverter leg interface
#include "owntech_leg_driver.h" // PWM management layer by inverter leg interface
// Application
#include "data_conversion.h"
// Current file header
#include "pid.h"
/////
// Defines
// block of values used with the PWM
#define LOW_DUTY 0.1
#define HIGH_DUTY 0.9
/////
// Global variables
/////
// Local variables
//PID variables
static float32_t Vref = 10;
static float32_t V1_value;
static float32_t error_pid;
static float32_t Kpgain; //Buck application gains calculated via matlab
static float32_t Kigain;
static float32_t Kdgain;
static arm_pid_instance_f32 PID_variables;
//PWM variables
static float32_t pwm_duty_cycle = 0.1; // pwm initialization value
static uint16_t pwm_pulse_width;
static uint16_t pwm_period, pwm_phase_shift, pwm_low_pulse_width, pwm_high_pulse_width;
/////
// Public Functions
/**
* This function Initialize all the parameters
* needed for the PID calculation for the buck topoligy
*/
void pid_init_buck()
{
pwm_period = leg_period();
pwm_phase_shift = pwm_period / 2;
pwm_low_pulse_width = pwm_period * LOW_DUTY;
pwm_high_pulse_width = pwm_period * HIGH_DUTY;
Kpgain = -0.00000235; //Buck application gains calculated via matlab
Kigain = -0.117;
Kdgain = 0;
PID_variables.Kp = Kpgain;
PID_variables.Ki = Kigain;
PID_variables.Kd = Kdgain;
arm_pid_init_f32(&PID_variables,1);
}
/**
* This function Initialize all the parameters
* needed for the PID calculation for the boost topoligy
*/
//NOTE: all the gain are set for the buck topology, they must be changed
void pid_init_boost()
{
pwm_phase_shift = pwm_period / 2;
pwm_low_pulse_width = pwm_period * LOW_DUTY;
pwm_high_pulse_width = pwm_period * HIGH_DUTY;
Kpgain = 0; //Boost application gains to be calculated
Kigain = 0;
Kdgain = 0;
PID_variables.Kp = Kpgain;
PID_variables.Ki = Kigain;
PID_variables.Kd = Kdgain;
arm_pid_init_f32(&PID_variables,1);
}
/**
* This function calculation of the PID for the chosen
* topology and set the new duty cycle in the hrtim PWM
*/
void pid_calculation_and_pwm_update()
{
//PID CALCUALTIONS
V1_value = get_value_converted(2 , 0);
arm_sub_f32(&V1_value, &Vref, &error_pid,1); //CALCULATING THE ERROR BASED ON THE REFERENCE
pwm_duty_cycle = arm_pid_f32(&PID_variables, 0.01*error_pid); //PID CALCULATIONS
//TESTING PWM VALUE TO AVOID OVERFLOW AND PWM UPDATE//
if (pwm_duty_cycle > HIGH_DUTY) //SATURATION CONDITIONS TO AVOID DIVERGENCE.
{
pwm_pulse_width = pwm_high_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
else if (pwm_duty_cycle < LOW_DUTY) //SATURATION CONDITIONS TO AVOID DIVERGENCE.
{
pwm_pulse_width = pwm_low_pulse_width;
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
else
{
pwm_pulse_width = (pwm_duty_cycle * pwm_period);
leg_set(TIMA, pwm_pulse_width, 0);
leg_set(TIMB, pwm_pulse_width, pwm_phase_shift);
}
}
\ No newline at end of file
/*
* Copyright (c) 2021 LAAS-CNRS
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 2.1 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* SPDX-License-Identifier: LGLPV2.1
*/
/**
* @author Jean Alinei <jean.alinei@laas.fr>
* @author Clément Foucher <clement.foucher@laas.fr>
* @author Antoine Boche <antoine.boche@laas.fr>
*/
#ifndef PID_H_
#define PID_H_
#include <arm_math.h> // adds all the CMSIS library
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief This function Initialize all the parameters
* needed for the PID calculation for the buck topoligy
*/
void pid_init_buck();
/**
* @brief This function Initialize all the parameters
* needed for the PID calculation for the buck topoligy
*/
void pid_init_boost();
/**
* @brief This function calculation of the PID for the chosen
* topology and set the new duty cycle in the hrtim PWM
*/
void pid_calculation_and_pwm_update();
#ifdef __cplusplus
}
#endif
#endif // PID_H_
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment