1
Dependencies: mbed-dev-f303 FastPWM3
Diff: MA700Sensor/MA700Sensor.cpp
- Revision:
- 48:1b51771c3647
diff -r 55bdc4d5096b -r 1b51771c3647 MA700Sensor/MA700Sensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MA700Sensor/MA700Sensor.cpp Fri Feb 07 11:31:37 2020 +0000 @@ -0,0 +1,342 @@ + +#include "mbed.h" +#include "MA700Sensor.h" +#include "math_ops.h" +#include "CAN_com.h" +//#include "offset_lut.h" +//#include <math.h> + +PositionSensorMA700::PositionSensorMA700(int CPR, float offset, int ppairs){ + //_CPR = CPR; + _CPR = CPR; + _ppairs = ppairs; + JointOffset = offset; + rotations = 0; + spi = new SPI(PA_7, PA_6, PA_5); + spi->format(16, 1); + // spi->format(16, 3); //shaorui modify // mbed v>127 breaks 16-bit spi, so transaction is broken into 2 8-bit words + spi->frequency(25000000); + + cs = new DigitalOut(PA_4); + cs->write(1); + } + +void PositionSensorMA700::Sample(float dt){ + GPIOA->ODR &= ~(1 << 4); + //raw = spi->write(readAngleCmd);//shaorui modify + //raw &= 0x3FFF; + raw = spi->write(0); + raw = raw>>1; //Extract last 14 bits + //raw = raw>>2; //Extract last 14 bits + GPIOA->ODR |= (1 << 4); + int off_1 = offset_lut[raw>>7]; + int off_2 = offset_lut[((raw>>7)+1)%128]; + int off_interp = off_1 + ((off_2 - off_1)*(raw - ((raw>>7)<<7))>>7); // Interpolate between lookup table entries + int angle = raw + off_interp; + //angle = 0.9*old_counts+0.1*angle; // Correct for nonlinearity with lookup table from calibration + if(angle - old_counts > _CPR/2){ + rotations -= 1; + // printf("old%d new%d\n\r",old_counts,angle); + } + else if (angle - old_counts < -_CPR/2){ + rotations += 1; + // printf("old%d new%d\n\r",old_counts,angle); + } + + old_counts = angle; + oldModPosition = modPosition; + modPosition = ((2.0f*PI * ((float) angle))/ (float)_CPR); + position = (2.0f*PI * ((float) angle+(_CPR*rotations)))/ (float)_CPR; + MechPosition = position - MechOffset; + float elec = ((2.0f*PI/(float)_CPR) * (float) ((_ppairs*angle)%_CPR)) + ElecOffset; + if(elec < 0) elec += 2.0f*PI; + else if(elec > 2.0f*PI) elec -= 2.0f*PI ; + ElecPosition = elec; + + float vel; + //if(modPosition<.1f && oldModPosition>6.1f){ + + if((modPosition-oldModPosition) < -3.0f){ + vel = (modPosition - oldModPosition + 2.0f*PI)/dt; + } + //else if(modPosition>6.1f && oldModPosition<0.1f){ + else if((modPosition - oldModPosition) > 3.0f){ + vel = (modPosition - oldModPosition - 2.0f*PI)/dt; + } + else{ + vel = (modPosition-oldModPosition)/dt; + } + + int n = 40; + float sum = vel; + for (int i = 1; i < (n); i++){ + velVec[n - i] = velVec[n-i-1]; + sum += velVec[n-i]; + } + velVec[0] = vel; + MechVelocity = sum/((float)n); + ElecVelocity = MechVelocity*_ppairs; + ElecVelocityFilt = 0.99f*ElecVelocityFilt + 0.01f*ElecVelocity; + } + +int PositionSensorMA700::GetRawPosition(){ + return raw; + } + +float PositionSensorMA700::GetMechPositionFixed(){ + return MechPosition+MechOffset; + } + +float PositionSensorMA700::GetMechPosition(){ + return MechPosition; + } + +float PositionSensorMA700::GetElecPosition(){ + return ElecPosition; + } + +float PositionSensorMA700::GetElecVelocity(){ + return ElecVelocity; + } + +float PositionSensorMA700::GetMechVelocity(){ + return MechVelocity; + } + +void PositionSensorMA700::ZeroPosition(){ + rotations = 0; + MechOffset = 0; + Sample(.00025f); + MechOffset = GetMechPosition(); + } + +void PositionSensorMA700:: SetElecOffset(float offset){ + ElecOffset = offset; + } +void PositionSensorMA700::SetMechOffset(float offset){ + MechOffset = offset; + } + +void PositionSensorMA700::WriteRegister( ControllerStruct * controller){ + BCT=0x2300|(controller->sidebct&0xff); + BCTREAD=0x1300; + int judge=(controller->sidebct&0xf00)>>8; + ETXY=0x2500|(judge<<4); + ETXYREAD=0x1500; + int ez; + // readAngleCmd = 0x1400; //shaorui modify + GPIOA->ODR &= ~(1 << 4); //shaorui ADD + spi->write( BCT); //shaorui ADD + GPIOA->ODR |= (1 << 4); //shaorui ADD + GPIOA->ODR &= ~(1 << 4); //shaorui ADD + _test=spi->write( BCTREAD); //shaorui ADD + GPIOA->ODR |= (1 << 4); //shaorui ADD + GPIOA->ODR &= ~(1 << 4); //shaorui ADD + _test1=ez=spi->write( ETXYREAD); //shaorui ADD + GPIOA->ODR |= (1 << 4); //shaorui ADD + ez&=0x000F; + GPIOA->ODR &= ~(1 << 4); //shaorui ADD + spi->write( ETXY|ez); //shaorui ADD //Extract last 14 bits + GPIOA->ODR |= (1 << 4); //shaorui ADD + + GPIOA->ODR &= ~(1 << 4); //shaorui ADD + _test2=spi->write( ETXYREAD); //shaorui ADD //Extract last 14 bits + GPIOA->ODR |= (1 << 4); //shaorui ADD + + + + + } + +int PositionSensorMA700::GetCPR(){ + return _CPR; + } + +void PositionSensorMA700::WriteLUT(int new_lut[128]){ + memcpy(offset_lut, new_lut, sizeof(offset_lut)); + } + +void PositionSensorMA700::ReadLUT(void){ + for(int z=0;z<128;z++) + { + printf("lut data:%d\n\r",offset_lut[z]);} + } + +int PositionSensorMA700::Gettest() { + return _test;} +int PositionSensorMA700::Gettest1() { + return _test1;} +int PositionSensorMA700::Gettest2() { + return _test2;} + + +/* + +PositionSensorEncoder::PositionSensorEncoder(int CPR, float offset, int ppairs) { + _ppairs = ppairs; + _CPR = CPR; + _offset = offset; + MechPosition = 0; + out_old = 0; + oldVel = 0; + raw = 0; + + // Enable clock for GPIOA + __GPIOA_CLK_ENABLE(); //equivalent from hal_rcc.h + + GPIOA->MODER |= GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1 ; //PA6 & PA7 as Alternate Function !< GPIO port mode register, Address offset: 0x00 + GPIOA->OTYPER |= GPIO_OTYPER_OT_6 | GPIO_OTYPER_OT_7 ; //PA6 & PA7 as Inputs !< GPIO port output type register, Address offset: 0x04 + GPIOA->OSPEEDR |= GPIO_OSPEEDER_OSPEEDR6 | GPIO_OSPEEDER_OSPEEDR7 ; //Low speed !< GPIO port output speed register, Address offset: 0x08 + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1 | GPIO_PUPDR_PUPDR7_1 ; //Pull Down !< GPIO port pull-up/pull-down register, Address offset: 0x0C + GPIOA->AFR[0] |= 0x22000000 ; //AF02 for PA6 & PA7 !< GPIO alternate function registers, Address offset: 0x20-0x24 + GPIOA->AFR[1] |= 0x00000000 ; //nibbles here refer to gpio8..15 !< GPIO alternate function registers, Address offset: 0x20-0x24 + + // configure TIM3 as Encoder input + // Enable clock for TIM3 + __TIM3_CLK_ENABLE(); + + TIM3->CR1 = 0x0001; // CEN(Counter ENable)='1' < TIM control register 1 + TIM3->SMCR = TIM_ENCODERMODE_TI12; // SMS='011' (Encoder mode 3) < TIM slave mode control register + TIM3->CCMR1 = 0x1111; // CC1S='01' CC2S='01' < TIM capture/compare mode register 1, maximum digital filtering + TIM3->CCMR2 = 0x0000; // < TIM capture/compare mode register 2 + TIM3->CCER = 0x0011; // CC1P CC2P < TIM capture/compare enable register + TIM3->PSC = 0x0000; // Prescaler = (0+1) < TIM prescaler + TIM3->ARR = CPR; // IM auto-reload register + + TIM3->CNT = 0x000; //reset the counter before we use it + + // Extra Timer for velocity measurement + + __TIM2_CLK_ENABLE(); + TIM3->CR2 = 0x030; //MMS = 101 + + TIM2->PSC = 0x03; + //TIM2->CR2 |= TIM_CR2_TI1S; + TIM2->SMCR = 0x24; //TS = 010 for ITR2, SMS = 100 (reset counter at edge) + TIM2->CCMR1 = 0x3; // CC1S = 11, IC1 mapped on TRC + + //TIM2->CR2 |= TIM_CR2_TI1S; + TIM2->CCER |= TIM_CCER_CC1P; + //TIM2->CCER |= TIM_CCER_CC1NP; + TIM2->CCER |= TIM_CCER_CC1E; + + + TIM2->CR1 = 0x01; //CEN, enable timer + + TIM3->CR1 = 0x01; // CEN + ZPulse = new InterruptIn(PC_4); + ZSense = new DigitalIn(PC_4); + //ZPulse = new InterruptIn(PB_0); + //ZSense = new DigitalIn(PB_0); + ZPulse->enable_irq(); + ZPulse->rise(this, &PositionSensorEncoder::ZeroEncoderCount); + //ZPulse->fall(this, &PositionSensorEncoder::ZeroEncoderCountDown); + ZPulse->mode(PullDown); + flag = 0; + + + //ZTest = new DigitalOut(PC_2); + //ZTest->write(1); + } + +void PositionSensorEncoder::Sample(float dt){ + + } + + +float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians. + int raw = TIM3->CNT; + float unsigned_mech = (6.28318530718f/(float)_CPR) * (float) ((raw)%_CPR); + return (float) unsigned_mech;// + 6.28318530718f* (float) rotations; +} + +float PositionSensorEncoder::GetElecPosition() { //returns rotor electrical angle in radians. + int raw = TIM3->CNT; + float elec = ((6.28318530718f/(float)_CPR) * (float) ((_ppairs*raw)%_CPR)) - _offset; + if(elec < 0) elec += 6.28318530718f; + return elec; +} + + + +float PositionSensorEncoder::GetMechVelocity(){ + + float out = 0; + float rawPeriod = TIM2->CCR1; //Clock Ticks + int currentTime = TIM2->CNT; + if(currentTime > 2000000){rawPeriod = currentTime;} + float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; // +/- 1 + float meas = dir*180000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; + if(isinf(meas)){ meas = 1;} + out = meas; + //if(meas == oldVel){ + // out = .9f*out_old; + // } + + + oldVel = meas; + out_old = out; + int n = 16; + float sum = out; + for (int i = 1; i < (n); i++){ + velVec[n - i] = velVec[n-i-1]; + sum += velVec[n-i]; + } + velVec[0] = out; + return sum/(float)n; + } + +float PositionSensorEncoder::GetElecVelocity(){ + return _ppairs*GetMechVelocity(); + } + +void PositionSensorEncoder::ZeroEncoderCount(void){ + if (ZSense->read() == 1 & flag == 0){ + if (ZSense->read() == 1){ + GPIOC->ODR ^= (1 << 4); + TIM3->CNT = 0x000; + //state = !state; + //ZTest->write(state); + GPIOC->ODR ^= (1 << 4); + //flag = 1; + } + } + } + +void PositionSensorEncoder::ZeroPosition(void){ + + } + +void PositionSensorEncoder::ZeroEncoderCountDown(void){ + if (ZSense->read() == 0){ + if (ZSense->read() == 0){ + GPIOC->ODR ^= (1 << 4); + flag = 0; + float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; + if(dir != dir){ + dir = dir; + rotations += dir; + } + + GPIOC->ODR ^= (1 << 4); + + } + } + } +void PositionSensorEncoder::SetElecOffset(float offset){ + + } + +int PositionSensorEncoder::GetRawPosition(void){ + return 0; + } + +int PositionSensorEncoder::GetCPR(){ + return _CPR; + } + + +void PositionSensorEncoder::WriteLUT(int new_lut[128]){ + memcpy(offset_lut, new_lut, sizeof(offset_lut)); + } +*/ \ No newline at end of file