/**
* @file
* Proportional-integral (PI) control law.
*
* This module implements a simple position-type PI controller:
* <pre>
* u = [ kp * e + ki * sum(e) ] >> shift
* </pre>
* <tt>shift</tt> is a right bit shift used to scale the output of the
* controller down from the 32-bit intermediate result.
*
* An anti-windup provision is implemented on the PI integrator to prevent
* deep saturation (aka integrator windup):
* - The new control output with the latest integrator value is computed.
* - If the control output exceeds either output limit, <i>and</i> the latest
* change in the integrator is in the same direction, then the new integrator
* value is not saved for the next call.
* - Otherwise, the integrator is saved for the next call.
*/
#include <stdbool.h>
#include "pi_control.h"
/**
* Proportional-integral (PI) control law.
*
* @param[in,out] p control parameter and state structure
* @param[in] e error signal
*
* @return control output <code>u</code>
*/
int pi_control (struct PIControl *p, int e)
{
bool int_ok; /* Whether or not the integrator should update */
long new_i; /* Proposed new integrator value */
long u; /* Control output */
/* Compute new integrator and the final control output. */
new_i = p->i + e;
u = (p->kp * (long)e + p->ki * new_i) >> p->shift;
/* Check for saturation. In the event of saturation in any one direction,
inhibit saving the integrator if doing so would deepen the saturation. */
int_ok = true;
/* Positive saturation? */
if (u > p->max)
{
/* Clamp the output */
u = p->max;
/* Error is the same sign? Inhibit integration. */
if (e > 0)
{
int_ok = false;
}
}
/* Repeat for negative sign */
else if (u < p->min)
{
u = p->min;
if (e < 0)
{
int_ok = false;
}
}
/* Update the integrator if allowed. */
if (int_ok)
{
p->i = new_i;
}
return (int)u;
}
/**
* Initializes the PI control.
*
* This function resets the PI integrator to zero.
*
* @param[in,out] p control parameter structure
*/
void pi_control_init (struct PIControl *p)
{
p->i = 0L;
}
/* Header file */
#if !defined(_PI_CONTROL_H)
#define _PI_CONTROL_H
/**
* @file
* Proportional-integral (PI) control law header file.
*/
/** PI control data structure. This structure contains configuration (the
proportional and integral gain, plus a final divisor), output limits, and
an integration accumulator (the PI controller's state variable). */
struct PIControl
{
int kp; /**< Proportional gain constant */
int ki; /**< Integral gain constant */
unsigned char shift; /**< Right shift to divide */
int max; /**< Maximum value */
int min; /**< Minimum value */
long i; /**< Current integrator value */
};
/* Prototypes */
int pi_control (struct PIControl *p, int e);
void pi_control_init (struct PIControl *p);
#endif /* _PI_CONTROL_H */
/**
* @file
* Table lookup with interpolation (1-D and 2-D).
*
* This is a 1/2-D table lookup facility. Each routine looks up data in a table
* structure, interpolating as needed between data points. The 2-D version
* looks up along 2 axes and interpolates in two dimensions.
*
* <h2>Limitations</h2>
* - The table axes (input values) must monotonically increase, or the lookup
* will fail.
* - The index data type is nominally 8 bits, limiting the table length to
* 256 elements. Change <code>index_t</code> if larger tables are needed.
*/
#include <stdint.h>
#include <stdbool.h>
#include "lookup.h"
/** Index data type */
typedef uint8_t index_t;
/**
* 1-D table lookup.
*
* This function performs a 1-D table lookup with interpolation. The output
* value is clamped to either of the table end values when the input value is
* out of bounds.
*
* @param[in] t table data structure
* @param[in] ix input (X-axis) value
* @param[out] o output data
*
* @retval true if the lookup result is suspect due to clipping
* @retval false on successful lookup
*/
bool lookup1d (Table1d *t, int ix, int *o)
{
index_t i;
/* ------------------------------------------------------------------------ */
/* Off the end of the table */
if (ix > t->columns[t->ncols - 1])
{
*o = t->table[t->ncols - 1];
return true;
}
/* Off beginning of the table */
else if (ix < t->columns[0])
{
*o = t->table[0];
return true;
}
/* Within the bounds of the table */
for (i = 0; i < t->ncols - 1; ++i)
{
if ( ix >= t->columns[i]
&& ix <= t->columns[i + 1])
{
/* Output (table) low value */
int o_low = t->table[i];
/* Input (X-axis) low value */
int i_low = t->columns[i];
/* Spead between the two adjacent input values */
int i_delta = t->columns[i + 1] - t->columns[i];
/* Spread between the two adjacent table output values */
int o_delta = t->table[i + 1] - t->table[i];
/* Prevent division by zero. We could get here if two consecutive
input values in the table are the same. */
if (o_delta == 0)
{
*o = o_low;
return true;
}
*o = o_low + ((ix - i_low) * (long)o_delta) / i_delta;
return false;
}
}
/* Didn't find it (we shouldn't ever get here). */
return true;
}
/**
* 2-D table lookup.
*
* This function performs a 2-D table lookup with interpolation. The output
* value is clamped to either of the table end values when the input value is
* out of bounds.
*
* @param[in] t table data structure
* @param[in] ix input (X-axis) value
* @param[in] iy input (Y-axis) value
* @param[out] o output value
*
* @retval true if the lookup result is suspect due to clipping
* @retval false on successful lookup
*/
bool lookup2d (Table2d *t, int ix, int iy, int *o)
{
/* The lower X and Y coordinates of the interpolation box */
index_t i, j;
/* Set whenever one of the lookups goes off the end of the table */
bool is_fault = false;
/* ------------------------------------------------------------------------ */
/* X axis coordinate lookup */
/* Off the end of the table */
if (ix > t->columns[t->ncols - 1])
{
/* Pretend the input value is right at the table edge so that interpolation
works as expected */
ix = t->columns[t->ncols - 1];
i = t->ncols - 1;
is_fault = true;
}
/* Off beginning of the table */
else if (ix < t->columns[0])
{
ix = t->columns[0];
i = 0;
is_fault = true;
}
/* Within the bounds of the table */
else
{
for (i = 0; i < t->ncols - 1; ++i)
{
if ( ix >= t->columns[i]
&& ix <= t->columns[i + 1])
{
break;
}
}
}
/* ------------------------------------------------------------------------ */
/* Y axis coordinate lookup */
/* Off the bottom of the table */
if (iy > t->rows[t->nrows - 1])
{
iy = t->rows[t->nrows - 1];
j = t->nrows - 1;
is_fault = true;
}
/* Off the top of the table */
else if (iy < t->rows[0])
{
iy = t->rows[0];
j = 0;
is_fault = true;
}
/* Within the bounds of the table */
else
{
for (j = 0; j < t->nrows - 1; ++j)
{
if ( iy >= t->rows[j]
&& iy <= t->rows[j + 1])
{
break;
}
}
}
/* ------------------------------------------------------------------------ */
/* 2-D interpolation */
/* At this point we know that the input X value is between
column[i] and column[i+1] and that the input Y value is between
row[j] and row[j+1]. Therefore we have a rectangle in which we need
to interpolate.
To do the interpolation, we first interpolate between column i and
column i+1 on the upper row j. Then, we interpolate between the same
columns on row j+1. Finally, we interpolate vertically between the two
rows based on the input Y value.
row0 is the upper row data and row1 is the lower (higher subscript) row
data. */
{
const int *row0 = &t->table[j * t->ncols];
const int *row1 = &row0[t->ncols];
/* Difference between the two adjacent column values */
int i_delta = t->columns[i + 1] - t->columns[i];
/* Difference between the two adjacent row values */
int j_delta = t->rows[j + 1] - t->rows[j];
/* Low column value */
int i_low = t->columns[i];
/* Low row value */
int j_low = t->rows[j];
/* Interpolation results for the upper and lower rows */
int o0, o1;
/* Prevent division by zero if the input values aren't increasing.
If no division by zero, interpolate between columns in the upper and
lower row. */
if (i_delta == 0)
{
o0 = row0[i];
o1 = row1[i];
is_fault = true;
}
else
{
/* Interpolate the upper row */
{
int o_low = row0[i]; /* Row value at low column # */
int o_delta = row0[i + 1] - row0[i]; /* Difference from next column */
o0 = o_low + ((ix - i_low) * (long)o_delta) / i_delta;
}
/* Interpolate the lower (higher subscript) row */
{
int o_low = row1[i]; /* Row value at low column # */
int o_delta = row1[i + 1] - row1[i]; /* Difference from next column */
o1 = o_low + ((ix - i_low) * (long)o_delta) / i_delta;
}
}
/* Guard against division by zero in the row axis. If all is well,
interpolate between the two row interpolation results from earlier. */
if (j_delta == 0)
{
*o = o0;
is_fault = true;
}
else
{
*o = o0 + ((iy - j_low) * (long)(o1 - o0)) / j_delta;
}
}
return is_fault;
}
/* Header file */
#if !defined(_LOOKUP_H)
#define _LOOKUP_H
/**
* @file
* Table lookup with interpolation (1-D and 2-D) header file.
*/
#include <stdbool.h>
/** One dimensional lookup table. */
typedef const struct
{
/** Number of elements in the table. This must be at least 2. */
unsigned char ncols;
/** List of input values. */
int *columns;
/** Table data (output values). The output values list must have the same
length as the input list. */
int *table;
} Table1d;
/** Two dimensional lookup table. */
typedef const struct
{
/** Number of columns (X values) in the table. Must be at least 2. */
unsigned char ncols;
/** Number of rows (Y values) in the table. Must be at least 2. */
unsigned char nrows;
/** X-axis input values list. */
int *columns;
/** Y-axis input values list. */
int *rows;
/** Table data. This is an array of <code>columns</code>X<code>rows</code>,
arranged in rows. For example, <code>table[1]</code> is the second
column in the first row. */
int *table;
} Table2d;
/* Prototypes */
bool lookup1d (Table1d *t, int ix, int *o);
bool lookup2d (Table2d *t, int ix, int iy, int *o);
#endif
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define PI 3.1415926536
double AWGN_generator()
{/* Generates additive white Gaussian Noise samples with zero mean and a standard deviation of 1. */
double temp1;
double temp2;
double result;
int p;
p = 1;
while( p > 0 )
{
temp2 = ( rand() / ( (double)RAND_MAX ) ); /* rand() function generates an
integer between 0 and RAND_MAX,
which is defined in stdlib.h.
*/
if ( temp2 == 0 )
{// temp2 is >= (RAND_MAX / 2)
p = 1;
}// end if
else
{// temp2 is < (RAND_MAX / 2)
p = -1;
}// end else
}// end while()
temp1 = cos( ( 2.0 * (double)PI ) * rand() / ( (double)RAND_MAX ) );
result = sqrt( -2.0 * log( temp2 ) ) * temp1;
return result; // return the generated random sample to the caller
}// end AWGN_generator()
// Get a bit field from a value
#define GetField(Var, Mask, Shift) \
(((Var) >> (Shift)) & (Mask))
// Set a bit field in a value
#define SetField(Var, Mask, Shift, Val) \
(Var) = (((Var) & ~((Mask) << (Shift))) | (((Val) & (Mask)) << (Shift)))
#define MAX_IR_INDEX 64
#define ESCAPE_COUNT 0x2200ul /* Could be reduced?*/
#pragma udata ircounts
unsigned int lcounts[MAX_IR_INDEX];
unsigned int hcounts[MAX_IR_INDEX];
#pragma interrupt high_isr /*=section(".tmpdata")*/
void high_isr(void) {
unsigned int l;
unsigned int c;
unsigned char th, tl;
l = 0;
TMR1H = 0; // Always write H before L
TMR1L = 0;
// Check that this is the INT0 interrupt
// IRRX is int0
if(INTCONbits.INT0IF)
{
// Disable the interrupt source
INTCONbits.INT0IE = 0;
// the code in the while needs to be balanced so that the ==0 and ==1 bits are the size number of cycles.
while(lindex < MAX_IR_INDEX)
{
while(PORTBbits.RB0 == 0)
{
l++;
if(l == ESCAPE_COUNT)
{
lcounts[lindex] = 0xffff;
goto done;
}
}
tl = TMR1L;
th = TMR1H; // << 8 ;// Always read L before H
lcounts[lindex++] = th * 256 + tl;
l = 0;
TMR1H = 0; // Always right H before L
TMR1L = 0;
while(PORTBbits.RB0 == 1)
{
l++;
if(l == ESCAPE_COUNT)
{
hcounts[hindex] = 0xffff;
goto done;
}
}
tl = TMR1L;
th = TMR1H; //<< 8 ;// Always read L before H
hcounts[hindex++] = th * 256 + tl;
l = 0;
TMR1H = 0; // Always write H before L
TMR1L = 0;
}
}
done:
codeReady = 1;
// reset interrupt status bit
//INTCONbits.INT0IE = 1;
//INTCONbits.INT0IF = 0;
return;
}
void zeroIR() {
unsigned char c;
// Initialize the IR Count holders
lindex = 0;
hindex = 0;
for(c = 0; c < MAX_IR_INDEX; c++)
{
lcounts[c] = 0xdead;
}
codeReady = 0;
// reset interrupt status bit
INTCONbits.INT0IE = 1;
INTCONbits.INT0IF = 0;
}
void transmitIR(void) {
unsigned char c;
// First check that the code is valid by examining the first LCOUNT
// if it is less than our defined threshold we will not transmit it.
// The first lcount holds the low-going preamble, it must be a minimum length or it does not
// represent a valid IR Transmission preamble.
if(lcounts[0] < PREAMBLE_MINIMUM)
{
return;
}
sprintf(serTX, (CAST) "$%c%cIC%d,", id1, id2, lindex);
rs485_puts(serTX);
for(c = 0; c < lindex; c++)
{
sprintf(serTX, (CAST) "%04x,%04x, ", lcounts[c], hcounts[c]);
rs485_puts(serTX);
}
sprintf(serTX, (CAST) "\n");
rs485_puts(serTX);
}
/* Basic bit manipulation macros
No one should ever have to rewrite these
*/
//Set bit y (0-indexed) of x to '1' by generating a a mask with a '1' in the proper bit location and ORing x with the mask.
#define SET(x,y) x |= (1 << y)
//Set bit y (0-indexed) of x to '0' by generating a mask with a '0' in the y position and 1's elsewhere then ANDing the mask with x.
#define CLEAR(x,y) x &= ~(1<< y)
//Return '1' if the bit value at position y within x is '1' and '0' if it's 0 by ANDing x with a bit mask where the bit in y's position is '1' and '0' elsewhere and comparing it to all 0's. Returns '1' in least significant bit position if the value of the bit is '1', '0' if it was '0'.
#define READ(x,y) ((0u == (x & (1<<y)))?0u:1u)
//Toggle bit y (0-index) of x to the inverse: '0' becomes '1', '1' becomes '0' by XORing x with a bitmask where the bit in position y is '1' and all others are '0'.
#define TOGGLE(x,y) (x ^= (1<<y))
clc
clear all
close all
s1 = serial('COM26', 'BaudRate', 57600);
set(s1,'Terminator',35);
fopen(s1);
V=[];
val='';
time=200;
count=0;
f=0;
% **********************************************************************
% Read Serial values
tic
tt=toc;
while(tt<time)
val=fscanf(s1);
tt=toc;
%########################################################################
A=[];
B=[];
C=[];
b=[];
x=[];
j=1;
if(j<numel(val))
while(val(j)~='$')
a=val(j);
b=[b,a];
j=j+1;
end
A=[A,str2num(b)];
j=j+1;
b=[];
while(val(j)~='*')
a=val(j);
b=[b,a];
j=j+1;
end
B=[B,str2num(b)];
j=j+1;
b=[];
while(val(j)~='#')
a=val(j);
b=[b,a];
j=j+1;
end
C=[C,str2num(b)];
i=j;
b=[];
f=f+i;
end
x=[x,round(toc)];
if(~(isempty(A)&&isempty(B)&&isempty(C)))
subplot(1,2,1)
plot(x,A,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',5);
hold on % if u want this in different graph remove hold on and add "figure" command before each graph;
plot(x,B,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','g','MarkerSize',5);
grid on;
subplot(1,2,2)
plot(x,(B/(A+B))*100,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','m','MarkerSize',5);
hold on
plot(x,C,'--rs','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','b','MarkerSize',5);
grid on;
pause(.01);
end
end
hold off
fclose(s1);
/*! \details This structure holds the data to run a
* floating point PID loop.
*/
typedef struct{
float max /*! \brief Max manipulated value */;
float min /*! \brief Miniumum manipulated value */;
float e /*! \brief Error value */;
float i /*! \brief Integrator value */;
float kp /*! \brief Proportional constant */;
float ki /*! \brief Integrator constant */;
float kd /*! \brief Differential constant */;
} pid_f_t;
/*! \details This function initializes the data in a PID structure.
*
*/
void pid_init_f(pid_f_t * ptr /*! A pointer to the PID data structure */,
float min /*! The manipulated variable's minimum value */,
float max /*! The manipulated variable's maximum value */){
memset(ptr, 0, sizeof(pid_f_t));
ptr->min = min;
ptr->max = max;
}
/*! \details This function updates the value of the manipulated variable (MV)
* based on the current state of the PID loop.
*/
float pid_update_f(float sp /*! The set point */,
float pv /*! The process variable */,
pid_f_t * ptr /*! A pointer to the PID constants */){
float temp;
float e;
float p;
float manp;
float tmpi;
//get the error from the last call
e = ptr->e;
//calculate the new error (set point - present value)
ptr->e = sp - pv;
//use a temp variable for the integrator
tmpi = ptr->i + ptr->e;
//update the manipulated process variable
manp = ptr->kp * ptr->e + ptr->ki * tmpi + ptr->kd * (ptr->e - e);
//the integrator is only updated if the manipulated process is within range
//otherwise the system will likely become unstable
if ( (manp < ptr->max) && (manp > ptr->min) ){
ptr->i = tmpi;
} else if ( manp > ptr->max ){
manp = ptr->max;
} else if ( manp < ptr->min ){
manp = ptr->min;
}
return manp;
}
