LEGO Mindstorms EV3


Speed Calculation
*
*  - Time is sampled every edge on the tacho
*      - Timer used is 64bit timer plus (P3) module (dual 32bit un-chained mode)
*      - 64bit timer is running 33Mhz (24Mhz (Osc) * 22 (Multiplier) / 2 (Post divider) / 2 (DIV2)) / 4 (T64 prescaler)
*      - When reading the timer it is divided by 256 => timer is a factor 256 slower
*
*
*  - Tacho counter is updated on every edge of the tacho INTx pin signal
*  - Time capture is updated on every edge of the tacho INTx pin signal
*
*
*  - Speed is calculated from the following parameters
*
*      - Time is measured edge to edge of the tacho interrupt pin. Average of time is always minimum 2 pulses
*        (1 high + 1 low period or 1 low + 1 high period) because the duty of the high and low period of the
*        tacho pulses are not always 50%.
*        - Average of the large motor
*          - Above speed 80 it is:          64 samples
*          - Between speed 60 - 80 it is:   32 samples
*          - Between speed 40 - 60 it is:   16 samples
*          - below speed 40 it is:           4 samples
*        - Average of the medium motor
*          - Above speed 80 it is:          16 samples
*          - Between speed 60 - 80 it is:    8 samples
*          - Between speed 40 - 60 it is:    4 samples
*          - below speed 40 it is:           2 sample
*
*      - Number of samples is always determined based on 1 sample meaning 1 low period or 1 high period,
*        this is to enable fast adoption to changes in speed. Medium motor has the critical timing because
*        it can change speed and direction very fast.
*
*      - Large Motor
*        - Maximum speed of the Large motor is approximately 2mS per tacho pulse (low + high period)
*          resulting in minimum timer value of: 2mS / (1/(33MHz / 256)) = 256 T64 timer ticks.
*          Because 1 sample is based on only half a period minimum speed is 256/2 = 128.
*        - Minimum speed of the large motor is a factor of 100 less than max. speed
*          max. speed timer ticks * 100 => 256 * 100 = 25600 T64 timer ticks
*          Because 1 sample is based on only half a period minimum speed is 25600/2 = 12800.
*
*
*      - Medium Motor
*        - Maximum speed of the medium motor is approximately 1,25mS per tacho pulse (low + high period)
*          resulting in minimum timer value og: 1,25mS / (1/(33MHz / 256)) = 162 (approximately)
*          Because 1 sample is based on only half a period minimum speed is 162/2 = 81.
*        - Minimum speed of the medium motor is a factor of 100 less than max. speed
*          max. speed timer ticks * 100 => 162 * 100 = 16200 T64 timer ticks
*          Because 1 sample is based on only half a period minimum speed is 16200/2 = 8100.
*
*      - Actual speed is then calculated as:
*        - Medium motor:
*          8100 * number of samples / actual time elapsed for number of samples
*        - Large motor:
*          12800 * number of samples / actual time elapsed for number of samples
*
*
*
*
*  - Tacho pulse examples:
*
*
*    - Normal
*
*      ----,     ,------,     ,------,     ,------,
*          |     |      |     |      |     |      |
*          |     |      |     |      |     |      |
*          '-----'      '-----'      '-----'      '-- DIRx signal
*
*
*         ----,     ,------,     ,------,     ,------ INTx signal
*             |     |      |     |      |     |
*             |     |      |     |      |     |
*             '-----'      '-----'      '-----'
*
*             ^     ^      ^     ^      ^     ^
*             |     |      |     |      |     |
*             |   Timer    |   Timer    |   Timer
*             |     +      |     +      |     +
*             |  Counter   |  Counter   |  Counter
*             |            |            |
*           Timer        Timer        Timer
*             +            +            +
*          Counter      Counter      Counter
*
*
*
*    - Direction change
*
*      DirChgPtr variable is used to indicate how many timer samples have been sampled
*      since direction has been changed. DirChgPtr is set to 0 when tacho interrupt detects
*      direction change and then it is counted up for every timer sample. So when DirChgPtr
*      has the value of 2 then there must be 2 timer samples in the the same direction
*      available.
*
*      ----,     ,------,     ,------,     ,------,     ,---
*          |     |      |     |      |     |      |     |
*          |     |      |     |      |     |      |     |
*          '-----'      '-----'      '-----'      '-----'   DIRx signal
*
*
*       ------,     ,-------------,     ,------,     ,------INTx signal
*             |     |             |     |      |     |
*             |     |             |     |      |     |
*             '-----'             '-----'      '-----'
*
*             ^     ^             ^     ^      ^     ^
*             |     |             |     |      |     |
*           Timer   |           Timer   |    Timer   |
*             +     |             +     |      +     |
*          Counter  |          Counter  |   Counter  |
*             +     |             +     |      +     |
*       DirChgPtr++ |       DirChgPtr=0 |DirChgPtr++ |
*                 Timer               Timer        Timer
*                   +                   +            +
*                Counter             Counter      Counter
*                   +                   +            +
*               DirChgPtr++         DirChgPtr++  DirChgPtr++
*
*
*
*
*      ----,     ,------,           ,------,      ,----
*          |     |      |           |      |      |
*          |     |      |           |      |      |
*          '-----'      '-----------'      '------'          DIRx signal
*
*
*       ------,     ,------,     ,------,     ,------,       INTx signal
*             |     |      |     |      |     |      |
*             |     |      |     |      |     |      |
*             '-----'      '-----'      '-----'      '----
*
*             ^     ^      ^     ^      ^     ^      ^
*             |     |      |     |      |     |      |
*           Timer   |    Timer   |    Timer   |    Timer
*             +     |      +     |      +     |      +
*          Counter  |   Counter  |   Counter  |   Counter
*             +     |      +     |      +     |      +
*        DirChgPtr++| DirChgPtr++| DirChgPtr++| DirChgPtr++
*                 Timer        Timer        Timer
*                   +            +            +
*                Counter      Counter      Counter
*                   +            +            +
*               DirChgPtr++  DirChgPtr=0  DirChgPtr++
*
* 

LEGO® Robotics Firmware Documentation
Confidential Information © 2013 The LEGO Group