supleed2/ELEC50003-P1-CW
1
0
Fork 0
mirror of https://github.com/supleed2/ELEC50003-P1-CW.git synced 2024-12-23 05:55:50 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add files via upload

Browse source
This commit is contained in:
NikitaK 2021-06-09 18:04:51 +01:00 committed by GitHub
parent 5411a247bc
commit be103e3072
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 798 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

798
Drive/20210706_Drive_Mars_Rover.ino Normal file
View file

@ -0,0 +1,798 @@
#include <Arduino.h>
#include <ArduinoJson.h>
#include <string>
#include <Wire.h>
#include <INA219_WE.h>
#include "SPI.h"
#define RXpin 0 // Define your RX pin here
#define TXpin 0 // Define your TX pin here
bool debug = true;
//TO IMPLEMENT
//DONE 2 way serial
//DONE F<>,B<>,S,L<>,R<>,p<0--1023>
//DONE Obtain current and power usage, get voltage from analog pin
//request angle facing
//DONE speed control 0-1
//speed calibration, 0 stop and max speed to match
//distance travveled and x and y at request
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
INA219_WE ina219; // this is the instantiation of the library for the current sensor
float open_loop, closed_loop; // Duty Cycles
float vpd, vb, vref, iL, dutyref, current_mA; // Measurement Variables
unsigned int sensorValue0, sensorValue1, sensorValue2, sensorValue3; // ADC sample values declaration
float ev = 0, cv = 0, ei = 0, oc = 0; //internal signals
float Ts = 0.0008; //1.25 kHz control frequency. It's better to design the control period as integral multiple of switching period.
float kpv = 0.05024, kiv = 15.78, kdv = 0; // voltage pid.
float u0v, u1v, delta_uv, e0v, e1v, e2v; // u->output; e->error; 0->this time; 1->last time; 2->last last time
float kpi = 0.02512, kii = 39.4, kdi = 0; // current pid.
float u0i, u1i, delta_ui, e0i, e1i, e2i; // Internal values for the current controller
float uv_max = 4, uv_min = 0; //anti-windup limitation
float ui_max = 1, ui_min = 0; //anti-windup limitation
float current_limit = 1.0;
boolean Boost_mode = 0;
boolean CL_mode = 0;
unsigned int loopTrigger;
unsigned int com_count = 0; // a variables to count the interrupts. Used for program debugging.
//************************** Motor Constants **************************//
unsigned long previousMillis = 0; //initializing time counter
const long f_i = 10000; //time to move in forward direction, please calculate the precision and conversion factor
const long r_i = 20000; //time to rotate clockwise
const long b_i = 30000; //time to move backwards
const long l_i = 40000; //time to move anticlockwise
const long s_i = 50000;
int DIRRstate = LOW; //initializing direction states
int DIRLstate = HIGH;
int DIRL = 20; //defining left direction pin
int DIRR = 21; //defining right direction pin
int pwmr = 5; //pin to control right wheel speed using pwm
int pwml = 9; //pin to control left wheel speed using pwm
//*******************************************************************//
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
//-------------------------------------------------------OPTICAL SENSOR CODE START------------------------------------------------------//
#define PIN_SS 10
#define PIN_MISO 12
#define PIN_MOSI 11
#define PIN_SCK 13
#define PIN_MOUSECAM_RESET 8
#define PIN_MOUSECAM_CS 7
#define ADNS3080_PIXELS_X 30
#define ADNS3080_PIXELS_Y 30
#define ADNS3080_PRODUCT_ID 0x00
#define ADNS3080_REVISION_ID 0x01
#define ADNS3080_MOTION 0x02
#define ADNS3080_DELTA_X 0x03
#define ADNS3080_DELTA_Y 0x04
#define ADNS3080_SQUAL 0x05
#define ADNS3080_PIXEL_SUM 0x06
#define ADNS3080_MAXIMUM_PIXEL 0x07
#define ADNS3080_CONFIGURATION_BITS 0x0a
#define ADNS3080_EXTENDED_CONFIG 0x0b
#define ADNS3080_DATA_OUT_LOWER 0x0c
#define ADNS3080_DATA_OUT_UPPER 0x0d
#define ADNS3080_SHUTTER_LOWER 0x0e
#define ADNS3080_SHUTTER_UPPER 0x0f
#define ADNS3080_FRAME_PERIOD_LOWER 0x10
#define ADNS3080_FRAME_PERIOD_UPPER 0x11
#define ADNS3080_MOTION_CLEAR 0x12
#define ADNS3080_FRAME_CAPTURE 0x13
#define ADNS3080_SROM_ENABLE 0x14
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_LOWER 0x19
#define ADNS3080_FRAME_PERIOD_MAX_BOUND_UPPER 0x1a
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_LOWER 0x1b
#define ADNS3080_FRAME_PERIOD_MIN_BOUND_UPPER 0x1c
#define ADNS3080_SHUTTER_MAX_BOUND_LOWER 0x1e
#define ADNS3080_SHUTTER_MAX_BOUND_UPPER 0x1e
#define ADNS3080_SROM_ID 0x1f
#define ADNS3080_OBSERVATION 0x3d
#define ADNS3080_INVERSE_PRODUCT_ID 0x3f
#define ADNS3080_PIXEL_BURST 0x40
#define ADNS3080_MOTION_BURST 0x50
#define ADNS3080_SROM_LOAD 0x60
#define ADNS3080_PRODUCT_ID_VAL 0x17
int total_x = 0;
int total_y = 0;
int total_x1 = 0;
int total_y1 = 0;
int x=0;
int y=0;
int a=0;
int b=0;
int distance_x=0;
int distance_y=0;
volatile byte movementflag=0;
volatile int xydat[2];
int convTwosComp(int b){
//Convert from 2's complement
if(b & 0x80){
b = -1 * ((b ^ 0xff) + 1);
}
return b;
}
int tdistance = 0;
void mousecam_reset()
{
digitalWrite(PIN_MOUSECAM_RESET,HIGH);
delay(1); // reset pulse >10us
digitalWrite(PIN_MOUSECAM_RESET,LOW);
delay(35); // 35ms from reset to functional
}
int mousecam_init()
{
pinMode(PIN_MOUSECAM_RESET,OUTPUT);
pinMode(PIN_MOUSECAM_CS,OUTPUT);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
mousecam_reset();
int pid = mousecam_read_reg(ADNS3080_PRODUCT_ID);
if(pid != ADNS3080_PRODUCT_ID_VAL)
return -1;
// turn on sensitive mode
mousecam_write_reg(ADNS3080_CONFIGURATION_BITS, 0x19);
return 0;
}
void mousecam_write_reg(int reg, int val)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg | 0x80);
SPI.transfer(val);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(50);
}
int mousecam_read_reg(int reg)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(reg);
delayMicroseconds(75);
int ret = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(1);
return ret;
}
struct MD
{
byte motion;
char dx, dy;
byte squal;
word shutter;
byte max_pix;
};
void mousecam_read_motion(struct MD *p)
{
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_MOTION_BURST);
delayMicroseconds(75);
p->motion = SPI.transfer(0xff);
p->dx = SPI.transfer(0xff);
p->dy = SPI.transfer(0xff);
p->squal = SPI.transfer(0xff);
p->shutter = SPI.transfer(0xff)<<8;
p->shutter |= SPI.transfer(0xff);
p->max_pix = SPI.transfer(0xff);
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(5);
}
// pdata must point to an array of size ADNS3080_PIXELS_X x ADNS3080_PIXELS_Y
// you must call mousecam_reset() after this if you want to go back to normal operation
int mousecam_frame_capture(byte *pdata)
{
mousecam_write_reg(ADNS3080_FRAME_CAPTURE,0x83);
digitalWrite(PIN_MOUSECAM_CS, LOW);
SPI.transfer(ADNS3080_PIXEL_BURST);
delayMicroseconds(50);
int pix;
byte started = 0;
int count;
int timeout = 0;
int ret = 0;
for(count = 0; count < ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y; )
{
pix = SPI.transfer(0xff);
delayMicroseconds(10);
if(started==0)
{
if(pix&0x40)
started = 1;
else
{
timeout++;
if(timeout==100)
{
ret = -1;
break;
}
}
}
if(started==1)
{
pdata[count++] = (pix & 0x3f)<<2; // scale to normal grayscale byte range
}
}
digitalWrite(PIN_MOUSECAM_CS,HIGH);
delayMicroseconds(14);
return ret;
}
//-------------------------------------------------------OPTICAL SENSOR CODE END------------------------------------------------------//
//Tracker Variables
int current_x = 0;
int current_y = 0;
int goal_x = 0;
int goal_y = 0;
int distanceGoal;
bool commandComplete = 1;
float powerUsage_mWh = 0;
int distTravelled_mm = 0;
bool initialAngleSet=false;
//calibration varibles
int angularDrift = 0; //+ve to right, -ve to left
int leftStart = 80; //pwm min for left motor
int leftStop = 255; //pwm max for left motor
int rightStart = 80; //pwm min for right motor
int rightStop = 255; //pwm max for right motor
//Energy Usage Variables
unsigned long previousMillis_Energy = 0; // will store last time energy use was updated
const long interval_Energy = 1000; //energy usaged update frequency
float totalEnergyUsed = 0;
float powerUsed = 0;
int loopCount = 0;
float motorVoltage = 0;
int getPWMfromSpeed(float speedr,bool left) {
if (speedr >= 1) {
return 512;
}
else if (speedr < 0) {
return 0;
}
else {
int speedpercentage = (speedr * 100);
if(left){
return map(speedpercentage,0,100,leftStart,leftStop);
}
else{
return map(speedpercentage,0,100,rightStart,rightStop);
}
}
}
void setup()
{
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
//************************** Motor Pins Defining **************************//
pinMode(DIRR, OUTPUT);
pinMode(DIRL, OUTPUT);
pinMode(pwmr, OUTPUT);
pinMode(pwml, OUTPUT);
digitalWrite(pwmr, HIGH); //setting right motor speed at maximum
digitalWrite(pwml, HIGH); //setting left motor speed at maximum
//*******************************************************************//
//Basic pin setups
noInterrupts(); //disable all interrupts
pinMode(13, OUTPUT); //Pin13 is used to time the loops of the controller
pinMode(3, INPUT_PULLUP); //Pin3 is the input from the Buck/Boost switch
pinMode(2, INPUT_PULLUP); // Pin 2 is the input from the CL/OL switch
analogReference(EXTERNAL); // We are using an external analogue reference for the ADC
// TimerA0 initialization for 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
pinMode(6, OUTPUT);
TCB0.CTRLA = TCB_CLKSEL_CLKDIV1_gc | TCB_ENABLE_bm; //62.5kHz
analogWrite(6, 120);
interrupts(); //enable interrupts.
Wire.begin(); // We need this for the i2c comms for the current sensor
ina219.init(); // this initiates the current sensor
Wire.setClock(700000); // set the comms speed for i2c
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
Serial.begin(115200); // Set up hardware UART0 (Connected to USB port)
Serial1.begin(9600); // Set up hardware UART1
//Serial.println(getPWMfromSpeed(-1));
//Serial.println(getPWMfromSpeed(256));
//Serial.println(getPWMfromSpeed(0.5));
// Other Drive setup stuff
/////////currentHeading = REQUEST HEADING HERE;
analogWrite(pwmr,0);
analogWrite(pwml,0);
//digitalWrite(DIRR, LOW);
//digitalWrite(DIRL, HIGH);
pinMode(PIN_SS,OUTPUT);
pinMode(PIN_MISO,INPUT);
pinMode(PIN_MOSI,OUTPUT);
pinMode(PIN_SCK,OUTPUT);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32);
SPI.setDataMode(SPI_MODE3);
SPI.setBitOrder(MSBFIRST);
Serial.begin(9600);
if(mousecam_init()==-1)
{
Serial.println("Mouse cam failed to init");
while(1);
}
}
bool commandCompletionStatus = 1; //0-No Command, 1-New Command, 2-Command being run, 3-Command Complete
int requiredHeading = 0;
int distance = 0;
float spd = 0;
int currentHeading = 0;
//reset variables for update on completion
unsigned long previousMillis_Command = 0;
const long interval_Command = 1000;
DeserializationError error;
char asciiart(int k)
{
static char foo[] = "WX86*3I>!;~:,`. ";
return foo[k>>4];
}
byte frame[ADNS3080_PIXELS_X * ADNS3080_PIXELS_Y];
void loop()
{
analogWrite(pwmr,512);
analogWrite(pwml,512);
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, LOW);
Serial.println(pwmr);
//Serial.println("Here");
DynamicJsonDocument rdoc(1024); // receive doc, not sure how big this needs to be
//deserializeJson(rdoc, Serial1); // Take JSON input from UART1
//Check for updates every 1s...
unsigned long currentMillis_Command = millis();
if (currentMillis_Command - previousMillis_Command >= interval_Command) {
// save the last time you blinked the LED
previousMillis_Command = currentMillis_Command;
error = deserializeJson(rdoc, Serial1);
//Serial.println("H");
}
// Test if parsing succeeds.
if (error) {
//Serial.print(F("deserializeJson() failed: "));
//Serial.println(error.f_str());
return;
}
else{
//parsing success, prepare command and pull request information
commandCompletionStatus = 1;
requiredHeading = rdoc["rH"];
distance = rdoc["dist"];
spd = rdoc["sp"];
currentHeading = rdoc["cH"];
//reset variables for update on completion
commandComplete = 0;
powerUsage_mWh = 0.0;
distTravelled_mm = 0;
}
//if(current_x!=goal_x)
// Do Drive stuff, set the 5 values above
if(commandCompletionStatus == 0){ //noCommand
//Do Nothing just wait
}
if(commandCompletionStatus == 1){ //newCommand
//set goals
goal_x += distance*sin(requiredHeading);
goal_y += distance*cos(requiredHeading);
total_y = 0;
total_x = 0;
commandCompletionStatus = 2;
initialAngleSet=false;
}
if(commandCompletionStatus == 2){ //ongoingCommand
//start moving towards goal
//set angle first
if(!initialAngleSet){
//turn to angle
currentHeading = getCurrentHeading();
if(currentHeading<requiredHeading){ //turn right
analogWrite(pwmr,getPWMfromSpeed(spd,false));
analogWrite(pwml,getPWMfromSpeed(spd,true));
digitalWrite(DIRR, LOW);
digitalWrite(DIRL, LOW);
}
else if(currentHeading>requiredHeading){ //turn left
analogWrite(pwmr,getPWMfromSpeed(spd,false));
analogWrite(pwml,getPWMfromSpeed(spd,true));
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, HIGH);
}
else{
//heading correct therefore move to next step...
//STOP!!!!
digitalWrite(pwmr,LOW);
digitalWrite(pwml,LOW);
initialAngleSet = true;
}
}
else{ //then move forwards but check angle for drift
if(total_x - distance > 0){ //go forwards
analogWrite(pwmr,getPWMfromSpeed(spd,false));
analogWrite(pwml,getPWMfromSpeed(spd,true));
digitalWrite(DIRR, HIGH);
digitalWrite(DIRL, LOW);
}
else if(total_x - distance < 0){//go backwards
analogWrite(pwmr,getPWMfromSpeed(spd,false));
analogWrite(pwml,getPWMfromSpeed(spd,true));
digitalWrite(DIRR, LOW);
digitalWrite(DIRL, HIGH);
}
else if((total_x == distance)) {//distance met
//STOP!!!!!
digitalWrite(pwmr,LOW);
digitalWrite(pwml,LOW);
commandCompletionStatus = 3;
initialAngleSet = true;
}
}
}
if(commandCompletionStatus == 3){ //currentPosMatchesOrExceedsRequest
///finish moving
//send update via UART
//prepare feedback variables
commandComplete = true;
current_x = goal_x;
current_y = goal_y;
distTravelled_mm += distance;
//compile energy use
unsigned long currentMillis_Energy = millis();
totalEnergyUsed += (currentMillis_Energy - previousMillis_Energy) * (powerUsed / loopCount) / 1000 / (60 * 60);
previousMillis_Energy = currentMillis_Energy;
if (debug) {
Serial.print(motorVoltage);
Serial.print("Energy Used: ");
Serial.print(totalEnergyUsed);
Serial.println("mWh");
}
loopCount = 0; //reset counter to zero
powerUsed = 0;//reset power usage
powerUsage_mWh = totalEnergyUsed;
totalEnergyUsed = 0;
DynamicJsonDocument tdoc(1024); // transmit doc, not sure how big this needs to be
tdoc["comp"] = commandComplete;
tdoc["mWh"] = powerUsage_mWh;
tdoc["mm"] = distTravelled_mm;
tdoc["pos"][0] = current_x;
tdoc["pos"][1] = current_y;
serializeJson(tdoc, Serial1); // Build JSON and send on UART1
commandCompletionStatus = 0;
}
//Handle power usage
//find motor voltage
//int motorVSensor = analogRead(A5);
//float motorVoltage = motorVSensor * (5.0 / 1023.0);
float motorVoltage = 5;
//find average power
if(current_mA >=0){
powerUsed += current_mA * motorVoltage;
}
Serial.println(powerUsed);
//calculate averages for energy use calculations
loopCount += 1; //handle loop quantity for averaging
//find average current
//find average voltage
//update command/control
if(movementflag){
tdistance = tdistance + convTwosComp(xydat[0]);
Serial.println("Distance = " + String(tdistance));
movementflag=0;
delay(3);
}
int val = mousecam_read_reg(ADNS3080_PIXEL_SUM);
MD md;
mousecam_read_motion(&md);
for(int i=0; i<md.squal/4; i++)
Serial.print('*');
Serial.print(' ');
Serial.print((val*100)/351);
Serial.print(' ');
Serial.print(md.shutter); Serial.print(" (");
Serial.print((int)md.dx); Serial.print(',');
Serial.print((int)md.dy); Serial.println(')');
// Serial.println(md.max_pix);
delay(100);
distance_x = md.dx; //convTwosComp(md.dx);
distance_y = md.dy; //convTwosComp(md.dy);
total_x1 = (total_x1 + distance_x);
total_y1 = (total_y1 + distance_y);
total_x = 10*total_x1/157; //Conversion from counts per inch to mm (400 counts per inch)
total_y = 10*total_y1/157; //Conversion from counts per inch to mm (400 counts per inch)
Serial.print('\n');
Serial.println("Distance_x = " + String(total_x));
Serial.println("Distance_y = " + String(total_y));
Serial.print('\n');
//-------------------------------------------------------SMPS & MOTOR CODE START------------------------------------------------------//
unsigned long currentMillis = millis();
if (loopTrigger) { // This loop is triggered, it wont run unless there is an interrupt
digitalWrite(13, HIGH); // set pin 13. Pin13 shows the time consumed by each control cycle. It's used for debugging.
// Sample all of the measurements and check which control mode we are in
sampling();
CL_mode = digitalRead(3); // input from the OL_CL switch
Boost_mode = digitalRead(2); // input from the Buck_Boost switch
if (Boost_mode) {
if (CL_mode) { //Closed Loop Boost
pwm_modulate(1); // This disables the Boost as we are not using this mode
} else { // Open Loop Boost
pwm_modulate(1); // This disables the Boost as we are not using this mode
}
} else {
if (CL_mode) { // Closed Loop Buck
// The closed loop path has a voltage controller cascaded with a current controller. The voltage controller
// creates a current demand based upon the voltage error. This demand is saturated to give current limiting.
// The current loop then gives a duty cycle demand based upon the error between demanded current and measured
// current
current_limit = 3; // Buck has a higher current limit
ev = vref - vb; //voltage error at this time
cv = pidv(ev); //voltage pid
cv = saturation(cv, current_limit, 0); //current demand saturation
ei = cv - iL; //current error
closed_loop = pidi(ei); //current pid
closed_loop = saturation(closed_loop, 0.99, 0.01); //duty_cycle saturation
pwm_modulate(closed_loop); //pwm modulation
} else { // Open Loop Buck
current_limit = 3; // Buck has a higher current limit
oc = iL - current_limit; // Calculate the difference between current measurement and current limit
if ( oc > 0) {
open_loop = open_loop - 0.001; // We are above the current limit so less duty cycle
} else {
open_loop = open_loop + 0.001; // We are below the current limit so more duty cycle
}
open_loop = saturation(open_loop, dutyref, 0.02); // saturate the duty cycle at the reference or a min of 0.01
pwm_modulate(open_loop); // and send it out
}
}
// closed loop control path
digitalWrite(13, LOW); // reset pin13.
loopTrigger = 0;
}
//************************** Motor Testing **************************//
//this part of the code decides the direction of motor rotations depending on the time lapsed. currentMillis records the time lapsed once it is called.
//*******************************************************************//
//-------------------------------------------------------SMPS & MOTOR CODE END------------------------------------------------------//
}
int getCurrentHeading(){
int currentAngle = 0; //-------------___<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
return currentAngle;
}
// Timer A CMP1 interrupt. Every 800us the program enters this interrupt.
// This, clears the incoming interrupt flag and triggers the main loop.
ISR(TCA0_CMP1_vect) {
TCA0.SINGLE.INTFLAGS |= TCA_SINGLE_CMP1_bm; //clear interrupt flag
loopTrigger = 1;
}
// This subroutine processes all of the analogue samples, creating the required values for the main loop
void sampling() {
// Make the initial sampling operations for the circuit measurements
sensorValue0 = analogRead(A0); //sample Vb
sensorValue2 = analogRead(A2); //sample Vref
sensorValue3 = analogRead(A3); //sample Vpd
current_mA = ina219.getCurrent_mA(); // sample the inductor current (via the sensor chip)
// Process the values so they are a bit more usable/readable
// The analogRead process gives a value between 0 and 1023
// representing a voltage between 0 and the analogue reference which is 4.096V
vb = sensorValue0 * (4.096 / 1023.0); // Convert the Vb sensor reading to volts
vref = sensorValue2 * (4.096 / 1023.0); // Convert the Vref sensor reading to volts
vpd = sensorValue3 * (4.096 / 1023.0); // Convert the Vpd sensor reading to volts
// The inductor current is in mA from the sensor so we need to convert to amps.
// We want to treat it as an input current in the Boost, so its also inverted
// For open loop control the duty cycle reference is calculated from the sensor
// differently from the Vref, this time scaled between zero and 1.
// The boost duty cycle needs to be saturated with a 0.33 minimum to prevent high output voltages
if (Boost_mode == 1) {
iL = -current_mA / 1000.0;
dutyref = saturation(sensorValue2 * (1.0 / 1023.0), 0.99, 0.33);
} else {
iL = current_mA / 1000.0;
dutyref = sensorValue2 * (1.0 / 1023.0);
}
}
float saturation( float sat_input, float uplim, float lowlim) { // Saturatio function
if (sat_input > uplim) sat_input = uplim;
else if (sat_input < lowlim ) sat_input = lowlim;
else;
return sat_input;
}
void pwm_modulate(float pwm_input) { // PWM function
analogWrite(6, (int)(255 - pwm_input * 255));
}
// This is a PID controller for the voltage
float pidv( float pid_input) {
float e_integration;
e0v = pid_input;
e_integration = e0v;
//anti-windup, if last-time pid output reaches the limitation, this time there won't be any intergrations.
if (u1v >= uv_max) {
e_integration = 0;
} else if (u1v <= uv_min) {
e_integration = 0;
}
delta_uv = kpv * (e0v - e1v) + kiv * Ts * e_integration + kdv / Ts * (e0v - 2 * e1v + e2v); //incremental PID programming avoids integrations.there is another PID program called positional PID.
u0v = u1v + delta_uv; //this time's control output
//output limitation
saturation(u0v, uv_max, uv_min);
u1v = u0v; //update last time's control output
e2v = e1v; //update last last time's error
e1v = e0v; // update last time's error
return u0v;
}
// This is a PID controller for the current
float pidi(float pid_input) {
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;
}