Shao Rui / Joint_control_2AS5147_DRV8323RH_GR49_HJB_2020_8_10

1

Dependencies:   mbed-dev-f303 FastPWM3

MA700Sensor/MA700Sensor.cpp@53:32218a36df05, 2020-09-15 (annotated)

Committer:
shaorui
Date:
Tue Sep 15 08:59:03 2020 +0000
Revision:
53:32218a36df05
Parent:
52:d4d5e3414865 
1

Who changed what in which revision?

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