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