Commit 2ee8c7e9 authored by Clément Foucher's avatar Clément Foucher
Browse files

Base code for Current mode PID.

parent f61dbfd5
.pio
.vscode
\ 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 Clément Foucher <clement.foucher@laas.fr>
*/
// Owntech modules
#include "data_acquisition.h"
#include "dac.h"
static const struct device* dac1 = NULL;
static const struct device* dac3 = NULL;
static double ihigh;
static double v1low;
static double v2low;
static double vhigh;
static double setPoint;
static double p;
static double i;
static double d;
static const double period = 50e-6;
static uint32_t reset_value;
/////
// Private functions
void _opalib_pid_current_acquire_dac_values()
{
static uint32_t v1LowRaw = 0;
static uint32_t v2LowRaw = 0;
static uint32_t vHighRaw = 0;
static uint32_t iHighRaw = 0;
// I
while (data_dispatch_get_values_available_in_adc1_channel(0) != 0)
{
// Ignore I1-low, just empty the buffer
data_dispatch_get_next_value_from_adc1_channel(0);
}
while (data_dispatch_get_values_available_in_adc1_channel(1) != 0)
{
// Ignore I2-low, just empty the buffer
data_dispatch_get_next_value_from_adc1_channel(1);
}
while (data_dispatch_get_values_available_in_adc1_channel(2) != 0)
{
// Acquire latest I-high data
iHighRaw = data_dispatch_get_next_value_from_adc1_channel(2);
}
// V
while (data_dispatch_get_values_available_in_adc2_channel(0) != 0)
{
// Acquire latest V1-low data
v1LowRaw = data_dispatch_get_next_value_from_adc2_channel(0);
}
while (data_dispatch_get_values_available_in_adc2_channel(1) != 0)
{
// Acquire latest V2-low data
v2LowRaw = data_dispatch_get_next_value_from_adc2_channel(1);
}
while (data_dispatch_get_values_available_in_adc2_channel(2) != 0)
{
// Acquire latest V-high data
vHighRaw = data_dispatch_get_next_value_from_adc2_channel(2);
}
/////
// Convert raw values
ihigh = (0.0303030312F * iHighRaw) - 56.121212F;
v1low = (0.0304878056F * v1LowRaw) + 0.000439482072F;
v2low = (0.0264900662F * v2LowRaw) - 51.6821175F;
vhigh = (0.0664451793F * vHighRaw) + 4.31893682F;
}
static void _opalib_pid_current_compute_slope()
{
// TODO calibration => update values
double slope1 = 200000.0F * (10638.2979F * setPoint);
uint32_t slope1Raw = ((int)(1638.4F * (0.2F * slope1))) % 65536;
if (slope1Raw > 4096)
slope1Raw = 4096;
double slope3 = slope1 * 0.0133333337F;
uint32_t slope3Raw = ((int)(1638.4F * (0.2F * slope3))) % 65536;
if (slope3Raw > 4096)
slope3Raw = 4096;
dac_function_update_step(dac1, 1, slope1Raw);
dac_function_update_step(dac3, 1, slope3Raw);
}
static void _opalib_pid_current_compute_reset_value()
{
static double integrator_mem = 0;
static double derivative_mem = 0;
/////
// Compute error
double error = setPoint - v1low;
/////
// Compute derivative term
double derivativeTerm = d * error / period;
double sum = (p * error) + integrator_mem + derivativeTerm - derivative_mem;
derivative_mem = derivativeTerm;
/////
// Compute reset value
// TODO: warning, values have changed!
reset_value = (uint32_t)(0.000625F * sum);
/////
// Compute integral term
integrator_mem += i * error * period;
}
static void _opalib_pid_current_update_reset_value()
{
dac_function_update_reset(dac1, 1, reset_value);
dac_function_update_reset(dac3, 1, reset_value);
}
/////
// Public API
void opalib_pid_current_init(double setpoint, double p_i, double i_i, double d_i)
{
dac1 = device_get_binding(DAC1_LABEL);
dac3 = device_get_binding(DAC3_LABEL);
setPoint = setpoint;
p = p_i;
i = i_i;
d = d_i;
_opalib_pid_current_compute_slope();
}
void opalib_pid_current_update_setpoint(double setpoint)
{
setPoint = setpoint;
_opalib_pid_current_compute_slope();
}
void opalib_pid_current_periodic_task()
{
_opalib_pid_current_acquire_dac_values();
_opalib_pid_current_compute_reset_value();
_opalib_pid_current_update_reset_value();
}
/*
* 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 Clément Foucher <clement.foucher@laas.fr>
*/
#ifndef OPALIB_PID_CURRENT_MODE_H_
#define OPALIB_PID_CURRENT_MODE_H_
#ifdef __cplusplus
extern "C" {
#endif
void opalib_pid_current_init(double setpoint, double p_i, double i_i, double d_i);
void opalib_pid_current_update_setpoint(double setpoint);
void opalib_pid_current_periodic_task();
#ifdef __cplusplus
}
#endif
#endif // OPALIB_PID_CURRENT_MODE_H_
Supports Markdown
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