Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Simplify and improve the writeMicroseconds() calculations #66

Merged
merged 3 commits into from
Nov 3, 2019

Conversation

photodude
Copy link
Contributor

@photodude photodude commented Oct 29, 2019

  • Describe the scope of your change
    Overall this change makes less calculation steps, and uses fewer variables. The change should be faster and more accurate to the intended results.

Checked the calculations by hand

Use Datasheet 7.3.5 Equation 1 for frequency from prescale
https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf
Frequency = (ClockFrequency/(PreScale+1))(1/BitRate)
and from the rest of the calculation
us per period = PulseLength/Frequency
us per bit = us per period/BitRate
we can combine to get
us per bit = (PulseLength/((ClockFrequency/(PreScale+1))
(1/BitRate)))*(1/BitRate)

This can be simplified to (PulseLength (PreScale + 1))/ClockFrequency
this formula was implemented in proposed code

checked by setting chip frequency to 60Hz
read prescale = 105
us per bit = 4.069 with 25Mhz (a PWM frequency of about 57.58hz)
Choosing the FREQUENCY_CALIBRATED constant listed at 104.3% higher or 26.075MHz
we get
us per bit = 4.1035 or a PWM frequency of about 60.056Hz much closer to the set 60Hz

Overall this change makes less calculation steps, and uses fewer variables. The change should be faster and more accurate to the intended results.

  • Describe any known limitations with your change.

    • Output is not precise due to the calculations involved (same as the example servo sketch's setServoPulse() method)
  • Please run any tests or examples that can exercise your modified code.
    Calculations were checked by hand and are an improvement

Checked the calculations by hand

Use Datasheet 7.3.5 Equation 1 for frequency from prescale
https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf
Frequency = (ClockFrequency/(PreScale+1))*(1/BitRate)
and from the rest of the calculation
us per period = PulseLength/Frequency
us per bit = us per period/BitRate
 we can combine to get
us per bit = (PulseLength/((ClockFrequency/(PreScale+1))*(1/BitRate)))*(1/BitRate)

This can be simplified to (PulseLength (PreScale + 1))/ClockFrequency 
this formula was implemented in proposed code

checked by setting chip frequency to 60Hz
read prescale = 105
us per bit = 4.069 with 25Mhz (a PWM frequency of about 57.3hz)
Choosing the FREQUENCY_CALIBRATED constant listed at 104.3% higher or 26.075MHz
we get 
us per bit = 4.1035 or a PWM frequency of about 60.056Hz much closer to the set 60Hz 

Overall this change makes less calculation steps, and uses fewer variables. The change should be faster and more accurate to the intended results.
debug statements should be Wrapped in `#ifdef ENABLE_DEBUG_OUTPUT`
@Bolukan
Copy link
Contributor

Bolukan commented Oct 29, 2019

I looked to it yesterday and had the same thoughts how it could be shortened. The code also runned in some rounding problems that seems to be solved now. PS: You could make prescale uint16_t

@photodude
Copy link
Contributor Author

@Bolukan Thanks. I'll make that change.
Prescale is 3 to 255 right? Would a unit8_t be all that's needed to store those values, or is there a reason to stick with unit16_t?

@Bolukan
Copy link
Contributor

Bolukan commented Oct 29, 2019

prescale += 1; is the reason you need 16 bits.

@photodude
Copy link
Contributor Author

@Bolukan Got it. Thanks.

Prescale values are integers 3 to 255 with a maximum of 255+1 for the calculations.
@photodude
Copy link
Contributor Author

@ladyada Let me know if there are any other changes. This should be good to go now.

@photodude
Copy link
Contributor Author

Changes tested on Arduino Uno and the Adafruit PCA9685 16-Channel Servo Driver
Also tested in a wrapped library as in issue #67 (@Bolukan found an amazing solution to solve the frequency lock issue)

This change is more robust and accurate.

@ladyada
Copy link
Member

ladyada commented Nov 3, 2019

ok is this ready for my review now?

@photodude
Copy link
Contributor Author

@ladyada Yes, it is ready for review.
Also confirmed this is working with an Arduino Uno and two Adafruit PCA9685 16-Channel Servo Drivers chained and setting the clocks individually.

@ladyada
Copy link
Member

ladyada commented Nov 3, 2019

ok i refactored the way we tell the PCA9685 the freq its running at. please check the version in 'master' now :)

@ladyada ladyada merged commit 33ec0da into adafruit:master Nov 3, 2019
@photodude photodude deleted the patch-1 branch November 4, 2019 01:13
@photodude
Copy link
Contributor Author

@ladyada Version 2.2.0 seems to be working as expected on my Uno with the two Adafruit PCA9685 16-Channel Servo Drivers I have.

With the refactored changes in 2.2.0 the new default after setting setPWMFreq(60); I get a return prescale of 101
After bumping the setOscillatorFrequency(26075000); I get a return prescale of 105

That matches what I was getting before, the refactor is nice.
Thanks for merging this PR.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants