LabVIEW_PID - Simple PID example for LabVIEW

Users » simon » Code » LabVIEW_PID

Simon Ford / Mbed 2 deprecated LabVIEW_PID

Simple PID example for LabVIEW

QEI.cpp@1:ddfe18427154, 2010-08-03 (annotated)

Committer:: simon
Date:: Tue Aug 03 15:32:30 2010 +0000
Revision:: 1:ddfe18427154
Parent:: 0:e3b759ab4b5c

Who changed what in which revision?

User	Revision	Line number	New contents of line
simon	0:e3b759ab4b5c	1	//****************************************************************************/
simon	0:e3b759ab4b5c	2	//@section LICENSE
simon	0:e3b759ab4b5c	3	//
simon	0:e3b759ab4b5c	4	//Copyright (c) 2010 ARM Limited
simon	0:e3b759ab4b5c	5	//
simon	0:e3b759ab4b5c	6	//Permission is hereby granted, free of charge, to any person obtaining a copy
simon	0:e3b759ab4b5c	7	//of this software and associated documentation files (the "Software"), to deal
simon	0:e3b759ab4b5c	8	//in the Software without restriction, including without limitation the rights
simon	0:e3b759ab4b5c	9	//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
simon	0:e3b759ab4b5c	10	//copies of the Software, and to permit persons to whom the Software is
simon	0:e3b759ab4b5c	11	//furnished to do so, subject to the following conditions:
simon	0:e3b759ab4b5c	12	//
simon	0:e3b759ab4b5c	13	//The above copyright notice and this permission notice shall be included in
simon	0:e3b759ab4b5c	14	//all copies or substantial portions of the Software.
simon	0:e3b759ab4b5c	15	//
simon	0:e3b759ab4b5c	16	//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
simon	0:e3b759ab4b5c	17	//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
simon	0:e3b759ab4b5c	18	//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
simon	0:e3b759ab4b5c	19	//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
simon	0:e3b759ab4b5c	20	//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
simon	0:e3b759ab4b5c	21	//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
simon	0:e3b759ab4b5c	22	//THE SOFTWARE.
simon	0:e3b759ab4b5c	23	//****************************************************************************/
simon	0:e3b759ab4b5c	24	//@section DESCRIPTION
simon	0:e3b759ab4b5c	25	//
simon	0:e3b759ab4b5c	26	// Quadrature Encoder Interface.
simon	0:e3b759ab4b5c	27	//
simon	0:e3b759ab4b5c	28	// A quadrature encoder consists of two code tracks on a disc which are 90
simon	0:e3b759ab4b5c	29	// degrees out of phase. It can be used to determine how far a wheel has
simon	0:e3b759ab4b5c	30	// rotated, relative to a known starting position.
simon	0:e3b759ab4b5c	31	//
simon	0:e3b759ab4b5c	32	// Only one code track changes at a time leading to a more robust system than
simon	0:e3b759ab4b5c	33	// a single track, because any jitter around any edge won't cause a state
simon	0:e3b759ab4b5c	34	// change as the other track will remain constant.
simon	0:e3b759ab4b5c	35	//
simon	0:e3b759ab4b5c	36	// Encoders can be a homebrew affair, consisting of infrared emitters/receivers
simon	0:e3b759ab4b5c	37	// and paper code tracks consisting of alternating black and white sections;
simon	0:e3b759ab4b5c	38	// alternatively, complete disk and PCB emitter/receiver encoder systems can
simon	0:e3b759ab4b5c	39	// be bought, but the interface, regardless of implementation is the same.
simon	0:e3b759ab4b5c	40	//
simon	0:e3b759ab4b5c	41	// +-----+ +-----+ +-----+
simon	0:e3b759ab4b5c	42	// Channel A \| ^ \| \| \| \| \|
simon	0:e3b759ab4b5c	43	// ---+ ^ +-----+ +-----+ +-----
simon	0:e3b759ab4b5c	44	// ^ ^
simon	0:e3b759ab4b5c	45	// ^ +-----+ +-----+ +-----+
simon	0:e3b759ab4b5c	46	// Channel B ^ \| \| \| \| \| \|
simon	0:e3b759ab4b5c	47	// ------+ +-----+ +-----+ +-----
simon	0:e3b759ab4b5c	48	// ^ ^
simon	0:e3b759ab4b5c	49	// ^ ^
simon	0:e3b759ab4b5c	50	// 90deg
simon	0:e3b759ab4b5c	51	//
simon	0:e3b759ab4b5c	52	// This interface uses X4 encoding which calculates the pulse count based on
simon	0:e3b759ab4b5c	53	// reading the current state after each rising and falling edge of either
simon	0:e3b759ab4b5c	54	// channel.
simon	0:e3b759ab4b5c	55	//
simon	0:e3b759ab4b5c	56	// +-----+ +-----+ +-----+
simon	0:e3b759ab4b5c	57	// Channel A \| \| \| \| \| \|
simon	0:e3b759ab4b5c	58	// ---+ +-----+ +-----+ +-----
simon	0:e3b759ab4b5c	59	// ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	60	// ^ +-----+ ^ +-----+ ^ +-----+
simon	0:e3b759ab4b5c	61	// Channel B ^ \| ^ \| ^ \| ^ \| ^ \| \|
simon	0:e3b759ab4b5c	62	// ------+ ^ +-----+ ^ +-----+ +--
simon	0:e3b759ab4b5c	63	// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	64	// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	65	// Pulse count 0 1 2 3 4 5 6 7 8 9 ...
simon	0:e3b759ab4b5c	66	//
simon	0:e3b759ab4b5c	67	// An optional index channel can be used which determines when a full
simon	0:e3b759ab4b5c	68	// revolution has occured.
simon	0:e3b759ab4b5c	69	//
simon	0:e3b759ab4b5c	70	// If a 4 pules per revolution encoder was used, the following would be
simon	0:e3b759ab4b5c	71	// observed.
simon	0:e3b759ab4b5c	72	//
simon	0:e3b759ab4b5c	73	// +-----+ +-----+ +-----+
simon	0:e3b759ab4b5c	74	// Channel A \| \| \| \| \| \|
simon	0:e3b759ab4b5c	75	// ---+ +-----+ +-----+ +-----
simon	0:e3b759ab4b5c	76	// ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	77	// ^ +-----+ ^ +-----+ ^ +-----+
simon	0:e3b759ab4b5c	78	// Channel B ^ \| ^ \| ^ \| ^ \| ^ \| \|
simon	0:e3b759ab4b5c	79	// ------+ ^ +-----+ ^ +-----+ +--
simon	0:e3b759ab4b5c	80	// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	81	// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	82	// ^ ^ ^ +--+ ^ ^ +--+ ^
simon	0:e3b759ab4b5c	83	// ^ ^ ^ \| \| ^ ^ \| \| ^
simon	0:e3b759ab4b5c	84	// Index ------------+ +--------+ +-----------
simon	0:e3b759ab4b5c	85	// ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
simon	0:e3b759ab4b5c	86	// Pulse count 0 1 2 3 4 5 6 7 8 9 ...
simon	0:e3b759ab4b5c	87	// Rev. count 0 1 2
simon	0:e3b759ab4b5c	88	//
simon	0:e3b759ab4b5c	89	// Rotational position in degrees can be calculated by:
simon	0:e3b759ab4b5c	90	//
simon	0:e3b759ab4b5c	91	// (pulse count / X * N) * 360
simon	0:e3b759ab4b5c	92	//
simon	0:e3b759ab4b5c	93	// Where X is the encoding type [in our case X=4], and N is the number of
simon	0:e3b759ab4b5c	94	// pulses per revolution.
simon	0:e3b759ab4b5c	95	//
simon	0:e3b759ab4b5c	96	// Linear position can be calculated by:
simon	0:e3b759ab4b5c	97	//
simon	0:e3b759ab4b5c	98	// (pulse count / X * N) * (1 / PPI)
simon	0:e3b759ab4b5c	99	//
simon	0:e3b759ab4b5c	100	// Where X is encoding type [in our case X=4], N is the number of pulses per
simon	0:e3b759ab4b5c	101	// revolution, and PPI is pulses per inch, or the equivalent for any other
simon	0:e3b759ab4b5c	102	// unit of displacement. PPI can be calculated by taking the circumference
simon	0:e3b759ab4b5c	103	// of the wheel or encoder disk and dividing it by the number of pulses per
simon	0:e3b759ab4b5c	104	// revolution.
simon	0:e3b759ab4b5c	105	//****************************************************************************/
simon	0:e3b759ab4b5c	106
simon	0:e3b759ab4b5c	107	//****************************************************************************/
simon	0:e3b759ab4b5c	108	// Includes
simon	0:e3b759ab4b5c	109	//****************************************************************************/
simon	0:e3b759ab4b5c	110	#include "QEI.h"
simon	0:e3b759ab4b5c	111
simon	0:e3b759ab4b5c	112	QEI::QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev) {
simon	0:e3b759ab4b5c	113
simon	0:e3b759ab4b5c	114	channelA_ = new InterruptIn(channelA);
simon	0:e3b759ab4b5c	115	channelB_ = new InterruptIn(channelB);
simon	0:e3b759ab4b5c	116	index_ = new InterruptIn(index);
simon	0:e3b759ab4b5c	117
simon	0:e3b759ab4b5c	118	pulses_ = 0;
simon	0:e3b759ab4b5c	119	revolutions_ = 0;
simon	0:e3b759ab4b5c	120	pulsesPerRev_ = pulsesPerRev;
simon	0:e3b759ab4b5c	121
simon	0:e3b759ab4b5c	122	//Workout what the current state is.
simon	0:e3b759ab4b5c	123	int chanA = channelA_->read();
simon	0:e3b759ab4b5c	124	int chanB = channelB_->read();
simon	0:e3b759ab4b5c	125
simon	0:e3b759ab4b5c	126	//2-bit state.
simon	0:e3b759ab4b5c	127	currState_ = (chanA << 1) \| (chanB);
simon	0:e3b759ab4b5c	128	prevState_ = currState_;
simon	0:e3b759ab4b5c	129
simon	0:e3b759ab4b5c	130	channelA_->rise(this, &QEI::encode);
simon	0:e3b759ab4b5c	131	channelA_->fall(this, &QEI::encode);
simon	0:e3b759ab4b5c	132	channelB_->rise(this, &QEI::encode);
simon	0:e3b759ab4b5c	133	channelB_->fall(this, &QEI::encode);
simon	0:e3b759ab4b5c	134	//Index is optional.
simon	0:e3b759ab4b5c	135	if (index != NC) {
simon	0:e3b759ab4b5c	136	index_->rise(this, &QEI::index);
simon	0:e3b759ab4b5c	137	}
simon	0:e3b759ab4b5c	138
simon	0:e3b759ab4b5c	139	}
simon	0:e3b759ab4b5c	140
simon	0:e3b759ab4b5c	141	void QEI::reset(void) {
simon	0:e3b759ab4b5c	142
simon	0:e3b759ab4b5c	143	pulses_ = 0;
simon	0:e3b759ab4b5c	144	revolutions_ = 0;
simon	0:e3b759ab4b5c	145
simon	0:e3b759ab4b5c	146	}
simon	0:e3b759ab4b5c	147
simon	0:e3b759ab4b5c	148	int QEI::getCurrentState(void) {
simon	0:e3b759ab4b5c	149
simon	0:e3b759ab4b5c	150	return currState_;
simon	0:e3b759ab4b5c	151
simon	0:e3b759ab4b5c	152	}
simon	0:e3b759ab4b5c	153
simon	0:e3b759ab4b5c	154	int QEI::getPulses(void) {
simon	0:e3b759ab4b5c	155
simon	0:e3b759ab4b5c	156	return pulses_;
simon	0:e3b759ab4b5c	157
simon	0:e3b759ab4b5c	158	}
simon	0:e3b759ab4b5c	159
simon	0:e3b759ab4b5c	160	// There are four possible states for a quadrature encoder which correspond to
simon	0:e3b759ab4b5c	161	// 2-bit gray code.
simon	0:e3b759ab4b5c	162	//
simon	0:e3b759ab4b5c	163	// A state change is only valid if of only one bit has changed.
simon	0:e3b759ab4b5c	164	// A state change is invalid if both bits have changed.
simon	0:e3b759ab4b5c	165	//
simon	0:e3b759ab4b5c	166	// Clockwise Rotation ->
simon	0:e3b759ab4b5c	167	//
simon	0:e3b759ab4b5c	168	// 00 01 11 10 00
simon	0:e3b759ab4b5c	169	//
simon	0:e3b759ab4b5c	170	// <- Counter Clockwise Rotation
simon	0:e3b759ab4b5c	171	//
simon	0:e3b759ab4b5c	172	// If we observe any valid state changes going from left to right, we have
simon	0:e3b759ab4b5c	173	// moved one pulse clockwise [we will consider this "backward" or "negative"].
simon	0:e3b759ab4b5c	174	//
simon	0:e3b759ab4b5c	175	// If we observe any valid state changes going from right to left we have
simon	0:e3b759ab4b5c	176	// moved one pulse counter clockwise [we will consider this "forward" or
simon	0:e3b759ab4b5c	177	// "positive"].
simon	0:e3b759ab4b5c	178	//
simon	0:e3b759ab4b5c	179	// We might enter an invalid state for a number of reasons which are hard to
simon	0:e3b759ab4b5c	180	// predict - if this is the case, it is generally safe to ignore it, update
simon	0:e3b759ab4b5c	181	// the state and carry on, with the error correcting itself shortly after.
simon	0:e3b759ab4b5c	182	void QEI::encode(void) {
simon	0:e3b759ab4b5c	183
simon	0:e3b759ab4b5c	184	int change = 0;
simon	0:e3b759ab4b5c	185	int chanA = channelA_->read();
simon	0:e3b759ab4b5c	186	int chanB = channelB_->read();
simon	0:e3b759ab4b5c	187
simon	0:e3b759ab4b5c	188	//2-bit state.
simon	0:e3b759ab4b5c	189	currState_ = (chanA << 1) \| (chanB);
simon	0:e3b759ab4b5c	190
simon	0:e3b759ab4b5c	191	//Entered an invalid state, or no change.
simon	0:e3b759ab4b5c	192	if ((currState_ ^ prevState_) == INVALID \|\| currState_ == prevState_) {
simon	0:e3b759ab4b5c	193	//Even if the state was invalid, it will eventually
simon	0:e3b759ab4b5c	194	//correct itself if we simply update the state.
simon	0:e3b759ab4b5c	195	prevState_ = currState_;
simon	0:e3b759ab4b5c	196	}
simon	0:e3b759ab4b5c	197	//Entered a valid state.
simon	0:e3b759ab4b5c	198	else {
simon	0:e3b759ab4b5c	199	//2 bit state. Right hand bit of prev XOR left hand bit of current
simon	0:e3b759ab4b5c	200	//gives 0 if clockwise rotation and 1 if counter clockwise rotation.
simon	0:e3b759ab4b5c	201	change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
simon	0:e3b759ab4b5c	202
simon	0:e3b759ab4b5c	203	if (change == 0) {
simon	0:e3b759ab4b5c	204	change = -1;
simon	0:e3b759ab4b5c	205	}
simon	0:e3b759ab4b5c	206
simon	0:e3b759ab4b5c	207	pulses_ -= change;
simon	0:e3b759ab4b5c	208	prevState_ = currState_;
simon	0:e3b759ab4b5c	209	}
simon	0:e3b759ab4b5c	210
simon	0:e3b759ab4b5c	211	}
simon	0:e3b759ab4b5c	212
simon	0:e3b759ab4b5c	213	void QEI::index(void) {
simon	0:e3b759ab4b5c	214
simon	0:e3b759ab4b5c	215	revolutions_++;
simon	0:e3b759ab4b5c	216
simon	0:e3b759ab4b5c	217	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	02 Aug 2010
Imports:	179
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	1
Followers:	4

QEI.cpp@1:ddfe18427154, 2010-08-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning