Mini Cheetah Actuator Branch Superseded by: https://github.com/bgkatz/motorcontrol
Dependencies: mbed-dev-f303 FastPWM3
Superseded by: https://github.com/bgkatz/motorcontrol
Revision 56:fe5056ac6740, committed 2019-10-10
- Comitter:
- benkatz
- Date:
- Thu Oct 10 15:03:12 2019 +0000
- Parent:
- 55:c4c9fec8539c
- Commit message:
- fixed position-sensor turn-on weirdness; ; improved output zeroing to work independent of encoder rollover angle
Changed in this revision
--- a/PositionSensor/PositionSensor.cpp Fri Oct 04 14:18:39 2019 +0000 +++ b/PositionSensor/PositionSensor.cpp Thu Oct 10 15:03:12 2019 +0000 @@ -23,6 +23,13 @@ oldModPosition = 0; oldVel = 0; raw = 0; + first_sample = 0; + for(int i = 0; i<100; i++) // Initial measurement is really noisy + { + spi->write(0); + wait_us(100); + } + } void PositionSensorAM5147::Sample(float dt){ @@ -36,12 +43,16 @@ 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; // Correct for nonlinearity with lookup table from calibration - if(angle - old_counts > _CPR/2){ - rotations -= 1; - } - else if (angle - old_counts < -_CPR/2){ - rotations += 1; - } + + if(first_sample){ + if(angle - old_counts > _CPR/2){ + rotations -= 1; + } + else if (angle - old_counts < -_CPR/2){ + rotations += 1; + } + } + if(!first_sample){first_sample = 1;} old_counts = angle; oldModPosition = modPosition; @@ -115,6 +126,7 @@ } void PositionSensorAM5147::SetMechOffset(float offset){ MechOffset = offset; + first_sample = 0; } int PositionSensorAM5147::GetCPR(){
--- a/PositionSensor/PositionSensor.h Fri Oct 04 14:18:39 2019 +0000 +++ b/PositionSensor/PositionSensor.h Thu Oct 10 15:03:12 2019 +0000 @@ -35,7 +35,7 @@ //DigitalOut *ZTest; virtual void ZeroEncoderCount(void); virtual void ZeroEncoderCountDown(void); - int _CPR, flag, rotations, _ppairs, raw; + int _CPR, flag, rotations, _ppairs, raw, first_sample; //int state; float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40]; int offset_lut[128]; @@ -58,7 +58,7 @@ virtual void WriteLUT(int new_lut[128]); private: float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt; - int raw, _CPR, rotations, old_counts, _ppairs; + int raw, _CPR, rotations, old_counts, _ppairs, first_sample; SPI *spi; DigitalOut *cs; int readAngleCmd;
--- a/main.cpp Fri Oct 04 14:18:39 2019 +0000 +++ b/main.cpp Thu Oct 10 15:03:12 2019 +0000 @@ -183,6 +183,10 @@ //for (delay = 0; delay < 55; delay++); spi.Sample(DT); // sample position sensor + /* + if(count < 10){printf("%d\n\r", spi.GetRawPosition());} + count ++; + */ controller.adc2_raw = ADC2->DR; // Read ADC Data Registers controller.adc1_raw = ADC1->DR; controller.adc3_raw = ADC3->DR; @@ -486,6 +490,10 @@ if(isnan(I_MAX_CONT) || I_MAX_CONT==-1){I_MAX_CONT = 14.0f;} spi.SetElecOffset(E_OFFSET); // Set position sensor offset spi.SetMechOffset(M_OFFSET); + spi.Sample(1.0f); + if(spi.GetMechPosition() > PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point + else if (spi.GetMechPosition() < -PI){spi.SetMechOffset(M_OFFSET-2.0f*PI);} + int lut[128] = {0}; memcpy(&lut, &ENCODER_LUT, sizeof(lut)); spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table