Test the set param mode
Diff: PositionSensor/PositionSensor.cpp
- Revision:
- 7:dc5f27756e02
- Parent:
- 6:4ee1cdc43aa8
- Child:
- 8:10ae7bc88d6e
--- a/PositionSensor/PositionSensor.cpp Sat Mar 12 19:55:52 2016 +0000 +++ b/PositionSensor/PositionSensor.cpp Tue Mar 29 01:05:46 2016 +0000 @@ -29,9 +29,9 @@ 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 = 0xffffffff; // reload at 0xfffffff < TIM auto-reload register + TIM3->ARR = 0xfffffff; // reload at 0xfffffff < TIM auto-reload register - TIM3->CNT = 0x8000; //reset the counter before we use it + TIM3->CNT = 0x000; //reset the counter before we use it // Extra Timer for velocity measurement /* @@ -57,7 +57,9 @@ 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); @@ -68,29 +70,15 @@ float PositionSensorEncoder::GetMechPosition() { //returns rotor angle in radians. int raw = TIM3->CNT-0x8000; - if (raw < 0) raw += _CPR; - if (raw >= _CPR) raw -= _CPR; - float signed_mech = fmod(((6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f); - if (signed_mech < 0){ - return signed_mech + 6.28318530718f; - } - else{ - return signed_mech; - } + 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-0x8000; - if (raw < 0) raw += _CPR; - if (raw >= _CPR) raw -= _CPR; - float signed_elec = fmod((7.0f*(6.28318530718f*(raw)/(float)_CPR + _offset)), 6.28318530718f); //7 pole pairs - //float signed_elec = (7*(6.28318530718*(TIM3->CNT-0x8000)/(float)_CPR + _offset)); - if (signed_elec < 0){ - return signed_elec + 6.28318530718f; - } - else{ - return signed_elec; - } + + int raw = TIM3->CNT; + float unsigned_elec = (6.28318530718f/(float)_CPR) * (float) ((7*raw)%_CPR); + return unsigned_elec; } float PositionSensorEncoder::GetElecVelocity(){ @@ -102,16 +90,42 @@ float PositionSensorEncoder::GetMechVelocity(){ float rawPeriod = TIM2->CCR1; //Clock Ticks float dir = -2.0f*(float)(((TIM3->CR1)>>4)&1)+1.0f; // +/- 1 - return dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; - + return dir*90000000.0f*(6.28318530718f/(float)_CPR)/rawPeriod; } void PositionSensorEncoder::ZeroEncoderCount(void){ - if (ZSense->read() == 1){ + if (ZSense->read() == 1 & flag == 0){ if (ZSense->read() == 1){ - TIM3->CNT=0x8000; + GPIOC->ODR ^= (1 << 4); + dir = -2*((int)(((TIM3->CR1)-0x000)>>4)&1)+1; + int old_count = _CPR*rotations + TIM3->CNT; + if( abs(_CPR*(rotations+dir) - old_count) <= _CPR>>2){ + rotations += dir; + } + + TIM3->CNT = 0x000; + //state = !state; //ZTest->write(state); + GPIOC->ODR ^= (1 << 4); + //flag = 1; + } + } + } + +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); + } } } \ No newline at end of file