Brian Pearson
Digital speed controller
main.cpp
- Committer:
- BPPearson
- Date:
- 2016-01-05
- Revision:
- 0:14f74b704963
File content as of revision 0:14f74b704963:
#include "mbed.h"
#include "USBSerial.h"
DigitalOut myled(LED1);
#include <float.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#define revString "V1.00"
//#define BAUD_RATE 19200 /* Serial baud rate */
#define DEBOUNCE_TIME 10 /* 10 * timer0Cntr1 cycle times */
#define OUTSIDE_COUNT_BAND_LIMIT 40 /* uchar counter limit */
#define MAX_COUNT_VALUE 150000 /* maximum difference between counts */
#define MIN_COUNT_VALUE 1000 /* minimum difference between counts */
#define OUTPUT_CALIBRATION_FACTOR 10.0 / 2048.0 /* 0..2047 binary to 0..10 V */
/* type definitions */
#define uchar unsigned char
#define uint unsigned int
/* boolean definitions */
#define FALSE 0
#define TRUE 1
/* ascii definitions */
#define CR '\x0D'
// peripherals and function blocks used on LPC1768
DigitalOut txLed(LED1);
DigitalOut rxLed(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut rs485Enable(p8);
DigitalOut d2aEnable1(p11);
DigitalOut d2aEnable2(p12);
DigitalOut scope1(p19);
DigitalOut scope2(p20);
InterruptIn sensor1(p15);
InterruptIn sensor2(p16);
InterruptIn sensor3(p17);
InterruptIn sensor4(p18);
InterruptIn sensor5(p21);
InterruptIn sensor6(p22);
InterruptIn sensor7(p23);
InterruptIn sensor8(p24);
PwmOut pulseEvery10ms(p25);
Serial host(p9, p10);
Serial cascade(p13, p14);
Serial pc(USBTX, USBRX); // tx, rx
SPI spi(p5,p6,p7);
Ticker oneMSec;
Ticker fiveMSec;
USBSerial serial;
/* port 3 bit definitions */
#define DTOA_DATA 0x20
#define DTOA_CLOCK 0x40
#define DTOA_SYNC 0x80
/* port 6 bit definitions */
#define WATCHDOG 0x40
/* lcd page definitions */
#define RESET_PAGE 0
#define SPEED_PAGE 1
#define COUNT_PAGE 2
#define DEBUG1_PAGE 3
#define COMMS_PAGE 4
#define LAST_PAGE 4
/* status bits */
#define h8Reset 0x01
/* host comms status definitions */
#define badCircumference 1
#define badSpeedSetpoint 2
#define badRatioSetpoint 3
#define badOutputSetpoint 4
#define badIntGainSetpoint 5
#define badPropGainSetpoint 6
#define badNumOfTeeth 7
#define badMaxSpeedSetpoint 8
#define unknownPacket 9
/* control modes */
#define outputMode 0
#define ratioMode 1
#define speedMode 2
#define thirdNipMode 3
const char *modestrings[4] = {"Out:","Rat:","Spd:","3rd:"};
#define OUTPUT_WITHIN 0
#define OUTPUT_LOW 1
#define OUTPUT_HIGH 2
#define commsBufLen 12 /* Anything over 12 will need display routine changed */
#define hexToBin(x) (x > '9') ? x - '7' : x - '0'
#define LAST_EXT_PAGE 28
const float teethOnEveryWheel[21] = { 0.0, 30.0, 30.0,
0.0, 0.0, 120.0, 60.0, 30.0,
20.0, 60.0, 60.0, 60.0,
60.0, 60.0, 35.0, 35.0,
35.0, 35.0, 35.0, 20.0,
20.0 };
const float pulleyRatios[21] = { 1.0, 1.0, 1.0,
1.0, 1.0,
1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0,
1.0, 1.0, 1.0, 1.0,
1.0 };
const float circumOfRoll[21] = { 0.0, 1.0, 1.0,
0.0, 0.0,
0.75084, 1.2252, 1.2252,
0.82090, 2.00000, 2.00000, 2.00000,
2.00000, 2.00000, 1.09955, 1.09955,
1.03044, 1.03044, 1.09955, 0.68420,
0.82090 };
/* Rope threader circ changed from 0.33500 to 0.42949 */
/* Then changed to 0.68420 as it was doing 137 but saying 86 */
/* because it needed a 0.78 ratio to run at the right speed */
const float maxSpeed[21] = { 1.0, 76.5, 76.5,
74.0, 74.0,
35.0, 220.0, 220.0,
220.0, 220.0, 220.0, 220.0,
220.0, 220.0, 220.0, 220.0,
220.0, 220.0, 220.0, 250.0,
220.0 };
const float propGains[21] = { 0.01, 0.015, 0.015,
0.015, 0.015,
0.015, 0.005, 0.015,
0.01, 0.01, 0.01, 0.01,
0.01, 0.01, 0.01, 0.01,
0.01, 0.01, 0.01, 0.005,
0.01};
const float intGains[21] = { 0.01, 0.006, 0.006,
0.006, 0.006,
0.006, 0.0010, 0.006,
0.003, 0.003, 0.003, 0.003,
0.003, 0.003, 0.003, 0.003,
0.003, 0.003, 0.003, 0.001,
0.003 };
const char *driveName[21] = { "All Drives ","Melt Pump A ","Melt Pump B ",
"Inner Extruder","Outer Extruder",
"First Nip ","Second Nip ","Third Nip ",
"4th Nip East ","1st Bole East ","2nd Bole East ","3rd Bole East ",
"4th Bole East ","5th Bole East ","Cooler 1 East ","Cooler 2 East ",
"Detreat 1 East","Detreat 2 East","Cooler 3 East ","E Rope Thread ",
"4th Nip West " };
const uchar modeAtReset[21] = { outputMode, speedMode, speedMode,
outputMode, outputMode, speedMode, ratioMode, thirdNipMode,
ratioMode, ratioMode, ratioMode, ratioMode,
ratioMode, ratioMode, ratioMode, ratioMode,
ratioMode, ratioMode, ratioMode, ratioMode,
ratioMode };
const int startupCount[21] = { 0, 4, 4,
0, 0, 20, 30, 30,
50, 100, 100, 100,
100, 100, 100, 100,
100, 100, 100, 100,
50 };
const char *debugtitles[28] = { "Invalid ",
"Mode ",
"Address ",
"Age ",
"In Band ",
"In Startup",
"Comms Stat",
"DtoA Stpt ",
"actual Cnt",
"cnt Stpt ",
"loCnt Lmt ",
"hiCnt Lmt ",
"last BCnt ",
"last SCnt ",
"Pull Ratio",
"Circumf ",
"Max Dv Spd",
"Teeth ",
"Start Cnt ",
"Prev Ratio",
"P.P.M.P.S.",
"Spd Stpt ",
"Spd Prev ",
"Act Spd ",
"Cnt error ",
"Int Sum ",
"propGain ",
"intGain "};
/* variables */
uchar commsBuf0[commsBufLen+1];
uchar commsCntr0;
uchar commsBuf1[commsBufLen+1];
uchar commsCntr1;
uchar boardAddress;
uchar activeControllers;
uchar hostCrRcvd;
uchar cascadeCrRcvd;
uchar updateLcdDue;
uchar updateAnalogOutput[8];
uchar lcdPage;
uchar overflowCntr;
uchar frtOverflowCntr;
uchar pageChanged;
uchar redrawLcd;
uchar disableHostComms;
uchar extended_lcd_page;
uchar xmitCascadeSpeed[8];
uchar recalcSpeedSetpoint[8];
uchar recalcFixedPart[8];
uchar recalcReqdCount[8];
uchar processFrtReading[8];
uchar withinControlBand[8]; /* flag */
uchar outsideCountBand[8]; /* counter */
uchar noPulseCntr[8];
uchar inStartupMode[8];
uchar startupCntr[8];
uchar controlMode[8];
uchar status;
uchar output_limit_status[8];
int hostCommsStatus;
int timer0Cntr;
int dtoaSetpnt[8];
int maxDtoa[8];
int minDtoa[8];
unsigned long prevFrtValue[8];
unsigned long currentFrtValue[8];
unsigned long currentCountDiff[8];
unsigned long actualCount[8];
unsigned long countSetpoint[8];
unsigned long loCountLimit[8];
unsigned long hiCountLimit[8];
unsigned long lastBigCount[8];
unsigned long lastSmallCount[8];
unsigned long ageInSecs;
float pulleyRatio[8];
float refFreq;
float rollCircum[8];
float maxSpeedOfDrive[8];
float teethOnWheel[8];
float ratioToPrev[8];
float pulsesPerMetrePerSec[8];
float speedSetpnt[8];
float speedOfPrev[8];
float measuredSpeed[8];
float errorCount[8];
float intSum[8];
float propGain[8];
float intGain[8];
/* Procedure declarations */
void pin15ISR();
void pin16ISR();
void pin17ISR();
void pin18ISR();
void pin21ISR();
void pin22ISR();
void pin23ISR();
void pin24ISR();
void acknowledge_host(int idx);
float calcActualSpeed();
void calcFixedPart(int idx);
void disable_host_comms();
void enable_host_comms();
void enable_interrupts();
void init_free_running_counter();
void init_locals();
void init_timer0();
void init_uarts();
void lcd_put( uchar dat );
void lcd_str( char *str );
void processCascadeComms();
void processHostComms();
void process_latest_count(int idx);
void putch0(uchar c);
void putch1(uchar c);
void putstr0(char *str);
void putstr1(char *str);
void serial_rcv_err_int0();
void serial_rcv_err_int1();
void serial_rcv_int0();
void serial_rcv_int1();
void repetitiveControlUpdate();
void repetitiveUpdateTimer();
void updateDtoaChip(int idx);
void xmit_cascade_speed(int idx);
int main()
{
/* initialize serial ports and vectors */
init_uarts(); /* initialize the uarts */
/* initialise locals */
init_locals();
/* initialize periodic timers */
oneMSec.attach(&repetitiveUpdateTimer, 0.001);
fiveMSec.attach(&repetitiveControlUpdate, 0.005);
/* enable free running counter */
LPC_SC->PCONP |= 1 < 1; //timer0 power on
LPC_TIM0->TCR = 1; //enable Timer2
// configure interrupt inputs
sensor1.mode(PullUp);
sensor1.fall(&pin15ISR);
sensor2.mode(PullUp);
sensor2.fall(&pin16ISR);
sensor3.mode(PullUp);
sensor3.fall(&pin17ISR);
sensor4.mode(PullUp);
sensor4.fall(&pin18ISR);
sensor5.mode(PullUp);
sensor5.fall(&pin21ISR);
sensor6.mode(PullUp);
sensor6.fall(&pin22ISR);
sensor7.mode(PullUp);
sensor7.fall(&pin23ISR);
sensor8.mode(PullUp);
sensor8.fall(&pin24ISR);
/* enable interrupts */
enable_interrupts();
// initialise D to A enables high
d2aEnable1 = 1;
d2aEnable2 = 1;
// configure serial peripheral interface: 16 bits clocked on first rising edge at a frequency of 1MHz
spi.format(16, 0);
spi.frequency(1000000);
// start pulse output to check interrupt inputs
pulseEvery10ms.period_ms(10);
pulseEvery10ms.write(0.1);
/* set bit in status to reflect reset of h8 */
status |= h8Reset;
while (1)
{
char ch = 0;
char buff[10];
int idx = 0;
while (ch != 0x0d && idx < 9)
{
ch = serial.getc();
buff[idx++] = ch;
}
switch (buff[0])
{
case '1':
serial.printf("Measured speed ");
for (int i=0; i<activeControllers - 1; i++)
serial.printf("%5.1f, ", measuredSpeed[i]);
serial.printf("%5.1f\n", measuredSpeed[activeControllers - 1]);
serial.printf("Actual count ");
for (int i=0; i<activeControllers - 1; i++)
serial.printf("%4d, ", actualCount[i]);
serial.printf("%4d\n", actualCount[activeControllers - 1]);
serial.printf("Count setpoint ");
for (int i=0; i<activeControllers - 1; i++)
serial.printf("%4d, ", countSetpoint[i]);
serial.printf("%4d\n", countSetpoint[activeControllers - 1]);
break;
default:
break;
}
// wait a while to allow other things to run
wait_ms(1);
}
}
void pin15ISR()
{
// read and save value of free running timer
currentFrtValue[0] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[0] = currentFrtValue[0] - prevFrtValue[0];
// save the current value to the previous for the next time
prevFrtValue[0] = currentFrtValue[0];
/* set flag for background routine to process */
processFrtReading[0] = TRUE;
/* clear no pulse counter */
noPulseCntr[0] = 0;
/* if in startup mode */
if (inStartupMode[0])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[0]++;
}
}
void pin16ISR()
{
currentFrtValue[1] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[1] = currentFrtValue[1] - prevFrtValue[1];
// save the current value to the previous for the next time
prevFrtValue[1] = currentFrtValue[1];
/* set flag for background routine to process */
processFrtReading[1] = TRUE;
/* clear no pulse counter */
noPulseCntr[1] = 0;
/* if in startup mode */
if (inStartupMode[1])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[1]++;
}
}
void pin17ISR()
{
currentFrtValue[2] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[2] = currentFrtValue[2] - prevFrtValue[2];
// save the current value to the previous for the next time
prevFrtValue[2] = currentFrtValue[2];
/* set flag for background routine to process */
processFrtReading[2] = TRUE;
/* clear no pulse counter */
noPulseCntr[2] = 0;
/* if in startup mode */
if (inStartupMode[2])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[2]++;
}
}
void pin18ISR()
{
currentFrtValue[3] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[3] = currentFrtValue[3] - prevFrtValue[3];
// save the current value to the previous for the next time
prevFrtValue[3] = currentFrtValue[3];
/* set flag for background routine to process */
processFrtReading[3] = TRUE;
/* clear no pulse counter */
noPulseCntr[3] = 0;
/* if in startup mode */
if (inStartupMode[3])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[3]++;
}
}
void pin21ISR()
{
currentFrtValue[4] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[4] = currentFrtValue[4] - prevFrtValue[4];
// save the current value to the previous for the next time
prevFrtValue[4] = currentFrtValue[4];
/* set flag for background routine to process */
processFrtReading[4] = TRUE;
/* clear no pulse counter */
noPulseCntr[4] = 0;
/* if in startup mode */
if (inStartupMode[4])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[4]++;
}
}
void pin22ISR()
{
currentFrtValue[5] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[5] = currentFrtValue[5] - prevFrtValue[5];
// save the current value to the previous for the next time
prevFrtValue[5] = currentFrtValue[5];
/* set flag for background routine to process */
processFrtReading[5] = TRUE;
/* clear no pulse counter */
noPulseCntr[5] = 0;
/* if in startup mode */
if (inStartupMode[5])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[5]++;
}
}
void pin23ISR()
{
currentFrtValue[6] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[6] = currentFrtValue[6] - prevFrtValue[6];
// save the current value to the previous for the next time
prevFrtValue[6] = currentFrtValue[6];
/* set flag for background routine to process */
processFrtReading[6] = TRUE;
/* clear no pulse counter */
noPulseCntr[6] = 0;
/* if in startup mode */
if (inStartupMode[6])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[6]++;
}
}
void pin24ISR()
{
currentFrtValue[7] = LPC_TIM0->TC;
// calculate difference in count from last interrupt pulse
currentCountDiff[7] = currentFrtValue[7] - prevFrtValue[7];
// save the current value to the previous for the next time
prevFrtValue[7] = currentFrtValue[7];
/* set flag for background routine to process */
processFrtReading[7] = TRUE;
/* clear no pulse counter */
noPulseCntr[7] = 0;
/* if in startup mode */
if (inStartupMode[7])
{
/* increment starup counter for each tooth of the wheel */
startupCntr[7]++;
}
}
void acknowledge_host(int idx)
{
/* enable host comms */
enable_host_comms();
/* acknowledge packet */
if (hostCommsStatus == 0)
{
putch0( '>' );
putch0( '0'+ output_limit_status[idx] );
putch0( CR );
}
else
{
putch0( '#' );
putch0( CR );
}
/* set counter to disable host comms xcvr in 2 timer periods */
/* the timer period should be optimised to get this down to msecs */
disableHostComms = 3;
}
float calcActualSpeed(int idx)
{
return (pulsesPerMetrePerSec[idx] / (float)actualCount[idx]);
}
void calcFixedPart(int idx)
{
pulsesPerMetrePerSec[idx] = ((refFreq * rollCircum[idx] * 60 * pulleyRatio[idx]) / teethOnWheel[idx]);
}
void disable_host_comms()
{
/* turn off tx led */
txLed = 0;
/* disable 485 */
rs485Enable = 0;
}
void enable_host_comms()
{
/* enable 485 */
rs485Enable = 1;
/* turn on tx led */
txLed = 1;
}
void enable_interrupts()
{
sensor1.enable_irq();
sensor2.enable_irq();
sensor3.enable_irq();
sensor4.enable_irq();
sensor5.enable_irq();
sensor6.enable_irq();
sensor7.enable_irq();
sensor8.enable_irq();
}
void init_locals()
{
commsBuf0[0] = 0;
commsBuf0[commsBufLen] = 0; /* Terminate for display on LCD */
commsCntr0 = 0;
commsBuf1[0] = 0;
commsBuf1[commsBufLen] = 0; /* Terminate for display on LCD */
commsCntr1 = 0;
boardAddress = 1; //******** read from the config file in nvram
activeControllers = 4; //******** read from the config file in nvram
for (int i=0; i<activeControllers; i++)
{
int idx = i + boardAddress;
controlMode[i] = modeAtReset[idx];
updateAnalogOutput[i] = TRUE;
xmitCascadeSpeed[i] = FALSE;
recalcSpeedSetpoint[i] = FALSE;
recalcFixedPart[i] = TRUE;
recalcReqdCount[i] = TRUE;
pulleyRatio[i] = pulleyRatios[idx];
rollCircum[i] = circumOfRoll[idx];
maxSpeedOfDrive[i] = maxSpeed[idx];
teethOnWheel[i] = teethOnEveryWheel[idx];
ratioToPrev[i] = 1.0;
pulsesPerMetrePerSec[i] = 30.0; /* some daft value */
speedSetpnt[i] = 8.0;
speedOfPrev[i] = 5.0;
measuredSpeed[i] = 1.0; /* some daft value */
intSum[i] = 0.0;
propGain[i] = propGains[idx];
intGain[i] = intGains[idx];
/* initialize but hopefully the next calc will set it to the right value */
dtoaSetpnt[i] = (int)(2047.0 * speedSetpnt[idx] / maxSpeedOfDrive[idx]);
maxDtoa[i] = dtoaSetpnt[i];
minDtoa[i] = dtoaSetpnt[i];
/* do crude bumpless transfer */
intSum[i] = (float)dtoaSetpnt[i] / intGain[i];
prevFrtValue[i] = 0L;
currentFrtValue[i] = 0;
actualCount[i] = 0;
countSetpoint[i] = 3000;
loCountLimit[i] = 2900;
hiCountLimit[i] = 3100;
lastBigCount[i] = MAX_COUNT_VALUE;
lastSmallCount[i] = MIN_COUNT_VALUE;
processFrtReading[i] = FALSE;
withinControlBand[i] = FALSE;
outsideCountBand[i] = 0;
noPulseCntr[i] = 0;
inStartupMode[i] = FALSE;
startupCntr[i] = 0;
output_limit_status[i] = 0;
}
status = 0;
hostCrRcvd = FALSE;
cascadeCrRcvd = FALSE;
updateLcdDue = FALSE;
lcdPage = RESET_PAGE;
overflowCntr = 0;
frtOverflowCntr = 0;
pageChanged = FALSE;
redrawLcd = FALSE;
disableHostComms = 0;
ageInSecs = 0;
extended_lcd_page = 0;
hostCommsStatus = 0;
timer0Cntr = 0;
refFreq = 307200.0;
}
void init_uarts()
{
// set host baud rate to 19200
host.baud(19200);
// attach serial port to handler
host.attach(&serial_rcv_int0);
// set cascade baud rate to 19200
cascade.baud(19200);
// attach serial port to handler
cascade.attach(&serial_rcv_int1);
}
void processCascadeComms()
{
/* Only called when a complete packet has been received */
/* Packet contains speed setpoint of previous drive */
/* Packet structure is listed below */
/* */
/* *1234<chk><chk><ret> where 1234 is speed setpoint * 10 */
int i;
uchar chkSum;
uchar pcktChkSum;
int speed;
/* check packet structure is valid, 8th char is <cr> */
if (commsBuf1[7] == CR)
{
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<5; i++)
{
chkSum ^= commsBuf1[i];
}
/* build packet checksum from both chars */
pcktChkSum = ((hexToBin(commsBuf1[5])) << 4);
pcktChkSum += (hexToBin(commsBuf1[6]));
/* if checksum ok */
if (chkSum == pcktChkSum)
{
/* initialize speed accumulator */
speed = 0;
/* extract speed from packet */
for (i=1; i<5; i++)
speed = (speed * 10) + (commsBuf1[i] - '0');
/* if valid speed */
if (speed >= 0 && speed < 3000)
{
/* save to speed setpoint of prev drive */
speedOfPrev[boardAddress] = (float)speed / 10.0;
/* set flag to recalculate if in ratio mode */
if (controlMode[boardAddress] == ratioMode)
{
recalcSpeedSetpoint[boardAddress] = TRUE;
}
}
}
}
}
void processHostComms()
{
/* Only called when a complete packet has been received */
/* All packets are same length to simplify protocol */
/* Valid packets are as listed below */
/* */
/* *01O1234<chk><chk><ret> sets analog output to 1234 */
/* *01S1234<chk><chk><ret> sets speed setpoint to 123.4 */
/* *01R1234<chk><chk><ret> sets ratio to prev drive to 1.234 */
/* *0131234<chk><chk><ret> sets third nip output to 1234 */
/* *01T1234<chk><chk><ret> sets number of teeth 1234 */
/* *01C1234<chk><chk><ret> sets circumference to 1234 mm */
/* *01P1234<chk><chk><ret> sets pulley ratio to 1.234 */
/* *01M1234<chk><chk><ret> sets max surface speed to 123.4 */
/* *01p1234<chk><chk><ret> sets prop gain to 1.234 */
/* *01i1234<chk><chk><ret> sets integral gain to 1.234 */
/* *01ENQ01<chk><chk><ret> requests current speed */
/* *01ENQ02<chk><chk><ret> requests host comms status */
/* *01ENQ03<chk><chk><ret> requests current status */
/* *01ENQ04<chk><chk><ret> requests max dtoa value */
/* *01ENQ05<chk><chk><ret> requests min dtoa value */
/* */
/* Sending a speed setpoint packet puts the controller into */
/* speed mode and so on for ratio and output packets. */
/* The controller responds with an > CR or # CR packet to */
/* the setpoint type of packets. */
/* The controller responds with a packet structure as below */
/* to the read packets */
/* */
/* *011234<chk><chk><ret> where 123.4 is current speed */
/* *010000<chk><chk><ret> where 0000 is host comms status */
/* *010000<chk><chk><ret> where 0000 is current status */
/* *011456<chk><chk><ret> where 1456 is max dtoa value */
/* *011234<chk><chk><ret> where 1234 is min dtoa value */
int i;
int address;
int idx;
int ival;
int intsp;
uchar chkSum;
uchar pcktChkSum;
char buf[12];
/* check packet structure is valid, 11th char is <cr> */
if (commsBuf0[10] == CR || commsBuf0[10] == 0x0a)
{
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<8; i++)
{
chkSum ^= commsBuf0[i];
}
/* build packet checksum from both chars */
pcktChkSum = ((hexToBin(commsBuf0[8])) << 4);
pcktChkSum += (hexToBin(commsBuf0[9]));
/* if checksum ok */
if (chkSum == pcktChkSum)
{
/* extract address from packet */
address = ((commsBuf0[1] - '0') * 10) + (commsBuf0[2] - '0');
//pc.printf("Address %d\n", address);
/* check if packet was for this controller or board set to reply to all addresses */
if ((address >= boardAddress && address < (boardAddress + activeControllers))|| boardAddress == 0 )
{
// index into array is address of loop - board address
idx = address - boardAddress;
//pc.printf("Idx %d\n", idx);
/* set status to zero */
hostCommsStatus = 0;
/* process packet request */
switch (commsBuf0[3])
{
case 'C':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival > 300 && ival < 2000)
{
/* put ivalerence into metres */
rollCircum[idx] = (float)ival / 1000.0;
/* set flag to force recalc */
recalcFixedPart[idx] = TRUE;
//pc.printf("circumference %5.3f\n", rollCircum[idx]);
}
else
{
hostCommsStatus = badCircumference;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'S':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 0 && ival < 3000)
{
/* scale new speed setpoint */
speedSetpnt[idx] = (float)ival * 0.1;
/* set to speed control mode */
controlMode[idx] = speedMode;
/* set flag to force recalc */
recalcSpeedSetpoint[idx] = TRUE;
//pc.printf("speed %5.3f\n", speedSetpnt[idx]);
}
else
{
hostCommsStatus = badSpeedSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'R':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 100 && ival < 9999)
{
/* scale to get max ratio of 9.999 */
ratioToPrev[idx] = (float)ival * 0.001;
/* set to ratio control mode */
controlMode[idx] = ratioMode;
/* set flag to force recalc */
recalcSpeedSetpoint[idx] = TRUE;
//pc.printf("Ratio to prev %5.3f\n", ratioToPrev[idx]);
}
else
{
hostCommsStatus = badRatioSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'O':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 0 && ival < 2048)
{
/* save for output to d/a */
dtoaSetpnt[idx] = ival;
/* do crude bumpless transfer */
intSum[idx] = (float)dtoaSetpnt[idx] / intGain[idx];
/* set to direct output mode */
controlMode[idx] = outputMode;
/* set flag to write to d/a chip */
updateAnalogOutput[idx] = TRUE;
}
else
{
hostCommsStatus = badOutputSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case '3':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 0 && ival < 2048)
{
/* save for output to d/a */
dtoaSetpnt[idx] = ival;
/* set to third nip mode */
controlMode[idx] = thirdNipMode;
/* set flag to write to d/a chip */
updateAnalogOutput[idx] = TRUE;
//pc.printf("3rd nip %d\n", dtoaSetpnt[idx]);
}
else
{
hostCommsStatus = badOutputSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'T':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 10 && ival < 180)
{
/* save into global variable teeth */
teethOnWheel[idx] = ival;
/* set flag to force recalc */
recalcFixedPart[idx] = TRUE;
}
else
{
hostCommsStatus = badNumOfTeeth;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'P':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 100 && ival < 9999)
{
/* scale to get max ratio of 9.999 */
pulleyRatio[idx] = (float)ival * 0.001;
/* set flag to force recalc */
recalcFixedPart[idx] = TRUE;
}
else
{
hostCommsStatus = badRatioSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'p':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 1 && ival < 9999)
{
/* scale to get max prop gain of 9.999 */
propGain[idx] = (float)ival * 0.001;
}
else
{
hostCommsStatus = badPropGainSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'i':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 1 && ival < 9999)
{
/* scale to get max integral gain of 9.999 */
intGain[idx] = (float)ival * 0.001;
}
else
{
hostCommsStatus = badIntGainSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'M':
ival = 0;
for (i=4; i<8; i++)
{
ival = ival * 10 + commsBuf0[i] - '0';
}
/* check value is sensible */
if (ival >= 100 && ival < 3000)
{
/* scale new max speed value */
maxSpeedOfDrive[idx] = (float)ival * 0.1;
}
else
{
hostCommsStatus = badMaxSpeedSetpoint;
}
/* reply to host with ack or nak */
acknowledge_host(idx);
break;
case 'E':
/* determine type of enquiry */
switch (commsBuf0[7])
{
/* if enquiry 1 return current speed */
case '1':
/* round up/down properly */
intsp = (int)(measuredSpeed[idx] * 10.0);
if (((measuredSpeed[idx] * 10.0) - intsp) >= 0.5 )
{
intsp = intsp + 1;
}
sprintf( buf, "*%02d%04d", address,
(int)(intsp));
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<7; i++)
{
chkSum ^= buf[i];
}
/* enable host comms */
enable_host_comms();
/* return packet to host */
putstr0( buf );
/* generate checksum */
sprintf( buf, "%02X%c", (int)chkSum, CR );
/* return packet to host */
putstr0( buf );
// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
disableHostComms = 6;
break;
/* if enquiry 2 return current host comms status */
case '2':
sprintf( buf, "*%02d%04d", address,
hostCommsStatus );
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<7; i++)
{
chkSum ^= buf[i];
}
/* enable host comms */
enable_host_comms();
/* return packet to host */
putstr0( buf );
sprintf( buf, "%02X%c", chkSum, CR );
/* return packet to host */
putstr0( buf );
// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
disableHostComms = 6;
break;
/* if enquiry 3 return current status */
case '3':
sprintf( buf, "*%02d%04d", address, status );
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<7; i++)
{
chkSum ^= buf[i];
}
/* enable host comms */
enable_host_comms();
/* return packet to host */
putstr0( buf );
sprintf( buf, "%02X%c", chkSum, CR );
/* return packet to host */
putstr0( buf );
// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
disableHostComms = 6;
/* reset status */
status = 0;
break;
/* if enquiry 4 return max dtoa value */
case '4':
sprintf( buf, "*%02d%04d", address, maxDtoa[idx] );
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<7; i++)
{
chkSum ^= buf[i];
}
/* enable host comms */
enable_host_comms();
/* return packet to host */
putstr0( buf );
// add checksum
sprintf( buf, "%02X%c", chkSum, CR );
/* return packet to host */
putstr0( buf );
// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
disableHostComms = 6;
break;
/* if enquiry 5 return current min dtoa value */
case '5':
sprintf( buf, "*%02d%04d", address, minDtoa[idx] );
/* set checksum accumulator to zero */
chkSum = 0;
/* checksum packet */
for (i=0; i<7; i++)
{
chkSum ^= buf[i];
}
/* enable host comms */
enable_host_comms();
/* return packet to host */
putstr0( buf );
// add checksum */
sprintf( buf, "%02X%c", chkSum, CR );
/* return packet to host */
putstr0( buf );
//******* consider implementing a timer to disable the 485 driver as using the counter is not deterministic
// the serial handler in mbed buffers the characters so the above code takes uSecs but the string can take 5mSecs to send
// and 2 tics of the disableHostComms may be as little as 1.6666mS up to 3.3333mS depending on the timing between the packet being sent and the timer
// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
disableHostComms = 6;
/* reset values */
maxDtoa[idx] = dtoaSetpnt[idx];
minDtoa[idx] = dtoaSetpnt[idx];
break;
}
break;
default:
hostCommsStatus = unknownPacket;
}
}
}
}
}
void process_latest_count(int idx)
{
unsigned long count;
// get local copy
count = currentCountDiff[idx];
/* limit count to maximum value */
if (count > MAX_COUNT_VALUE)
{
count = MAX_COUNT_VALUE;
}
else
{
/* limit count to minimum value */
if (count < MIN_COUNT_VALUE)
{
count = MIN_COUNT_VALUE;
}
}
/* if count > previous biggest count */
if (count > lastBigCount[idx])
{
/* adjust lastBigCount by one eigth of count value */
lastBigCount[idx] = lastBigCount[idx] + (count / 8);
}
else
{
/* if count < previous smallest count */
if (count < lastSmallCount[idx])
{
/* adjust lastSmallCount by one eigth of count value */
lastSmallCount[idx] = lastSmallCount[idx] - (count / 8);
}
else
{
/* bring limits back in around current count */
lastBigCount[idx] = count + (count / 8);
lastSmallCount[idx]= count - (count / 8);
/* process count for mode controller is in */
switch (controlMode[idx])
{
case speedMode:
case ratioMode:
/* filter the current count into the actual count value */
actualCount[idx] = ((actualCount[idx] / 8) * 7) + (count / 8);
/* check if within band */
if (actualCount[idx] > loCountLimit[idx] && actualCount[idx] < hiCountLimit[idx])
{
/* if greater than zero */
if (outsideCountBand[idx] > 0)
{
/* decrement counter */
outsideCountBand[idx]--;
if (outsideCountBand[idx] == 0)
{
/* set flag for within band */
withinControlBand[idx] = TRUE;
}
}
}
else
{
/* check if not reached limit */
if (outsideCountBand[idx] < OUTSIDE_COUNT_BAND_LIMIT)
{
/* increment out of band counter */
outsideCountBand[idx]++;
/* check if reached limit */
if (outsideCountBand[idx] >= OUTSIDE_COUNT_BAND_LIMIT)
{
/* set flag for not within band */
withinControlBand[idx] = FALSE;
}
}
}
/* if not in startup mode */
if (!inStartupMode[idx])
{
/* get count error (override to long to get sign back)*/
errorCount[idx] = (long)(actualCount[idx] - countSetpoint[idx]);
/* perform P, I and D */
intSum[idx] += errorCount[idx];
dtoaSetpnt[idx] = (int)((propGain[idx] * errorCount[idx]) + (intGain[idx] * intSum[idx]));
/* check within output limits */
if (dtoaSetpnt[idx] < 0)
{
dtoaSetpnt[idx] = 0;
output_limit_status[idx] = OUTPUT_LOW;
/* do crude bumpless transfer */
intSum[idx] = 0.0;
}
else
{
if (dtoaSetpnt[idx] > 2047)
{
dtoaSetpnt[idx] = 2047;
output_limit_status[idx] = OUTPUT_HIGH;
/* do crude bumpless transfer */
intSum[idx] = (float)dtoaSetpnt[idx] / intGain[idx];
}
else
{
output_limit_status[idx] = OUTPUT_WITHIN;
}
}
/* check new dtoa value against monitored limits */
if (dtoaSetpnt[idx] > maxDtoa[idx])
{
/* set new maximum */
maxDtoa[idx] = dtoaSetpnt[idx];
}
else
{
if (dtoaSetpnt[idx] < minDtoa[idx])
{
/* set new minimum */
minDtoa[idx] = dtoaSetpnt[idx];
}
}
/* set flag to force update */
updateAnalogOutput[idx] = TRUE;
/* calculate speed for logging */
measuredSpeed[idx] = calcActualSpeed(idx);
}
break;
case thirdNipMode:
/* filter count into actual count */
actualCount[idx] = ((actualCount[idx] / 8) * 7) + (count / 8);
/* calculate speed */
measuredSpeed[idx] = calcActualSpeed(idx);
/* filter into speed setpoint to be cascaded to next drive */
speedSetpnt[idx] = (speedSetpnt[idx] * 0.875) + (measuredSpeed[idx] * 0.125);
/* make speedSetpoint the same as measured speed */
/* speedSetpnt = measuredSpeed; */
/* force speed to be cascaded */
xmitCascadeSpeed[idx] = TRUE;
break;
default: ;
}
}
}
}
void putch0(uchar c)
{
host.putc(c);
}
void putch1(uchar c)
{
cascade.putc(c);
}
void putstr0(char* str )
{
while (*str)
{
putch0( *str++ );
}
}
void putstr1( char* str )
{
while (*str)
{
putch1( *str++ );
}
}
void serial_rcv_int0()
{
uchar c, loop;
/* if not processing a packet */
if (hostCrRcvd == FALSE)
{
/* get char into local */
c = host.getc();
/* if first char received */
if (commsCntr0 == 0)
{
/* set receive led on */
rxLed = 1;
}
/* if space in buffer */
if (commsCntr0 < (commsBufLen - 1))
{
/* put char in buffer and increment pointer */
commsBuf0[commsCntr0++] = c;
}
/* if char is end of packet */
if (c == CR || c == 0x0a)
{
// clear to the end of the packet
for (loop = commsCntr0; loop<commsBufLen; loop++)
{
commsBuf0[loop] = 0;
}
/* set flag to process packet */
hostCrRcvd = TRUE;
/* turn receive led off */
rxLed = 0;
}
}
else
{
/* if the end of the packet */
if (c == CR)
{
/* reset char pointer */
commsCntr0 = 0;
/* turn receive led off */
rxLed = 0;
}
}
/* reset receive data register full flag */
//SSR0 &= ~RDRF;
}
void serial_rcv_int1()
{
uchar c, loop;
/* if not processing a packet */
if (cascadeCrRcvd == FALSE)
{
/* get char into local */
c = cascade.getc();
/* if space in buffer */
if (commsCntr1 < (commsBufLen - 1))
{
/* put char in buffer and increment pointer */
commsBuf1[commsCntr1++] = c;
}
/* if char is end of packet */
if (c == CR)
{
for (loop = commsCntr1; loop<commsBufLen; loop++)
{
commsBuf1[loop] = 0;
}
/* reset char pointer */
commsCntr1 = 0;
/* set flag to process packet */
cascadeCrRcvd = TRUE;
}
}
else
{
/* if the end of the packet */
if (c == CR)
{
/* reset char pointer */
commsCntr1 = 0;
}
}
}
void repetitiveControlUpdate()
{
//scope1 = 1;
for (int i=0; i<activeControllers; i++)
{
/* if time to transmit cascade speed */
if (xmitCascadeSpeed[i])
{
// if this is the last controller on this card
if (i == (activeControllers - 1))
/* send current speed setpoint by comms to next card */
xmit_cascade_speed(i);
else
// save the value to internal registers to be used for all drives controlled by this card except the last one
speedOfPrev[i + 1] = speedSetpnt[i];
/* clear flag */
xmitCascadeSpeed[i] = FALSE;
}
/* recalculate fixed part of speed calculation */
if (recalcFixedPart[i])
{
/* calculate fixed part of speed calc */
calcFixedPart(i);
/* clear flag */
recalcFixedPart[i] = FALSE;
}
/* recalculate speed setpoint */
if (recalcSpeedSetpoint[i])
{
/* only recalculate if in ratio mode */
if (controlMode[i] == ratioMode)
{
/* calculate new speed setpoint */
speedSetpnt[i] = speedOfPrev[i] * ratioToPrev[i];
}
/* set flag to send new speed setpoint on to next drive */
xmitCascadeSpeed[i] = TRUE;
/* force recalc of required count */
recalcReqdCount[i] = TRUE;
/* clear flag */
recalcSpeedSetpoint[i] = FALSE;
}
/* if speed or ratio changed */
if (recalcReqdCount[i])
{
/* calculate count using current speed setpoint */
/* check for zero speed setpoint */
if (speedSetpnt[i] > 0.0)
{
/* calculate required count */
countSetpoint[i] = (unsigned long)(pulsesPerMetrePerSec[i] / speedSetpnt[i]);
}
/* calculate 12% control bands */
hiCountLimit[i] = countSetpoint[i] + (countSetpoint[i] / 8);
loCountLimit[i] = countSetpoint[i] - (countSetpoint[i] / 8);
/* clear flag */
recalcReqdCount[i] = FALSE;
}
/* if new reading from free running timer */
if (processFrtReading[i])
{
/* if in speed control or ratio mode */
switch (controlMode[i])
{
case speedMode:
case ratioMode:
case thirdNipMode:
/* calculate new output */
process_latest_count(i);
break;
case outputMode:
break;
default: ;
}
/* clear flag */
processFrtReading[i] = FALSE;
}
/* if no pulses from toothed wheel in last 3 seconds */
if (noPulseCntr[i] > 6)
{
/* if in speed control or ratio mode */
switch (controlMode[i])
{
case speedMode:
case ratioMode:
/* set default output */
dtoaSetpnt[i] = (int)(2047.0 * speedSetpnt[i] / maxSpeedOfDrive[i]);
if ( dtoaSetpnt[i] > 2047 )
dtoaSetpnt[i] = 2047;
/* do crude bumpless transfer */
intSum[i] = (float)dtoaSetpnt[i] / intGain[i];
/* set flag to update analog output */
updateAnalogOutput[i] = TRUE;
break;
case thirdNipMode:
case outputMode:
break;
default: ;
}
/* set flag to indicate starting from cold */
inStartupMode[i] = TRUE;
/* clear within band flag */
withinControlBand[i] = FALSE;
/* zero startup counter */
startupCntr[i] = 0;
/* clear flag */
noPulseCntr[i] = 2;
}
/* if new analog output value flag set */
if (updateAnalogOutput[i])
{
/* write binary value to d/a chip */
updateDtoaChip(i);
/* clear flag */
updateAnalogOutput[i] = FALSE;
}
}
/* process host comms */
if (hostCrRcvd)
{
/* process packet received from host */
processHostComms();
/* reset flag */
hostCrRcvd = FALSE;
/* reset input pointer */
commsCntr0 = 0;
}
/* process cascade comms */
if (cascadeCrRcvd)
{
/* process the packet received through cascade port */
processCascadeComms();
/* clear flag */
cascadeCrRcvd = FALSE;
}
for (int i=0; i<activeControllers; i++)
{
/* if in startup mode and startup counter reached limit (pulses from wheel) */
if (inStartupMode[i] && startupCntr[i] > startupCount[boardAddress + i])
{
/* drive should have started by now so clear flag */
inStartupMode[i] = FALSE;
}
}
//scope1 = 0;
}
void repetitiveUpdateTimer()
{
/* called every ms */
/* increment counter */
timer0Cntr++;
/* if host comms xmit driver enabled and waiting for timeout */
if (disableHostComms > 0)
{
/* decrement timer */
disableHostComms--;
/* if timer is zero */
if (disableHostComms == 0)
{
/* disable the driver */
disable_host_comms();
}
}
/* check for 8th pulse (8 * 1.666 ms = every 13 msec) */
if ((timer0Cntr & 0x0F) == 0x08)
{
for (int i=0; i<activeControllers; i++)
{
/* set flag to force update of analog output */
updateAnalogOutput[i] = TRUE;
}
}
/* check for 64th pulse (64 * 1.666 ms = every 0.106 sec) */
if ((timer0Cntr & 0x7F) == 0x40)
{
for (int i=0; i<activeControllers; i++)
/* set flag to resend speed setpoint */
xmitCascadeSpeed[i] = 1;
}
/* check for 256th pulse (256 * 1.666 ms = every 0.426 sec) */
if ((timer0Cntr & 0x1FF) == 0x100)
{
for (int i=0; i<activeControllers; i++)
{
/* increment no pulse counter */
noPulseCntr[i]++;
}
}
/* check for 512th pulse (512 * 1.666 ms = every 0.852 sec) */
if ((timer0Cntr & 0x3FF) == 0x200)
{
/* increment age since reset */
ageInSecs++;
}
}
void updateDtoaChip(int idx)
{
int d2aCmd;
// build command with the following format: aaccddddddddddss
// where aa = output address
// cc = 3 - update all outputs when value loaded into buffer register
// dddddddddd - 10 bit output value
// ss = 0 - not used but must be zero
d2aCmd = ((idx & 3) << 14) | (3 << 12) | ((dtoaSetpnt[idx] & 0x7fe) << 1);
//select correct d/a chip
if (idx > 3)
d2aEnable2 = 0;
else
d2aEnable1 = 0;
// write the command to the d/a chip over the serial peripheral interface
spi.write(d2aCmd);
//deselect correct d/a chip
if (idx > 3)
d2aEnable2 = 1;
else
d2aEnable1 = 1;
}
void xmit_cascade_speed(int idx)
{
char str[20];
char chksum;
char *pstr;
int intsp;
intsp = (int)(speedSetpnt[idx] * 10.0);
if (((speedSetpnt[idx] * 10.0) - intsp) >= 0.5 )
{
intsp = intsp + 1;
}
/* build up packet of current speed setpoint */
sprintf( str, "*%04d", (int)(intsp) );
/* init pointer to string */
pstr = str;
/* clear checksum accumulator */
chksum = 0;
/* checksum string */
while (*pstr)
{
chksum ^= *pstr++;
}
/* send the current speed setpoint */
putstr1( str );
/* build checksum part of string */
sprintf( str, "%02X\x0d", chksum );
/* send the checksum */
putstr1( str );
}