Energy stuff added

This commit is contained in:
IssaBqain 2021-06-08 02:10:13 +01:00
parent 02aed60775
commit f2f1dfa80f
3 changed files with 785 additions and 0 deletions

View file

@ -0,0 +1,340 @@
#include <Wire.h>
#include <INA219_WE.h>
#include <SPI.h>
#include <SD.h>
INA219_WE ina219; // this is the instantiation of the library for the current sensor
// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;
const int chipSelect = 10;
unsigned int rest_timer;
unsigned int loop_trigger;
unsigned int int_count = 0; // a variables to count the interrupts. Used for program debugging.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float ui_max = 1, ui_min = 0; //anti-windup limitation
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float Ts = 0.001; //1 kHz control frequency.
float current_measure, current_ref = 0, error_amps; // Current Control
float pwm_out;
float V_Bat;
boolean input_switch;
int state_num=0,next_state;
String dataString;
float cell_1_read = 0;
float cell_2_read = 0;
int balance_counter = 0;
float initial_V_lookup[] = {1} ;
float initial_SOC_lookup[] = {1};
float initial_SOC = 0;
float coloumb_total = 0;
float coloumb_change = 0;
float current_SOC = 0;
float battery_capacity = 1700000;
int initial_SOC_counter = 1;
void setup() {
//Some General Setup Stuff
Wire.begin(); // We need this for the i2c comms for the current sensor
Wire.setClock(700000); // set the comms speed for i2c
ina219.init(); // this initiates the current sensor
Serial.begin(9600); // USB Communications
//Check for the SD Card
Serial.println("\nInitializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println("* is a card inserted?");
while (true) {} //It will stick here FOREVER if no SD is in on boot
} else {
Serial.println("Wiring is correct and a card is present.");
}
if (SD.exists("BatCycle.csv")) { // Wipe the datalog when starting
SD.remove("BatCycle.csv");
}
noInterrupts(); //disable all interrupts
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
//SMPS Pins
pinMode(13, OUTPUT); // Using the LED on Pin D13 to indicate status
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
pinMode(6, OUTPUT); // This is the PWM Pin
//LEDs on pin 7 and 8
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
//Analogue input, the battery voltage (also port B voltage)
pinMode(A0, INPUT);
/// Battery manage
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(9, OUTPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
// TimerA0 initialization for 1kHz control-loop interrupt.
TCA0.SINGLE.PER = 999; //
TCA0.SINGLE.CMP1 = 999; //
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
// TimerB0 initialization for PWM output
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
interrupts(); //enable interrupts.
analogWrite(6, 120); //just a default state to start with
}
void loop() {
if (loop_trigger == 1){ // FAST LOOP (1kHZ)
state_num = next_state; //state transition
V_Bat = analogRead(A0)*4.096/1.03; //check the battery voltage (1.03 is a correction for measurement error, you need to check this works for you)
if ((V_Bat > 3700 || V_Bat < 2400)) { //Checking for Error states (just battery voltage for now)
state_num = 5; //go directly to jail
next_state = 5; // stay in jail
digitalWrite(7,true); //turn on the red LED
current_ref = 0; // no current
}
current_measure = (ina219.getCurrent_mA()); // sample the inductor current (via the sensor chip)
error_amps = (current_ref - current_measure) / 1000; //PID error calculation
pwm_out = pidi(error_amps); //Perform the PID controller calculation
pwm_out = saturation(pwm_out, 0.99, 0.01); //duty_cycle saturation
analogWrite(6, (int)(255 - pwm_out * 255)); // write it out (inverting for the Buck here)
int_count++; //count how many interrupts since this was last reset to zero
loop_trigger = 0; //reset the trigger and move on with life
}
if (int_count == 1000) { // SLOW LOOP (1Hz)
input_switch = digitalRead(2); //get the OL/CL switch status
switch (state_num) { // STATE MACHINE (see diagram)
case 0:{ // Start state (no current, no LEDs)
current_ref = 0;
if (input_switch == 1) { // if switch, move to charge
next_state = 1;
digitalWrite(8,true);
} else { // otherwise stay put
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 1:{ // Charge state (250mA and a green LED)
current_ref = 250;
balance_counter = balance_counter + 1;
if(balance_counter == 20){
manage_battery();
}
if( initial_SOC_counter == 1){
initial_SOC = get_initial_SOC();
initial_SOC_counter == 0;
}
get_current_SOC();
if (V_Bat < 3550) { // if not charged, stay put
next_state = 1;
digitalWrite(8,true);
} else { // otherwise go to charge rest
next_state = 2;
digitalWrite(8,false);
}
if(input_switch == 0){ // UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 2:{ // Charge Rest, green LED is off and no current
current_ref = 0;
if (rest_timer < 30) { // Stay here if timer < 30
next_state = 2;
digitalWrite(8,false);
rest_timer++;
} else { // Or move to discharge (and reset the timer)
next_state = 3;
digitalWrite(8,false);
rest_timer = 0;
}
if(input_switch == 0){ // UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 3:{ //Discharge state (-250mA and no LEDs)
current_ref = -250;
balance_counter = balance_counter + 1;
if(balance_counter == 5){
manage_battery();
}
initial_SOC = get_initial_SOC();
get_current_SOC();
if (V_Bat > 2500) { // While not at minimum volts, stay here
next_state = 3;
digitalWrite(8,false);
} else { // If we reach full discharged, move to rest
next_state = 4;
digitalWrite(8,false);
}
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 4:{ // Discharge rest, no LEDs no current
current_ref = 0;
if (rest_timer < 30) { // Rest here for 30s like before
next_state = 4;
digitalWrite(8,false);
rest_timer++;
} else { // When thats done, move back to charging (and light the green LED)
next_state = 1;
digitalWrite(8,true);
rest_timer = 0;
}
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 5: { // ERROR state RED led and no current
current_ref = 0;
next_state = 5; // Always stay here
digitalWrite(7,true);
digitalWrite(8,false);
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(7,false);
}
break;
}
default :{ // Should not end up here ....
Serial.println("Boop");
current_ref = 0;
next_state = 5; // So if we are here, we go to error
digitalWrite(7,true);
}
}
dataString = String(state_num) + "," + String(V_Bat) + "," + String(balance_counter) + "," + String(cell_1_read) + "," + String(cell_2_read) + "," + String(current_ref) + "," + String(current_measure) + "," + String(initial_SOC) + "," + String( current_SOC) ; //build a datastring for the CSV file
Serial.println(dataString); // send it to serial as well in case a computer is connected
File dataFile = SD.open("BatCycle.csv", FILE_WRITE); // open our CSV file
if (dataFile){ //If we succeeded (usually this fails if the SD card is out)
dataFile.println(dataString); // print the data
} else {
Serial.println("File not open"); //otherwise print an error
}
dataFile.close(); // close the file
int_count = 0; // reset the interrupt count so we dont come back here for 1000ms
}
}
// Timer A CMP1 interrupt. Every 1000us the program enters this interrupt. This is the fast 1kHz loop
ISR(TCA0_CMP1_vect) {
loop_trigger = 1; //trigger the loop when we are back in normal flow
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
}
float saturation( float sat_input, float uplim, float lowlim) { // Saturation function
if (sat_input > uplim) sat_input = uplim;
else if (sat_input < lowlim ) sat_input = lowlim;
else;
return sat_input;
}
float pidi(float pid_input) { // discrete PID function
float e_integration;
e0i = pid_input;
e_integration = e0i;
//anti-windup
if (u1i >= ui_max) {
e_integration = 0;
} else if (u1i <= ui_min) {
e_integration = 0;
}
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
u0i = u1i + delta_ui; //this time's control output
//output limitation
saturation(u0i, ui_max, ui_min);
u1i = u0i; //update last time's control output
e2i = e1i; //update last last time's error
e1i = e0i; // update last time's error
return u0i;
}
void manage_battery(){
// First battery
//measure first battery and balance if needed ( relay = D4 , discharge = D5, Measure= A1)
digitalWrite(4, HIGH);
cell_1_read = analogRead(A1)*4.096/1.03;;
digitalWrite(4, LOW);
if( cell_1_read > 3600){
digitalWrite(5, HIGH);
}
if(cell_1_read < 2500){
next_state = 1;
}
//second battery
//measure second battery and balance if needed ( relay = D3 , discharge = D9, Measure= A2)
digitalWrite(3, HIGH);
cell_2_read = analogRead(A2)*4.096/1.03;
digitalWrite(3,LOW);
if( cell_2_read > 3600){
digitalWrite(9, HIGH);
}
if( cell_2_read < 2500){
next_state = 1;
}
// reset counter
balance_counter = 0;
}
float get_initial_SOC(){
float initial = 0.3;
return initial;
}
void get_current_SOC(){
coloumb_total = coloumb_total + abs(current_measure);
coloumb_change = coloumb_change + current_measure;
current_SOC = initial_SOC + ((coloumb_change)/(battery_capacity));
}

View file

@ -0,0 +1,246 @@
#include <Wire.h>
#include <INA219_WE.h>
#include <SPI.h>
#include <SD.h>
INA219_WE ina219; // this is the instantiation of the library for the current sensor
// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;
const int chipSelect = 10;
unsigned int rest_timer;
unsigned int loop_trigger;
unsigned int int_count = 0; // a variables to count the interrupts. Used for program debugging.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float ui_max = 1, ui_min = 0; //anti-windup limitation
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float Ts = 0.001; //1 kHz control frequency.
float current_measure, current_ref = 0, error_amps; // Current Control
float pwm_out;
float V_Bat;
boolean input_switch;
int state_num=0,next_state;
String dataString;
void setup() {
//Some General Setup Stuff
Wire.begin(); // We need this for the i2c comms for the current sensor
Wire.setClock(700000); // set the comms speed for i2c
ina219.init(); // this initiates the current sensor
Serial.begin(9600); // USB Communications
//Check for the SD Card
Serial.println("\nInitializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println("* is a card inserted?");
while (true) {} //It will stick here FOREVER if no SD is in on boot
} else {
Serial.println("Wiring is correct and a card is present.");
}
if (SD.exists("BatCycle.csv")) { // Wipe the datalog when starting
SD.remove("BatCycle.csv");
}
noInterrupts(); //disable all interrupts
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
//SMPS Pins
pinMode(13, OUTPUT); // Using the LED on Pin D13 to indicate status
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
pinMode(6, OUTPUT); // This is the PWM Pin
//LEDs on pin 7 and 8
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
//Analogue input, the battery voltage (also port B voltage)
pinMode(A0, INPUT);
// TimerA0 initialization for 1kHz control-loop interrupt.
TCA0.SINGLE.PER = 999; //
TCA0.SINGLE.CMP1 = 999; //
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
// TimerB0 initialization for PWM output
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
interrupts(); //enable interrupts.
analogWrite(6, 120); //just a default state to start with
}
void loop() {
if (loop_trigger == 1){ // FAST LOOP (1kHZ)
state_num = next_state; //state transition
V_Bat = analogRead(A0)*4.096/1.03; //check the battery voltage (1.03 is a correction for measurement error, you need to check this works for you)
if ((V_Bat > 3700 || V_Bat < 2400)) { //Checking for Error states (just battery voltage for now)
state_num = 5; //go directly to jail
next_state = 5; // stay in jail
digitalWrite(7,true); //turn on the red LED
current_ref = 0; // no current
}
current_measure = (ina219.getCurrent_mA()); // sample the inductor current (via the sensor chip)
error_amps = (current_ref - current_measure) / 1000; //PID error calculation
pwm_out = pidi(error_amps); //Perform the PID controller calculation
pwm_out = saturation(pwm_out, 0.99, 0.01); //duty_cycle saturation
analogWrite(6, (int)(255 - pwm_out * 255)); // write it out (inverting for the Buck here)
int_count++; //count how many interrupts since this was last reset to zero
loop_trigger = 0; //reset the trigger and move on with life
}
if (int_count == 1000) { // SLOW LOOP (1Hz)
input_switch = digitalRead(2); //get the OL/CL switch status
switch (state_num) { // STATE MACHINE (see diagram)
case 0:{ // Start state (no current, no LEDs)
current_ref = 0;
if (input_switch == 1) { // if switch, move to charge
next_state = 1;
digitalWrite(8,true);
} else { // otherwise stay put
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 1:{ // Charge state (250mA and a green LED)
current_ref = 400;
if (V_Bat < 3600) { // if not charged, stay put
next_state = 1;
digitalWrite(8,true);
} else { // otherwise go to charge rest
next_state = 2;
digitalWrite(8,false);
}
if(input_switch == 0){ // UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 2:{ // Charge Rest, green LED is off and no current
current_ref = 0;
if (rest_timer < 30) { // Stay here if timer < 30
next_state = 2;
digitalWrite(8,false);
rest_timer++;
} else { // Or move to discharge (and reset the timer)
next_state = 3;
digitalWrite(8,false);
rest_timer = 0;
}
if(input_switch == 0){ // UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 3:{ //Discharge state (-250mA and no LEDs)
current_ref = -400;
if (V_Bat > 2500) { // While not at minimum volts, stay here
next_state = 3;
digitalWrite(8,false);
} else { // If we reach full discharged, move to rest
next_state = 4;
digitalWrite(8,false);
}
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 4:{ // Discharge rest, no LEDs no current
current_ref = 0;
if (rest_timer < 30) { // Rest here for 30s like before
next_state = 4;
digitalWrite(8,false);
rest_timer++;
} else { // When thats done, move back to charging (and light the green LED)
next_state = 1;
digitalWrite(8,true);
rest_timer = 0;
}
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 5: { // ERROR state RED led and no current
current_ref = 0;
next_state = 5; // Always stay here
digitalWrite(7,true);
digitalWrite(8,false);
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(7,false);
}
break;
}
default :{ // Should not end up here ....
Serial.println("Boop");
current_ref = 0;
next_state = 5; // So if we are here, we go to error
digitalWrite(7,true);
}
}
dataString = String(state_num) + "," + String(V_Bat) + "," + String(current_ref) + "," + String(current_measure); //build a datastring for the CSV file
Serial.println(dataString); // send it to serial as well in case a computer is connected
File dataFile = SD.open("BatCycle.csv", FILE_WRITE); // open our CSV file
if (dataFile){ //If we succeeded (usually this fails if the SD card is out)
dataFile.println(dataString); // print the data
} else {
Serial.println("File not open"); //otherwise print an error
}
dataFile.close(); // close the file
int_count = 0; // reset the interrupt count so we dont come back here for 1000ms
}
}
// Timer A CMP1 interrupt. Every 1000us the program enters this interrupt. This is the fast 1kHz loop
ISR(TCA0_CMP1_vect) {
loop_trigger = 1; //trigger the loop when we are back in normal flow
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
}
float saturation( float sat_input, float uplim, float lowlim) { // Saturation function
if (sat_input > uplim) sat_input = uplim;
else if (sat_input < lowlim ) sat_input = lowlim;
else;
return sat_input;
}
float pidi(float pid_input) { // discrete PID function
float e_integration;
e0i = pid_input;
e_integration = e0i;
//anti-windup
if (u1i >= ui_max) {
e_integration = 0;
} else if (u1i <= ui_min) {
e_integration = 0;
}
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
u0i = u1i + delta_ui; //this time's control output
//output limitation
saturation(u0i, ui_max, ui_min);
u1i = u0i; //update last time's control output
e2i = e1i; //update last last time's error
e1i = e0i; // update last time's error
return u0i;
}

199
Energy/PV Analyses.ino Normal file
View file

@ -0,0 +1,199 @@
#include <Wire.h>
#include <INA219_WE.h>
#include <SPI.h>
#include <SD.h>
INA219_WE ina219; // this is the instantiation of the library for the current sensor
// set up variables using the SD utility library functions:
Sd2Card card;
SdVolume volume;
SdFile root;
const int chipSelect = 10;
unsigned int rest_timer;
unsigned int loop_trigger;
unsigned int int_count = 0; // a variables to count the interrupts. Used for program debugging.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float ui_max = 1, ui_min = 0; //anti-windup limitation
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float Ts = 0.001; //1 kHz control frequency.
float current_measure, current_ref = 0, error_amps; // Current Control
float pwm_out;
float V_Bat;
boolean input_switch;
int state_num=0,next_state;
String dataString;
int PWM_in = 1;
float duty_cycle = 0;
int counter = 5;
void setup() {
//Some General Setup Stuff
Wire.begin(); // We need this for the i2c comms for the current sensor
Wire.setClock(700000); // set the comms speed for i2c
ina219.init(); // this initiates the current sensor
Serial.begin(9600); // USB Communications
//Check for the SD Card
Serial.println("\nInitializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println("* is a card inserted?");
while (true) {} //It will stick here FOREVER if no SD is in on boot
} else {
Serial.println("Wiring is correct and a card is present.");
}
if (SD.exists("BatCycle.csv")) { // Wipe the datalog when starting
SD.remove("BatCycle.csv");
}
noInterrupts(); //disable all interrupts
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
//SMPS Pins
pinMode(13, OUTPUT); // Using the LED on Pin D13 to indicate status
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
pinMode(6, OUTPUT); // This is the PWM Pin
//LEDs on pin 7 and 8
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
//Analogue input, the battery voltage (also port B voltage)
pinMode(A0, INPUT);
// TimerA0 initialization for 1kHz control-loop interrupt.
TCA0.SINGLE.PER = 999; //
TCA0.SINGLE.CMP1 = 999; //
TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV16_gc | TCA_SINGLE_ENABLE_bm; //16 prescaler, 1M.
TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1_bm;
// TimerB0 initialization for PWM output
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
interrupts(); //enable interrupts.
analogWrite(6, 120); //just a default state to start with
}
void loop() {
if (loop_trigger == 1){ // FAST LOOP (1kHZ)
state_num = next_state; //state transition
V_Bat = analogRead(A0)*4.096/1.03; //check the battery voltage (1.03 is a correction for measurement error, you need to check this works for you)
current_measure = (ina219.getCurrent_mA()); // sample the inductor current (via the sensor chip)
int_count++; //count how many interrupts since this was last reset to zero
loop_trigger = 0; //reset the trigger and move on with life
}
if (int_count == 1000) { // SLOW LOOP (1Hz)
input_switch = digitalRead(2); //get the OL/CL switch status
switch (state_num) { // STATE MACHINE (see diagram)
case 0:{ // Start state (no current, no LEDs)
current_ref = 0;
if (input_switch == 1) { // if switch, move to charge
next_state = 1;
digitalWrite(8,true);
} else { // otherwise stay put
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 1:{ // Charge state (250mA and a green LED)
analogWrite(6,PWM_in);
if(counter == 5){
if(PWM_in < 255) {
PWM_in = PWM_in + 1;
}
else {
PWM_in = 1;
}
counter = 0;
}
counter = counter +1;
duty_cycle = (PWM_in /255)*100;
if(input_switch == 0){ // UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(8,false);
}
break;
}
case 5: { // ERROR state RED led and no current
current_ref = 0;
next_state = 5; // Always stay here
digitalWrite(7,true);
digitalWrite(8,false);
if(input_switch == 0){ //UNLESS the switch = 0, then go back to start
next_state = 0;
digitalWrite(7,false);
}
break;
}
default :{ // Should not end up here ....
Serial.println("Boop");
current_ref = 0;
next_state = 5; // So if we are here, we go to error
digitalWrite(7,true);
}
}
dataString = String(state_num) + "," + String(V_Bat) + "," + String(current_measure) + "," + String(PWM_in) + "," + String(duty_cycle); ; //build a datastring for the CSV file
Serial.println(dataString); // send it to serial as well in case a computer is connected
File dataFile = SD.open("BatCycle.csv", FILE_WRITE); // open our CSV file
if (dataFile){ //If we succeeded (usually this fails if the SD card is out)
dataFile.println(dataString); // print the data
} else {
Serial.println("File not open"); //otherwise print an error
}
dataFile.close(); // close the file
int_count = 0; // reset the interrupt count so we dont come back here for 1000ms
}
}
// Timer A CMP1 interrupt. Every 1000us the program enters this interrupt. This is the fast 1kHz loop
ISR(TCA0_CMP1_vect) {
loop_trigger = 1; //trigger the loop when we are back in normal flow
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
}
float saturation( float sat_input, float uplim, float lowlim) { // Saturation function
if (sat_input > uplim) sat_input = uplim;
else if (sat_input < lowlim ) sat_input = lowlim;
else;
return sat_input;
}
float pidi(float pid_input) { // discrete PID function
float e_integration;
e0i = pid_input;
e_integration = e0i;
//anti-windup
if (u1i >= ui_max) {
e_integration = 0;
} else if (u1i <= ui_min) {
e_integration = 0;
}
delta_ui = kpi * (e0i - e1i) + kii * Ts * e_integration + kdi / Ts * (e0i - 2 * e1i + e2i); //incremental PID programming avoids integrations.
u0i = u1i + delta_ui; //this time's control output
//output limitation
saturation(u0i, ui_max, ui_min);
u1i = u0i; //update last time's control output
e2i = e1i; //update last last time's error
e1i = e0i; // update last time's error
return u0i;
}