Shao Rui / Motor_200N_DRV8323RS

1

Dependencies:   mbed-dev-f303 FastPWM3

Position_Torque_Sensor/Position_Torque_Sensor.cpp@56:4f73fa55baa0, 2020-05-14 (annotated)

Committer:
shaorui
Date:
Thu May 14 10:44:41 2020 +0000
Revision:
56:4f73fa55baa0
1

Who changed what in which revision?

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