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