robocon_Bteam / LPC1549QEI

LPC1549　QEI

QEI.cpp@5:a7972b08669b, 2020-01-17 (annotated)

Committer:
YutaTogashi
Date:
Fri Jan 17 08:47:03 2020 +0000
Revision:
5:a7972b08669b
Parent:
4:4092d7f5ca0c
Child:
6:f4c910df1172
20200117 getRotationFunction ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kotakku 0:26f0e94f01e0 1 #include "QEI.h"
kotakku 0:26f0e94f01e0 2
kotakku 0:26f0e94f01e0 3 #ifdef TARGET_LPC1549
kotakku 0:26f0e94f01e0 4
kotakku 0:26f0e94f01e0 5 //UM p329～
kotakku 0:26f0e94f01e0 6
kotakku 0:26f0e94f01e0 7 QEI::QEI(PinName phaseA,
kotakku 0:26f0e94f01e0 8 PinName phaseB,
kotakku 0:26f0e94f01e0 9 PinName index,
kotakku 0:26f0e94f01e0 10 uint32_t pulsePerRotation,
kotakku 0:26f0e94f01e0 11 Encoding encoding,
kotakku 0:26f0e94f01e0 12 SignalMode signalMode):
kotakku 0:26f0e94f01e0 13 _phaseA(phaseA), _phaseB(phaseB), _index(index),
kotakku 0:26f0e94f01e0 14 _encoding(encoding), _signalMode(signalMode)
kotakku 0:26f0e94f01e0 15 {
kotakku 0:26f0e94f01e0 16 // Basic configration
kotakku 0:26f0e94f01e0 17 qei_init(&_qei, _phaseA, _phaseB, _index);
kotakku 0:26f0e94f01e0 18
kotakku 0:26f0e94f01e0 19 // Set pulse num per revolution
kotakku 0:26f0e94f01e0 20 uint32_t maxPos = pulsePerRotation;
YutaTogashi 5:a7972b08669b 21 ppr = pulsePerRotation;
kotakku 0:26f0e94f01e0 22 switch(_encoding)
kotakku 0:26f0e94f01e0 23 {
kotakku 0:26f0e94f01e0 24 case X2_ENCODING:
kotakku 0:26f0e94f01e0 25 maxPos *= 2;
kotakku 0:26f0e94f01e0 26 maxPos -= 1;
YutaTogashi 5:a7972b08669b 27 ppr *= 2;
kotakku 0:26f0e94f01e0 28 break;
kotakku 0:26f0e94f01e0 29 case X4_ENCODING:
kotakku 0:26f0e94f01e0 30 maxPos *= 4;
kotakku 0:26f0e94f01e0 31 maxPos -= 1;
YutaTogashi 5:a7972b08669b 32 ppr *= 4;
kotakku 0:26f0e94f01e0 33 break;
kotakku 0:26f0e94f01e0 34 default:
kotakku 0:26f0e94f01e0 35 maxPos = 0;
kotakku 0:26f0e94f01e0 36 }
YutaTogashi 4:4092d7f5ca0c 37 setMaxPos(/*maxPos*/0xFFFFFFFF); //togashi改善 バク注意
kotakku 0:26f0e94f01e0 38
kotakku 0:26f0e94f01e0 39 // Set encoding
kotakku 0:26f0e94f01e0 40 setEncoding(_encoding);
kotakku 0:26f0e94f01e0 41
kotakku 0:26f0e94f01e0 42 // Set signal mode
kotakku 0:26f0e94f01e0 43 setSignalMode(_signalMode);
kotakku 0:26f0e94f01e0 44
kotakku 0:26f0e94f01e0 45 // Set default velocity capture period
kotakku 0:26f0e94f01e0 46 setVelocityTimerPeriod_us(10000);
kotakku 0:26f0e94f01e0 47
kotakku 0:26f0e94f01e0 48 // Peripheral reset
kotakku 0:26f0e94f01e0 49 reset();
kotakku 0:26f0e94f01e0 50 }
kotakku 0:26f0e94f01e0 51
kotakku 0:26f0e94f01e0 52 QEI::~QEI()
kotakku 0:26f0e94f01e0 53 {
kotakku 0:26f0e94f01e0 54 // Distaching interrupts
kotakku 0:26f0e94f01e0 55 for(int irq = 0; irq < IrqCnt; irq++)
kotakku 0:26f0e94f01e0 56 attach(NULL, (IrqType)irq);
kotakku 0:26f0e94f01e0 57
kotakku 0:26f0e94f01e0 58 }
kotakku 0:26f0e94f01e0 59
kotakku 0:26f0e94f01e0 60 void QEI::reset()
kotakku 0:26f0e94f01e0 61 {
kotakku 0:26f0e94f01e0 62 qei_reset();
kotakku 0:26f0e94f01e0 63 }
kotakku 0:26f0e94f01e0 64
kotakku 0:26f0e94f01e0 65 ////////////////////////// set //////////////////////////
kotakku 0:26f0e94f01e0 66
kotakku 0:26f0e94f01e0 67 void QEI::setMaxPos(uint32_t num)
kotakku 0:26f0e94f01e0 68 {
kotakku 0:26f0e94f01e0 69 qei_set_maxpos(&_qei, num);
kotakku 0:26f0e94f01e0 70 }
kotakku 0:26f0e94f01e0 71
kotakku 0:26f0e94f01e0 72 void QEI::setInvertDirection(bool isInvert)
kotakku 0:26f0e94f01e0 73 {
kotakku 0:26f0e94f01e0 74 qei_set_invert_direction(isInvert);
kotakku 0:26f0e94f01e0 75 }
kotakku 0:26f0e94f01e0 76
kotakku 0:26f0e94f01e0 77 void QEI::setInvertIndex(bool isInvert)
kotakku 0:26f0e94f01e0 78 {
kotakku 0:26f0e94f01e0 79 qei_set_invert_index(isInvert);
kotakku 0:26f0e94f01e0 80 }
kotakku 0:26f0e94f01e0 81
kotakku 0:26f0e94f01e0 82 void QEI::setSignalMode(SignalMode signalMode)
kotakku 0:26f0e94f01e0 83 {
kotakku 0:26f0e94f01e0 84 qei_set_signal_mode((QEISignalMode)signalMode);
kotakku 0:26f0e94f01e0 85 }
kotakku 0:26f0e94f01e0 86
kotakku 0:26f0e94f01e0 87 void QEI::setEncoding(Encoding encoding)
kotakku 0:26f0e94f01e0 88 {
kotakku 0:26f0e94f01e0 89 qei_set_encoding(&_qei, (QEIEncoding)encoding);
kotakku 0:26f0e94f01e0 90 }
kotakku 0:26f0e94f01e0 91
kotakku 0:26f0e94f01e0 92 // What is Bit 3 of LPCQEI->CONF ?
kotakku 0:26f0e94f01e0 93
kotakku 0:26f0e94f01e0 94 void QEI::setVelocityTimerPeriod_us(uint32_t us)
kotakku 0:26f0e94f01e0 95 {
kotakku 0:26f0e94f01e0 96 #warning find period to value.
kotakku 0:26f0e94f01e0 97 // p346 22.7.3 example
kotakku 0:26f0e94f01e0 98 // load = PCLK / キャプチャー周波数
kotakku 0:26f0e94f01e0 99 uint32_t regVal = ((double)_qei.PCLK * us / 1000000);
kotakku 0:26f0e94f01e0 100
kotakku 0:26f0e94f01e0 101 if(regVal == 0)
kotakku 0:26f0e94f01e0 102 regVal = 1;
kotakku 0:26f0e94f01e0 103
kotakku 0:26f0e94f01e0 104 qei_set_timer_load(&_qei, regVal);
kotakku 0:26f0e94f01e0 105 }
kotakku 0:26f0e94f01e0 106
kotakku 0:26f0e94f01e0 107 ////////////////////////// get //////////////////////////
kotakku 0:26f0e94f01e0 108 /*
kotakku 0:26f0e94f01e0 109 uint32_t QEI::getInterrupts()
kotakku 0:26f0e94f01e0 110 {
kotakku 0:26f0e94f01e0 111 return LPC_QEI->IE;
kotakku 0:26f0e94f01e0 112 }
kotakku 0:26f0e94f01e0 113
kotakku 0:26f0e94f01e0 114 uint32_t QEI::getInterruptStatus()
kotakku 0:26f0e94f01e0 115 {
kotakku 0:26f0e94f01e0 116 return LPC_QEI->INTSTAT;
kotakku 0:26f0e94f01e0 117 }
kotakku 0:26f0e94f01e0 118 */
kotakku 0:26f0e94f01e0 119
kotakku 0:26f0e94f01e0 120 /*
kotakku 0:26f0e94f01e0 121 int QEI::getPhase(Channel channel)
kotakku 0:26f0e94f01e0 122 {
kotakku 0:26f0e94f01e0 123 uint32_t phaseState = LPC_QEI->CONF;
kotakku 0:26f0e94f01e0 124 switch(channel)
kotakku 0:26f0e94f01e0 125 {
kotakku 0:26f0e94f01e0 126 case Phase_A:
kotakku 0:26f0e94f01e0 127 if(phaseState & (1 << 17) || phaseState & (1 << 16))
kotakku 0:26f0e94f01e0 128 return 1;
kotakku 0:26f0e94f01e0 129 else
kotakku 0:26f0e94f01e0 130 return 0;
kotakku 0:26f0e94f01e0 131
kotakku 0:26f0e94f01e0 132 case Phase_B:
kotakku 0:26f0e94f01e0 133 if(phaseState & (1 << 18) || phaseState & (1 << 17))
kotakku 0:26f0e94f01e0 134 return 1;
kotakku 0:26f0e94f01e0 135 else
kotakku 0:26f0e94f01e0 136 return 0;
kotakku 0:26f0e94f01e0 137 }
kotakku 0:26f0e94f01e0 138 return -1;
kotakku 0:26f0e94f01e0 139 }*/
kotakku 0:26f0e94f01e0 140
kotakku 0:26f0e94f01e0 141 int QEI::getDirection()
kotakku 0:26f0e94f01e0 142 {
kotakku 0:26f0e94f01e0 143 int value = 0;
kotakku 0:26f0e94f01e0 144
kotakku 0:26f0e94f01e0 145 if(getVelocity())
kotakku 0:26f0e94f01e0 146 {
kotakku 0:26f0e94f01e0 147 value = (LPC_QEI->STAT & 0x00000001) ? -1 : 1;
kotakku 0:26f0e94f01e0 148
kotakku 0:26f0e94f01e0 149 if(getInvertDirection())
kotakku 0:26f0e94f01e0 150 value *= -1;
kotakku 0:26f0e94f01e0 151 }
kotakku 0:26f0e94f01e0 152 return value;
kotakku 0:26f0e94f01e0 153 }
kotakku 0:26f0e94f01e0 154
kotakku 0:26f0e94f01e0 155 uint32_t QEI::getPosition()
kotakku 0:26f0e94f01e0 156 {
kotakku 0:26f0e94f01e0 157 return qei_get_position();
kotakku 0:26f0e94f01e0 158 }
kotakku 0:26f0e94f01e0 159
kotakku 0:26f0e94f01e0 160 uint32_t QEI::getIndex()
kotakku 0:26f0e94f01e0 161 {
kotakku 0:26f0e94f01e0 162 return qei_get_index();
kotakku 0:26f0e94f01e0 163 }
kotakku 0:26f0e94f01e0 164
kotakku 0:26f0e94f01e0 165 uint32_t QEI::getVelocity()
kotakku 0:26f0e94f01e0 166 {
kotakku 0:26f0e94f01e0 167 return qei_get_velocity();
kotakku 0:26f0e94f01e0 168 }
kotakku 0:26f0e94f01e0 169
kotakku 0:26f0e94f01e0 170 double QEI::getRotation()
kotakku 0:26f0e94f01e0 171 {
YutaTogashi 5:a7972b08669b 172 return (double)((float)((int)getPosition()) / (/*_qei.maxPos */(float)ppr));
kotakku 0:26f0e94f01e0 173 }
kotakku 0:26f0e94f01e0 174
kotakku 0:26f0e94f01e0 175 double QEI::getRPM()
kotakku 0:26f0e94f01e0 176 {
kotakku 0:26f0e94f01e0 177 return (60 * getRPS());
kotakku 0:26f0e94f01e0 178 }
kotakku 0:26f0e94f01e0 179
kotakku 0:26f0e94f01e0 180 double QEI::getRPS()
kotakku 0:26f0e94f01e0 181 {
kotakku 0:26f0e94f01e0 182 /*
kotakku 0:26f0e94f01e0 183 RPM = (PCLK * Speed * 60) / (Load * PPR * Edges)
kotakku 0:26f0e94f01e0 184
kotakku 0:26f0e94f01e0 185 PCLK is the QEI controller clock.
kotakku 0:26f0e94f01e0 186 PPR is the number of pulses per revolution of the physical encoder.
kotakku 0:26f0e94f01e0 187 Edges 2 or 4 (x2 or x4)
kotakku 0:26f0e94f01e0 188
kotakku 0:26f0e94f01e0 189 PPR * Edges = 400?
kotakku 0:26f0e94f01e0 190 */
kotakku 0:26f0e94f01e0 191
YutaTogashi 5:a7972b08669b 192 double rps = ( (double)_qei.PCLK * getVelocity() ) / ( _qei.load * (_qei.maxPos/* + 1*/) );
YutaTogashi 1:6a70ea5c215a 193
YutaTogashi 1:6a70ea5c215a 194 /**Plus by Togashi**/
YutaTogashi 1:6a70ea5c215a 195 if(LPC_QEI->STAT & 0x00000001) {
YutaTogashi 1:6a70ea5c215a 196 if(rps >= 0.0f) {
YutaTogashi 1:6a70ea5c215a 197 rps *= -1;
YutaTogashi 1:6a70ea5c215a 198 }
YutaTogashi 1:6a70ea5c215a 199 }
kotakku 0:26f0e94f01e0 200 return rps;
YutaTogashi 5:a7972b08669b 201 //return _qei.maxPos;
kotakku 0:26f0e94f01e0 202 }
kotakku 0:26f0e94f01e0 203
kotakku 0:26f0e94f01e0 204 bool QEI::getInvertDirection()
kotakku 0:26f0e94f01e0 205 {
kotakku 0:26f0e94f01e0 206 return qei_get_invert_direction_status();
kotakku 0:26f0e94f01e0 207 }
kotakku 0:26f0e94f01e0 208
kotakku 0:26f0e94f01e0 209 bool QEI::getInvertIndex()
kotakku 0:26f0e94f01e0 210 {
kotakku 0:26f0e94f01e0 211 return qei_get_invert_index_status();
kotakku 0:26f0e94f01e0 212 }
kotakku 0:26f0e94f01e0 213
kotakku 0:26f0e94f01e0 214 ////////////////////////// Irq //////////////////////////
kotakku 0:26f0e94f01e0 215
kotakku 0:26f0e94f01e0 216 void QEI::attach(Callback<void()> func, IrqType type)
kotakku 0:26f0e94f01e0 217 {
kotakku 0:26f0e94f01e0 218
kotakku 0:26f0e94f01e0 219 }
kotakku 0:26f0e94f01e0 220 /*
kotakku 0:26f0e94f01e0 221 void QEI::setInterrupts(uint32_t symbol)
kotakku 0:26f0e94f01e0 222 {
kotakku 0:26f0e94f01e0 223 LPC_QEI->IES |= symbol;
kotakku 0:26f0e94f01e0 224 }
kotakku 0:26f0e94f01e0 225
kotakku 0:26f0e94f01e0 226 void QEI::clearInterrupts(uint32_t symbol)
kotakku 0:26f0e94f01e0 227 {
kotakku 0:26f0e94f01e0 228 LPC_QEI->IEC |= symbol;
kotakku 0:26f0e94f01e0 229 }
kotakku 0:26f0e94f01e0 230
kotakku 0:26f0e94f01e0 231 void QEI::setInterruptStatus(uint32_t symbol)
kotakku 0:26f0e94f01e0 232 {
kotakku 0:26f0e94f01e0 233 LPC_QEI->SET |= symbol;
kotakku 0:26f0e94f01e0 234 }
kotakku 0:26f0e94f01e0 235
kotakku 0:26f0e94f01e0 236 void QEI::clearInterruptStatus(uint32_t symbol)
kotakku 0:26f0e94f01e0 237 {
kotakku 0:26f0e94f01e0 238 LPC_QEI->CLR |= symbol;
kotakku 0:26f0e94f01e0 239 }
kotakku 0:26f0e94f01e0 240 */
kotakku 0:26f0e94f01e0 241
kotakku 0:26f0e94f01e0 242 #endif //TARGET_LPC1549