LPC1768DSC - Digital speed controller

Users » BPPearson » Code » LPC1768DSC

Brian Pearson / Mbed 2 deprecated LPC1768DSC

Digital speed controller

main.cpp@0:14f74b704963, 2016-01-05 (annotated)

Committer:: BPPearson
Date:: Tue Jan 05 16:43:35 2016 +0000
Revision:: 0:14f74b704963

Digital speed controller

Who changed what in which revision?

User	Revision	Line number	New contents of line
BPPearson	0:14f74b704963	1	#include "mbed.h"
BPPearson	0:14f74b704963	2	#include "USBSerial.h"
BPPearson	0:14f74b704963	3
BPPearson	0:14f74b704963	4	DigitalOut myled(LED1);
BPPearson	0:14f74b704963	5
BPPearson	0:14f74b704963	6
BPPearson	0:14f74b704963	7	#include <float.h>
BPPearson	0:14f74b704963	8	#include <math.h>
BPPearson	0:14f74b704963	9	#include <stdio.h>
BPPearson	0:14f74b704963	10	#include <string.h>
BPPearson	0:14f74b704963	11
BPPearson	0:14f74b704963	12
BPPearson	0:14f74b704963	13	#define revString "V1.00"
BPPearson	0:14f74b704963	14
BPPearson	0:14f74b704963	15
BPPearson	0:14f74b704963	16	//#define BAUD_RATE 19200 /* Serial baud rate */
BPPearson	0:14f74b704963	17	#define DEBOUNCE_TIME 10 /* 10 * timer0Cntr1 cycle times */
BPPearson	0:14f74b704963	18	#define OUTSIDE_COUNT_BAND_LIMIT 40 /* uchar counter limit */
BPPearson	0:14f74b704963	19	#define MAX_COUNT_VALUE 150000 /* maximum difference between counts */
BPPearson	0:14f74b704963	20	#define MIN_COUNT_VALUE 1000 /* minimum difference between counts */
BPPearson	0:14f74b704963	21	#define OUTPUT_CALIBRATION_FACTOR 10.0 / 2048.0 /* 0..2047 binary to 0..10 V */
BPPearson	0:14f74b704963	22
BPPearson	0:14f74b704963	23	/* type definitions */
BPPearson	0:14f74b704963	24	#define uchar unsigned char
BPPearson	0:14f74b704963	25	#define uint unsigned int
BPPearson	0:14f74b704963	26
BPPearson	0:14f74b704963	27	/* boolean definitions */
BPPearson	0:14f74b704963	28	#define FALSE 0
BPPearson	0:14f74b704963	29	#define TRUE 1
BPPearson	0:14f74b704963	30
BPPearson	0:14f74b704963	31
BPPearson	0:14f74b704963	32	/* ascii definitions */
BPPearson	0:14f74b704963	33	#define CR '\x0D'
BPPearson	0:14f74b704963	34
BPPearson	0:14f74b704963	35
BPPearson	0:14f74b704963	36	// peripherals and function blocks used on LPC1768
BPPearson	0:14f74b704963	37	DigitalOut txLed(LED1);
BPPearson	0:14f74b704963	38	DigitalOut rxLed(LED2);
BPPearson	0:14f74b704963	39	DigitalOut led3(LED3);
BPPearson	0:14f74b704963	40	DigitalOut led4(LED4);
BPPearson	0:14f74b704963	41	DigitalOut rs485Enable(p8);
BPPearson	0:14f74b704963	42	DigitalOut d2aEnable1(p11);
BPPearson	0:14f74b704963	43	DigitalOut d2aEnable2(p12);
BPPearson	0:14f74b704963	44
BPPearson	0:14f74b704963	45	DigitalOut scope1(p19);
BPPearson	0:14f74b704963	46	DigitalOut scope2(p20);
BPPearson	0:14f74b704963	47
BPPearson	0:14f74b704963	48	InterruptIn sensor1(p15);
BPPearson	0:14f74b704963	49	InterruptIn sensor2(p16);
BPPearson	0:14f74b704963	50	InterruptIn sensor3(p17);
BPPearson	0:14f74b704963	51	InterruptIn sensor4(p18);
BPPearson	0:14f74b704963	52	InterruptIn sensor5(p21);
BPPearson	0:14f74b704963	53	InterruptIn sensor6(p22);
BPPearson	0:14f74b704963	54	InterruptIn sensor7(p23);
BPPearson	0:14f74b704963	55	InterruptIn sensor8(p24);
BPPearson	0:14f74b704963	56
BPPearson	0:14f74b704963	57	PwmOut pulseEvery10ms(p25);
BPPearson	0:14f74b704963	58
BPPearson	0:14f74b704963	59	Serial host(p9, p10);
BPPearson	0:14f74b704963	60	Serial cascade(p13, p14);
BPPearson	0:14f74b704963	61	Serial pc(USBTX, USBRX); // tx, rx
BPPearson	0:14f74b704963	62
BPPearson	0:14f74b704963	63	SPI spi(p5,p6,p7);
BPPearson	0:14f74b704963	64
BPPearson	0:14f74b704963	65	Ticker oneMSec;
BPPearson	0:14f74b704963	66	Ticker fiveMSec;
BPPearson	0:14f74b704963	67
BPPearson	0:14f74b704963	68	USBSerial serial;
BPPearson	0:14f74b704963	69
BPPearson	0:14f74b704963	70	/* port 3 bit definitions */
BPPearson	0:14f74b704963	71	#define DTOA_DATA 0x20
BPPearson	0:14f74b704963	72	#define DTOA_CLOCK 0x40
BPPearson	0:14f74b704963	73	#define DTOA_SYNC 0x80
BPPearson	0:14f74b704963	74
BPPearson	0:14f74b704963	75	/* port 6 bit definitions */
BPPearson	0:14f74b704963	76	#define WATCHDOG 0x40
BPPearson	0:14f74b704963	77
BPPearson	0:14f74b704963	78	/* lcd page definitions */
BPPearson	0:14f74b704963	79	#define RESET_PAGE 0
BPPearson	0:14f74b704963	80	#define SPEED_PAGE 1
BPPearson	0:14f74b704963	81	#define COUNT_PAGE 2
BPPearson	0:14f74b704963	82	#define DEBUG1_PAGE 3
BPPearson	0:14f74b704963	83	#define COMMS_PAGE 4
BPPearson	0:14f74b704963	84	#define LAST_PAGE 4
BPPearson	0:14f74b704963	85
BPPearson	0:14f74b704963	86	/* status bits */
BPPearson	0:14f74b704963	87	#define h8Reset 0x01
BPPearson	0:14f74b704963	88
BPPearson	0:14f74b704963	89	/* host comms status definitions */
BPPearson	0:14f74b704963	90	#define badCircumference 1
BPPearson	0:14f74b704963	91	#define badSpeedSetpoint 2
BPPearson	0:14f74b704963	92	#define badRatioSetpoint 3
BPPearson	0:14f74b704963	93	#define badOutputSetpoint 4
BPPearson	0:14f74b704963	94	#define badIntGainSetpoint 5
BPPearson	0:14f74b704963	95	#define badPropGainSetpoint 6
BPPearson	0:14f74b704963	96	#define badNumOfTeeth 7
BPPearson	0:14f74b704963	97	#define badMaxSpeedSetpoint 8
BPPearson	0:14f74b704963	98	#define unknownPacket 9
BPPearson	0:14f74b704963	99
BPPearson	0:14f74b704963	100	/* control modes */
BPPearson	0:14f74b704963	101	#define outputMode 0
BPPearson	0:14f74b704963	102	#define ratioMode 1
BPPearson	0:14f74b704963	103	#define speedMode 2
BPPearson	0:14f74b704963	104	#define thirdNipMode 3
BPPearson	0:14f74b704963	105
BPPearson	0:14f74b704963	106	const char *modestrings[4] = {"Out:","Rat:","Spd:","3rd:"};
BPPearson	0:14f74b704963	107
BPPearson	0:14f74b704963	108	#define OUTPUT_WITHIN 0
BPPearson	0:14f74b704963	109	#define OUTPUT_LOW 1
BPPearson	0:14f74b704963	110	#define OUTPUT_HIGH 2
BPPearson	0:14f74b704963	111
BPPearson	0:14f74b704963	112	#define commsBufLen 12 /* Anything over 12 will need display routine changed */
BPPearson	0:14f74b704963	113
BPPearson	0:14f74b704963	114	#define hexToBin(x) (x > '9') ? x - '7' : x - '0'
BPPearson	0:14f74b704963	115
BPPearson	0:14f74b704963	116	#define LAST_EXT_PAGE 28
BPPearson	0:14f74b704963	117
BPPearson	0:14f74b704963	118
BPPearson	0:14f74b704963	119	const float teethOnEveryWheel[21] = { 0.0, 30.0, 30.0,
BPPearson	0:14f74b704963	120	0.0, 0.0, 120.0, 60.0, 30.0,
BPPearson	0:14f74b704963	121	20.0, 60.0, 60.0, 60.0,
BPPearson	0:14f74b704963	122	60.0, 60.0, 35.0, 35.0,
BPPearson	0:14f74b704963	123	35.0, 35.0, 35.0, 20.0,
BPPearson	0:14f74b704963	124	20.0 };
BPPearson	0:14f74b704963	125
BPPearson	0:14f74b704963	126	const float pulleyRatios[21] = { 1.0, 1.0, 1.0,
BPPearson	0:14f74b704963	127	1.0, 1.0,
BPPearson	0:14f74b704963	128	1.0, 1.0, 1.0,
BPPearson	0:14f74b704963	129	1.0, 1.0, 1.0, 1.0,
BPPearson	0:14f74b704963	130	1.0, 1.0, 1.0, 1.0,
BPPearson	0:14f74b704963	131	1.0, 1.0, 1.0, 1.0,
BPPearson	0:14f74b704963	132	1.0 };
BPPearson	0:14f74b704963	133
BPPearson	0:14f74b704963	134	const float circumOfRoll[21] = { 0.0, 1.0, 1.0,
BPPearson	0:14f74b704963	135	0.0, 0.0,
BPPearson	0:14f74b704963	136	0.75084, 1.2252, 1.2252,
BPPearson	0:14f74b704963	137	0.82090, 2.00000, 2.00000, 2.00000,
BPPearson	0:14f74b704963	138	2.00000, 2.00000, 1.09955, 1.09955,
BPPearson	0:14f74b704963	139	1.03044, 1.03044, 1.09955, 0.68420,
BPPearson	0:14f74b704963	140	0.82090 };
BPPearson	0:14f74b704963	141
BPPearson	0:14f74b704963	142	/* Rope threader circ changed from 0.33500 to 0.42949 */
BPPearson	0:14f74b704963	143	/* Then changed to 0.68420 as it was doing 137 but saying 86 */
BPPearson	0:14f74b704963	144
BPPearson	0:14f74b704963	145	/* because it needed a 0.78 ratio to run at the right speed */
BPPearson	0:14f74b704963	146
BPPearson	0:14f74b704963	147
BPPearson	0:14f74b704963	148	const float maxSpeed[21] = { 1.0, 76.5, 76.5,
BPPearson	0:14f74b704963	149	74.0, 74.0,
BPPearson	0:14f74b704963	150	35.0, 220.0, 220.0,
BPPearson	0:14f74b704963	151	220.0, 220.0, 220.0, 220.0,
BPPearson	0:14f74b704963	152	220.0, 220.0, 220.0, 220.0,
BPPearson	0:14f74b704963	153	220.0, 220.0, 220.0, 250.0,
BPPearson	0:14f74b704963	154	220.0 };
BPPearson	0:14f74b704963	155
BPPearson	0:14f74b704963	156	const float propGains[21] = { 0.01, 0.015, 0.015,
BPPearson	0:14f74b704963	157	0.015, 0.015,
BPPearson	0:14f74b704963	158	0.015, 0.005, 0.015,
BPPearson	0:14f74b704963	159	0.01, 0.01, 0.01, 0.01,
BPPearson	0:14f74b704963	160	0.01, 0.01, 0.01, 0.01,
BPPearson	0:14f74b704963	161	0.01, 0.01, 0.01, 0.005,
BPPearson	0:14f74b704963	162	0.01};
BPPearson	0:14f74b704963	163
BPPearson	0:14f74b704963	164	const float intGains[21] = { 0.01, 0.006, 0.006,
BPPearson	0:14f74b704963	165	0.006, 0.006,
BPPearson	0:14f74b704963	166	0.006, 0.0010, 0.006,
BPPearson	0:14f74b704963	167	0.003, 0.003, 0.003, 0.003,
BPPearson	0:14f74b704963	168	0.003, 0.003, 0.003, 0.003,
BPPearson	0:14f74b704963	169	0.003, 0.003, 0.003, 0.001,
BPPearson	0:14f74b704963	170	0.003 };
BPPearson	0:14f74b704963	171
BPPearson	0:14f74b704963	172	const char *driveName[21] = { "All Drives ","Melt Pump A ","Melt Pump B ",
BPPearson	0:14f74b704963	173	"Inner Extruder","Outer Extruder",
BPPearson	0:14f74b704963	174	"First Nip ","Second Nip ","Third Nip ",
BPPearson	0:14f74b704963	175	"4th Nip East ","1st Bole East ","2nd Bole East ","3rd Bole East ",
BPPearson	0:14f74b704963	176	"4th Bole East ","5th Bole East ","Cooler 1 East ","Cooler 2 East ",
BPPearson	0:14f74b704963	177	"Detreat 1 East","Detreat 2 East","Cooler 3 East ","E Rope Thread ",
BPPearson	0:14f74b704963	178	"4th Nip West " };
BPPearson	0:14f74b704963	179
BPPearson	0:14f74b704963	180	const uchar modeAtReset[21] = { outputMode, speedMode, speedMode,
BPPearson	0:14f74b704963	181	outputMode, outputMode, speedMode, ratioMode, thirdNipMode,
BPPearson	0:14f74b704963	182	ratioMode, ratioMode, ratioMode, ratioMode,
BPPearson	0:14f74b704963	183	ratioMode, ratioMode, ratioMode, ratioMode,
BPPearson	0:14f74b704963	184	ratioMode, ratioMode, ratioMode, ratioMode,
BPPearson	0:14f74b704963	185	ratioMode };
BPPearson	0:14f74b704963	186
BPPearson	0:14f74b704963	187	const int startupCount[21] = { 0, 4, 4,
BPPearson	0:14f74b704963	188	0, 0, 20, 30, 30,
BPPearson	0:14f74b704963	189	50, 100, 100, 100,
BPPearson	0:14f74b704963	190	100, 100, 100, 100,
BPPearson	0:14f74b704963	191	100, 100, 100, 100,
BPPearson	0:14f74b704963	192	50 };
BPPearson	0:14f74b704963	193
BPPearson	0:14f74b704963	194	const char *debugtitles[28] = { "Invalid ",
BPPearson	0:14f74b704963	195	"Mode ",
BPPearson	0:14f74b704963	196	"Address ",
BPPearson	0:14f74b704963	197	"Age ",
BPPearson	0:14f74b704963	198	"In Band ",
BPPearson	0:14f74b704963	199	"In Startup",
BPPearson	0:14f74b704963	200	"Comms Stat",
BPPearson	0:14f74b704963	201	"DtoA Stpt ",
BPPearson	0:14f74b704963	202	"actual Cnt",
BPPearson	0:14f74b704963	203	"cnt Stpt ",
BPPearson	0:14f74b704963	204	"loCnt Lmt ",
BPPearson	0:14f74b704963	205	"hiCnt Lmt ",
BPPearson	0:14f74b704963	206	"last BCnt ",
BPPearson	0:14f74b704963	207	"last SCnt ",
BPPearson	0:14f74b704963	208	"Pull Ratio",
BPPearson	0:14f74b704963	209	"Circumf ",
BPPearson	0:14f74b704963	210	"Max Dv Spd",
BPPearson	0:14f74b704963	211	"Teeth ",
BPPearson	0:14f74b704963	212	"Start Cnt ",
BPPearson	0:14f74b704963	213	"Prev Ratio",
BPPearson	0:14f74b704963	214	"P.P.M.P.S.",
BPPearson	0:14f74b704963	215	"Spd Stpt ",
BPPearson	0:14f74b704963	216	"Spd Prev ",
BPPearson	0:14f74b704963	217	"Act Spd ",
BPPearson	0:14f74b704963	218	"Cnt error ",
BPPearson	0:14f74b704963	219	"Int Sum ",
BPPearson	0:14f74b704963	220	"propGain ",
BPPearson	0:14f74b704963	221	"intGain "};
BPPearson	0:14f74b704963	222
BPPearson	0:14f74b704963	223	/* variables */
BPPearson	0:14f74b704963	224	uchar commsBuf0[commsBufLen+1];
BPPearson	0:14f74b704963	225	uchar commsCntr0;
BPPearson	0:14f74b704963	226	uchar commsBuf1[commsBufLen+1];
BPPearson	0:14f74b704963	227	uchar commsCntr1;
BPPearson	0:14f74b704963	228	uchar boardAddress;
BPPearson	0:14f74b704963	229	uchar activeControllers;
BPPearson	0:14f74b704963	230	uchar hostCrRcvd;
BPPearson	0:14f74b704963	231	uchar cascadeCrRcvd;
BPPearson	0:14f74b704963	232	uchar updateLcdDue;
BPPearson	0:14f74b704963	233	uchar updateAnalogOutput[8];
BPPearson	0:14f74b704963	234	uchar lcdPage;
BPPearson	0:14f74b704963	235	uchar overflowCntr;
BPPearson	0:14f74b704963	236	uchar frtOverflowCntr;
BPPearson	0:14f74b704963	237	uchar pageChanged;
BPPearson	0:14f74b704963	238	uchar redrawLcd;
BPPearson	0:14f74b704963	239	uchar disableHostComms;
BPPearson	0:14f74b704963	240	uchar extended_lcd_page;
BPPearson	0:14f74b704963	241
BPPearson	0:14f74b704963	242	uchar xmitCascadeSpeed[8];
BPPearson	0:14f74b704963	243	uchar recalcSpeedSetpoint[8];
BPPearson	0:14f74b704963	244	uchar recalcFixedPart[8];
BPPearson	0:14f74b704963	245	uchar recalcReqdCount[8];
BPPearson	0:14f74b704963	246	uchar processFrtReading[8];
BPPearson	0:14f74b704963	247	uchar withinControlBand[8]; /* flag */
BPPearson	0:14f74b704963	248	uchar outsideCountBand[8]; /* counter */
BPPearson	0:14f74b704963	249	uchar noPulseCntr[8];
BPPearson	0:14f74b704963	250	uchar inStartupMode[8];
BPPearson	0:14f74b704963	251	uchar startupCntr[8];
BPPearson	0:14f74b704963	252	uchar controlMode[8];
BPPearson	0:14f74b704963	253	uchar status;
BPPearson	0:14f74b704963	254	uchar output_limit_status[8];
BPPearson	0:14f74b704963	255
BPPearson	0:14f74b704963	256
BPPearson	0:14f74b704963	257	int hostCommsStatus;
BPPearson	0:14f74b704963	258	int timer0Cntr;
BPPearson	0:14f74b704963	259	int dtoaSetpnt[8];
BPPearson	0:14f74b704963	260	int maxDtoa[8];
BPPearson	0:14f74b704963	261	int minDtoa[8];
BPPearson	0:14f74b704963	262
BPPearson	0:14f74b704963	263
BPPearson	0:14f74b704963	264	unsigned long prevFrtValue[8];
BPPearson	0:14f74b704963	265	unsigned long currentFrtValue[8];
BPPearson	0:14f74b704963	266	unsigned long currentCountDiff[8];
BPPearson	0:14f74b704963	267
BPPearson	0:14f74b704963	268	unsigned long actualCount[8];
BPPearson	0:14f74b704963	269	unsigned long countSetpoint[8];
BPPearson	0:14f74b704963	270	unsigned long loCountLimit[8];
BPPearson	0:14f74b704963	271	unsigned long hiCountLimit[8];
BPPearson	0:14f74b704963	272	unsigned long lastBigCount[8];
BPPearson	0:14f74b704963	273	unsigned long lastSmallCount[8];
BPPearson	0:14f74b704963	274	unsigned long ageInSecs;
BPPearson	0:14f74b704963	275
BPPearson	0:14f74b704963	276
BPPearson	0:14f74b704963	277	float pulleyRatio[8];
BPPearson	0:14f74b704963	278	float refFreq;
BPPearson	0:14f74b704963	279	float rollCircum[8];
BPPearson	0:14f74b704963	280	float maxSpeedOfDrive[8];
BPPearson	0:14f74b704963	281	float teethOnWheel[8];
BPPearson	0:14f74b704963	282	float ratioToPrev[8];
BPPearson	0:14f74b704963	283	float pulsesPerMetrePerSec[8];
BPPearson	0:14f74b704963	284	float speedSetpnt[8];
BPPearson	0:14f74b704963	285	float speedOfPrev[8];
BPPearson	0:14f74b704963	286	float measuredSpeed[8];
BPPearson	0:14f74b704963	287
BPPearson	0:14f74b704963	288	float errorCount[8];
BPPearson	0:14f74b704963	289	float intSum[8];
BPPearson	0:14f74b704963	290	float propGain[8];
BPPearson	0:14f74b704963	291	float intGain[8];
BPPearson	0:14f74b704963	292
BPPearson	0:14f74b704963	293
BPPearson	0:14f74b704963	294	/* Procedure declarations */
BPPearson	0:14f74b704963	295	void pin15ISR();
BPPearson	0:14f74b704963	296	void pin16ISR();
BPPearson	0:14f74b704963	297	void pin17ISR();
BPPearson	0:14f74b704963	298	void pin18ISR();
BPPearson	0:14f74b704963	299	void pin21ISR();
BPPearson	0:14f74b704963	300	void pin22ISR();
BPPearson	0:14f74b704963	301	void pin23ISR();
BPPearson	0:14f74b704963	302	void pin24ISR();
BPPearson	0:14f74b704963	303	void acknowledge_host(int idx);
BPPearson	0:14f74b704963	304	float calcActualSpeed();
BPPearson	0:14f74b704963	305	void calcFixedPart(int idx);
BPPearson	0:14f74b704963	306	void disable_host_comms();
BPPearson	0:14f74b704963	307	void enable_host_comms();
BPPearson	0:14f74b704963	308	void enable_interrupts();
BPPearson	0:14f74b704963	309	void init_free_running_counter();
BPPearson	0:14f74b704963	310	void init_locals();
BPPearson	0:14f74b704963	311	void init_timer0();
BPPearson	0:14f74b704963	312	void init_uarts();
BPPearson	0:14f74b704963	313	void lcd_put( uchar dat );
BPPearson	0:14f74b704963	314	void lcd_str( char *str );
BPPearson	0:14f74b704963	315	void processCascadeComms();
BPPearson	0:14f74b704963	316	void processHostComms();
BPPearson	0:14f74b704963	317	void process_latest_count(int idx);
BPPearson	0:14f74b704963	318	void putch0(uchar c);
BPPearson	0:14f74b704963	319	void putch1(uchar c);
BPPearson	0:14f74b704963	320	void putstr0(char *str);
BPPearson	0:14f74b704963	321	void putstr1(char *str);
BPPearson	0:14f74b704963	322	void serial_rcv_err_int0();
BPPearson	0:14f74b704963	323	void serial_rcv_err_int1();
BPPearson	0:14f74b704963	324	void serial_rcv_int0();
BPPearson	0:14f74b704963	325	void serial_rcv_int1();
BPPearson	0:14f74b704963	326	void repetitiveControlUpdate();
BPPearson	0:14f74b704963	327	void repetitiveUpdateTimer();
BPPearson	0:14f74b704963	328	void updateDtoaChip(int idx);
BPPearson	0:14f74b704963	329	void xmit_cascade_speed(int idx);
BPPearson	0:14f74b704963	330
BPPearson	0:14f74b704963	331
BPPearson	0:14f74b704963	332
BPPearson	0:14f74b704963	333	int main()
BPPearson	0:14f74b704963	334
BPPearson	0:14f74b704963	335	{
BPPearson	0:14f74b704963	336
BPPearson	0:14f74b704963	337	/* initialize serial ports and vectors */
BPPearson	0:14f74b704963	338	init_uarts(); /* initialize the uarts */
BPPearson	0:14f74b704963	339
BPPearson	0:14f74b704963	340	/* initialise locals */
BPPearson	0:14f74b704963	341	init_locals();
BPPearson	0:14f74b704963	342
BPPearson	0:14f74b704963	343	/* initialize periodic timers */
BPPearson	0:14f74b704963	344	oneMSec.attach(&repetitiveUpdateTimer, 0.001);
BPPearson	0:14f74b704963	345	fiveMSec.attach(&repetitiveControlUpdate, 0.005);
BPPearson	0:14f74b704963	346
BPPearson	0:14f74b704963	347	/* enable free running counter */
BPPearson	0:14f74b704963	348	LPC_SC->PCONP \|= 1 < 1; //timer0 power on
BPPearson	0:14f74b704963	349	LPC_TIM0->TCR = 1; //enable Timer2
BPPearson	0:14f74b704963	350
BPPearson	0:14f74b704963	351	// configure interrupt inputs
BPPearson	0:14f74b704963	352	sensor1.mode(PullUp);
BPPearson	0:14f74b704963	353	sensor1.fall(&pin15ISR);
BPPearson	0:14f74b704963	354	sensor2.mode(PullUp);
BPPearson	0:14f74b704963	355	sensor2.fall(&pin16ISR);
BPPearson	0:14f74b704963	356	sensor3.mode(PullUp);
BPPearson	0:14f74b704963	357	sensor3.fall(&pin17ISR);
BPPearson	0:14f74b704963	358	sensor4.mode(PullUp);
BPPearson	0:14f74b704963	359	sensor4.fall(&pin18ISR);
BPPearson	0:14f74b704963	360	sensor5.mode(PullUp);
BPPearson	0:14f74b704963	361	sensor5.fall(&pin21ISR);
BPPearson	0:14f74b704963	362	sensor6.mode(PullUp);
BPPearson	0:14f74b704963	363	sensor6.fall(&pin22ISR);
BPPearson	0:14f74b704963	364	sensor7.mode(PullUp);
BPPearson	0:14f74b704963	365	sensor7.fall(&pin23ISR);
BPPearson	0:14f74b704963	366	sensor8.mode(PullUp);
BPPearson	0:14f74b704963	367	sensor8.fall(&pin24ISR);
BPPearson	0:14f74b704963	368
BPPearson	0:14f74b704963	369	/* enable interrupts */
BPPearson	0:14f74b704963	370	enable_interrupts();
BPPearson	0:14f74b704963	371
BPPearson	0:14f74b704963	372	// initialise D to A enables high
BPPearson	0:14f74b704963	373	d2aEnable1 = 1;
BPPearson	0:14f74b704963	374	d2aEnable2 = 1;
BPPearson	0:14f74b704963	375
BPPearson	0:14f74b704963	376	// configure serial peripheral interface: 16 bits clocked on first rising edge at a frequency of 1MHz
BPPearson	0:14f74b704963	377	spi.format(16, 0);
BPPearson	0:14f74b704963	378	spi.frequency(1000000);
BPPearson	0:14f74b704963	379
BPPearson	0:14f74b704963	380	// start pulse output to check interrupt inputs
BPPearson	0:14f74b704963	381	pulseEvery10ms.period_ms(10);
BPPearson	0:14f74b704963	382	pulseEvery10ms.write(0.1);
BPPearson	0:14f74b704963	383
BPPearson	0:14f74b704963	384	/* set bit in status to reflect reset of h8 */
BPPearson	0:14f74b704963	385	status \|= h8Reset;
BPPearson	0:14f74b704963	386
BPPearson	0:14f74b704963	387	while (1)
BPPearson	0:14f74b704963	388	{
BPPearson	0:14f74b704963	389	char ch = 0;
BPPearson	0:14f74b704963	390	char buff[10];
BPPearson	0:14f74b704963	391	int idx = 0;
BPPearson	0:14f74b704963	392
BPPearson	0:14f74b704963	393	while (ch != 0x0d && idx < 9)
BPPearson	0:14f74b704963	394	{
BPPearson	0:14f74b704963	395	ch = serial.getc();
BPPearson	0:14f74b704963	396
BPPearson	0:14f74b704963	397	buff[idx++] = ch;
BPPearson	0:14f74b704963	398	}
BPPearson	0:14f74b704963	399
BPPearson	0:14f74b704963	400	switch (buff[0])
BPPearson	0:14f74b704963	401	{
BPPearson	0:14f74b704963	402	case '1':
BPPearson	0:14f74b704963	403	serial.printf("Measured speed ");
BPPearson	0:14f74b704963	404	for (int i=0; i<activeControllers - 1; i++)
BPPearson	0:14f74b704963	405	serial.printf("%5.1f, ", measuredSpeed[i]);
BPPearson	0:14f74b704963	406
BPPearson	0:14f74b704963	407	serial.printf("%5.1f\n", measuredSpeed[activeControllers - 1]);
BPPearson	0:14f74b704963	408
BPPearson	0:14f74b704963	409	serial.printf("Actual count ");
BPPearson	0:14f74b704963	410	for (int i=0; i<activeControllers - 1; i++)
BPPearson	0:14f74b704963	411	serial.printf("%4d, ", actualCount[i]);
BPPearson	0:14f74b704963	412
BPPearson	0:14f74b704963	413	serial.printf("%4d\n", actualCount[activeControllers - 1]);
BPPearson	0:14f74b704963	414
BPPearson	0:14f74b704963	415	serial.printf("Count setpoint ");
BPPearson	0:14f74b704963	416	for (int i=0; i<activeControllers - 1; i++)
BPPearson	0:14f74b704963	417	serial.printf("%4d, ", countSetpoint[i]);
BPPearson	0:14f74b704963	418
BPPearson	0:14f74b704963	419	serial.printf("%4d\n", countSetpoint[activeControllers - 1]);
BPPearson	0:14f74b704963	420	break;
BPPearson	0:14f74b704963	421	default:
BPPearson	0:14f74b704963	422	break;
BPPearson	0:14f74b704963	423	}
BPPearson	0:14f74b704963	424
BPPearson	0:14f74b704963	425	// wait a while to allow other things to run
BPPearson	0:14f74b704963	426	wait_ms(1);
BPPearson	0:14f74b704963	427	}
BPPearson	0:14f74b704963	428	}
BPPearson	0:14f74b704963	429
BPPearson	0:14f74b704963	430
BPPearson	0:14f74b704963	431
BPPearson	0:14f74b704963	432	void pin15ISR()
BPPearson	0:14f74b704963	433	{
BPPearson	0:14f74b704963	434
BPPearson	0:14f74b704963	435	// read and save value of free running timer
BPPearson	0:14f74b704963	436	currentFrtValue[0] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	437
BPPearson	0:14f74b704963	438	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	439	currentCountDiff[0] = currentFrtValue[0] - prevFrtValue[0];
BPPearson	0:14f74b704963	440
BPPearson	0:14f74b704963	441	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	442	prevFrtValue[0] = currentFrtValue[0];
BPPearson	0:14f74b704963	443
BPPearson	0:14f74b704963	444	/* set flag for background routine to process */
BPPearson	0:14f74b704963	445	processFrtReading[0] = TRUE;
BPPearson	0:14f74b704963	446
BPPearson	0:14f74b704963	447	/* clear no pulse counter */
BPPearson	0:14f74b704963	448	noPulseCntr[0] = 0;
BPPearson	0:14f74b704963	449
BPPearson	0:14f74b704963	450	/* if in startup mode */
BPPearson	0:14f74b704963	451	if (inStartupMode[0])
BPPearson	0:14f74b704963	452	{
BPPearson	0:14f74b704963	453	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	454	startupCntr[0]++;
BPPearson	0:14f74b704963	455	}
BPPearson	0:14f74b704963	456	}
BPPearson	0:14f74b704963	457
BPPearson	0:14f74b704963	458
BPPearson	0:14f74b704963	459
BPPearson	0:14f74b704963	460	void pin16ISR()
BPPearson	0:14f74b704963	461	{
BPPearson	0:14f74b704963	462
BPPearson	0:14f74b704963	463	currentFrtValue[1] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	464
BPPearson	0:14f74b704963	465	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	466	currentCountDiff[1] = currentFrtValue[1] - prevFrtValue[1];
BPPearson	0:14f74b704963	467
BPPearson	0:14f74b704963	468	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	469	prevFrtValue[1] = currentFrtValue[1];
BPPearson	0:14f74b704963	470
BPPearson	0:14f74b704963	471	/* set flag for background routine to process */
BPPearson	0:14f74b704963	472	processFrtReading[1] = TRUE;
BPPearson	0:14f74b704963	473
BPPearson	0:14f74b704963	474	/* clear no pulse counter */
BPPearson	0:14f74b704963	475	noPulseCntr[1] = 0;
BPPearson	0:14f74b704963	476
BPPearson	0:14f74b704963	477	/* if in startup mode */
BPPearson	0:14f74b704963	478	if (inStartupMode[1])
BPPearson	0:14f74b704963	479	{
BPPearson	0:14f74b704963	480	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	481	startupCntr[1]++;
BPPearson	0:14f74b704963	482	}
BPPearson	0:14f74b704963	483	}
BPPearson	0:14f74b704963	484
BPPearson	0:14f74b704963	485
BPPearson	0:14f74b704963	486
BPPearson	0:14f74b704963	487	void pin17ISR()
BPPearson	0:14f74b704963	488	{
BPPearson	0:14f74b704963	489
BPPearson	0:14f74b704963	490	currentFrtValue[2] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	491
BPPearson	0:14f74b704963	492	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	493	currentCountDiff[2] = currentFrtValue[2] - prevFrtValue[2];
BPPearson	0:14f74b704963	494
BPPearson	0:14f74b704963	495	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	496	prevFrtValue[2] = currentFrtValue[2];
BPPearson	0:14f74b704963	497
BPPearson	0:14f74b704963	498	/* set flag for background routine to process */
BPPearson	0:14f74b704963	499	processFrtReading[2] = TRUE;
BPPearson	0:14f74b704963	500
BPPearson	0:14f74b704963	501	/* clear no pulse counter */
BPPearson	0:14f74b704963	502	noPulseCntr[2] = 0;
BPPearson	0:14f74b704963	503
BPPearson	0:14f74b704963	504	/* if in startup mode */
BPPearson	0:14f74b704963	505	if (inStartupMode[2])
BPPearson	0:14f74b704963	506	{
BPPearson	0:14f74b704963	507	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	508	startupCntr[2]++;
BPPearson	0:14f74b704963	509	}
BPPearson	0:14f74b704963	510	}
BPPearson	0:14f74b704963	511
BPPearson	0:14f74b704963	512
BPPearson	0:14f74b704963	513
BPPearson	0:14f74b704963	514	void pin18ISR()
BPPearson	0:14f74b704963	515	{
BPPearson	0:14f74b704963	516
BPPearson	0:14f74b704963	517	currentFrtValue[3] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	518
BPPearson	0:14f74b704963	519	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	520	currentCountDiff[3] = currentFrtValue[3] - prevFrtValue[3];
BPPearson	0:14f74b704963	521
BPPearson	0:14f74b704963	522	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	523	prevFrtValue[3] = currentFrtValue[3];
BPPearson	0:14f74b704963	524
BPPearson	0:14f74b704963	525	/* set flag for background routine to process */
BPPearson	0:14f74b704963	526	processFrtReading[3] = TRUE;
BPPearson	0:14f74b704963	527
BPPearson	0:14f74b704963	528	/* clear no pulse counter */
BPPearson	0:14f74b704963	529	noPulseCntr[3] = 0;
BPPearson	0:14f74b704963	530
BPPearson	0:14f74b704963	531	/* if in startup mode */
BPPearson	0:14f74b704963	532	if (inStartupMode[3])
BPPearson	0:14f74b704963	533	{
BPPearson	0:14f74b704963	534	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	535	startupCntr[3]++;
BPPearson	0:14f74b704963	536	}
BPPearson	0:14f74b704963	537	}
BPPearson	0:14f74b704963	538
BPPearson	0:14f74b704963	539
BPPearson	0:14f74b704963	540
BPPearson	0:14f74b704963	541	void pin21ISR()
BPPearson	0:14f74b704963	542	{
BPPearson	0:14f74b704963	543
BPPearson	0:14f74b704963	544	currentFrtValue[4] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	545
BPPearson	0:14f74b704963	546	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	547	currentCountDiff[4] = currentFrtValue[4] - prevFrtValue[4];
BPPearson	0:14f74b704963	548
BPPearson	0:14f74b704963	549	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	550	prevFrtValue[4] = currentFrtValue[4];
BPPearson	0:14f74b704963	551
BPPearson	0:14f74b704963	552	/* set flag for background routine to process */
BPPearson	0:14f74b704963	553	processFrtReading[4] = TRUE;
BPPearson	0:14f74b704963	554
BPPearson	0:14f74b704963	555	/* clear no pulse counter */
BPPearson	0:14f74b704963	556	noPulseCntr[4] = 0;
BPPearson	0:14f74b704963	557
BPPearson	0:14f74b704963	558	/* if in startup mode */
BPPearson	0:14f74b704963	559	if (inStartupMode[4])
BPPearson	0:14f74b704963	560	{
BPPearson	0:14f74b704963	561	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	562	startupCntr[4]++;
BPPearson	0:14f74b704963	563	}
BPPearson	0:14f74b704963	564	}
BPPearson	0:14f74b704963	565
BPPearson	0:14f74b704963	566
BPPearson	0:14f74b704963	567
BPPearson	0:14f74b704963	568	void pin22ISR()
BPPearson	0:14f74b704963	569	{
BPPearson	0:14f74b704963	570
BPPearson	0:14f74b704963	571	currentFrtValue[5] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	572
BPPearson	0:14f74b704963	573	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	574	currentCountDiff[5] = currentFrtValue[5] - prevFrtValue[5];
BPPearson	0:14f74b704963	575
BPPearson	0:14f74b704963	576	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	577	prevFrtValue[5] = currentFrtValue[5];
BPPearson	0:14f74b704963	578
BPPearson	0:14f74b704963	579	/* set flag for background routine to process */
BPPearson	0:14f74b704963	580	processFrtReading[5] = TRUE;
BPPearson	0:14f74b704963	581
BPPearson	0:14f74b704963	582	/* clear no pulse counter */
BPPearson	0:14f74b704963	583	noPulseCntr[5] = 0;
BPPearson	0:14f74b704963	584
BPPearson	0:14f74b704963	585	/* if in startup mode */
BPPearson	0:14f74b704963	586	if (inStartupMode[5])
BPPearson	0:14f74b704963	587	{
BPPearson	0:14f74b704963	588	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	589	startupCntr[5]++;
BPPearson	0:14f74b704963	590	}
BPPearson	0:14f74b704963	591	}
BPPearson	0:14f74b704963	592
BPPearson	0:14f74b704963	593
BPPearson	0:14f74b704963	594
BPPearson	0:14f74b704963	595	void pin23ISR()
BPPearson	0:14f74b704963	596	{
BPPearson	0:14f74b704963	597
BPPearson	0:14f74b704963	598	currentFrtValue[6] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	599
BPPearson	0:14f74b704963	600	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	601	currentCountDiff[6] = currentFrtValue[6] - prevFrtValue[6];
BPPearson	0:14f74b704963	602
BPPearson	0:14f74b704963	603	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	604	prevFrtValue[6] = currentFrtValue[6];
BPPearson	0:14f74b704963	605
BPPearson	0:14f74b704963	606	/* set flag for background routine to process */
BPPearson	0:14f74b704963	607	processFrtReading[6] = TRUE;
BPPearson	0:14f74b704963	608
BPPearson	0:14f74b704963	609	/* clear no pulse counter */
BPPearson	0:14f74b704963	610	noPulseCntr[6] = 0;
BPPearson	0:14f74b704963	611
BPPearson	0:14f74b704963	612	/* if in startup mode */
BPPearson	0:14f74b704963	613	if (inStartupMode[6])
BPPearson	0:14f74b704963	614	{
BPPearson	0:14f74b704963	615	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	616	startupCntr[6]++;
BPPearson	0:14f74b704963	617	}
BPPearson	0:14f74b704963	618	}
BPPearson	0:14f74b704963	619
BPPearson	0:14f74b704963	620
BPPearson	0:14f74b704963	621
BPPearson	0:14f74b704963	622	void pin24ISR()
BPPearson	0:14f74b704963	623	{
BPPearson	0:14f74b704963	624
BPPearson	0:14f74b704963	625	currentFrtValue[7] = LPC_TIM0->TC;
BPPearson	0:14f74b704963	626
BPPearson	0:14f74b704963	627	// calculate difference in count from last interrupt pulse
BPPearson	0:14f74b704963	628	currentCountDiff[7] = currentFrtValue[7] - prevFrtValue[7];
BPPearson	0:14f74b704963	629
BPPearson	0:14f74b704963	630	// save the current value to the previous for the next time
BPPearson	0:14f74b704963	631	prevFrtValue[7] = currentFrtValue[7];
BPPearson	0:14f74b704963	632
BPPearson	0:14f74b704963	633	/* set flag for background routine to process */
BPPearson	0:14f74b704963	634	processFrtReading[7] = TRUE;
BPPearson	0:14f74b704963	635
BPPearson	0:14f74b704963	636	/* clear no pulse counter */
BPPearson	0:14f74b704963	637	noPulseCntr[7] = 0;
BPPearson	0:14f74b704963	638
BPPearson	0:14f74b704963	639	/* if in startup mode */
BPPearson	0:14f74b704963	640	if (inStartupMode[7])
BPPearson	0:14f74b704963	641	{
BPPearson	0:14f74b704963	642	/* increment starup counter for each tooth of the wheel */
BPPearson	0:14f74b704963	643	startupCntr[7]++;
BPPearson	0:14f74b704963	644	}
BPPearson	0:14f74b704963	645	}
BPPearson	0:14f74b704963	646
BPPearson	0:14f74b704963	647
BPPearson	0:14f74b704963	648
BPPearson	0:14f74b704963	649	void acknowledge_host(int idx)
BPPearson	0:14f74b704963	650	{
BPPearson	0:14f74b704963	651
BPPearson	0:14f74b704963	652	/* enable host comms */
BPPearson	0:14f74b704963	653	enable_host_comms();
BPPearson	0:14f74b704963	654
BPPearson	0:14f74b704963	655	/* acknowledge packet */
BPPearson	0:14f74b704963	656	if (hostCommsStatus == 0)
BPPearson	0:14f74b704963	657	{
BPPearson	0:14f74b704963	658	putch0( '>' );
BPPearson	0:14f74b704963	659	putch0( '0'+ output_limit_status[idx] );
BPPearson	0:14f74b704963	660	putch0( CR );
BPPearson	0:14f74b704963	661	}
BPPearson	0:14f74b704963	662	else
BPPearson	0:14f74b704963	663	{
BPPearson	0:14f74b704963	664	putch0( '#' );
BPPearson	0:14f74b704963	665	putch0( CR );
BPPearson	0:14f74b704963	666	}
BPPearson	0:14f74b704963	667
BPPearson	0:14f74b704963	668	/* set counter to disable host comms xcvr in 2 timer periods */
BPPearson	0:14f74b704963	669	/* the timer period should be optimised to get this down to msecs */
BPPearson	0:14f74b704963	670	disableHostComms = 3;
BPPearson	0:14f74b704963	671	}
BPPearson	0:14f74b704963	672
BPPearson	0:14f74b704963	673
BPPearson	0:14f74b704963	674
BPPearson	0:14f74b704963	675	float calcActualSpeed(int idx)
BPPearson	0:14f74b704963	676	{
BPPearson	0:14f74b704963	677
BPPearson	0:14f74b704963	678	return (pulsesPerMetrePerSec[idx] / (float)actualCount[idx]);
BPPearson	0:14f74b704963	679	}
BPPearson	0:14f74b704963	680
BPPearson	0:14f74b704963	681
BPPearson	0:14f74b704963	682
BPPearson	0:14f74b704963	683	void calcFixedPart(int idx)
BPPearson	0:14f74b704963	684	{
BPPearson	0:14f74b704963	685
BPPearson	0:14f74b704963	686	pulsesPerMetrePerSec[idx] = ((refFreq * rollCircum[idx] * 60 * pulleyRatio[idx]) / teethOnWheel[idx]);
BPPearson	0:14f74b704963	687	}
BPPearson	0:14f74b704963	688
BPPearson	0:14f74b704963	689
BPPearson	0:14f74b704963	690
BPPearson	0:14f74b704963	691	void disable_host_comms()
BPPearson	0:14f74b704963	692	{
BPPearson	0:14f74b704963	693
BPPearson	0:14f74b704963	694	/* turn off tx led */
BPPearson	0:14f74b704963	695	txLed = 0;
BPPearson	0:14f74b704963	696
BPPearson	0:14f74b704963	697	/* disable 485 */
BPPearson	0:14f74b704963	698	rs485Enable = 0;
BPPearson	0:14f74b704963	699	}
BPPearson	0:14f74b704963	700
BPPearson	0:14f74b704963	701
BPPearson	0:14f74b704963	702
BPPearson	0:14f74b704963	703	void enable_host_comms()
BPPearson	0:14f74b704963	704	{
BPPearson	0:14f74b704963	705
BPPearson	0:14f74b704963	706	/* enable 485 */
BPPearson	0:14f74b704963	707	rs485Enable = 1;
BPPearson	0:14f74b704963	708
BPPearson	0:14f74b704963	709	/* turn on tx led */
BPPearson	0:14f74b704963	710	txLed = 1;
BPPearson	0:14f74b704963	711	}
BPPearson	0:14f74b704963	712
BPPearson	0:14f74b704963	713
BPPearson	0:14f74b704963	714
BPPearson	0:14f74b704963	715	void enable_interrupts()
BPPearson	0:14f74b704963	716	{
BPPearson	0:14f74b704963	717
BPPearson	0:14f74b704963	718	sensor1.enable_irq();
BPPearson	0:14f74b704963	719
BPPearson	0:14f74b704963	720	sensor2.enable_irq();
BPPearson	0:14f74b704963	721
BPPearson	0:14f74b704963	722	sensor3.enable_irq();
BPPearson	0:14f74b704963	723
BPPearson	0:14f74b704963	724	sensor4.enable_irq();
BPPearson	0:14f74b704963	725
BPPearson	0:14f74b704963	726	sensor5.enable_irq();
BPPearson	0:14f74b704963	727
BPPearson	0:14f74b704963	728	sensor6.enable_irq();
BPPearson	0:14f74b704963	729
BPPearson	0:14f74b704963	730	sensor7.enable_irq();
BPPearson	0:14f74b704963	731
BPPearson	0:14f74b704963	732	sensor8.enable_irq();
BPPearson	0:14f74b704963	733	}
BPPearson	0:14f74b704963	734
BPPearson	0:14f74b704963	735
BPPearson	0:14f74b704963	736
BPPearson	0:14f74b704963	737	void init_locals()
BPPearson	0:14f74b704963	738	{
BPPearson	0:14f74b704963	739
BPPearson	0:14f74b704963	740	commsBuf0[0] = 0;
BPPearson	0:14f74b704963	741	commsBuf0[commsBufLen] = 0; /* Terminate for display on LCD */
BPPearson	0:14f74b704963	742	commsCntr0 = 0;
BPPearson	0:14f74b704963	743	commsBuf1[0] = 0;
BPPearson	0:14f74b704963	744	commsBuf1[commsBufLen] = 0; /* Terminate for display on LCD */
BPPearson	0:14f74b704963	745	commsCntr1 = 0;
BPPearson	0:14f74b704963	746	boardAddress = 1; //******** read from the config file in nvram
BPPearson	0:14f74b704963	747	activeControllers = 4; //******** read from the config file in nvram
BPPearson	0:14f74b704963	748
BPPearson	0:14f74b704963	749	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	750	{
BPPearson	0:14f74b704963	751	int idx = i + boardAddress;
BPPearson	0:14f74b704963	752	controlMode[i] = modeAtReset[idx];
BPPearson	0:14f74b704963	753	updateAnalogOutput[i] = TRUE;
BPPearson	0:14f74b704963	754	xmitCascadeSpeed[i] = FALSE;
BPPearson	0:14f74b704963	755	recalcSpeedSetpoint[i] = FALSE;
BPPearson	0:14f74b704963	756	recalcFixedPart[i] = TRUE;
BPPearson	0:14f74b704963	757	recalcReqdCount[i] = TRUE;
BPPearson	0:14f74b704963	758
BPPearson	0:14f74b704963	759	pulleyRatio[i] = pulleyRatios[idx];
BPPearson	0:14f74b704963	760	rollCircum[i] = circumOfRoll[idx];
BPPearson	0:14f74b704963	761	maxSpeedOfDrive[i] = maxSpeed[idx];
BPPearson	0:14f74b704963	762	teethOnWheel[i] = teethOnEveryWheel[idx];
BPPearson	0:14f74b704963	763	ratioToPrev[i] = 1.0;
BPPearson	0:14f74b704963	764	pulsesPerMetrePerSec[i] = 30.0; /* some daft value */
BPPearson	0:14f74b704963	765	speedSetpnt[i] = 8.0;
BPPearson	0:14f74b704963	766	speedOfPrev[i] = 5.0;
BPPearson	0:14f74b704963	767	measuredSpeed[i] = 1.0; /* some daft value */
BPPearson	0:14f74b704963	768	intSum[i] = 0.0;
BPPearson	0:14f74b704963	769	propGain[i] = propGains[idx];
BPPearson	0:14f74b704963	770	intGain[i] = intGains[idx];
BPPearson	0:14f74b704963	771
BPPearson	0:14f74b704963	772	/* initialize but hopefully the next calc will set it to the right value */
BPPearson	0:14f74b704963	773	dtoaSetpnt[i] = (int)(2047.0 * speedSetpnt[idx] / maxSpeedOfDrive[idx]);
BPPearson	0:14f74b704963	774	maxDtoa[i] = dtoaSetpnt[i];
BPPearson	0:14f74b704963	775	minDtoa[i] = dtoaSetpnt[i];
BPPearson	0:14f74b704963	776
BPPearson	0:14f74b704963	777	/* do crude bumpless transfer */
BPPearson	0:14f74b704963	778	intSum[i] = (float)dtoaSetpnt[i] / intGain[i];
BPPearson	0:14f74b704963	779
BPPearson	0:14f74b704963	780	prevFrtValue[i] = 0L;
BPPearson	0:14f74b704963	781	currentFrtValue[i] = 0;
BPPearson	0:14f74b704963	782	actualCount[i] = 0;
BPPearson	0:14f74b704963	783	countSetpoint[i] = 3000;
BPPearson	0:14f74b704963	784	loCountLimit[i] = 2900;
BPPearson	0:14f74b704963	785	hiCountLimit[i] = 3100;
BPPearson	0:14f74b704963	786	lastBigCount[i] = MAX_COUNT_VALUE;
BPPearson	0:14f74b704963	787	lastSmallCount[i] = MIN_COUNT_VALUE;
BPPearson	0:14f74b704963	788	processFrtReading[i] = FALSE;
BPPearson	0:14f74b704963	789	withinControlBand[i] = FALSE;
BPPearson	0:14f74b704963	790	outsideCountBand[i] = 0;
BPPearson	0:14f74b704963	791	noPulseCntr[i] = 0;
BPPearson	0:14f74b704963	792	inStartupMode[i] = FALSE;
BPPearson	0:14f74b704963	793	startupCntr[i] = 0;
BPPearson	0:14f74b704963	794	output_limit_status[i] = 0;
BPPearson	0:14f74b704963	795	}
BPPearson	0:14f74b704963	796
BPPearson	0:14f74b704963	797	status = 0;
BPPearson	0:14f74b704963	798	hostCrRcvd = FALSE;
BPPearson	0:14f74b704963	799	cascadeCrRcvd = FALSE;
BPPearson	0:14f74b704963	800	updateLcdDue = FALSE;
BPPearson	0:14f74b704963	801	lcdPage = RESET_PAGE;
BPPearson	0:14f74b704963	802	overflowCntr = 0;
BPPearson	0:14f74b704963	803	frtOverflowCntr = 0;
BPPearson	0:14f74b704963	804	pageChanged = FALSE;
BPPearson	0:14f74b704963	805	redrawLcd = FALSE;
BPPearson	0:14f74b704963	806	disableHostComms = 0;
BPPearson	0:14f74b704963	807	ageInSecs = 0;
BPPearson	0:14f74b704963	808
BPPearson	0:14f74b704963	809	extended_lcd_page = 0;
BPPearson	0:14f74b704963	810
BPPearson	0:14f74b704963	811	hostCommsStatus = 0;
BPPearson	0:14f74b704963	812	timer0Cntr = 0;
BPPearson	0:14f74b704963	813
BPPearson	0:14f74b704963	814	refFreq = 307200.0;
BPPearson	0:14f74b704963	815
BPPearson	0:14f74b704963	816	}
BPPearson	0:14f74b704963	817
BPPearson	0:14f74b704963	818
BPPearson	0:14f74b704963	819
BPPearson	0:14f74b704963	820	void init_uarts()
BPPearson	0:14f74b704963	821	{
BPPearson	0:14f74b704963	822
BPPearson	0:14f74b704963	823	// set host baud rate to 19200
BPPearson	0:14f74b704963	824	host.baud(19200);
BPPearson	0:14f74b704963	825
BPPearson	0:14f74b704963	826	// attach serial port to handler
BPPearson	0:14f74b704963	827	host.attach(&serial_rcv_int0);
BPPearson	0:14f74b704963	828
BPPearson	0:14f74b704963	829	// set cascade baud rate to 19200
BPPearson	0:14f74b704963	830	cascade.baud(19200);
BPPearson	0:14f74b704963	831
BPPearson	0:14f74b704963	832	// attach serial port to handler
BPPearson	0:14f74b704963	833	cascade.attach(&serial_rcv_int1);
BPPearson	0:14f74b704963	834	}
BPPearson	0:14f74b704963	835
BPPearson	0:14f74b704963	836
BPPearson	0:14f74b704963	837
BPPearson	0:14f74b704963	838	void processCascadeComms()
BPPearson	0:14f74b704963	839	{
BPPearson	0:14f74b704963	840	/* Only called when a complete packet has been received */
BPPearson	0:14f74b704963	841	/* Packet contains speed setpoint of previous drive */
BPPearson	0:14f74b704963	842	/* Packet structure is listed below */
BPPearson	0:14f74b704963	843	/* */
BPPearson	0:14f74b704963	844	/* 1234<chk><chk><ret> where 1234 is speed setpoint 10 */
BPPearson	0:14f74b704963	845
BPPearson	0:14f74b704963	846	int i;
BPPearson	0:14f74b704963	847	uchar chkSum;
BPPearson	0:14f74b704963	848	uchar pcktChkSum;
BPPearson	0:14f74b704963	849	int speed;
BPPearson	0:14f74b704963	850
BPPearson	0:14f74b704963	851	/* check packet structure is valid, 8th char is <cr> */
BPPearson	0:14f74b704963	852	if (commsBuf1[7] == CR)
BPPearson	0:14f74b704963	853	{
BPPearson	0:14f74b704963	854	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	855	chkSum = 0;
BPPearson	0:14f74b704963	856
BPPearson	0:14f74b704963	857	/* checksum packet */
BPPearson	0:14f74b704963	858	for (i=0; i<5; i++)
BPPearson	0:14f74b704963	859	{
BPPearson	0:14f74b704963	860	chkSum ^= commsBuf1[i];
BPPearson	0:14f74b704963	861	}
BPPearson	0:14f74b704963	862
BPPearson	0:14f74b704963	863	/* build packet checksum from both chars */
BPPearson	0:14f74b704963	864	pcktChkSum = ((hexToBin(commsBuf1[5])) << 4);
BPPearson	0:14f74b704963	865	pcktChkSum += (hexToBin(commsBuf1[6]));
BPPearson	0:14f74b704963	866
BPPearson	0:14f74b704963	867	/* if checksum ok */
BPPearson	0:14f74b704963	868	if (chkSum == pcktChkSum)
BPPearson	0:14f74b704963	869	{
BPPearson	0:14f74b704963	870	/* initialize speed accumulator */
BPPearson	0:14f74b704963	871	speed = 0;
BPPearson	0:14f74b704963	872
BPPearson	0:14f74b704963	873	/* extract speed from packet */
BPPearson	0:14f74b704963	874	for (i=1; i<5; i++)
BPPearson	0:14f74b704963	875	speed = (speed * 10) + (commsBuf1[i] - '0');
BPPearson	0:14f74b704963	876
BPPearson	0:14f74b704963	877	/* if valid speed */
BPPearson	0:14f74b704963	878	if (speed >= 0 && speed < 3000)
BPPearson	0:14f74b704963	879	{
BPPearson	0:14f74b704963	880	/* save to speed setpoint of prev drive */
BPPearson	0:14f74b704963	881	speedOfPrev[boardAddress] = (float)speed / 10.0;
BPPearson	0:14f74b704963	882
BPPearson	0:14f74b704963	883	/* set flag to recalculate if in ratio mode */
BPPearson	0:14f74b704963	884	if (controlMode[boardAddress] == ratioMode)
BPPearson	0:14f74b704963	885	{
BPPearson	0:14f74b704963	886	recalcSpeedSetpoint[boardAddress] = TRUE;
BPPearson	0:14f74b704963	887	}
BPPearson	0:14f74b704963	888	}
BPPearson	0:14f74b704963	889	}
BPPearson	0:14f74b704963	890
BPPearson	0:14f74b704963	891	}
BPPearson	0:14f74b704963	892	}
BPPearson	0:14f74b704963	893
BPPearson	0:14f74b704963	894
BPPearson	0:14f74b704963	895
BPPearson	0:14f74b704963	896	void processHostComms()
BPPearson	0:14f74b704963	897	{
BPPearson	0:14f74b704963	898	/* Only called when a complete packet has been received */
BPPearson	0:14f74b704963	899	/* All packets are same length to simplify protocol */
BPPearson	0:14f74b704963	900	/* Valid packets are as listed below */
BPPearson	0:14f74b704963	901	/* */
BPPearson	0:14f74b704963	902	/* 01O1234<chk><chk><ret> sets analog output to 1234 /
BPPearson	0:14f74b704963	903	/* 01S1234<chk><chk><ret> sets speed setpoint to 123.4 /
BPPearson	0:14f74b704963	904	/* 01R1234<chk><chk><ret> sets ratio to prev drive to 1.234 /
BPPearson	0:14f74b704963	905	/* 0131234<chk><chk><ret> sets third nip output to 1234 /
BPPearson	0:14f74b704963	906	/* 01T1234<chk><chk><ret> sets number of teeth 1234 /
BPPearson	0:14f74b704963	907	/* 01C1234<chk><chk><ret> sets circumference to 1234 mm /
BPPearson	0:14f74b704963	908	/* 01P1234<chk><chk><ret> sets pulley ratio to 1.234 /
BPPearson	0:14f74b704963	909	/* 01M1234<chk><chk><ret> sets max surface speed to 123.4 /
BPPearson	0:14f74b704963	910	/* 01p1234<chk><chk><ret> sets prop gain to 1.234 /
BPPearson	0:14f74b704963	911	/* 01i1234<chk><chk><ret> sets integral gain to 1.234 /
BPPearson	0:14f74b704963	912	/* 01ENQ01<chk><chk><ret> requests current speed /
BPPearson	0:14f74b704963	913	/* 01ENQ02<chk><chk><ret> requests host comms status /
BPPearson	0:14f74b704963	914	/* 01ENQ03<chk><chk><ret> requests current status /
BPPearson	0:14f74b704963	915	/* 01ENQ04<chk><chk><ret> requests max dtoa value /
BPPearson	0:14f74b704963	916	/* 01ENQ05<chk><chk><ret> requests min dtoa value /
BPPearson	0:14f74b704963	917	/* */
BPPearson	0:14f74b704963	918	/* Sending a speed setpoint packet puts the controller into */
BPPearson	0:14f74b704963	919	/* speed mode and so on for ratio and output packets. */
BPPearson	0:14f74b704963	920	/* The controller responds with an > CR or # CR packet to */
BPPearson	0:14f74b704963	921	/* the setpoint type of packets. */
BPPearson	0:14f74b704963	922	/* The controller responds with a packet structure as below */
BPPearson	0:14f74b704963	923	/* to the read packets */
BPPearson	0:14f74b704963	924	/* */
BPPearson	0:14f74b704963	925	/* 011234<chk><chk><ret> where 123.4 is current speed /
BPPearson	0:14f74b704963	926	/* 010000<chk><chk><ret> where 0000 is host comms status /
BPPearson	0:14f74b704963	927	/* 010000<chk><chk><ret> where 0000 is current status /
BPPearson	0:14f74b704963	928	/* 011456<chk><chk><ret> where 1456 is max dtoa value /
BPPearson	0:14f74b704963	929	/* 011234<chk><chk><ret> where 1234 is min dtoa value /
BPPearson	0:14f74b704963	930
BPPearson	0:14f74b704963	931	int i;
BPPearson	0:14f74b704963	932	int address;
BPPearson	0:14f74b704963	933	int idx;
BPPearson	0:14f74b704963	934	int ival;
BPPearson	0:14f74b704963	935	int intsp;
BPPearson	0:14f74b704963	936	uchar chkSum;
BPPearson	0:14f74b704963	937	uchar pcktChkSum;
BPPearson	0:14f74b704963	938	char buf[12];
BPPearson	0:14f74b704963	939
BPPearson	0:14f74b704963	940	/* check packet structure is valid, 11th char is <cr> */
BPPearson	0:14f74b704963	941	if (commsBuf0[10] == CR \|\| commsBuf0[10] == 0x0a)
BPPearson	0:14f74b704963	942	{
BPPearson	0:14f74b704963	943	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	944	chkSum = 0;
BPPearson	0:14f74b704963	945
BPPearson	0:14f74b704963	946	/* checksum packet */
BPPearson	0:14f74b704963	947	for (i=0; i<8; i++)
BPPearson	0:14f74b704963	948	{
BPPearson	0:14f74b704963	949	chkSum ^= commsBuf0[i];
BPPearson	0:14f74b704963	950	}
BPPearson	0:14f74b704963	951
BPPearson	0:14f74b704963	952	/* build packet checksum from both chars */
BPPearson	0:14f74b704963	953	pcktChkSum = ((hexToBin(commsBuf0[8])) << 4);
BPPearson	0:14f74b704963	954	pcktChkSum += (hexToBin(commsBuf0[9]));
BPPearson	0:14f74b704963	955
BPPearson	0:14f74b704963	956	/* if checksum ok */
BPPearson	0:14f74b704963	957	if (chkSum == pcktChkSum)
BPPearson	0:14f74b704963	958	{
BPPearson	0:14f74b704963	959	/* extract address from packet */
BPPearson	0:14f74b704963	960	address = ((commsBuf0[1] - '0') * 10) + (commsBuf0[2] - '0');
BPPearson	0:14f74b704963	961
BPPearson	0:14f74b704963	962	//pc.printf("Address %d\n", address);
BPPearson	0:14f74b704963	963
BPPearson	0:14f74b704963	964	/* check if packet was for this controller or board set to reply to all addresses */
BPPearson	0:14f74b704963	965	if ((address >= boardAddress && address < (boardAddress + activeControllers))\|\| boardAddress == 0 )
BPPearson	0:14f74b704963	966	{
BPPearson	0:14f74b704963	967	// index into array is address of loop - board address
BPPearson	0:14f74b704963	968	idx = address - boardAddress;
BPPearson	0:14f74b704963	969
BPPearson	0:14f74b704963	970	//pc.printf("Idx %d\n", idx);
BPPearson	0:14f74b704963	971
BPPearson	0:14f74b704963	972	/* set status to zero */
BPPearson	0:14f74b704963	973	hostCommsStatus = 0;
BPPearson	0:14f74b704963	974
BPPearson	0:14f74b704963	975	/* process packet request */
BPPearson	0:14f74b704963	976	switch (commsBuf0[3])
BPPearson	0:14f74b704963	977	{
BPPearson	0:14f74b704963	978	case 'C':
BPPearson	0:14f74b704963	979	ival = 0;
BPPearson	0:14f74b704963	980	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	981	{
BPPearson	0:14f74b704963	982	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	983	}
BPPearson	0:14f74b704963	984	/* check value is sensible */
BPPearson	0:14f74b704963	985	if (ival > 300 && ival < 2000)
BPPearson	0:14f74b704963	986	{
BPPearson	0:14f74b704963	987	/* put ivalerence into metres */
BPPearson	0:14f74b704963	988	rollCircum[idx] = (float)ival / 1000.0;
BPPearson	0:14f74b704963	989
BPPearson	0:14f74b704963	990	/* set flag to force recalc */
BPPearson	0:14f74b704963	991	recalcFixedPart[idx] = TRUE;
BPPearson	0:14f74b704963	992
BPPearson	0:14f74b704963	993	//pc.printf("circumference %5.3f\n", rollCircum[idx]);
BPPearson	0:14f74b704963	994	}
BPPearson	0:14f74b704963	995	else
BPPearson	0:14f74b704963	996	{
BPPearson	0:14f74b704963	997	hostCommsStatus = badCircumference;
BPPearson	0:14f74b704963	998	}
BPPearson	0:14f74b704963	999	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1000	acknowledge_host(idx);
BPPearson	0:14f74b704963	1001	break;
BPPearson	0:14f74b704963	1002	case 'S':
BPPearson	0:14f74b704963	1003	ival = 0;
BPPearson	0:14f74b704963	1004	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1005	{
BPPearson	0:14f74b704963	1006	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1007	}
BPPearson	0:14f74b704963	1008	/* check value is sensible */
BPPearson	0:14f74b704963	1009	if (ival >= 0 && ival < 3000)
BPPearson	0:14f74b704963	1010	{
BPPearson	0:14f74b704963	1011	/* scale new speed setpoint */
BPPearson	0:14f74b704963	1012	speedSetpnt[idx] = (float)ival * 0.1;
BPPearson	0:14f74b704963	1013
BPPearson	0:14f74b704963	1014	/* set to speed control mode */
BPPearson	0:14f74b704963	1015	controlMode[idx] = speedMode;
BPPearson	0:14f74b704963	1016
BPPearson	0:14f74b704963	1017	/* set flag to force recalc */
BPPearson	0:14f74b704963	1018	recalcSpeedSetpoint[idx] = TRUE;
BPPearson	0:14f74b704963	1019
BPPearson	0:14f74b704963	1020	//pc.printf("speed %5.3f\n", speedSetpnt[idx]);
BPPearson	0:14f74b704963	1021	}
BPPearson	0:14f74b704963	1022	else
BPPearson	0:14f74b704963	1023	{
BPPearson	0:14f74b704963	1024	hostCommsStatus = badSpeedSetpoint;
BPPearson	0:14f74b704963	1025	}
BPPearson	0:14f74b704963	1026	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1027	acknowledge_host(idx);
BPPearson	0:14f74b704963	1028	break;
BPPearson	0:14f74b704963	1029	case 'R':
BPPearson	0:14f74b704963	1030	ival = 0;
BPPearson	0:14f74b704963	1031	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1032	{
BPPearson	0:14f74b704963	1033	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1034	}
BPPearson	0:14f74b704963	1035	/* check value is sensible */
BPPearson	0:14f74b704963	1036	if (ival >= 100 && ival < 9999)
BPPearson	0:14f74b704963	1037	{
BPPearson	0:14f74b704963	1038	/* scale to get max ratio of 9.999 */
BPPearson	0:14f74b704963	1039	ratioToPrev[idx] = (float)ival * 0.001;
BPPearson	0:14f74b704963	1040
BPPearson	0:14f74b704963	1041	/* set to ratio control mode */
BPPearson	0:14f74b704963	1042	controlMode[idx] = ratioMode;
BPPearson	0:14f74b704963	1043
BPPearson	0:14f74b704963	1044	/* set flag to force recalc */
BPPearson	0:14f74b704963	1045	recalcSpeedSetpoint[idx] = TRUE;
BPPearson	0:14f74b704963	1046	//pc.printf("Ratio to prev %5.3f\n", ratioToPrev[idx]);
BPPearson	0:14f74b704963	1047	}
BPPearson	0:14f74b704963	1048	else
BPPearson	0:14f74b704963	1049	{
BPPearson	0:14f74b704963	1050	hostCommsStatus = badRatioSetpoint;
BPPearson	0:14f74b704963	1051	}
BPPearson	0:14f74b704963	1052	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1053	acknowledge_host(idx);
BPPearson	0:14f74b704963	1054	break;
BPPearson	0:14f74b704963	1055	case 'O':
BPPearson	0:14f74b704963	1056	ival = 0;
BPPearson	0:14f74b704963	1057	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1058	{
BPPearson	0:14f74b704963	1059	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1060	}
BPPearson	0:14f74b704963	1061	/* check value is sensible */
BPPearson	0:14f74b704963	1062	if (ival >= 0 && ival < 2048)
BPPearson	0:14f74b704963	1063	{
BPPearson	0:14f74b704963	1064	/* save for output to d/a */
BPPearson	0:14f74b704963	1065	dtoaSetpnt[idx] = ival;
BPPearson	0:14f74b704963	1066
BPPearson	0:14f74b704963	1067	/* do crude bumpless transfer */
BPPearson	0:14f74b704963	1068	intSum[idx] = (float)dtoaSetpnt[idx] / intGain[idx];
BPPearson	0:14f74b704963	1069
BPPearson	0:14f74b704963	1070	/* set to direct output mode */
BPPearson	0:14f74b704963	1071	controlMode[idx] = outputMode;
BPPearson	0:14f74b704963	1072
BPPearson	0:14f74b704963	1073	/* set flag to write to d/a chip */
BPPearson	0:14f74b704963	1074	updateAnalogOutput[idx] = TRUE;
BPPearson	0:14f74b704963	1075	}
BPPearson	0:14f74b704963	1076	else
BPPearson	0:14f74b704963	1077	{
BPPearson	0:14f74b704963	1078	hostCommsStatus = badOutputSetpoint;
BPPearson	0:14f74b704963	1079	}
BPPearson	0:14f74b704963	1080	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1081	acknowledge_host(idx);
BPPearson	0:14f74b704963	1082	break;
BPPearson	0:14f74b704963	1083	case '3':
BPPearson	0:14f74b704963	1084	ival = 0;
BPPearson	0:14f74b704963	1085	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1086	{
BPPearson	0:14f74b704963	1087	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1088	}
BPPearson	0:14f74b704963	1089
BPPearson	0:14f74b704963	1090	/* check value is sensible */
BPPearson	0:14f74b704963	1091	if (ival >= 0 && ival < 2048)
BPPearson	0:14f74b704963	1092	{
BPPearson	0:14f74b704963	1093	/* save for output to d/a */
BPPearson	0:14f74b704963	1094	dtoaSetpnt[idx] = ival;
BPPearson	0:14f74b704963	1095
BPPearson	0:14f74b704963	1096	/* set to third nip mode */
BPPearson	0:14f74b704963	1097	controlMode[idx] = thirdNipMode;
BPPearson	0:14f74b704963	1098
BPPearson	0:14f74b704963	1099	/* set flag to write to d/a chip */
BPPearson	0:14f74b704963	1100	updateAnalogOutput[idx] = TRUE;
BPPearson	0:14f74b704963	1101	//pc.printf("3rd nip %d\n", dtoaSetpnt[idx]);
BPPearson	0:14f74b704963	1102	}
BPPearson	0:14f74b704963	1103	else
BPPearson	0:14f74b704963	1104	{
BPPearson	0:14f74b704963	1105	hostCommsStatus = badOutputSetpoint;
BPPearson	0:14f74b704963	1106	}
BPPearson	0:14f74b704963	1107	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1108	acknowledge_host(idx);
BPPearson	0:14f74b704963	1109	break;
BPPearson	0:14f74b704963	1110	case 'T':
BPPearson	0:14f74b704963	1111	ival = 0;
BPPearson	0:14f74b704963	1112	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1113	{
BPPearson	0:14f74b704963	1114	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1115	}
BPPearson	0:14f74b704963	1116	/* check value is sensible */
BPPearson	0:14f74b704963	1117	if (ival >= 10 && ival < 180)
BPPearson	0:14f74b704963	1118	{
BPPearson	0:14f74b704963	1119	/* save into global variable teeth */
BPPearson	0:14f74b704963	1120	teethOnWheel[idx] = ival;
BPPearson	0:14f74b704963	1121
BPPearson	0:14f74b704963	1122	/* set flag to force recalc */
BPPearson	0:14f74b704963	1123	recalcFixedPart[idx] = TRUE;
BPPearson	0:14f74b704963	1124	}
BPPearson	0:14f74b704963	1125	else
BPPearson	0:14f74b704963	1126	{
BPPearson	0:14f74b704963	1127	hostCommsStatus = badNumOfTeeth;
BPPearson	0:14f74b704963	1128	}
BPPearson	0:14f74b704963	1129	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1130	acknowledge_host(idx);
BPPearson	0:14f74b704963	1131	break;
BPPearson	0:14f74b704963	1132	case 'P':
BPPearson	0:14f74b704963	1133	ival = 0;
BPPearson	0:14f74b704963	1134	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1135	{
BPPearson	0:14f74b704963	1136	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1137	}
BPPearson	0:14f74b704963	1138	/* check value is sensible */
BPPearson	0:14f74b704963	1139	if (ival >= 100 && ival < 9999)
BPPearson	0:14f74b704963	1140	{
BPPearson	0:14f74b704963	1141	/* scale to get max ratio of 9.999 */
BPPearson	0:14f74b704963	1142	pulleyRatio[idx] = (float)ival * 0.001;
BPPearson	0:14f74b704963	1143
BPPearson	0:14f74b704963	1144	/* set flag to force recalc */
BPPearson	0:14f74b704963	1145	recalcFixedPart[idx] = TRUE;
BPPearson	0:14f74b704963	1146	}
BPPearson	0:14f74b704963	1147	else
BPPearson	0:14f74b704963	1148	{
BPPearson	0:14f74b704963	1149	hostCommsStatus = badRatioSetpoint;
BPPearson	0:14f74b704963	1150	}
BPPearson	0:14f74b704963	1151	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1152	acknowledge_host(idx);
BPPearson	0:14f74b704963	1153	break;
BPPearson	0:14f74b704963	1154	case 'p':
BPPearson	0:14f74b704963	1155	ival = 0;
BPPearson	0:14f74b704963	1156	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1157	{
BPPearson	0:14f74b704963	1158	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1159	}
BPPearson	0:14f74b704963	1160	/* check value is sensible */
BPPearson	0:14f74b704963	1161	if (ival >= 1 && ival < 9999)
BPPearson	0:14f74b704963	1162	{
BPPearson	0:14f74b704963	1163	/* scale to get max prop gain of 9.999 */
BPPearson	0:14f74b704963	1164	propGain[idx] = (float)ival * 0.001;
BPPearson	0:14f74b704963	1165	}
BPPearson	0:14f74b704963	1166	else
BPPearson	0:14f74b704963	1167	{
BPPearson	0:14f74b704963	1168	hostCommsStatus = badPropGainSetpoint;
BPPearson	0:14f74b704963	1169	}
BPPearson	0:14f74b704963	1170	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1171	acknowledge_host(idx);
BPPearson	0:14f74b704963	1172	break;
BPPearson	0:14f74b704963	1173	case 'i':
BPPearson	0:14f74b704963	1174	ival = 0;
BPPearson	0:14f74b704963	1175	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1176	{
BPPearson	0:14f74b704963	1177	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1178	}
BPPearson	0:14f74b704963	1179	/* check value is sensible */
BPPearson	0:14f74b704963	1180	if (ival >= 1 && ival < 9999)
BPPearson	0:14f74b704963	1181	{
BPPearson	0:14f74b704963	1182	/* scale to get max integral gain of 9.999 */
BPPearson	0:14f74b704963	1183	intGain[idx] = (float)ival * 0.001;
BPPearson	0:14f74b704963	1184	}
BPPearson	0:14f74b704963	1185	else
BPPearson	0:14f74b704963	1186	{
BPPearson	0:14f74b704963	1187	hostCommsStatus = badIntGainSetpoint;
BPPearson	0:14f74b704963	1188	}
BPPearson	0:14f74b704963	1189	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1190	acknowledge_host(idx);
BPPearson	0:14f74b704963	1191	break;
BPPearson	0:14f74b704963	1192	case 'M':
BPPearson	0:14f74b704963	1193	ival = 0;
BPPearson	0:14f74b704963	1194	for (i=4; i<8; i++)
BPPearson	0:14f74b704963	1195	{
BPPearson	0:14f74b704963	1196	ival = ival * 10 + commsBuf0[i] - '0';
BPPearson	0:14f74b704963	1197	}
BPPearson	0:14f74b704963	1198	/* check value is sensible */
BPPearson	0:14f74b704963	1199	if (ival >= 100 && ival < 3000)
BPPearson	0:14f74b704963	1200	{
BPPearson	0:14f74b704963	1201	/* scale new max speed value */
BPPearson	0:14f74b704963	1202	maxSpeedOfDrive[idx] = (float)ival * 0.1;
BPPearson	0:14f74b704963	1203	}
BPPearson	0:14f74b704963	1204	else
BPPearson	0:14f74b704963	1205	{
BPPearson	0:14f74b704963	1206	hostCommsStatus = badMaxSpeedSetpoint;
BPPearson	0:14f74b704963	1207	}
BPPearson	0:14f74b704963	1208	/* reply to host with ack or nak */
BPPearson	0:14f74b704963	1209	acknowledge_host(idx);
BPPearson	0:14f74b704963	1210	break;
BPPearson	0:14f74b704963	1211	case 'E':
BPPearson	0:14f74b704963	1212	/* determine type of enquiry */
BPPearson	0:14f74b704963	1213	switch (commsBuf0[7])
BPPearson	0:14f74b704963	1214	{
BPPearson	0:14f74b704963	1215	/* if enquiry 1 return current speed */
BPPearson	0:14f74b704963	1216	case '1':
BPPearson	0:14f74b704963	1217
BPPearson	0:14f74b704963	1218	/* round up/down properly */
BPPearson	0:14f74b704963	1219	intsp = (int)(measuredSpeed[idx] * 10.0);
BPPearson	0:14f74b704963	1220	if (((measuredSpeed[idx] * 10.0) - intsp) >= 0.5 )
BPPearson	0:14f74b704963	1221	{
BPPearson	0:14f74b704963	1222	intsp = intsp + 1;
BPPearson	0:14f74b704963	1223	}
BPPearson	0:14f74b704963	1224
BPPearson	0:14f74b704963	1225	sprintf( buf, "*%02d%04d", address,
BPPearson	0:14f74b704963	1226	(int)(intsp));
BPPearson	0:14f74b704963	1227	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	1228	chkSum = 0;
BPPearson	0:14f74b704963	1229
BPPearson	0:14f74b704963	1230	/* checksum packet */
BPPearson	0:14f74b704963	1231	for (i=0; i<7; i++)
BPPearson	0:14f74b704963	1232	{
BPPearson	0:14f74b704963	1233	chkSum ^= buf[i];
BPPearson	0:14f74b704963	1234	}
BPPearson	0:14f74b704963	1235
BPPearson	0:14f74b704963	1236	/* enable host comms */
BPPearson	0:14f74b704963	1237	enable_host_comms();
BPPearson	0:14f74b704963	1238
BPPearson	0:14f74b704963	1239	/* return packet to host */
BPPearson	0:14f74b704963	1240	putstr0( buf );
BPPearson	0:14f74b704963	1241
BPPearson	0:14f74b704963	1242	/* generate checksum */
BPPearson	0:14f74b704963	1243	sprintf( buf, "%02X%c", (int)chkSum, CR );
BPPearson	0:14f74b704963	1244
BPPearson	0:14f74b704963	1245	/* return packet to host */
BPPearson	0:14f74b704963	1246	putstr0( buf );
BPPearson	0:14f74b704963	1247
BPPearson	0:14f74b704963	1248	// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
BPPearson	0:14f74b704963	1249	disableHostComms = 6;
BPPearson	0:14f74b704963	1250	break;
BPPearson	0:14f74b704963	1251	/* if enquiry 2 return current host comms status */
BPPearson	0:14f74b704963	1252	case '2':
BPPearson	0:14f74b704963	1253	sprintf( buf, "*%02d%04d", address,
BPPearson	0:14f74b704963	1254	hostCommsStatus );
BPPearson	0:14f74b704963	1255	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	1256	chkSum = 0;
BPPearson	0:14f74b704963	1257
BPPearson	0:14f74b704963	1258	/* checksum packet */
BPPearson	0:14f74b704963	1259	for (i=0; i<7; i++)
BPPearson	0:14f74b704963	1260	{
BPPearson	0:14f74b704963	1261	chkSum ^= buf[i];
BPPearson	0:14f74b704963	1262	}
BPPearson	0:14f74b704963	1263
BPPearson	0:14f74b704963	1264	/* enable host comms */
BPPearson	0:14f74b704963	1265	enable_host_comms();
BPPearson	0:14f74b704963	1266
BPPearson	0:14f74b704963	1267	/* return packet to host */
BPPearson	0:14f74b704963	1268	putstr0( buf );
BPPearson	0:14f74b704963	1269
BPPearson	0:14f74b704963	1270	sprintf( buf, "%02X%c", chkSum, CR );
BPPearson	0:14f74b704963	1271
BPPearson	0:14f74b704963	1272	/* return packet to host */
BPPearson	0:14f74b704963	1273	putstr0( buf );
BPPearson	0:14f74b704963	1274
BPPearson	0:14f74b704963	1275	// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
BPPearson	0:14f74b704963	1276	disableHostComms = 6;
BPPearson	0:14f74b704963	1277	break;
BPPearson	0:14f74b704963	1278	/* if enquiry 3 return current status */
BPPearson	0:14f74b704963	1279	case '3':
BPPearson	0:14f74b704963	1280	sprintf( buf, "*%02d%04d", address, status );
BPPearson	0:14f74b704963	1281
BPPearson	0:14f74b704963	1282	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	1283	chkSum = 0;
BPPearson	0:14f74b704963	1284
BPPearson	0:14f74b704963	1285	/* checksum packet */
BPPearson	0:14f74b704963	1286	for (i=0; i<7; i++)
BPPearson	0:14f74b704963	1287	{
BPPearson	0:14f74b704963	1288	chkSum ^= buf[i];
BPPearson	0:14f74b704963	1289	}
BPPearson	0:14f74b704963	1290
BPPearson	0:14f74b704963	1291	/* enable host comms */
BPPearson	0:14f74b704963	1292	enable_host_comms();
BPPearson	0:14f74b704963	1293
BPPearson	0:14f74b704963	1294	/* return packet to host */
BPPearson	0:14f74b704963	1295	putstr0( buf );
BPPearson	0:14f74b704963	1296
BPPearson	0:14f74b704963	1297	sprintf( buf, "%02X%c", chkSum, CR );
BPPearson	0:14f74b704963	1298
BPPearson	0:14f74b704963	1299	/* return packet to host */
BPPearson	0:14f74b704963	1300	putstr0( buf );
BPPearson	0:14f74b704963	1301
BPPearson	0:14f74b704963	1302	// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
BPPearson	0:14f74b704963	1303	disableHostComms = 6;
BPPearson	0:14f74b704963	1304
BPPearson	0:14f74b704963	1305	/* reset status */
BPPearson	0:14f74b704963	1306	status = 0;
BPPearson	0:14f74b704963	1307	break;
BPPearson	0:14f74b704963	1308	/* if enquiry 4 return max dtoa value */
BPPearson	0:14f74b704963	1309	case '4':
BPPearson	0:14f74b704963	1310	sprintf( buf, "*%02d%04d", address, maxDtoa[idx] );
BPPearson	0:14f74b704963	1311
BPPearson	0:14f74b704963	1312	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	1313	chkSum = 0;
BPPearson	0:14f74b704963	1314
BPPearson	0:14f74b704963	1315	/* checksum packet */
BPPearson	0:14f74b704963	1316	for (i=0; i<7; i++)
BPPearson	0:14f74b704963	1317	{
BPPearson	0:14f74b704963	1318	chkSum ^= buf[i];
BPPearson	0:14f74b704963	1319	}
BPPearson	0:14f74b704963	1320
BPPearson	0:14f74b704963	1321	/* enable host comms */
BPPearson	0:14f74b704963	1322	enable_host_comms();
BPPearson	0:14f74b704963	1323
BPPearson	0:14f74b704963	1324	/* return packet to host */
BPPearson	0:14f74b704963	1325	putstr0( buf );
BPPearson	0:14f74b704963	1326
BPPearson	0:14f74b704963	1327	// add checksum
BPPearson	0:14f74b704963	1328	sprintf( buf, "%02X%c", chkSum, CR );
BPPearson	0:14f74b704963	1329
BPPearson	0:14f74b704963	1330	/* return packet to host */
BPPearson	0:14f74b704963	1331	putstr0( buf );
BPPearson	0:14f74b704963	1332
BPPearson	0:14f74b704963	1333	// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
BPPearson	0:14f74b704963	1334	disableHostComms = 6;
BPPearson	0:14f74b704963	1335	break;
BPPearson	0:14f74b704963	1336	/* if enquiry 5 return current min dtoa value */
BPPearson	0:14f74b704963	1337	case '5':
BPPearson	0:14f74b704963	1338	sprintf( buf, "*%02d%04d", address, minDtoa[idx] );
BPPearson	0:14f74b704963	1339
BPPearson	0:14f74b704963	1340	/* set checksum accumulator to zero */
BPPearson	0:14f74b704963	1341	chkSum = 0;
BPPearson	0:14f74b704963	1342
BPPearson	0:14f74b704963	1343	/* checksum packet */
BPPearson	0:14f74b704963	1344	for (i=0; i<7; i++)
BPPearson	0:14f74b704963	1345	{
BPPearson	0:14f74b704963	1346	chkSum ^= buf[i];
BPPearson	0:14f74b704963	1347	}
BPPearson	0:14f74b704963	1348
BPPearson	0:14f74b704963	1349	/* enable host comms */
BPPearson	0:14f74b704963	1350	enable_host_comms();
BPPearson	0:14f74b704963	1351
BPPearson	0:14f74b704963	1352	/* return packet to host */
BPPearson	0:14f74b704963	1353	putstr0( buf );
BPPearson	0:14f74b704963	1354
BPPearson	0:14f74b704963	1355	// add checksum */
BPPearson	0:14f74b704963	1356	sprintf( buf, "%02X%c", chkSum, CR );
BPPearson	0:14f74b704963	1357
BPPearson	0:14f74b704963	1358	/* return packet to host */
BPPearson	0:14f74b704963	1359	putstr0( buf );
BPPearson	0:14f74b704963	1360
BPPearson	0:14f74b704963	1361	//******* consider implementing a timer to disable the 485 driver as using the counter is not deterministic
BPPearson	0:14f74b704963	1362	// the serial handler in mbed buffers the characters so the above code takes uSecs but the string can take 5mSecs to send
BPPearson	0:14f74b704963	1363	// 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
BPPearson	0:14f74b704963	1364
BPPearson	0:14f74b704963	1365	// disable host comms after 6mS allowing 10 chars to be sent at 19200 baud
BPPearson	0:14f74b704963	1366	disableHostComms = 6;
BPPearson	0:14f74b704963	1367
BPPearson	0:14f74b704963	1368	/* reset values */
BPPearson	0:14f74b704963	1369	maxDtoa[idx] = dtoaSetpnt[idx];
BPPearson	0:14f74b704963	1370	minDtoa[idx] = dtoaSetpnt[idx];
BPPearson	0:14f74b704963	1371	break;
BPPearson	0:14f74b704963	1372	}
BPPearson	0:14f74b704963	1373	break;
BPPearson	0:14f74b704963	1374	default:
BPPearson	0:14f74b704963	1375	hostCommsStatus = unknownPacket;
BPPearson	0:14f74b704963	1376	}
BPPearson	0:14f74b704963	1377	}
BPPearson	0:14f74b704963	1378	}
BPPearson	0:14f74b704963	1379	}
BPPearson	0:14f74b704963	1380	}
BPPearson	0:14f74b704963	1381
BPPearson	0:14f74b704963	1382
BPPearson	0:14f74b704963	1383
BPPearson	0:14f74b704963	1384
BPPearson	0:14f74b704963	1385
BPPearson	0:14f74b704963	1386
BPPearson	0:14f74b704963	1387	void process_latest_count(int idx)
BPPearson	0:14f74b704963	1388
BPPearson	0:14f74b704963	1389	{
BPPearson	0:14f74b704963	1390	unsigned long count;
BPPearson	0:14f74b704963	1391
BPPearson	0:14f74b704963	1392	// get local copy
BPPearson	0:14f74b704963	1393	count = currentCountDiff[idx];
BPPearson	0:14f74b704963	1394
BPPearson	0:14f74b704963	1395	/* limit count to maximum value */
BPPearson	0:14f74b704963	1396	if (count > MAX_COUNT_VALUE)
BPPearson	0:14f74b704963	1397	{
BPPearson	0:14f74b704963	1398	count = MAX_COUNT_VALUE;
BPPearson	0:14f74b704963	1399	}
BPPearson	0:14f74b704963	1400	else
BPPearson	0:14f74b704963	1401	{
BPPearson	0:14f74b704963	1402	/* limit count to minimum value */
BPPearson	0:14f74b704963	1403	if (count < MIN_COUNT_VALUE)
BPPearson	0:14f74b704963	1404	{
BPPearson	0:14f74b704963	1405	count = MIN_COUNT_VALUE;
BPPearson	0:14f74b704963	1406	}
BPPearson	0:14f74b704963	1407	}
BPPearson	0:14f74b704963	1408
BPPearson	0:14f74b704963	1409	/* if count > previous biggest count */
BPPearson	0:14f74b704963	1410	if (count > lastBigCount[idx])
BPPearson	0:14f74b704963	1411	{
BPPearson	0:14f74b704963	1412	/* adjust lastBigCount by one eigth of count value */
BPPearson	0:14f74b704963	1413	lastBigCount[idx] = lastBigCount[idx] + (count / 8);
BPPearson	0:14f74b704963	1414	}
BPPearson	0:14f74b704963	1415	else
BPPearson	0:14f74b704963	1416	{
BPPearson	0:14f74b704963	1417	/* if count < previous smallest count */
BPPearson	0:14f74b704963	1418	if (count < lastSmallCount[idx])
BPPearson	0:14f74b704963	1419	{
BPPearson	0:14f74b704963	1420	/* adjust lastSmallCount by one eigth of count value */
BPPearson	0:14f74b704963	1421	lastSmallCount[idx] = lastSmallCount[idx] - (count / 8);
BPPearson	0:14f74b704963	1422	}
BPPearson	0:14f74b704963	1423	else
BPPearson	0:14f74b704963	1424	{
BPPearson	0:14f74b704963	1425	/* bring limits back in around current count */
BPPearson	0:14f74b704963	1426	lastBigCount[idx] = count + (count / 8);
BPPearson	0:14f74b704963	1427	lastSmallCount[idx]= count - (count / 8);
BPPearson	0:14f74b704963	1428
BPPearson	0:14f74b704963	1429	/* process count for mode controller is in */
BPPearson	0:14f74b704963	1430	switch (controlMode[idx])
BPPearson	0:14f74b704963	1431	{
BPPearson	0:14f74b704963	1432	case speedMode:
BPPearson	0:14f74b704963	1433	case ratioMode:
BPPearson	0:14f74b704963	1434	/* filter the current count into the actual count value */
BPPearson	0:14f74b704963	1435	actualCount[idx] = ((actualCount[idx] / 8) * 7) + (count / 8);
BPPearson	0:14f74b704963	1436
BPPearson	0:14f74b704963	1437	/* check if within band */
BPPearson	0:14f74b704963	1438	if (actualCount[idx] > loCountLimit[idx] && actualCount[idx] < hiCountLimit[idx])
BPPearson	0:14f74b704963	1439	{
BPPearson	0:14f74b704963	1440	/* if greater than zero */
BPPearson	0:14f74b704963	1441	if (outsideCountBand[idx] > 0)
BPPearson	0:14f74b704963	1442	{
BPPearson	0:14f74b704963	1443	/* decrement counter */
BPPearson	0:14f74b704963	1444	outsideCountBand[idx]--;
BPPearson	0:14f74b704963	1445
BPPearson	0:14f74b704963	1446	if (outsideCountBand[idx] == 0)
BPPearson	0:14f74b704963	1447	{
BPPearson	0:14f74b704963	1448	/* set flag for within band */
BPPearson	0:14f74b704963	1449	withinControlBand[idx] = TRUE;
BPPearson	0:14f74b704963	1450	}
BPPearson	0:14f74b704963	1451	}
BPPearson	0:14f74b704963	1452	}
BPPearson	0:14f74b704963	1453	else
BPPearson	0:14f74b704963	1454	{
BPPearson	0:14f74b704963	1455	/* check if not reached limit */
BPPearson	0:14f74b704963	1456	if (outsideCountBand[idx] < OUTSIDE_COUNT_BAND_LIMIT)
BPPearson	0:14f74b704963	1457	{
BPPearson	0:14f74b704963	1458	/* increment out of band counter */
BPPearson	0:14f74b704963	1459	outsideCountBand[idx]++;
BPPearson	0:14f74b704963	1460
BPPearson	0:14f74b704963	1461	/* check if reached limit */
BPPearson	0:14f74b704963	1462	if (outsideCountBand[idx] >= OUTSIDE_COUNT_BAND_LIMIT)
BPPearson	0:14f74b704963	1463	{
BPPearson	0:14f74b704963	1464	/* set flag for not within band */
BPPearson	0:14f74b704963	1465	withinControlBand[idx] = FALSE;
BPPearson	0:14f74b704963	1466	}
BPPearson	0:14f74b704963	1467	}
BPPearson	0:14f74b704963	1468	}
BPPearson	0:14f74b704963	1469
BPPearson	0:14f74b704963	1470	/* if not in startup mode */
BPPearson	0:14f74b704963	1471	if (!inStartupMode[idx])
BPPearson	0:14f74b704963	1472	{
BPPearson	0:14f74b704963	1473	/* get count error (override to long to get sign back)*/
BPPearson	0:14f74b704963	1474	errorCount[idx] = (long)(actualCount[idx] - countSetpoint[idx]);
BPPearson	0:14f74b704963	1475
BPPearson	0:14f74b704963	1476	/* perform P, I and D */
BPPearson	0:14f74b704963	1477	intSum[idx] += errorCount[idx];
BPPearson	0:14f74b704963	1478	dtoaSetpnt[idx] = (int)((propGain[idx] * errorCount[idx]) + (intGain[idx] * intSum[idx]));
BPPearson	0:14f74b704963	1479
BPPearson	0:14f74b704963	1480	/* check within output limits */
BPPearson	0:14f74b704963	1481	if (dtoaSetpnt[idx] < 0)
BPPearson	0:14f74b704963	1482	{
BPPearson	0:14f74b704963	1483	dtoaSetpnt[idx] = 0;
BPPearson	0:14f74b704963	1484	output_limit_status[idx] = OUTPUT_LOW;
BPPearson	0:14f74b704963	1485	/* do crude bumpless transfer */
BPPearson	0:14f74b704963	1486	intSum[idx] = 0.0;
BPPearson	0:14f74b704963	1487	}
BPPearson	0:14f74b704963	1488	else
BPPearson	0:14f74b704963	1489	{
BPPearson	0:14f74b704963	1490	if (dtoaSetpnt[idx] > 2047)
BPPearson	0:14f74b704963	1491	{
BPPearson	0:14f74b704963	1492	dtoaSetpnt[idx] = 2047;
BPPearson	0:14f74b704963	1493	output_limit_status[idx] = OUTPUT_HIGH;
BPPearson	0:14f74b704963	1494	/* do crude bumpless transfer */
BPPearson	0:14f74b704963	1495	intSum[idx] = (float)dtoaSetpnt[idx] / intGain[idx];
BPPearson	0:14f74b704963	1496	}
BPPearson	0:14f74b704963	1497	else
BPPearson	0:14f74b704963	1498	{
BPPearson	0:14f74b704963	1499	output_limit_status[idx] = OUTPUT_WITHIN;
BPPearson	0:14f74b704963	1500	}
BPPearson	0:14f74b704963	1501	}
BPPearson	0:14f74b704963	1502
BPPearson	0:14f74b704963	1503	/* check new dtoa value against monitored limits */
BPPearson	0:14f74b704963	1504	if (dtoaSetpnt[idx] > maxDtoa[idx])
BPPearson	0:14f74b704963	1505	{
BPPearson	0:14f74b704963	1506	/* set new maximum */
BPPearson	0:14f74b704963	1507	maxDtoa[idx] = dtoaSetpnt[idx];
BPPearson	0:14f74b704963	1508	}
BPPearson	0:14f74b704963	1509	else
BPPearson	0:14f74b704963	1510	{
BPPearson	0:14f74b704963	1511	if (dtoaSetpnt[idx] < minDtoa[idx])
BPPearson	0:14f74b704963	1512	{
BPPearson	0:14f74b704963	1513	/* set new minimum */
BPPearson	0:14f74b704963	1514	minDtoa[idx] = dtoaSetpnt[idx];
BPPearson	0:14f74b704963	1515	}
BPPearson	0:14f74b704963	1516	}
BPPearson	0:14f74b704963	1517
BPPearson	0:14f74b704963	1518	/* set flag to force update */
BPPearson	0:14f74b704963	1519	updateAnalogOutput[idx] = TRUE;
BPPearson	0:14f74b704963	1520
BPPearson	0:14f74b704963	1521	/* calculate speed for logging */
BPPearson	0:14f74b704963	1522	measuredSpeed[idx] = calcActualSpeed(idx);
BPPearson	0:14f74b704963	1523	}
BPPearson	0:14f74b704963	1524	break;
BPPearson	0:14f74b704963	1525	case thirdNipMode:
BPPearson	0:14f74b704963	1526	/* filter count into actual count */
BPPearson	0:14f74b704963	1527	actualCount[idx] = ((actualCount[idx] / 8) * 7) + (count / 8);
BPPearson	0:14f74b704963	1528
BPPearson	0:14f74b704963	1529	/* calculate speed */
BPPearson	0:14f74b704963	1530	measuredSpeed[idx] = calcActualSpeed(idx);
BPPearson	0:14f74b704963	1531
BPPearson	0:14f74b704963	1532	/* filter into speed setpoint to be cascaded to next drive */
BPPearson	0:14f74b704963	1533	speedSetpnt[idx] = (speedSetpnt[idx] * 0.875) + (measuredSpeed[idx] * 0.125);
BPPearson	0:14f74b704963	1534	/* make speedSetpoint the same as measured speed */
BPPearson	0:14f74b704963	1535	/* speedSetpnt = measuredSpeed; */
BPPearson	0:14f74b704963	1536
BPPearson	0:14f74b704963	1537	/* force speed to be cascaded */
BPPearson	0:14f74b704963	1538	xmitCascadeSpeed[idx] = TRUE;
BPPearson	0:14f74b704963	1539	break;
BPPearson	0:14f74b704963	1540	default: ;
BPPearson	0:14f74b704963	1541	}
BPPearson	0:14f74b704963	1542	}
BPPearson	0:14f74b704963	1543	}
BPPearson	0:14f74b704963	1544	}
BPPearson	0:14f74b704963	1545
BPPearson	0:14f74b704963	1546
BPPearson	0:14f74b704963	1547
BPPearson	0:14f74b704963	1548	void putch0(uchar c)
BPPearson	0:14f74b704963	1549	{
BPPearson	0:14f74b704963	1550
BPPearson	0:14f74b704963	1551	host.putc(c);
BPPearson	0:14f74b704963	1552	}
BPPearson	0:14f74b704963	1553
BPPearson	0:14f74b704963	1554
BPPearson	0:14f74b704963	1555
BPPearson	0:14f74b704963	1556	void putch1(uchar c)
BPPearson	0:14f74b704963	1557	{
BPPearson	0:14f74b704963	1558
BPPearson	0:14f74b704963	1559	cascade.putc(c);
BPPearson	0:14f74b704963	1560	}
BPPearson	0:14f74b704963	1561
BPPearson	0:14f74b704963	1562
BPPearson	0:14f74b704963	1563
BPPearson	0:14f74b704963	1564	void putstr0(char* str )
BPPearson	0:14f74b704963	1565	{
BPPearson	0:14f74b704963	1566
BPPearson	0:14f74b704963	1567	while (*str)
BPPearson	0:14f74b704963	1568	{
BPPearson	0:14f74b704963	1569	putch0( *str++ );
BPPearson	0:14f74b704963	1570	}
BPPearson	0:14f74b704963	1571	}
BPPearson	0:14f74b704963	1572
BPPearson	0:14f74b704963	1573
BPPearson	0:14f74b704963	1574
BPPearson	0:14f74b704963	1575	void putstr1( char* str )
BPPearson	0:14f74b704963	1576	{
BPPearson	0:14f74b704963	1577
BPPearson	0:14f74b704963	1578	while (*str)
BPPearson	0:14f74b704963	1579	{
BPPearson	0:14f74b704963	1580	putch1( *str++ );
BPPearson	0:14f74b704963	1581	}
BPPearson	0:14f74b704963	1582	}
BPPearson	0:14f74b704963	1583
BPPearson	0:14f74b704963	1584
BPPearson	0:14f74b704963	1585
BPPearson	0:14f74b704963	1586	void serial_rcv_int0()
BPPearson	0:14f74b704963	1587	{
BPPearson	0:14f74b704963	1588	uchar c, loop;
BPPearson	0:14f74b704963	1589
BPPearson	0:14f74b704963	1590	/* if not processing a packet */
BPPearson	0:14f74b704963	1591	if (hostCrRcvd == FALSE)
BPPearson	0:14f74b704963	1592	{
BPPearson	0:14f74b704963	1593	/* get char into local */
BPPearson	0:14f74b704963	1594	c = host.getc();
BPPearson	0:14f74b704963	1595
BPPearson	0:14f74b704963	1596	/* if first char received */
BPPearson	0:14f74b704963	1597	if (commsCntr0 == 0)
BPPearson	0:14f74b704963	1598	{
BPPearson	0:14f74b704963	1599	/* set receive led on */
BPPearson	0:14f74b704963	1600	rxLed = 1;
BPPearson	0:14f74b704963	1601	}
BPPearson	0:14f74b704963	1602
BPPearson	0:14f74b704963	1603	/* if space in buffer */
BPPearson	0:14f74b704963	1604	if (commsCntr0 < (commsBufLen - 1))
BPPearson	0:14f74b704963	1605	{
BPPearson	0:14f74b704963	1606	/* put char in buffer and increment pointer */
BPPearson	0:14f74b704963	1607	commsBuf0[commsCntr0++] = c;
BPPearson	0:14f74b704963	1608	}
BPPearson	0:14f74b704963	1609
BPPearson	0:14f74b704963	1610	/* if char is end of packet */
BPPearson	0:14f74b704963	1611	if (c == CR \|\| c == 0x0a)
BPPearson	0:14f74b704963	1612	{
BPPearson	0:14f74b704963	1613	// clear to the end of the packet
BPPearson	0:14f74b704963	1614	for (loop = commsCntr0; loop<commsBufLen; loop++)
BPPearson	0:14f74b704963	1615	{
BPPearson	0:14f74b704963	1616	commsBuf0[loop] = 0;
BPPearson	0:14f74b704963	1617	}
BPPearson	0:14f74b704963	1618
BPPearson	0:14f74b704963	1619	/* set flag to process packet */
BPPearson	0:14f74b704963	1620	hostCrRcvd = TRUE;
BPPearson	0:14f74b704963	1621
BPPearson	0:14f74b704963	1622	/* turn receive led off */
BPPearson	0:14f74b704963	1623	rxLed = 0;
BPPearson	0:14f74b704963	1624	}
BPPearson	0:14f74b704963	1625
BPPearson	0:14f74b704963	1626	}
BPPearson	0:14f74b704963	1627	else
BPPearson	0:14f74b704963	1628	{
BPPearson	0:14f74b704963	1629	/* if the end of the packet */
BPPearson	0:14f74b704963	1630	if (c == CR)
BPPearson	0:14f74b704963	1631	{
BPPearson	0:14f74b704963	1632	/* reset char pointer */
BPPearson	0:14f74b704963	1633	commsCntr0 = 0;
BPPearson	0:14f74b704963	1634
BPPearson	0:14f74b704963	1635	/* turn receive led off */
BPPearson	0:14f74b704963	1636	rxLed = 0;
BPPearson	0:14f74b704963	1637	}
BPPearson	0:14f74b704963	1638	}
BPPearson	0:14f74b704963	1639
BPPearson	0:14f74b704963	1640	/* reset receive data register full flag */
BPPearson	0:14f74b704963	1641	//SSR0 &= ~RDRF;
BPPearson	0:14f74b704963	1642	}
BPPearson	0:14f74b704963	1643
BPPearson	0:14f74b704963	1644
BPPearson	0:14f74b704963	1645
BPPearson	0:14f74b704963	1646	void serial_rcv_int1()
BPPearson	0:14f74b704963	1647	{
BPPearson	0:14f74b704963	1648	uchar c, loop;
BPPearson	0:14f74b704963	1649
BPPearson	0:14f74b704963	1650	/* if not processing a packet */
BPPearson	0:14f74b704963	1651	if (cascadeCrRcvd == FALSE)
BPPearson	0:14f74b704963	1652	{
BPPearson	0:14f74b704963	1653	/* get char into local */
BPPearson	0:14f74b704963	1654	c = cascade.getc();
BPPearson	0:14f74b704963	1655
BPPearson	0:14f74b704963	1656	/* if space in buffer */
BPPearson	0:14f74b704963	1657	if (commsCntr1 < (commsBufLen - 1))
BPPearson	0:14f74b704963	1658	{
BPPearson	0:14f74b704963	1659	/* put char in buffer and increment pointer */
BPPearson	0:14f74b704963	1660	commsBuf1[commsCntr1++] = c;
BPPearson	0:14f74b704963	1661	}
BPPearson	0:14f74b704963	1662
BPPearson	0:14f74b704963	1663	/* if char is end of packet */
BPPearson	0:14f74b704963	1664	if (c == CR)
BPPearson	0:14f74b704963	1665	{
BPPearson	0:14f74b704963	1666	for (loop = commsCntr1; loop<commsBufLen; loop++)
BPPearson	0:14f74b704963	1667	{
BPPearson	0:14f74b704963	1668	commsBuf1[loop] = 0;
BPPearson	0:14f74b704963	1669	}
BPPearson	0:14f74b704963	1670
BPPearson	0:14f74b704963	1671	/* reset char pointer */
BPPearson	0:14f74b704963	1672	commsCntr1 = 0;
BPPearson	0:14f74b704963	1673
BPPearson	0:14f74b704963	1674	/* set flag to process packet */
BPPearson	0:14f74b704963	1675	cascadeCrRcvd = TRUE;
BPPearson	0:14f74b704963	1676	}
BPPearson	0:14f74b704963	1677
BPPearson	0:14f74b704963	1678	}
BPPearson	0:14f74b704963	1679	else
BPPearson	0:14f74b704963	1680	{
BPPearson	0:14f74b704963	1681
BPPearson	0:14f74b704963	1682	/* if the end of the packet */
BPPearson	0:14f74b704963	1683	if (c == CR)
BPPearson	0:14f74b704963	1684	{
BPPearson	0:14f74b704963	1685
BPPearson	0:14f74b704963	1686	/* reset char pointer */
BPPearson	0:14f74b704963	1687	commsCntr1 = 0;
BPPearson	0:14f74b704963	1688	}
BPPearson	0:14f74b704963	1689	}
BPPearson	0:14f74b704963	1690	}
BPPearson	0:14f74b704963	1691
BPPearson	0:14f74b704963	1692
BPPearson	0:14f74b704963	1693
BPPearson	0:14f74b704963	1694	void repetitiveControlUpdate()
BPPearson	0:14f74b704963	1695	{
BPPearson	0:14f74b704963	1696
BPPearson	0:14f74b704963	1697	//scope1 = 1;
BPPearson	0:14f74b704963	1698
BPPearson	0:14f74b704963	1699	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	1700	{
BPPearson	0:14f74b704963	1701	/* if time to transmit cascade speed */
BPPearson	0:14f74b704963	1702	if (xmitCascadeSpeed[i])
BPPearson	0:14f74b704963	1703	{
BPPearson	0:14f74b704963	1704	// if this is the last controller on this card
BPPearson	0:14f74b704963	1705	if (i == (activeControllers - 1))
BPPearson	0:14f74b704963	1706	/* send current speed setpoint by comms to next card */
BPPearson	0:14f74b704963	1707	xmit_cascade_speed(i);
BPPearson	0:14f74b704963	1708	else
BPPearson	0:14f74b704963	1709	// save the value to internal registers to be used for all drives controlled by this card except the last one
BPPearson	0:14f74b704963	1710	speedOfPrev[i + 1] = speedSetpnt[i];
BPPearson	0:14f74b704963	1711
BPPearson	0:14f74b704963	1712	/* clear flag */
BPPearson	0:14f74b704963	1713	xmitCascadeSpeed[i] = FALSE;
BPPearson	0:14f74b704963	1714	}
BPPearson	0:14f74b704963	1715
BPPearson	0:14f74b704963	1716	/* recalculate fixed part of speed calculation */
BPPearson	0:14f74b704963	1717	if (recalcFixedPart[i])
BPPearson	0:14f74b704963	1718	{
BPPearson	0:14f74b704963	1719	/* calculate fixed part of speed calc */
BPPearson	0:14f74b704963	1720	calcFixedPart(i);
BPPearson	0:14f74b704963	1721
BPPearson	0:14f74b704963	1722	/* clear flag */
BPPearson	0:14f74b704963	1723	recalcFixedPart[i] = FALSE;
BPPearson	0:14f74b704963	1724	}
BPPearson	0:14f74b704963	1725
BPPearson	0:14f74b704963	1726	/* recalculate speed setpoint */
BPPearson	0:14f74b704963	1727	if (recalcSpeedSetpoint[i])
BPPearson	0:14f74b704963	1728	{
BPPearson	0:14f74b704963	1729	/* only recalculate if in ratio mode */
BPPearson	0:14f74b704963	1730	if (controlMode[i] == ratioMode)
BPPearson	0:14f74b704963	1731	{
BPPearson	0:14f74b704963	1732	/* calculate new speed setpoint */
BPPearson	0:14f74b704963	1733	speedSetpnt[i] = speedOfPrev[i] * ratioToPrev[i];
BPPearson	0:14f74b704963	1734	}
BPPearson	0:14f74b704963	1735
BPPearson	0:14f74b704963	1736	/* set flag to send new speed setpoint on to next drive */
BPPearson	0:14f74b704963	1737	xmitCascadeSpeed[i] = TRUE;
BPPearson	0:14f74b704963	1738
BPPearson	0:14f74b704963	1739	/* force recalc of required count */
BPPearson	0:14f74b704963	1740	recalcReqdCount[i] = TRUE;
BPPearson	0:14f74b704963	1741
BPPearson	0:14f74b704963	1742	/* clear flag */
BPPearson	0:14f74b704963	1743	recalcSpeedSetpoint[i] = FALSE;
BPPearson	0:14f74b704963	1744	}
BPPearson	0:14f74b704963	1745
BPPearson	0:14f74b704963	1746	/* if speed or ratio changed */
BPPearson	0:14f74b704963	1747	if (recalcReqdCount[i])
BPPearson	0:14f74b704963	1748	{
BPPearson	0:14f74b704963	1749	/* calculate count using current speed setpoint */
BPPearson	0:14f74b704963	1750	/* check for zero speed setpoint */
BPPearson	0:14f74b704963	1751	if (speedSetpnt[i] > 0.0)
BPPearson	0:14f74b704963	1752	{
BPPearson	0:14f74b704963	1753	/* calculate required count */
BPPearson	0:14f74b704963	1754	countSetpoint[i] = (unsigned long)(pulsesPerMetrePerSec[i] / speedSetpnt[i]);
BPPearson	0:14f74b704963	1755	}
BPPearson	0:14f74b704963	1756
BPPearson	0:14f74b704963	1757	/* calculate 12% control bands */
BPPearson	0:14f74b704963	1758	hiCountLimit[i] = countSetpoint[i] + (countSetpoint[i] / 8);
BPPearson	0:14f74b704963	1759	loCountLimit[i] = countSetpoint[i] - (countSetpoint[i] / 8);
BPPearson	0:14f74b704963	1760
BPPearson	0:14f74b704963	1761	/* clear flag */
BPPearson	0:14f74b704963	1762	recalcReqdCount[i] = FALSE;
BPPearson	0:14f74b704963	1763	}
BPPearson	0:14f74b704963	1764
BPPearson	0:14f74b704963	1765	/* if new reading from free running timer */
BPPearson	0:14f74b704963	1766	if (processFrtReading[i])
BPPearson	0:14f74b704963	1767	{
BPPearson	0:14f74b704963	1768	/* if in speed control or ratio mode */
BPPearson	0:14f74b704963	1769	switch (controlMode[i])
BPPearson	0:14f74b704963	1770	{
BPPearson	0:14f74b704963	1771	case speedMode:
BPPearson	0:14f74b704963	1772	case ratioMode:
BPPearson	0:14f74b704963	1773	case thirdNipMode:
BPPearson	0:14f74b704963	1774	/* calculate new output */
BPPearson	0:14f74b704963	1775	process_latest_count(i);
BPPearson	0:14f74b704963	1776	break;
BPPearson	0:14f74b704963	1777	case outputMode:
BPPearson	0:14f74b704963	1778	break;
BPPearson	0:14f74b704963	1779	default: ;
BPPearson	0:14f74b704963	1780	}
BPPearson	0:14f74b704963	1781
BPPearson	0:14f74b704963	1782	/* clear flag */
BPPearson	0:14f74b704963	1783	processFrtReading[i] = FALSE;
BPPearson	0:14f74b704963	1784	}
BPPearson	0:14f74b704963	1785
BPPearson	0:14f74b704963	1786	/* if no pulses from toothed wheel in last 3 seconds */
BPPearson	0:14f74b704963	1787	if (noPulseCntr[i] > 6)
BPPearson	0:14f74b704963	1788	{
BPPearson	0:14f74b704963	1789	/* if in speed control or ratio mode */
BPPearson	0:14f74b704963	1790	switch (controlMode[i])
BPPearson	0:14f74b704963	1791	{
BPPearson	0:14f74b704963	1792	case speedMode:
BPPearson	0:14f74b704963	1793	case ratioMode:
BPPearson	0:14f74b704963	1794	/* set default output */
BPPearson	0:14f74b704963	1795	dtoaSetpnt[i] = (int)(2047.0 * speedSetpnt[i] / maxSpeedOfDrive[i]);
BPPearson	0:14f74b704963	1796
BPPearson	0:14f74b704963	1797	if ( dtoaSetpnt[i] > 2047 )
BPPearson	0:14f74b704963	1798	dtoaSetpnt[i] = 2047;
BPPearson	0:14f74b704963	1799
BPPearson	0:14f74b704963	1800	/* do crude bumpless transfer */
BPPearson	0:14f74b704963	1801	intSum[i] = (float)dtoaSetpnt[i] / intGain[i];
BPPearson	0:14f74b704963	1802
BPPearson	0:14f74b704963	1803	/* set flag to update analog output */
BPPearson	0:14f74b704963	1804	updateAnalogOutput[i] = TRUE;
BPPearson	0:14f74b704963	1805	break;
BPPearson	0:14f74b704963	1806	case thirdNipMode:
BPPearson	0:14f74b704963	1807	case outputMode:
BPPearson	0:14f74b704963	1808	break;
BPPearson	0:14f74b704963	1809	default: ;
BPPearson	0:14f74b704963	1810	}
BPPearson	0:14f74b704963	1811
BPPearson	0:14f74b704963	1812	/* set flag to indicate starting from cold */
BPPearson	0:14f74b704963	1813	inStartupMode[i] = TRUE;
BPPearson	0:14f74b704963	1814
BPPearson	0:14f74b704963	1815	/* clear within band flag */
BPPearson	0:14f74b704963	1816	withinControlBand[i] = FALSE;
BPPearson	0:14f74b704963	1817
BPPearson	0:14f74b704963	1818	/* zero startup counter */
BPPearson	0:14f74b704963	1819	startupCntr[i] = 0;
BPPearson	0:14f74b704963	1820
BPPearson	0:14f74b704963	1821	/* clear flag */
BPPearson	0:14f74b704963	1822	noPulseCntr[i] = 2;
BPPearson	0:14f74b704963	1823	}
BPPearson	0:14f74b704963	1824
BPPearson	0:14f74b704963	1825
BPPearson	0:14f74b704963	1826	/* if new analog output value flag set */
BPPearson	0:14f74b704963	1827	if (updateAnalogOutput[i])
BPPearson	0:14f74b704963	1828	{
BPPearson	0:14f74b704963	1829	/* write binary value to d/a chip */
BPPearson	0:14f74b704963	1830	updateDtoaChip(i);
BPPearson	0:14f74b704963	1831
BPPearson	0:14f74b704963	1832	/* clear flag */
BPPearson	0:14f74b704963	1833	updateAnalogOutput[i] = FALSE;
BPPearson	0:14f74b704963	1834	}
BPPearson	0:14f74b704963	1835	}
BPPearson	0:14f74b704963	1836
BPPearson	0:14f74b704963	1837	/* process host comms */
BPPearson	0:14f74b704963	1838	if (hostCrRcvd)
BPPearson	0:14f74b704963	1839	{
BPPearson	0:14f74b704963	1840	/* process packet received from host */
BPPearson	0:14f74b704963	1841	processHostComms();
BPPearson	0:14f74b704963	1842
BPPearson	0:14f74b704963	1843	/* reset flag */
BPPearson	0:14f74b704963	1844	hostCrRcvd = FALSE;
BPPearson	0:14f74b704963	1845
BPPearson	0:14f74b704963	1846	/* reset input pointer */
BPPearson	0:14f74b704963	1847	commsCntr0 = 0;
BPPearson	0:14f74b704963	1848	}
BPPearson	0:14f74b704963	1849
BPPearson	0:14f74b704963	1850	/* process cascade comms */
BPPearson	0:14f74b704963	1851	if (cascadeCrRcvd)
BPPearson	0:14f74b704963	1852	{
BPPearson	0:14f74b704963	1853	/* process the packet received through cascade port */
BPPearson	0:14f74b704963	1854	processCascadeComms();
BPPearson	0:14f74b704963	1855
BPPearson	0:14f74b704963	1856	/* clear flag */
BPPearson	0:14f74b704963	1857	cascadeCrRcvd = FALSE;
BPPearson	0:14f74b704963	1858	}
BPPearson	0:14f74b704963	1859
BPPearson	0:14f74b704963	1860	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	1861	{
BPPearson	0:14f74b704963	1862	/* if in startup mode and startup counter reached limit (pulses from wheel) */
BPPearson	0:14f74b704963	1863	if (inStartupMode[i] && startupCntr[i] > startupCount[boardAddress + i])
BPPearson	0:14f74b704963	1864	{
BPPearson	0:14f74b704963	1865	/* drive should have started by now so clear flag */
BPPearson	0:14f74b704963	1866	inStartupMode[i] = FALSE;
BPPearson	0:14f74b704963	1867	}
BPPearson	0:14f74b704963	1868	}
BPPearson	0:14f74b704963	1869
BPPearson	0:14f74b704963	1870	//scope1 = 0;
BPPearson	0:14f74b704963	1871	}
BPPearson	0:14f74b704963	1872
BPPearson	0:14f74b704963	1873
BPPearson	0:14f74b704963	1874
BPPearson	0:14f74b704963	1875	void repetitiveUpdateTimer()
BPPearson	0:14f74b704963	1876
BPPearson	0:14f74b704963	1877	{
BPPearson	0:14f74b704963	1878
BPPearson	0:14f74b704963	1879	/* called every ms */
BPPearson	0:14f74b704963	1880	/* increment counter */
BPPearson	0:14f74b704963	1881	timer0Cntr++;
BPPearson	0:14f74b704963	1882
BPPearson	0:14f74b704963	1883	/* if host comms xmit driver enabled and waiting for timeout */
BPPearson	0:14f74b704963	1884	if (disableHostComms > 0)
BPPearson	0:14f74b704963	1885	{
BPPearson	0:14f74b704963	1886	/* decrement timer */
BPPearson	0:14f74b704963	1887	disableHostComms--;
BPPearson	0:14f74b704963	1888
BPPearson	0:14f74b704963	1889	/* if timer is zero */
BPPearson	0:14f74b704963	1890	if (disableHostComms == 0)
BPPearson	0:14f74b704963	1891	{
BPPearson	0:14f74b704963	1892	/* disable the driver */
BPPearson	0:14f74b704963	1893	disable_host_comms();
BPPearson	0:14f74b704963	1894	}
BPPearson	0:14f74b704963	1895	}
BPPearson	0:14f74b704963	1896
BPPearson	0:14f74b704963	1897	/* check for 8th pulse (8 * 1.666 ms = every 13 msec) */
BPPearson	0:14f74b704963	1898	if ((timer0Cntr & 0x0F) == 0x08)
BPPearson	0:14f74b704963	1899	{
BPPearson	0:14f74b704963	1900	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	1901	{
BPPearson	0:14f74b704963	1902	/* set flag to force update of analog output */
BPPearson	0:14f74b704963	1903	updateAnalogOutput[i] = TRUE;
BPPearson	0:14f74b704963	1904	}
BPPearson	0:14f74b704963	1905	}
BPPearson	0:14f74b704963	1906
BPPearson	0:14f74b704963	1907	/* check for 64th pulse (64 * 1.666 ms = every 0.106 sec) */
BPPearson	0:14f74b704963	1908	if ((timer0Cntr & 0x7F) == 0x40)
BPPearson	0:14f74b704963	1909	{
BPPearson	0:14f74b704963	1910	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	1911	/* set flag to resend speed setpoint */
BPPearson	0:14f74b704963	1912	xmitCascadeSpeed[i] = 1;
BPPearson	0:14f74b704963	1913	}
BPPearson	0:14f74b704963	1914
BPPearson	0:14f74b704963	1915	/* check for 256th pulse (256 * 1.666 ms = every 0.426 sec) */
BPPearson	0:14f74b704963	1916	if ((timer0Cntr & 0x1FF) == 0x100)
BPPearson	0:14f74b704963	1917	{
BPPearson	0:14f74b704963	1918	for (int i=0; i<activeControllers; i++)
BPPearson	0:14f74b704963	1919	{
BPPearson	0:14f74b704963	1920	/* increment no pulse counter */
BPPearson	0:14f74b704963	1921	noPulseCntr[i]++;
BPPearson	0:14f74b704963	1922	}
BPPearson	0:14f74b704963	1923	}
BPPearson	0:14f74b704963	1924
BPPearson	0:14f74b704963	1925	/* check for 512th pulse (512 * 1.666 ms = every 0.852 sec) */
BPPearson	0:14f74b704963	1926	if ((timer0Cntr & 0x3FF) == 0x200)
BPPearson	0:14f74b704963	1927	{
BPPearson	0:14f74b704963	1928	/* increment age since reset */
BPPearson	0:14f74b704963	1929	ageInSecs++;
BPPearson	0:14f74b704963	1930	}
BPPearson	0:14f74b704963	1931	}
BPPearson	0:14f74b704963	1932
BPPearson	0:14f74b704963	1933
BPPearson	0:14f74b704963	1934
BPPearson	0:14f74b704963	1935	void updateDtoaChip(int idx)
BPPearson	0:14f74b704963	1936	{
BPPearson	0:14f74b704963	1937	int d2aCmd;
BPPearson	0:14f74b704963	1938
BPPearson	0:14f74b704963	1939	// build command with the following format: aaccddddddddddss
BPPearson	0:14f74b704963	1940	// where aa = output address
BPPearson	0:14f74b704963	1941	// cc = 3 - update all outputs when value loaded into buffer register
BPPearson	0:14f74b704963	1942	// dddddddddd - 10 bit output value
BPPearson	0:14f74b704963	1943	// ss = 0 - not used but must be zero
BPPearson	0:14f74b704963	1944	d2aCmd = ((idx & 3) << 14) \| (3 << 12) \| ((dtoaSetpnt[idx] & 0x7fe) << 1);
BPPearson	0:14f74b704963	1945
BPPearson	0:14f74b704963	1946	//select correct d/a chip
BPPearson	0:14f74b704963	1947	if (idx > 3)
BPPearson	0:14f74b704963	1948	d2aEnable2 = 0;
BPPearson	0:14f74b704963	1949	else
BPPearson	0:14f74b704963	1950	d2aEnable1 = 0;
BPPearson	0:14f74b704963	1951
BPPearson	0:14f74b704963	1952	// write the command to the d/a chip over the serial peripheral interface
BPPearson	0:14f74b704963	1953	spi.write(d2aCmd);
BPPearson	0:14f74b704963	1954
BPPearson	0:14f74b704963	1955	//deselect correct d/a chip
BPPearson	0:14f74b704963	1956	if (idx > 3)
BPPearson	0:14f74b704963	1957	d2aEnable2 = 1;
BPPearson	0:14f74b704963	1958	else
BPPearson	0:14f74b704963	1959	d2aEnable1 = 1;
BPPearson	0:14f74b704963	1960	}
BPPearson	0:14f74b704963	1961
BPPearson	0:14f74b704963	1962
BPPearson	0:14f74b704963	1963
BPPearson	0:14f74b704963	1964	void xmit_cascade_speed(int idx)
BPPearson	0:14f74b704963	1965	{
BPPearson	0:14f74b704963	1966	char str[20];
BPPearson	0:14f74b704963	1967	char chksum;
BPPearson	0:14f74b704963	1968	char *pstr;
BPPearson	0:14f74b704963	1969	int intsp;
BPPearson	0:14f74b704963	1970
BPPearson	0:14f74b704963	1971	intsp = (int)(speedSetpnt[idx] * 10.0);
BPPearson	0:14f74b704963	1972	if (((speedSetpnt[idx] * 10.0) - intsp) >= 0.5 )
BPPearson	0:14f74b704963	1973	{
BPPearson	0:14f74b704963	1974	intsp = intsp + 1;
BPPearson	0:14f74b704963	1975	}
BPPearson	0:14f74b704963	1976
BPPearson	0:14f74b704963	1977	/* build up packet of current speed setpoint */
BPPearson	0:14f74b704963	1978	sprintf( str, "*%04d", (int)(intsp) );
BPPearson	0:14f74b704963	1979
BPPearson	0:14f74b704963	1980	/* init pointer to string */
BPPearson	0:14f74b704963	1981	pstr = str;
BPPearson	0:14f74b704963	1982
BPPearson	0:14f74b704963	1983	/* clear checksum accumulator */
BPPearson	0:14f74b704963	1984	chksum = 0;
BPPearson	0:14f74b704963	1985
BPPearson	0:14f74b704963	1986	/* checksum string */
BPPearson	0:14f74b704963	1987	while (*pstr)
BPPearson	0:14f74b704963	1988	{
BPPearson	0:14f74b704963	1989	chksum ^= *pstr++;
BPPearson	0:14f74b704963	1990	}
BPPearson	0:14f74b704963	1991
BPPearson	0:14f74b704963	1992	/* send the current speed setpoint */
BPPearson	0:14f74b704963	1993	putstr1( str );
BPPearson	0:14f74b704963	1994
BPPearson	0:14f74b704963	1995	/* build checksum part of string */
BPPearson	0:14f74b704963	1996	sprintf( str, "%02X\x0d", chksum );
BPPearson	0:14f74b704963	1997
BPPearson	0:14f74b704963	1998	/* send the checksum */
BPPearson	0:14f74b704963	1999	putstr1( str );
BPPearson	0:14f74b704963	2000	}
BPPearson	0:14f74b704963	2001

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	05 Jan 2016
Imports:	5
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@0:14f74b704963, 2016-01-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning