Shao Rui / Mbed 2 deprecated AGV

1

Dependencies:   mbed

PositionSensor/PositionSensor.cpp@0:571a1835428e, 2021-01-25 (annotated)

Committer:
shaorui
Date:
Mon Jan 25 08:36:48 2021 +0000
Revision:
0:571a1835428e
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shaorui 0:571a1835428e 1
shaorui 0:571a1835428e 2 #include "mbed.h"
shaorui 0:571a1835428e 3 #include "PositionSensor.h"
shaorui 0:571a1835428e 4 //#include "math_ops.h"
shaorui 0:571a1835428e 5 //#include "offset_lut.h"
shaorui 0:571a1835428e 6 //#include <math.h>
shaorui 0:571a1835428e 7
shaorui 0:571a1835428e 8 PositionSensorAM5147::PositionSensorAM5147(int CPR, float offset, int ppairs){
shaorui 0:571a1835428e 9 //_CPR = CPR;
shaorui 0:571a1835428e 10 _CPR = CPR;
shaorui 0:571a1835428e 11 _ppairs = ppairs;
shaorui 0:571a1835428e 12 ElecOffset = offset;
shaorui 0:571a1835428e 13 rotations = 0;
shaorui 0:571a1835428e 14 spi = new SPI(PC_12, PC_11, PC_10);
shaorui 0:571a1835428e 15 spi->format(16, 1); // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words
shaorui 0:571a1835428e 16 spi->frequency(25000000);
shaorui 0:571a1835428e 17
shaorui 0:571a1835428e 18 cs = new DigitalOut(PA_15);
shaorui 0:571a1835428e 19 cs->write(1);
shaorui 0:571a1835428e 20 readAngleCmd = 0xffff;
shaorui 0:571a1835428e 21 MechOffset = offset;
shaorui 0:571a1835428e 22 modPosition = 0;
shaorui 0:571a1835428e 23 oldModPosition = 0;
shaorui 0:571a1835428e 24 oldVel = 0;
shaorui 0:571a1835428e 25 raw = 0;
shaorui 0:571a1835428e 26 }
shaorui 0:571a1835428e 27
shaorui 0:571a1835428e 28 void PositionSensorAM5147::Sample(float dt){
shaorui 0:571a1835428e 29 GPIOA->ODR &= ~(1 << 15);
shaorui 0:571a1835428e 30 raw = spi->write(readAngleCmd);
shaorui 0:571a1835428e 31 raw &= 0x3FFF;
shaorui 0:571a1835428e 32 //raw = spi->write(0);
shaorui 0:571a1835428e 33 //raw = raw>>2; //Extract last 14 bits
shaorui 0:571a1835428e 34 GPIOA->ODR |= (1 << 15);
shaorui 0:571a1835428e 35 int off_1 = offset_lut[raw>>7];
shaorui 0:571a1835428e 36 int off_2 = offset_lut[((raw>>7)+1)%128];
shaorui 0:571a1835428e 37 int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7); // Interpolate between lookup table entries
shaorui 0:571a1835428e 38 int angle = raw + off_interp; // Correct for nonlinearity with lookup table from calibration
shaorui 0:571a1835428e 39 if(angle - old_counts > _CPR/2){
shaorui 0:571a1835428e 40 rotations -= 1;
shaorui 0:571a1835428e 41 }
shaorui 0:571a1835428e 42 else if (angle - old_counts < -_CPR/2){
shaorui 0:571a1835428e 43 rotations += 1;
shaorui 0:571a1835428e 44 }
shaorui 0:571a1835428e 45
shaorui 0:571a1835428e 46 old_counts = angle;
shaorui 0:571a1835428e 47 oldModPosition = modPosition;
shaorui 0:571a1835428e 48 modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR);
shaorui 0:571a1835428e 49 position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR;
shaorui 0:571a1835428e 50 MechPosition = position - MechOffset;
shaorui 0:571a1835428e 51 float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset;
shaorui 0:571a1835428e 52 if(elec < 0) elec += 2.0f*PI;
shaorui 0:571a1835428e 53 else if(elec > 2.0f*PI) elec -= 2.0f*PI ;
shaorui 0:571a1835428e 54 ElecPosition = elec;
shaorui 0:571a1835428e 55
shaorui 0:571a1835428e 56 float vel;
shaorui 0:571a1835428e 57 //if(modPosition<.1f && oldModPosition>6.1f){
shaorui 0:571a1835428e 58
shaorui 0:571a1835428e 59 if((modPosition-oldModPosition) < -3.0f){
shaorui 0:571a1835428e 60 vel = (modPosition - oldModPosition + 2.0f*PI)/dt;
shaorui 0:571a1835428e 61 }
shaorui 0:571a1835428e 62 //else if(modPosition>6.1f && oldModPosition<0.1f){
shaorui 0:571a1835428e 63 else if((modPosition - oldModPosition) > 3.0f){
shaorui 0:571a1835428e 64 vel = (modPosition - oldModPosition - 2.0f*PI)/dt;
shaorui 0:571a1835428e 65 }
shaorui 0:571a1835428e 66 else{
shaorui 0:571a1835428e 67 vel = (modPosition-oldModPosition)/dt;
shaorui 0:571a1835428e 68 }
shaorui 0:571a1835428e 69
shaorui 0:571a1835428e 70 int n = 40;
shaorui 0:571a1835428e 71 float sum = vel;
shaorui 0:571a1835428e 72 for (int i = 1; i < (n); i++){
shaorui 0:571a1835428e 73 velVec[n - i] = velVec[n-i-1];
shaorui 0:571a1835428e 74 sum += velVec[n-i];
shaorui 0:571a1835428e 75 }
shaorui 0:571a1835428e 76 velVec[0] = vel;
shaorui 0:571a1835428e 77 MechVelocity = sum/((float)n);
shaorui 0:571a1835428e 78 ElecVelocity = MechVelocity*_ppairs;
shaorui 0:571a1835428e 79 ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity;
shaorui 0:571a1835428e 80 }
shaorui 0:571a1835428e 81
shaorui 0:571a1835428e 82 int PositionSensorAM5147::GetRawPosition(){
shaorui 0:571a1835428e 83 return raw;
shaorui 0:571a1835428e 84 }
shaorui 0:571a1835428e 85
shaorui 0:571a1835428e 86 float PositionSensorAM5147::GetMechPositionFixed(){
shaorui 0:571a1835428e 87 return MechPosition+MechOffset;
shaorui 0:571a1835428e 88 }
shaorui 0:571a1835428e 89
shaorui 0:571a1835428e 90 float PositionSensorAM5147::GetMechPosition(){
shaorui 0:571a1835428e 91 return MechPosition;
shaorui 0:571a1835428e 92 }
shaorui 0:571a1835428e 93
shaorui 0:571a1835428e 94 float PositionSensorAM5147::GetElecPosition(){
shaorui 0:571a1835428e 95 return ElecPosition;
shaorui 0:571a1835428e 96 }
shaorui 0:571a1835428e 97
shaorui 0:571a1835428e 98 float PositionSensorAM5147::GetElecVelocity(){
shaorui 0:571a1835428e 99 return ElecVelocity;
shaorui 0:571a1835428e 100 }
shaorui 0:571a1835428e 101
shaorui 0:571a1835428e 102 float PositionSensorAM5147::GetMechVelocity(){
shaorui 0:571a1835428e 103 return MechVelocity;
shaorui 0:571a1835428e 104 }
shaorui 0:571a1835428e 105
shaorui 0:571a1835428e 106 void PositionSensorAM5147::ZeroPosition(){
shaorui 0:571a1835428e 107 rotations = 0;
shaorui 0:571a1835428e 108 MechOffset = 0;
shaorui 0:571a1835428e 109 Sample(.00025f);
shaorui 0:571a1835428e 110 MechOffset = GetMechPosition();
shaorui 0:571a1835428e 111 }
shaorui 0:571a1835428e 112
shaorui 0:571a1835428e 113 void PositionSensorAM5147::SetElecOffset(float offset){
shaorui 0:571a1835428e 114 ElecOffset = offset;
shaorui 0:571a1835428e 115 }
shaorui 0:571a1835428e 116 void PositionSensorAM5147::SetMechOffset(float offset){
shaorui 0:571a1835428e 117 MechOffset = offset;
shaorui 0:571a1835428e 118 }
shaorui 0:571a1835428e 119
shaorui 0:571a1835428e 120 int PositionSensorAM5147::GetCPR(){
shaorui 0:571a1835428e 121 return _CPR;
shaorui 0:571a1835428e 122 }
shaorui 0:571a1835428e 123
shaorui 0:571a1835428e 124
shaorui 0:571a1835428e 125 void PositionSensorAM5147::WriteLUT(int new_lut[128]){
shaorui 0:571a1835428e 126 memcpy(offset_lut, new_lut, sizeof(offset_lut));
shaorui 0:571a1835428e 127 }
shaorui 0:571a1835428e 128
shaorui 0:571a1835428e 129
shaorui 0:571a1835428e 130
shaorui 0:571a1835428e 131 PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) {
shaorui 0:571a1835428e 132 _ppairs = ppairs;
shaorui 0:571a1835428e 133 _CPR = CPR;
shaorui 0:571a1835428e 134 _offset = offset;
shaorui 0:571a1835428e 135 MechPosition = 0;
shaorui 0:571a1835428e 136 out_old = 0;
shaorui 0:571a1835428e 137 oldVel = 0;
shaorui 0:571a1835428e 138 raw = 0;
shaorui 0:571a1835428e 139
shaorui 0:571a1835428e 140 // Enable clock for GPIOA
shaorui 0:571a1835428e 141 __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h
shaorui 0:571a1835428e 142
shaorui 0:571a1835428e 143 GPIOA->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ; //PA6 & PA7 as Alternate Function /*!< GPIO port mode register, Address offset: 0x00 */
shaorui 0:571a1835428e 144 GPIOA->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ; //PA6 & PA7 as Inputs /*!< GPIO port output type register, Address offset: 0x04 */
shaorui 0:571a1835428e 145 GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ; //Low speed /*!< GPIO port output speed register, Address offset: 0x08 */
shaorui 0:571a1835428e 146 GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ; //Pull Down /*!< GPIO port pull-up/pull-down register, Address offset: 0x0C */
shaorui 0:571a1835428e 147 GPIOA->AFR[0] |= 0x22000000 ; //AF02 for PA6 & PA7 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
shaorui 0:571a1835428e 148 GPIOA->AFR[1] |= 0x00000000 ; //nibbles here refer to gpio8..15 /*!< GPIO alternate function registers, Address offset: 0x20-0x24 */
shaorui 0:571a1835428e 149
shaorui 0:571a1835428e 150 // configure TIM3 as Encoder input
shaorui 0:571a1835428e 151 // Enable clock for TIM3
shaorui 0:571a1835428e 152 __TIM3_CLK_ENABLE();
shaorui 0:571a1835428e 153
shaorui 0:571a1835428e 154 TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1
shaorui 0:571a1835428e 155 TIM3->SMCR = TIM_ENCODERMODE_TI12; // SMS='011' (Encoder mode 3) < TIM slave mode control register
shaorui 0:571a1835428e 156 TIM3->CCMR1 = 0x1111; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1, maximum digital filtering
shaorui 0:571a1835428e 157 TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2
shaorui 0:571a1835428e 158 TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register
shaorui 0:571a1835428e 159 TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler
shaorui 0:571a1835428e 160 TIM3->ARR = CPR; // IM auto-reload register
shaorui 0:571a1835428e 161
shaorui 0:571a1835428e 162 TIM3->CNT = 0x000; //reset the counter before we use it
shaorui 0:571a1835428e 163
shaorui 0:571a1835428e 164 // Extra Timer for velocity measurement
shaorui 0:571a1835428e 165
shaorui 0:571a1835428e 166 __TIM2_CLK_ENABLE();
shaorui 0:571a1835428e 167 TIM3->CR2 = 0x030; //MMS = 101
shaorui 0:571a1835428e 168
shaorui 0:571a1835428e 169 TIM2->PSC = 0x03;
shaorui 0:571a1835428e 170 //TIM2->CR2 |= TIM_CR2_TI1S;
shaorui 0:571a1835428e 171 TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge)
shaorui 0:571a1835428e 172 TIM2->CCMR1 = 0x3; // CC1S = 11, IC1 mapped on TRC
shaorui 0:571a1835428e 173
shaorui 0:571a1835428e 174 //TIM2->CR2 |= TIM_CR2_TI1S;
shaorui 0:571a1835428e 175 TIM2->CCER |= TIM_CCER_CC1P;
shaorui 0:571a1835428e 176 //TIM2->CCER |= TIM_CCER_CC1NP;
shaorui 0:571a1835428e 177 TIM2->CCER |= TIM_CCER_CC1E;
shaorui 0:571a1835428e 178
shaorui 0:571a1835428e 179
shaorui 0:571a1835428e 180 TIM2->CR1 = 0x01; //CEN, enable timer
shaorui 0:571a1835428e 181
shaorui 0:571a1835428e 182 TIM3->CR1 = 0x01; // CEN
shaorui 0:571a1835428e 183 ZPulse = new InterruptIn(PC_4);
shaorui 0:571a1835428e 184 ZSense = new DigitalIn(PC_4);
shaorui 0:571a1835428e 185 //ZPulse = new InterruptIn(PB_0);
shaorui 0:571a1835428e 186 //ZSense = new DigitalIn(PB_0);
shaorui 0:571a1835428e 187 ZPulse->enable_irq();
shaorui 0:571a1835428e 188 ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount);
shaorui 0:571a1835428e 189 //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown);
shaorui 0:571a1835428e 190 ZPulse->mode(PullDown);
shaorui 0:571a1835428e 191 flag = 0;
shaorui 0:571a1835428e 192
shaorui 0:571a1835428e 193
shaorui 0:571a1835428e 194 //ZTest = new DigitalOut(PC_2);
shaorui 0:571a1835428e 195 //ZTest->write(1);
shaorui 0:571a1835428e 196 }
shaorui 0:571a1835428e 197
shaorui 0:571a1835428e 198 void PositionSensorEncoder::Sample(float dt){
shaorui 0:571a1835428e 199
shaorui 0:571a1835428e 200 }
shaorui 0:571a1835428e 201
shaorui 0:571a1835428e 202
shaorui 0:571a1835428e 203 float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians.
shaorui 0:571a1835428e 204 int raw = TIM3->CNT;
shaorui 0:571a1835428e 205 float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR);
shaorui 0:571a1835428e 206 return (float) unsigned_mech;// + 6.28318530718f* (float) rotations;
shaorui 0:571a1835428e 207 }
shaorui 0:571a1835428e 208
shaorui 0:571a1835428e 209 float PositionSensorEncoder::GetElecPosition() { //returns rotor electrical angle in radians.
shaorui 0:571a1835428e 210 int raw = TIM3->CNT;
shaorui 0:571a1835428e 211 float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset;
shaorui 0:571a1835428e 212 if(elec < 0) elec += 6.28318530718f;
shaorui 0:571a1835428e 213 return elec;
shaorui 0:571a1835428e 214 }
shaorui 0:571a1835428e 215
shaorui 0:571a1835428e 216
shaorui 0:571a1835428e 217
shaorui 0:571a1835428e 218 float PositionSensorEncoder::GetMechVelocity(){
shaorui 0:571a1835428e 219
shaorui 0:571a1835428e 220 float out = 0;
shaorui 0:571a1835428e 221 float rawPeriod = TIM2->CCR1; //Clock Ticks
shaorui 0:571a1835428e 222 int currentTime = TIM2->CNT;
shaorui 0:571a1835428e 223 if(currentTime > 2000000){rawPeriod = currentTime;}
shaorui 0:571a1835428e 224 float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; // +/- 1
shaorui 0:571a1835428e 225 float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod;
shaorui 0:571a1835428e 226 if(isinf(meas)){ meas = 1;}
shaorui 0:571a1835428e 227 out = meas;
shaorui 0:571a1835428e 228 //if(meas == oldVel){
shaorui 0:571a1835428e 229 // out = .9f*out_old;
shaorui 0:571a1835428e 230 // }
shaorui 0:571a1835428e 231
shaorui 0:571a1835428e 232
shaorui 0:571a1835428e 233 oldVel = meas;
shaorui 0:571a1835428e 234 out_old = out;
shaorui 0:571a1835428e 235 int n = 16;
shaorui 0:571a1835428e 236 float sum = out;
shaorui 0:571a1835428e 237 for (int i = 1; i < (n); i++){
shaorui 0:571a1835428e 238 velVec[n - i] = velVec[n-i-1];
shaorui 0:571a1835428e 239 sum += velVec[n-i];
shaorui 0:571a1835428e 240 }
shaorui 0:571a1835428e 241 velVec[0] = out;
shaorui 0:571a1835428e 242 return sum/(float)n;
shaorui 0:571a1835428e 243 }
shaorui 0:571a1835428e 244
shaorui 0:571a1835428e 245 float PositionSensorEncoder::GetElecVelocity(){
shaorui 0:571a1835428e 246 return _ppairs*GetMechVelocity();
shaorui 0:571a1835428e 247 }
shaorui 0:571a1835428e 248
shaorui 0:571a1835428e 249 void PositionSensorEncoder::ZeroEncoderCount(void){
shaorui 0:571a1835428e 250 if (ZSense->read() == 1 & flag == 0){
shaorui 0:571a1835428e 251 if (ZSense->read() == 1){
shaorui 0:571a1835428e 252 GPIOC->ODR ^= (1 << 4);
shaorui 0:571a1835428e 253 TIM3->CNT = 0x000;
shaorui 0:571a1835428e 254 //state = !state;
shaorui 0:571a1835428e 255 //ZTest->write(state);
shaorui 0:571a1835428e 256 GPIOC->ODR ^= (1 << 4);
shaorui 0:571a1835428e 257 //flag = 1;
shaorui 0:571a1835428e 258 }
shaorui 0:571a1835428e 259 }
shaorui 0:571a1835428e 260 }
shaorui 0:571a1835428e 261
shaorui 0:571a1835428e 262 void PositionSensorEncoder::ZeroPosition(void){
shaorui 0:571a1835428e 263
shaorui 0:571a1835428e 264 }
shaorui 0:571a1835428e 265
shaorui 0:571a1835428e 266 void PositionSensorEncoder::ZeroEncoderCountDown(void){
shaorui 0:571a1835428e 267 if (ZSense->read() == 0){
shaorui 0:571a1835428e 268 if (ZSense->read() == 0){
shaorui 0:571a1835428e 269 GPIOC->ODR ^= (1 << 4);
shaorui 0:571a1835428e 270 flag = 0;
shaorui 0:571a1835428e 271 float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f;
shaorui 0:571a1835428e 272 if(dir != dir){
shaorui 0:571a1835428e 273 dir = dir;
shaorui 0:571a1835428e 274 rotations += dir;
shaorui 0:571a1835428e 275 }
shaorui 0:571a1835428e 276
shaorui 0:571a1835428e 277 GPIOC->ODR ^= (1 << 4);
shaorui 0:571a1835428e 278
shaorui 0:571a1835428e 279 }
shaorui 0:571a1835428e 280 }
shaorui 0:571a1835428e 281 }
shaorui 0:571a1835428e 282 void PositionSensorEncoder::SetElecOffset(float offset){
shaorui 0:571a1835428e 283
shaorui 0:571a1835428e 284 }
shaorui 0:571a1835428e 285
shaorui 0:571a1835428e 286 int PositionSensorEncoder::GetRawPosition(void){
shaorui 0:571a1835428e 287 return 0;
shaorui 0:571a1835428e 288 }
shaorui 0:571a1835428e 289
shaorui 0:571a1835428e 290 int PositionSensorEncoder::GetCPR(){
shaorui 0:571a1835428e 291 return _CPR;
shaorui 0:571a1835428e 292 }
shaorui 0:571a1835428e 293
shaorui 0:571a1835428e 294
shaorui 0:571a1835428e 295 void PositionSensorEncoder::WriteLUT(int new_lut[128]){
shaorui 0:571a1835428e 296 memcpy(offset_lut, new_lut, sizeof(offset_lut));
shaorui 0:571a1835428e 297 }