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