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