14bit position sensor version.
Dependencies: mbed-dev-f303 FastPWM3
PositionSensor.cpp
00001 00002 #include "mbed.h" 00003 #include "PositionSensor.h" 00004 #include "../math_ops.h" 00005 //#include "offset_lut.h" 00006 //#include <math.h> 00007 00008 PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){ 00009 //_CPR = CPR; 00010 _CPR = CPR; 00011 _ppairs = ppairs; 00012 ElecOffset = offset; 00013 rotations = 0; 00014 spi = new SPI(PC_12, PC_11, PC_10); 00015 spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words 00016 spi->frequency(25000000); 00017 00018 cs = new DigitalOut(PA_15); 00019 cs->write(1); 00020 readAngleCmd = 0xffff; 00021 MechOffset = offset; 00022 modPosition = 0; 00023 oldModPosition = 0; 00024 oldVel = 0; 00025 raw = 0; 00026 } 00027 00028 void PositionSensorAM5147::Sample(float dt){ 00029 GPIOA->ODR &= ~(1 << 15); 00030 raw = spi->write(readAngleCmd); 00031 raw &= 0x3FFF; 00032 //Extract last 14 bits 00033 GPIOA->ODR |= (1 << 15); 00034 int off_1 = offset_lut[raw>>7]; 00035 int off_2 = offset_lut[((raw>>7)+1)%128]; 00036 int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7); // Interpolate between lookup table entries 00037 int angle = raw + off_interp; // Correct for nonlinearity with lookup table from calibration 00038 if(angle - old_counts > _CPR/2){ 00039 rotations -= 1; 00040 } 00041 else if (angle - old_counts < -_CPR/2){ 00042 rotations += 1; 00043 } 00044 00045 old_counts = angle; 00046 oldModPosition = modPosition; 00047 modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR); 00048 position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR; 00049 MechPosition = position - MechOffset; 00050 float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset; 00051 if(elec < 0) elec += 2.0f*PI; 00052 else if(elec > 2.0f*PI) elec -= 2.0f*PI ; 00053 ElecPosition = elec; 00054 00055 float vel; 00056 //if(modPosition<.1f && oldModPosition>6.1f){ 00057 00058 if((modPosition-oldModPosition) < -3.0f){ 00059 vel = (modPosition - oldModPosition + 2.0f*PI)/dt; 00060 } 00061 //else if(modPosition>6.1f && oldModPosition<0.1f){ 00062 else if((modPosition - oldModPosition) > 3.0f){ 00063 vel = (modPosition - oldModPosition - 2.0f*PI)/dt; 00064 } 00065 else{ 00066 vel = (modPosition-oldModPosition)/dt; 00067 } 00068 00069 int n = 40; 00070 float sum = vel; 00071 for (int i = 1; i < (n); i++){ 00072 velVec[n - i] = velVec[n-i-1]; 00073 sum += velVec[n-i]; 00074 } 00075 velVec[0] = vel; 00076 MechVelocity = sum/((float)n); 00077 ElecVelocity = MechVelocity*_ppairs; 00078 ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity; 00079 } 00080 00081 int PositionSensorAM5147::GetRawPosition(){ 00082 return raw; 00083 } 00084 00085 float PositionSensorAM5147::GetMechPositionFixed(){ 00086 return MechPosition+MechOffset; 00087 } 00088 00089 float PositionSensorAM5147::GetMechPosition(){ 00090 return MechPosition; 00091 } 00092 00093 float PositionSensorAM5147::GetElecPosition(){ 00094 return ElecPosition; 00095 } 00096 00097 float PositionSensorAM5147::GetElecVelocity(){ 00098 return ElecVelocity; 00099 } 00100 00101 float PositionSensorAM5147::GetMechVelocity(){ 00102 return MechVelocity; 00103 } 00104 00105 void PositionSensorAM5147::ZeroPosition(){ 00106 rotations = 0; 00107 MechOffset = 0; 00108 Sample(.00025f); 00109 MechOffset = GetMechPosition(); 00110 } 00111 00112 void PositionSensorAM5147::SetElecOffset(float offset){ 00113 ElecOffset = offset; 00114 } 00115 void PositionSensorAM5147::SetMechOffset(float offset){ 00116 MechOffset = offset; 00117 } 00118 00119 int PositionSensorAM5147::GetCPR(){ 00120 return _CPR; 00121 } 00122 00123 00124 void PositionSensorAM5147::WriteLUT(int new_lut[128]){ 00125 memcpy(offset_lut, new_lut, sizeof(offset_lut)); 00126 } 00127 00128 00129 00130 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) { 00131 _ppairs = ppairs; 00132 _CPR = CPR; 00133 _offset = offset; 00134 MechPosition = 0; 00135 out_old = 0; 00136 oldVel = 0; 00137 raw = 0; 00138 00139 // Enable clock for GPIOA 00140 __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h 00141 00142 GPIOA->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ; //PA6 & PA7 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */ 00143 GPIOA->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ; //PA6 & PA7 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */ 00144 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ; //Low speed /*!< GPIO port output speed register, Address offset: 0x08 */ 00145 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ; //Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */ 00146 GPIOA->AFR[0] |= 0x22000000 ; //AF02 for PA6 & PA7 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ 00147 GPIOA->AFR[1] |= 0x00000000 ; //nibbles here refer to gpio8..15 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */ 00148 00149 // configure TIM3 as Encoder input 00150 // Enable clock for TIM3 00151 __TIM3_CLK_ENABLE(); 00152 00153 TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1 00154 TIM3->SMCR = TIM_ENCODERMODE_TI12; // SMS='011' (Encoder mode 3) < TIM slave mode control register 00155 TIM3->CCMR1 = 0x1111; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1, maximum digital filtering 00156 TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 00157 TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register 00158 TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler 00159 TIM3->ARR = CPR; // IM auto-reload register 00160 00161 TIM3->CNT = 0x000; //reset the counter before we use it 00162 00163 // Extra Timer for velocity measurement 00164 00165 __TIM2_CLK_ENABLE(); 00166 TIM3->CR2 = 0x030; //MMS = 101 00167 00168 TIM2->PSC = 0x03; 00169 //TIM2->CR2 |= TIM_CR2_TI1S; 00170 TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge) 00171 TIM2->CCMR1 = 0x3; // CC1S = 11, IC1 mapped on TRC 00172 00173 //TIM2->CR2 |= TIM_CR2_TI1S; 00174 TIM2->CCER |= TIM_CCER_CC1P; 00175 //TIM2->CCER |= TIM_CCER_CC1NP; 00176 TIM2->CCER |= TIM_CCER_CC1E; 00177 00178 00179 TIM2->CR1 = 0x01; //CEN, enable timer 00180 00181 TIM3->CR1 = 0x01; // CEN 00182 ZPulse = new InterruptIn(PC_4); 00183 ZSense = new DigitalIn(PC_4); 00184 //ZPulse = new InterruptIn(PB_0); 00185 //ZSense = new DigitalIn(PB_0); 00186 ZPulse->enable_irq(); 00187 ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount); 00188 //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown); 00189 ZPulse->mode(PullDown); 00190 flag = 0; 00191 00192 00193 //ZTest = new DigitalOut(PC_2); 00194 //ZTest->write(1); 00195 } 00196 00197 void PositionSensorEncoder::Sample(float dt){ 00198 00199 } 00200 00201 00202 float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians. 00203 int raw = TIM3->CNT; 00204 float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR); 00205 return (float) unsigned_mech;// + 6.28318530718f* (float) rotations; 00206 } 00207 00208 float PositionSensorEncoder::GetElecPosition() { //returns rotor electrical angle in radians. 00209 int raw = TIM3->CNT; 00210 float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset; 00211 if(elec < 0) elec += 6.28318530718f; 00212 return elec; 00213 } 00214 00215 00216 00217 float PositionSensorEncoder::GetMechVelocity(){ 00218 00219 float out = 0; 00220 float rawPeriod = TIM2->CCR1; //Clock Ticks 00221 int currentTime = TIM2->CNT; 00222 if(currentTime > 2000000){rawPeriod = currentTime;} 00223 float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; // +/- 1 00224 float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; 00225 if(isinf(meas)){ meas = 1;} 00226 out = meas; 00227 //if(meas == oldVel){ 00228 // out = .9f*out_old; 00229 // } 00230 00231 00232 oldVel = meas; 00233 out_old = out; 00234 int n = 16; 00235 float sum = out; 00236 for (int i = 1; i < (n); i++){ 00237 velVec[n - i] = velVec[n-i-1]; 00238 sum += velVec[n-i]; 00239 } 00240 velVec[0] = out; 00241 return sum/(float)n; 00242 } 00243 00244 float PositionSensorEncoder::GetElecVelocity(){ 00245 return _ppairs*GetMechVelocity(); 00246 } 00247 00248 void PositionSensorEncoder::ZeroEncoderCount(void){ 00249 if (ZSense->read() == 1 & flag == 0){ 00250 if (ZSense->read() == 1){ 00251 GPIOC->ODR ^= (1 << 4); 00252 TIM3->CNT = 0x000; 00253 //state = !state; 00254 //ZTest->write(state); 00255 GPIOC->ODR ^= (1 << 4); 00256 //flag = 1; 00257 } 00258 } 00259 } 00260 00261 void PositionSensorEncoder::ZeroPosition(void){ 00262 00263 } 00264 00265 void PositionSensorEncoder::ZeroEncoderCountDown(void){ 00266 if (ZSense->read() == 0){ 00267 if (ZSense->read() == 0){ 00268 GPIOC->ODR ^= (1 << 4); 00269 flag = 0; 00270 float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; 00271 if(dir != dir){ 00272 dir = dir; 00273 rotations += dir; 00274 } 00275 00276 GPIOC->ODR ^= (1 << 4); 00277 00278 } 00279 } 00280 } 00281 void PositionSensorEncoder::SetElecOffset(float offset){ 00282 00283 } 00284 00285 int PositionSensorEncoder::GetRawPosition(void){ 00286 return 0; 00287 } 00288 00289 int PositionSensorEncoder::GetCPR(){ 00290 return _CPR; 00291 } 00292 00293 00294 void PositionSensorEncoder::WriteLUT(int new_lut[128]){ 00295 memcpy(offset_lut, new_lut, sizeof(offset_lut)); 00296 }
Generated on Fri Jul 15 2022 09:32:51 by 1.7.2