From f2f1dfa80fe40ddb4fe8443a7db73b187e6b6e2e Mon Sep 17 00:00:00 2001 From: IssaBqain Date: Tue, 8 Jun 2021 02:10:13 +0100 Subject: [PATCH] Energy stuff added --- Energy/Balance+SOH estimation.ino | 340 ++++++++++++++++++++++++++++ Energy/Battery Characterisation.ino | 246 ++++++++++++++++++++ Energy/PV Analyses.ino | 199 ++++++++++++++++ 3 files changed, 785 insertions(+) create mode 100644 Energy/Balance+SOH estimation.ino create mode 100644 Energy/Battery Characterisation.ino create mode 100644 Energy/PV Analyses.ino diff --git a/Energy/Balance+SOH estimation.ino b/Energy/Balance+SOH estimation.ino new file mode 100644 index 0000000..d44db7f --- /dev/null +++ b/Energy/Balance+SOH estimation.ino @@ -0,0 +1,340 @@ +#include +#include +#include +#include + +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)); +} diff --git a/Energy/Battery Characterisation.ino b/Energy/Battery Characterisation.ino new file mode 100644 index 0000000..2dd32a6 --- /dev/null +++ b/Energy/Battery Characterisation.ino @@ -0,0 +1,246 @@ +#include +#include +#include +#include + +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; +} diff --git a/Energy/PV Analyses.ino b/Energy/PV Analyses.ino new file mode 100644 index 0000000..89f893d --- /dev/null +++ b/Energy/PV Analyses.ino @@ -0,0 +1,199 @@ +#include +#include +#include +#include + +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; +}