Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

PEC.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:
12:35f5cb75743e 
Fix bug in nudging with small speeds mentioned in the last commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 1 /*
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 2 * PEC.cpp
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 3 *
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 4 * Created on: Sep 9, 2018
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 5 * Author: caoyu
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 6 */
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 7
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 8 #include "PEC.h"
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 9 #include "Axis.h"
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 10 #include "TelescopeConfiguration.h"
caoyuan9642 12:35f5cb75743e 11 #include <math.h>
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 12
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 13 PEC::PEC(Axis &a) :
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 14 axis(a), enabled(false), indexOffset(0) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 15 thread = new Thread(osPriorityAboveNormal,
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 16 OS_STACK_SIZE, NULL, "PEC Thread");
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 17 thread->start(callback(this, &PEC::task));
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 18
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 19 granularity = TelescopeConfiguration::getInt("pec_granularity");
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 20 pecData = new float[granularity];
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 21 for (int i = 0; i < granularity; i++) {
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 22 pecData[i] = 0.0f;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 23 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 24 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 25
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 26 void PEC::task() {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 27
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 28 Timer t;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 29 t.start();
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 30 while (true) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 31 if (enabled) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 32 double degTeeth = 360.0
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 33 / TelescopeConfiguration::getDouble("worm_teeth"); // Degree per teeth
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 34 double period = degTeeth
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 35 / (axis.getTrackSpeedSidereal() * sidereal_speed); // Worm rotation period
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 36
caoyuan9642 12:35f5cb75743e 37 int currentIndex = (int) floor(
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 38 remainder((axis.getAngleDeg() - indexOffset), degTeeth)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 39 * granularity);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 40 if (currentIndex < 0)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 41 currentIndex += granularity;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 42
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 43 double interval = period / granularity; // Time to wait between executing each step
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 44
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 45 uint64_t startTimeUs = t.read_high_resolution_us();
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 46 uint64_t count = 0;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 47 double intervalUs = interval * 1000000.0;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 48
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 49 while (enabled) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 50 float correction = pecData[currentIndex]; // Correction in arcseconds
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 51 if (++currentIndex >= granularity)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 52 currentIndex -= granularity;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 53
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 54 if (correction != 0.0f) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 55 // Calculate guide time in milliseconds
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 56 // 3.6 = 3600 (arcsec/degree) * 1000 (ms/s)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 57 int guidetime = correction / float(axis.guideSpeed) / 3.6f;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 58
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 59 // Positive guidetime means needs to correct towards east
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 60 // Negative guidetime means needs to correct towards west
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 61 axis.guide(AXIS_ROTATE_NEGATIVE, guidetime);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 62 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 63
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 64 // Make sure mean interval is very accurate
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 65 count++;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 66 while ((t.read_high_resolution_us() - startTimeUs)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 67 < intervalUs * count) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 68 Thread::wait(1);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 69 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 70 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 71 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 72 Thread::wait(10);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 73 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 74 }