Telescope Control Library
StepOut.cpp@19:fd854309cb4c, 2018-09-24 (annotated)
- Committer:
- caoyu@caoyuan9642-desktop.MIT.EDU
- Date:
- Mon Sep 24 19:36:48 2018 -0400
- Revision:
- 19:fd854309cb4c
- Parent:
- 17:7e47bc1630c0
Fix bug in nudging with small speeds mentioned in the last commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
caoyuan9642 | 0:6cb2eaf8b133 | 1 | /* |
caoyuan9642 | 0:6cb2eaf8b133 | 2 | #include <StepOut.h> |
caoyuan9642 | 0:6cb2eaf8b133 | 3 | * ControllablePWMOut.cpp |
caoyuan9642 | 0:6cb2eaf8b133 | 4 | * |
caoyuan9642 | 0:6cb2eaf8b133 | 5 | * Created on: 2018Äê2ÔÂ9ÈÕ |
caoyuan9642 | 0:6cb2eaf8b133 | 6 | * Author: caoyuan9642 |
caoyuan9642 | 0:6cb2eaf8b133 | 7 | */ |
caoyuan9642 | 0:6cb2eaf8b133 | 8 | |
caoyuan9642 | 0:6cb2eaf8b133 | 9 | #include "mbed.h" |
caoyuan9642 | 0:6cb2eaf8b133 | 10 | #include <StepOut.h> |
caoyuan9642 | 0:6cb2eaf8b133 | 11 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 19:fd854309cb4c | 12 | #define MAX_PERIOD 1000000LL |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 13 | |
caoyuan9642 | 0:6cb2eaf8b133 | 14 | void StepOut::start() |
caoyuan9642 | 0:6cb2eaf8b133 | 15 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 16 | if (status == IDLE && freq > 0) // Start only when idle and frequency is not zero |
caoyuan9642 | 0:6cb2eaf8b133 | 17 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 18 | core_util_critical_section_enter(); |
caoyuan9642 | 0:6cb2eaf8b133 | 19 | status = STEPPING; |
caoyuan9642 | 0:6cb2eaf8b133 | 20 | this->write(0.5f); |
caoyuan9642 | 0:6cb2eaf8b133 | 21 | tim.reset(); |
caoyuan9642 | 0:6cb2eaf8b133 | 22 | core_util_critical_section_exit(); |
caoyuan9642 | 0:6cb2eaf8b133 | 23 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 24 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 25 | |
caoyuan9642 | 0:6cb2eaf8b133 | 26 | void StepOut::stop() |
caoyuan9642 | 0:6cb2eaf8b133 | 27 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 28 | if (status == STEPPING) |
caoyuan9642 | 0:6cb2eaf8b133 | 29 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 30 | core_util_critical_section_enter(); |
caoyuan9642 | 0:6cb2eaf8b133 | 31 | status = IDLE; |
caoyuan9642 | 0:6cb2eaf8b133 | 32 | this->write(0); |
caoyuan9642 | 0:6cb2eaf8b133 | 33 | stepCount += (int64_t) (freq * tim.read_high_resolution_us() / 1.0E6); |
caoyuan9642 | 0:6cb2eaf8b133 | 34 | core_util_critical_section_exit(); |
caoyuan9642 | 0:6cb2eaf8b133 | 35 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 36 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 37 | |
caoyuan9642 | 0:6cb2eaf8b133 | 38 | double StepOut::setFrequency(double frequency) |
caoyuan9642 | 0:6cb2eaf8b133 | 39 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 40 | if (frequency > 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 41 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 42 | int64_t us_period = ceil(1.0E6 / frequency); /*Ceil to the next microsecond*/ |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 43 | if (us_period > MAX_PERIOD) |
caoyuan9642 | 0:6cb2eaf8b133 | 44 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 45 | // Prevent overflow |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 46 | us_period = MAX_PERIOD; |
caoyuan9642 | 0:6cb2eaf8b133 | 47 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 48 | if (status == IDLE) |
caoyuan9642 | 0:6cb2eaf8b133 | 49 | this->period_us(us_period); |
caoyuan9642 | 0:6cb2eaf8b133 | 50 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 51 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 52 | core_util_critical_section_enter(); |
caoyuan9642 | 0:6cb2eaf8b133 | 53 | stop(); /*Stop to correctly update the stepCount*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 54 | this->period_us(us_period); |
caoyuan9642 | 0:6cb2eaf8b133 | 55 | start(); |
caoyuan9642 | 0:6cb2eaf8b133 | 56 | core_util_critical_section_exit(); |
caoyuan9642 | 0:6cb2eaf8b133 | 57 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 58 | freq = 1.0E6 / us_period; // get CORRECT frequency! |
caoyuan9642 | 0:6cb2eaf8b133 | 59 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 60 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 61 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 62 | // frequency=0 effectively means stop |
caoyuan9642 | 0:6cb2eaf8b133 | 63 | freq = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 64 | if (status == STEPPING) |
caoyuan9642 | 0:6cb2eaf8b133 | 65 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 66 | stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 67 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 68 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 69 | return freq; // Return the accurate period |
caoyuan9642 | 0:6cb2eaf8b133 | 70 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 71 | |
caoyuan9642 | 0:6cb2eaf8b133 | 72 | void StepOut::resetCount() |
caoyuan9642 | 0:6cb2eaf8b133 | 73 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 74 | core_util_critical_section_enter(); |
caoyuan9642 | 0:6cb2eaf8b133 | 75 | stepCount = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 76 | if (status == STEPPING) |
caoyuan9642 | 0:6cb2eaf8b133 | 77 | tim.reset(); |
caoyuan9642 | 0:6cb2eaf8b133 | 78 | core_util_critical_section_exit(); |
caoyuan9642 | 0:6cb2eaf8b133 | 79 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 80 | |
caoyuan9642 | 0:6cb2eaf8b133 | 81 | int64_t StepOut::getCount() |
caoyuan9642 | 0:6cb2eaf8b133 | 82 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 83 | if (status == IDLE) |
caoyuan9642 | 0:6cb2eaf8b133 | 84 | return stepCount; |
caoyuan9642 | 0:6cb2eaf8b133 | 85 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 86 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 87 | // Fixed: use read_high_resolution_us() to prevent overflow every ~30min |
caoyuan9642 | 0:6cb2eaf8b133 | 88 | return stepCount |
caoyuan9642 | 0:6cb2eaf8b133 | 89 | + (int64_t) (freq * tim.read_high_resolution_us() / 1.0E6); /*Calculate count at now*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 90 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 91 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 92 | |
caoyuan9642 | 0:6cb2eaf8b133 | 93 |