Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

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?

UserRevisionLine numberNew 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