Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

Axis.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:
15:0bdda8dadafe 
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 * Axis.cpp
caoyuan9642 0:6cb2eaf8b133 3 *
caoyuan9642 2:2ee28add0821 4 * Created on: 2018��2��24��
caoyuan9642 0:6cb2eaf8b133 5 * Author: caoyuan9642
caoyuan9642 0:6cb2eaf8b133 6 */
caoyuan9642 0:6cb2eaf8b133 7
caoyuan9642 0:6cb2eaf8b133 8 #include <Axis.h>
caoyuan9642 0:6cb2eaf8b133 9
caoyuan9642 2:2ee28add0821 10 #define AXIS_DEBUG 0
caoyuan9642 0:6cb2eaf8b133 11
caoyuan9642 0:6cb2eaf8b133 12 static inline double min(double x, double y)
caoyuan9642 0:6cb2eaf8b133 13 {
caoyuan9642 0:6cb2eaf8b133 14 return (x < y) ? x : y;
caoyuan9642 0:6cb2eaf8b133 15 }
caoyuan9642 0:6cb2eaf8b133 16 Axis::Axis(double stepsPerDeg, StepperMotor *stepper, const char *name) :
caoyuan9642 0:6cb2eaf8b133 17 stepsPerDeg(stepsPerDeg), stepper(stepper), axisName(name), currentSpeed(
caoyuan9642 0:6cb2eaf8b133 18 0), currentDirection(AXIS_ROTATE_POSITIVE), slewSpeed(
caoyuan9642 0:6cb2eaf8b133 19 TelescopeConfiguration::getDouble("default_slew_speed")), trackSpeed(
caoyuan9642 0:6cb2eaf8b133 20 TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 21 "default_track_speed_sidereal") * sidereal_speed), guideSpeed(
caoyuan9642 0:6cb2eaf8b133 22 TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 23 "default_guide_speed_sidereal") * sidereal_speed), status(
caoyuan9642 0:6cb2eaf8b133 24 AXIS_STOPPED), slewState(AXIS_NOT_SLEWING), slew_finish_sem(0,
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 25 1), slew_finish_state(FINISH_COMPLETE), guiding(false), pec(NULL), pecEnabled(false)
caoyuan9642 0:6cb2eaf8b133 26 {
caoyuan9642 0:6cb2eaf8b133 27 if (stepsPerDeg <= 0)
caoyuan9642 0:6cb2eaf8b133 28 error("Axis: steps per degree must be > 0");
caoyuan9642 0:6cb2eaf8b133 29
caoyuan9642 0:6cb2eaf8b133 30 if (!stepper)
caoyuan9642 0:6cb2eaf8b133 31 error("Axis: stepper must be defined");
caoyuan9642 0:6cb2eaf8b133 32
caoyuan9642 0:6cb2eaf8b133 33 taskName = new char[strlen(name) + 10];
caoyuan9642 0:6cb2eaf8b133 34 strcpy(taskName, name);
caoyuan9642 0:6cb2eaf8b133 35 strcat(taskName, " task");
caoyuan9642 0:6cb2eaf8b133 36 /*Start the task-handling thread*/
caoyuan9642 0:6cb2eaf8b133 37 task_thread = new Thread(osPriorityAboveNormal,
caoyuan9642 0:6cb2eaf8b133 38 OS_STACK_SIZE, NULL, taskName);
caoyuan9642 0:6cb2eaf8b133 39 task_thread->start(callback(this, &Axis::task));
caoyuan9642 0:6cb2eaf8b133 40 tim.start();
caoyuan9642 0:6cb2eaf8b133 41 }
caoyuan9642 0:6cb2eaf8b133 42
caoyuan9642 0:6cb2eaf8b133 43 Axis::~Axis()
caoyuan9642 0:6cb2eaf8b133 44 {
caoyuan9642 0:6cb2eaf8b133 45 // Wait until the axis is stopped
caoyuan9642 0:6cb2eaf8b133 46 if (status != AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 47 {
caoyuan9642 0:6cb2eaf8b133 48 stop();
caoyuan9642 0:6cb2eaf8b133 49 while (status != AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 50 {
caoyuan9642 0:6cb2eaf8b133 51 Thread::wait(100);
caoyuan9642 0:6cb2eaf8b133 52 }
caoyuan9642 0:6cb2eaf8b133 53 }
caoyuan9642 0:6cb2eaf8b133 54
caoyuan9642 0:6cb2eaf8b133 55 // Terminate the task thread to prevent illegal access to destroyed objects.
caoyuan9642 0:6cb2eaf8b133 56 task_thread->terminate();
caoyuan9642 0:6cb2eaf8b133 57 delete task_thread;
caoyuan9642 0:6cb2eaf8b133 58 delete taskName;
caoyuan9642 0:6cb2eaf8b133 59 }
caoyuan9642 0:6cb2eaf8b133 60
caoyuan9642 0:6cb2eaf8b133 61 void Axis::task()
caoyuan9642 0:6cb2eaf8b133 62 {
caoyuan9642 0:6cb2eaf8b133 63
caoyuan9642 0:6cb2eaf8b133 64 /*Main loop of RotationAxis*/
caoyuan9642 0:6cb2eaf8b133 65 while (true)
caoyuan9642 0:6cb2eaf8b133 66 {
caoyuan9642 0:6cb2eaf8b133 67 /*Get next message*/
caoyuan9642 0:6cb2eaf8b133 68 msg_t *message;
caoyuan9642 0:6cb2eaf8b133 69 enum msg_t::sig_t signal;
caoyuan9642 0:6cb2eaf8b133 70 float value;
caoyuan9642 0:6cb2eaf8b133 71 axisrotdir_t dir;
caoyuan9642 0:6cb2eaf8b133 72 bool wc;
caoyuan9642 0:6cb2eaf8b133 73
caoyuan9642 0:6cb2eaf8b133 74 // Wait for next message
caoyuan9642 0:6cb2eaf8b133 75 osEvent evt = task_queue.get();
caoyuan9642 0:6cb2eaf8b133 76 if (evt.status == osEventMessage)
caoyuan9642 0:6cb2eaf8b133 77 {
caoyuan9642 0:6cb2eaf8b133 78 /*New message arrived. Copy the data and free is asap*/
caoyuan9642 0:6cb2eaf8b133 79 message = (msg_t*) evt.value.p;
caoyuan9642 0:6cb2eaf8b133 80 signal = message->signal;
caoyuan9642 0:6cb2eaf8b133 81 value = message->value;
caoyuan9642 0:6cb2eaf8b133 82 dir = message->dir;
caoyuan9642 0:6cb2eaf8b133 83 wc = message->withCorrection;
caoyuan9642 0:6cb2eaf8b133 84 task_pool.free(message);
caoyuan9642 0:6cb2eaf8b133 85 }
caoyuan9642 0:6cb2eaf8b133 86 else
caoyuan9642 0:6cb2eaf8b133 87 {
caoyuan9642 0:6cb2eaf8b133 88 /*Error*/
caoyuan9642 0:6cb2eaf8b133 89 debug("%s: Error fetching the task queue.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 90 continue;
caoyuan9642 0:6cb2eaf8b133 91 }
caoyuan9642 0:6cb2eaf8b133 92 debug_if(AXIS_DEBUG, "%s: MSG %d %f %d 0x%8x\n", axisName, signal,
caoyuan9642 0:6cb2eaf8b133 93 value, dir);
caoyuan9642 0:6cb2eaf8b133 94
caoyuan9642 0:6cb2eaf8b133 95 /*Check the type of the signal, and start corresponding operations*/
caoyuan9642 0:6cb2eaf8b133 96 switch (signal)
caoyuan9642 0:6cb2eaf8b133 97 {
caoyuan9642 0:6cb2eaf8b133 98 case msg_t::SIGNAL_SLEW_TO:
caoyuan9642 0:6cb2eaf8b133 99 if (status == AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 100 {
caoyuan9642 0:6cb2eaf8b133 101 slew(dir, value, false, wc);
caoyuan9642 0:6cb2eaf8b133 102 }
caoyuan9642 0:6cb2eaf8b133 103 else
caoyuan9642 0:6cb2eaf8b133 104 {
caoyuan9642 0:6cb2eaf8b133 105 debug("%s: being slewed while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 106 axisName);
caoyuan9642 0:6cb2eaf8b133 107 }
caoyuan9642 0:6cb2eaf8b133 108 debug_if(0, "%s: SIG SLEW 0x%08x\n", axisName, Thread::gettid());
caoyuan9642 0:6cb2eaf8b133 109 slew_finish_sem.release(); /*Send a signal so that the caller is free to run*/
caoyuan9642 0:6cb2eaf8b133 110 break;
caoyuan9642 0:6cb2eaf8b133 111 case msg_t::SIGNAL_SLEW_INDEFINITE:
caoyuan9642 0:6cb2eaf8b133 112 if (status == AXIS_STOPPED || status == AXIS_INERTIAL)
caoyuan9642 0:6cb2eaf8b133 113 {
caoyuan9642 0:6cb2eaf8b133 114 slew(dir, 0.0, true, false);
caoyuan9642 0:6cb2eaf8b133 115 }
caoyuan9642 0:6cb2eaf8b133 116 else
caoyuan9642 0:6cb2eaf8b133 117 {
caoyuan9642 0:6cb2eaf8b133 118 debug("%s: being slewed while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 119 axisName);
caoyuan9642 0:6cb2eaf8b133 120 }
caoyuan9642 0:6cb2eaf8b133 121 break;
caoyuan9642 0:6cb2eaf8b133 122 case msg_t::SIGNAL_TRACK:
caoyuan9642 0:6cb2eaf8b133 123 if (status == AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 124 {
caoyuan9642 0:6cb2eaf8b133 125 track(dir);
caoyuan9642 0:6cb2eaf8b133 126 }
caoyuan9642 0:6cb2eaf8b133 127 else
caoyuan9642 0:6cb2eaf8b133 128 {
caoyuan9642 0:6cb2eaf8b133 129 debug("%s: trying to track while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 130 axisName);
caoyuan9642 0:6cb2eaf8b133 131 }
caoyuan9642 0:6cb2eaf8b133 132 break;
caoyuan9642 0:6cb2eaf8b133 133 default:
caoyuan9642 0:6cb2eaf8b133 134 debug("%s: undefined signal %d\n", axisName, message->signal);
caoyuan9642 0:6cb2eaf8b133 135 }
caoyuan9642 0:6cb2eaf8b133 136 }
caoyuan9642 0:6cb2eaf8b133 137 }
caoyuan9642 0:6cb2eaf8b133 138
caoyuan9642 0:6cb2eaf8b133 139 void Axis::slew(axisrotdir_t dir, double dest, bool indefinite,
caoyuan9642 0:6cb2eaf8b133 140 bool useCorrection)
caoyuan9642 0:6cb2eaf8b133 141 {
caoyuan9642 0:6cb2eaf8b133 142 if (!indefinite && (isnan(dest) || isinf(dest)))
caoyuan9642 0:6cb2eaf8b133 143 {
caoyuan9642 0:6cb2eaf8b133 144 debug("%s: invalid angle.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 145 return;
caoyuan9642 0:6cb2eaf8b133 146 }
caoyuan9642 0:6cb2eaf8b133 147 if (dir == AXIS_ROTATE_STOP)
caoyuan9642 0:6cb2eaf8b133 148 {
caoyuan9642 0:6cb2eaf8b133 149 debug("%s: skipped.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 150 return;
caoyuan9642 0:6cb2eaf8b133 151 }
caoyuan9642 0:6cb2eaf8b133 152
caoyuan9642 0:6cb2eaf8b133 153 slew_mode(); // Switch to slew mode
caoyuan9642 0:6cb2eaf8b133 154 Thread::signal_clr(
caoyuan9642 0:6cb2eaf8b133 155 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_SPEEDCHANGE_SIGNAL); // Clear flags
caoyuan9642 0:6cb2eaf8b133 156 bool isInertial = (status == AXIS_INERTIAL);
caoyuan9642 0:6cb2eaf8b133 157 status = AXIS_SLEWING;
caoyuan9642 0:6cb2eaf8b133 158 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 159 slew_finish_state = FINISH_COMPLETE;
caoyuan9642 0:6cb2eaf8b133 160 currentDirection = dir;
caoyuan9642 0:6cb2eaf8b133 161 stepdir_t sd = (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
caoyuan9642 0:6cb2eaf8b133 162
caoyuan9642 0:6cb2eaf8b133 163 /* Calculate the angle to rotate*/
caoyuan9642 0:6cb2eaf8b133 164 bool skip_slew = false;
caoyuan9642 0:6cb2eaf8b133 165 double angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 166 double delta;
caoyuan9642 0:6cb2eaf8b133 167 delta = (dest - angleDeg) * (dir == AXIS_ROTATE_POSITIVE ? 1 : -1); /*delta is the actual angle to rotate*/
caoyuan9642 0:6cb2eaf8b133 168 if (fabs(delta) < 1e-5)
caoyuan9642 0:6cb2eaf8b133 169 { // FIX: delta 0->360degrees when angleDeg is essentially equal to dest
caoyuan9642 0:6cb2eaf8b133 170 delta = 0;
caoyuan9642 0:6cb2eaf8b133 171 }
caoyuan9642 0:6cb2eaf8b133 172 delta = remainder(delta - 180.0, 360.0) + 180.0; /*Shift to 0-360 deg*/
caoyuan9642 0:6cb2eaf8b133 173 debug_if(AXIS_DEBUG, "%s: start=%f, end=%f, delta=%f\n", axisName, angleDeg,
caoyuan9642 0:6cb2eaf8b133 174 dest, delta);
caoyuan9642 0:6cb2eaf8b133 175
caoyuan9642 0:6cb2eaf8b133 176 double startSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 177 double endSpeed = slewSpeed, waitTime;
caoyuan9642 0:6cb2eaf8b133 178 unsigned int ramp_steps;
caoyuan9642 0:6cb2eaf8b133 179 double acceleration = TelescopeConfiguration::getDouble("acceleration");
caoyuan9642 0:6cb2eaf8b133 180
caoyuan9642 0:6cb2eaf8b133 181 if (!indefinite)
caoyuan9642 0:6cb2eaf8b133 182 {
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 183 // Use the GOTO speed from configuration for slewing
caoyu@caoyuan9642-desktop.MIT.EDU 15:0bdda8dadafe 184 endSpeed = TelescopeConfiguration::getDouble("goto_slew_speed");
caoyuan9642 0:6cb2eaf8b133 185 // Ensure that delta is more than the minimum slewing angle, calculate the correct endSpeed and waitTime
caoyuan9642 0:6cb2eaf8b133 186 if (delta > TelescopeConfiguration::getDouble("min_slew_angle"))
caoyuan9642 0:6cb2eaf8b133 187 {
caoyuan9642 0:6cb2eaf8b133 188 /*The motion angle is decreased to ensure the correction step is in the same direction*/
caoyuan9642 0:6cb2eaf8b133 189 delta = delta
caoyuan9642 0:6cb2eaf8b133 190 - 0.5 * TelescopeConfiguration::getDouble("min_slew_angle");
caoyuan9642 0:6cb2eaf8b133 191
caoyuan9642 0:6cb2eaf8b133 192 double angleRotatedDuringAcceleration;
caoyuan9642 0:6cb2eaf8b133 193
caoyuan9642 0:6cb2eaf8b133 194 /* The actual endSpeed we get might be different than this due to the finite time resolution of the stepper driver
caoyuan9642 0:6cb2eaf8b133 195 * Here we set the dummy frequency and obtain the actual frequency it will be set to, so that the slewing time will be more accurate
caoyuan9642 0:6cb2eaf8b133 196 */
caoyuan9642 0:6cb2eaf8b133 197
caoyuan9642 0:6cb2eaf8b133 198 // Calculate the desired endSpeed. If delta is small, then endSpeed will correspondingly be reduced
caoyuan9642 0:6cb2eaf8b133 199 endSpeed = min(sqrt(delta * acceleration),
caoyuan9642 0:6cb2eaf8b133 200 stepper->setFrequency(stepsPerDeg * endSpeed)
caoyuan9642 0:6cb2eaf8b133 201 / stepsPerDeg);
caoyuan9642 0:6cb2eaf8b133 202 ramp_steps = (unsigned int) (endSpeed
caoyuan9642 0:6cb2eaf8b133 203 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 204 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 205 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 206 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 207
caoyuan9642 0:6cb2eaf8b133 208 angleRotatedDuringAcceleration = 0;
caoyuan9642 0:6cb2eaf8b133 209 /*Simulate the slewing process to get an accurate estimate of the actual angle that will be slewed*/
caoyuan9642 0:6cb2eaf8b133 210 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 211 {
caoyuan9642 0:6cb2eaf8b133 212 double speed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 213 stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 214 angleRotatedDuringAcceleration += speed
caoyuan9642 0:6cb2eaf8b133 215 * (TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 216 "acceleration_step_time") / 1000.0)
caoyuan9642 0:6cb2eaf8b133 217 * (i == ramp_steps ? 1 : 2); // Count both acceleration and deceleration
caoyuan9642 0:6cb2eaf8b133 218 }
caoyuan9642 0:6cb2eaf8b133 219
caoyuan9642 0:6cb2eaf8b133 220 waitTime = (delta - angleRotatedDuringAcceleration) / endSpeed;
caoyuan9642 0:6cb2eaf8b133 221 if (waitTime < 0.0)
caoyuan9642 0:6cb2eaf8b133 222 waitTime = 0.0; // With the above calculations, waitTime should no longer be zero. But if it happens to be so, let the correction do the job
caoyuan9642 0:6cb2eaf8b133 223
caoyuan9642 0:6cb2eaf8b133 224 debug_if(AXIS_DEBUG, "%s: endspeed = %f deg/s, time=%f, acc=%f\n",
caoyuan9642 0:6cb2eaf8b133 225 axisName, endSpeed, waitTime, acceleration);
caoyuan9642 0:6cb2eaf8b133 226 }
caoyuan9642 0:6cb2eaf8b133 227 else
caoyuan9642 0:6cb2eaf8b133 228 {
caoyuan9642 0:6cb2eaf8b133 229 // Angle difference is too small, skip slewing
caoyuan9642 0:6cb2eaf8b133 230 skip_slew = true;
caoyuan9642 0:6cb2eaf8b133 231 }
caoyuan9642 0:6cb2eaf8b133 232 }
caoyuan9642 0:6cb2eaf8b133 233 else
caoyuan9642 0:6cb2eaf8b133 234 {
caoyuan9642 0:6cb2eaf8b133 235 // Indefinite slewing mode
caoyuan9642 0:6cb2eaf8b133 236 waitTime = INFINITY;
caoyuan9642 0:6cb2eaf8b133 237 // If was in inertial mode, use startSpeed
caoyuan9642 0:6cb2eaf8b133 238 if (isInertial)
caoyuan9642 0:6cb2eaf8b133 239 startSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 240 // No need for correction
caoyuan9642 0:6cb2eaf8b133 241 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 242 }
caoyuan9642 0:6cb2eaf8b133 243
caoyuan9642 0:6cb2eaf8b133 244 /*Slewing -> accel, wait, decel*/
caoyuan9642 0:6cb2eaf8b133 245 if (!skip_slew)
caoyuan9642 0:6cb2eaf8b133 246 {
caoyuan9642 0:6cb2eaf8b133 247 int wait_ms;
caoyuan9642 0:6cb2eaf8b133 248 uint32_t flags;
caoyuan9642 0:6cb2eaf8b133 249 /*Acceleration*/
caoyuan9642 0:6cb2eaf8b133 250 slewState = AXIS_SLEW_ACCELERATING;
caoyuan9642 0:6cb2eaf8b133 251 ramp_steps = (unsigned int) ((endSpeed - startSpeed)
caoyuan9642 0:6cb2eaf8b133 252 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 253 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 254
caoyuan9642 0:6cb2eaf8b133 255 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 256 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 257
caoyuan9642 0:6cb2eaf8b133 258 debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n", axisName,
caoyuan9642 0:6cb2eaf8b133 259 ramp_steps); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 260
caoyuan9642 0:6cb2eaf8b133 261 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 262 {
caoyuan9642 0:6cb2eaf8b133 263 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 264 stepsPerDeg
caoyuan9642 0:6cb2eaf8b133 265 * ((endSpeed - startSpeed) / ramp_steps * i
caoyuan9642 0:6cb2eaf8b133 266 + startSpeed)) / stepsPerDeg; // Set and update currentSpeed with actual speed
caoyuan9642 0:6cb2eaf8b133 267
caoyuan9642 0:6cb2eaf8b133 268 if (i == 1)
caoyuan9642 0:6cb2eaf8b133 269 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 270
caoyuan9642 0:6cb2eaf8b133 271 /*Monitor whether there is a stop/emerge stop signal*/
caoyuan9642 0:6cb2eaf8b133 272 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 273 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 274 TelescopeConfiguration::getInt("acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 275
caoyuan9642 0:6cb2eaf8b133 276 if (flags == osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 277 {
caoyuan9642 0:6cb2eaf8b133 278 /*Nothing happened, we're good*/
caoyuan9642 0:6cb2eaf8b133 279 continue;
caoyuan9642 0:6cb2eaf8b133 280 }
caoyuan9642 0:6cb2eaf8b133 281 else if ((flags & osFlagsError) == 0)
caoyuan9642 0:6cb2eaf8b133 282 {
caoyuan9642 0:6cb2eaf8b133 283 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 284 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 285 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 286 {
caoyuan9642 0:6cb2eaf8b133 287 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 288 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 289 }
caoyuan9642 0:6cb2eaf8b133 290 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 291 {
caoyuan9642 0:6cb2eaf8b133 292 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 293 goto stop;
caoyuan9642 0:6cb2eaf8b133 294 }
caoyuan9642 0:6cb2eaf8b133 295 }
caoyuan9642 0:6cb2eaf8b133 296 }
caoyuan9642 0:6cb2eaf8b133 297
caoyuan9642 0:6cb2eaf8b133 298 /*Keep slewing and wait*/
caoyuan9642 0:6cb2eaf8b133 299 slewState = AXIS_SLEW_CONSTANT_SPEED;
caoyuan9642 0:6cb2eaf8b133 300 debug_if(AXIS_DEBUG, "%s: wait for %f\n", axisName, waitTime); // TODO
caoyuan9642 0:6cb2eaf8b133 301 wait_ms = (isinf(waitTime)) ? osWaitForever : (int) (waitTime * 1000);
caoyuan9642 0:6cb2eaf8b133 302
caoyuan9642 0:6cb2eaf8b133 303 tim.reset();
caoyuan9642 0:6cb2eaf8b133 304
caoyuan9642 0:6cb2eaf8b133 305 while (wait_ms)
caoyuan9642 0:6cb2eaf8b133 306 {
caoyuan9642 0:6cb2eaf8b133 307 flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 308 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL
caoyuan9642 0:6cb2eaf8b133 309 | (indefinite ? AXIS_SPEEDCHANGE_SIGNAL : 0),
caoyuan9642 0:6cb2eaf8b133 310 osFlagsWaitAny, wait_ms); /*Wait the remaining time*/
caoyuan9642 0:6cb2eaf8b133 311 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 312 {
caoyuan9642 0:6cb2eaf8b133 313 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 314 {
caoyuan9642 0:6cb2eaf8b133 315 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 316 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 317 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 318 }
caoyuan9642 0:6cb2eaf8b133 319 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 320 {
caoyuan9642 0:6cb2eaf8b133 321 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 322 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 323 goto stop;
caoyuan9642 0:6cb2eaf8b133 324 }
caoyuan9642 0:6cb2eaf8b133 325 else if (flags & AXIS_SPEEDCHANGE_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 326 {
caoyuan9642 0:6cb2eaf8b133 327 // Change speed, therefore also changing waittime. Only applies to indefinite slew
caoyuan9642 0:6cb2eaf8b133 328 double newSpeed = slewSpeed;
caoyuan9642 0:6cb2eaf8b133 329 startSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 330 endSpeed = newSpeed;
caoyuan9642 0:6cb2eaf8b133 331
caoyuan9642 0:6cb2eaf8b133 332 ramp_steps = (unsigned int) (fabs(endSpeed - startSpeed)
caoyuan9642 0:6cb2eaf8b133 333 / (TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 334 "acceleration_step_time") / 1000.0)
caoyuan9642 0:6cb2eaf8b133 335 / acceleration);
caoyuan9642 0:6cb2eaf8b133 336
caoyuan9642 0:6cb2eaf8b133 337 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 338 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 339
caoyuan9642 0:6cb2eaf8b133 340 debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n",
caoyuan9642 0:6cb2eaf8b133 341 axisName, ramp_steps); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 342
caoyuan9642 0:6cb2eaf8b133 343 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 344 {
caoyuan9642 0:6cb2eaf8b133 345 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 346 stepsPerDeg
caoyuan9642 0:6cb2eaf8b133 347 * ((endSpeed - startSpeed) / ramp_steps
caoyuan9642 0:6cb2eaf8b133 348 * i + startSpeed))
caoyuan9642 0:6cb2eaf8b133 349 / stepsPerDeg; // Set and update currentSpeed with actual speed
caoyuan9642 0:6cb2eaf8b133 350
caoyuan9642 0:6cb2eaf8b133 351 /*Monitor whether there is a stop/emerge stop signal*/
caoyuan9642 0:6cb2eaf8b133 352 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 353 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 354 osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 355 TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 356 "acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 357
caoyuan9642 0:6cb2eaf8b133 358 if (flags == osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 359 {
caoyuan9642 0:6cb2eaf8b133 360 /*Nothing happened, we're good*/
caoyuan9642 0:6cb2eaf8b133 361 continue;
caoyuan9642 0:6cb2eaf8b133 362 }
caoyuan9642 0:6cb2eaf8b133 363 else if ((flags & osFlagsError) == 0)
caoyuan9642 0:6cb2eaf8b133 364 {
caoyuan9642 0:6cb2eaf8b133 365 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 366 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 367 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 368 {
caoyuan9642 0:6cb2eaf8b133 369 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 370 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 371 }
caoyuan9642 0:6cb2eaf8b133 372 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 373 {
caoyuan9642 0:6cb2eaf8b133 374 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 375 goto stop;
caoyuan9642 0:6cb2eaf8b133 376 }
caoyuan9642 0:6cb2eaf8b133 377 }
caoyuan9642 0:6cb2eaf8b133 378 }
caoyuan9642 0:6cb2eaf8b133 379 }
caoyuan9642 0:6cb2eaf8b133 380 }
caoyuan9642 0:6cb2eaf8b133 381 if (!indefinite)
caoyuan9642 0:6cb2eaf8b133 382 {
caoyuan9642 0:6cb2eaf8b133 383 wait_ms -= tim.read_ms();
caoyuan9642 0:6cb2eaf8b133 384 tim.reset();
caoyuan9642 0:6cb2eaf8b133 385 }
caoyuan9642 0:6cb2eaf8b133 386 }
caoyuan9642 0:6cb2eaf8b133 387
caoyuan9642 0:6cb2eaf8b133 388 stop:
caoyuan9642 0:6cb2eaf8b133 389 /*Now deceleration*/
caoyuan9642 0:6cb2eaf8b133 390 slewState = AXIS_SLEW_DECELERATING;
caoyuan9642 0:6cb2eaf8b133 391 endSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 392 ramp_steps = (unsigned int) (currentSpeed
caoyuan9642 0:6cb2eaf8b133 393 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 394 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 395
caoyuan9642 0:6cb2eaf8b133 396 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 397 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 398
caoyuan9642 0:6cb2eaf8b133 399 debug_if(AXIS_DEBUG, "%s: decelerate in %d steps from %f\n", axisName,
caoyuan9642 0:6cb2eaf8b133 400 ramp_steps, endSpeed); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 401
caoyuan9642 0:6cb2eaf8b133 402 for (unsigned int i = ramp_steps - 1; i >= 1; i--)
caoyuan9642 0:6cb2eaf8b133 403 {
caoyuan9642 0:6cb2eaf8b133 404 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 405 stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg; // set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 406 // Wait. Now we only handle EMERGENCY STOP signal, since stop has been handled already
caoyuan9642 0:6cb2eaf8b133 407 flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 408 AXIS_EMERGE_STOP_SIGNAL | AXIS_STOP_KEEPSPEED_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 409 osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 410 TelescopeConfiguration::getInt("acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 411
caoyuan9642 0:6cb2eaf8b133 412 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 413 {
caoyuan9642 0:6cb2eaf8b133 414 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 415 {
caoyuan9642 0:6cb2eaf8b133 416 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 417 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 418 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 419 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 420 }
caoyuan9642 0:6cb2eaf8b133 421 else if (flags & AXIS_STOP_KEEPSPEED_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 422 {
caoyuan9642 0:6cb2eaf8b133 423 // Keep current speed
caoyuan9642 0:6cb2eaf8b133 424 status = AXIS_INERTIAL;
caoyuan9642 0:6cb2eaf8b133 425 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 426 return;
caoyuan9642 0:6cb2eaf8b133 427 }
caoyuan9642 0:6cb2eaf8b133 428 }
caoyuan9642 0:6cb2eaf8b133 429 }
caoyuan9642 0:6cb2eaf8b133 430
caoyuan9642 0:6cb2eaf8b133 431 emerge_stop:
caoyuan9642 0:6cb2eaf8b133 432 /*Fully pull-over*/
caoyuan9642 0:6cb2eaf8b133 433 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 434 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 435 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 436 }
caoyuan9642 0:6cb2eaf8b133 437
caoyuan9642 0:6cb2eaf8b133 438 if (useCorrection)
caoyuan9642 0:6cb2eaf8b133 439 {
caoyuan9642 0:6cb2eaf8b133 440 // Switch mode
caoyuan9642 0:6cb2eaf8b133 441 correction_mode();
caoyuan9642 0:6cb2eaf8b133 442 double correctionSpeed = TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 443 "correction_speed_sidereal") * sidereal_speed;
caoyuan9642 0:6cb2eaf8b133 444 /*Use correction to goto the final angle with high resolution*/
caoyuan9642 0:6cb2eaf8b133 445 angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 446 debug_if(AXIS_DEBUG, "%s: correct from %f to %f deg\n", axisName,
caoyuan9642 0:6cb2eaf8b133 447 angleDeg, dest); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 448
caoyuan9642 0:6cb2eaf8b133 449 double diff = remainder(angleDeg - dest, 360.0);
caoyuan9642 0:6cb2eaf8b133 450 if (diff > TelescopeConfiguration::getDouble("max_correction_angle"))
caoyuan9642 0:6cb2eaf8b133 451 {
caoyuan9642 0:6cb2eaf8b133 452 debug(
caoyuan9642 0:6cb2eaf8b133 453 "%s: correction too large: %f. Check hardware configuration.\n",
caoyuan9642 0:6cb2eaf8b133 454 axisName, diff);
caoyuan9642 0:6cb2eaf8b133 455 stop();
caoyuan9642 0:6cb2eaf8b133 456 idle_mode();
caoyuan9642 0:6cb2eaf8b133 457 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 458 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 459 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 460 return;
caoyuan9642 0:6cb2eaf8b133 461 }
caoyuan9642 0:6cb2eaf8b133 462
caoyuan9642 0:6cb2eaf8b133 463 int nTry = 3; // Try 3 corrections at most
caoyuan9642 0:6cb2eaf8b133 464 while (--nTry
caoyuan9642 0:6cb2eaf8b133 465 && fabsf(diff)
caoyuan9642 0:6cb2eaf8b133 466 > TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 467 "correction_tolerance"))
caoyuan9642 0:6cb2eaf8b133 468 {
caoyuan9642 0:6cb2eaf8b133 469 /*Determine correction direction and time*/
caoyuan9642 0:6cb2eaf8b133 470 sd = (diff > 0.0) ? STEP_BACKWARD : STEP_FORWARD;
caoyuan9642 0:6cb2eaf8b133 471
caoyuan9642 0:6cb2eaf8b133 472 /*Perform correction*/
caoyuan9642 0:6cb2eaf8b133 473 currentSpeed = stepper->setFrequency(stepsPerDeg * correctionSpeed)
caoyuan9642 0:6cb2eaf8b133 474 / stepsPerDeg; // Set and update actual speed
caoyuan9642 0:6cb2eaf8b133 475
caoyuan9642 0:6cb2eaf8b133 476 int correctionTime_ms = (int) (fabs(diff) / currentSpeed * 1000); // Use the accurate speed for calculating time
caoyuan9642 0:6cb2eaf8b133 477
caoyuan9642 0:6cb2eaf8b133 478 debug_if(AXIS_DEBUG,
caoyuan9642 0:6cb2eaf8b133 479 "%s: correction: from %f to %f deg. time=%d ms\n", axisName,
caoyuan9642 0:6cb2eaf8b133 480 angleDeg, dest, correctionTime_ms); //TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 481 if (correctionTime_ms
caoyuan9642 0:6cb2eaf8b133 482 < TelescopeConfiguration::getInt("min_correction_time"))
caoyuan9642 0:6cb2eaf8b133 483 {
caoyuan9642 0:6cb2eaf8b133 484 break;
caoyuan9642 0:6cb2eaf8b133 485 }
caoyuan9642 0:6cb2eaf8b133 486
caoyuan9642 0:6cb2eaf8b133 487 /*Start, wait, stop*/
caoyuan9642 0:6cb2eaf8b133 488 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 489 uint32_t flags = osThreadFlagsWait(AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 490 osFlagsWaitAny, correctionTime_ms);
caoyuan9642 0:6cb2eaf8b133 491 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 492 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 493 {
caoyuan9642 0:6cb2eaf8b133 494 // Emergency stop!
caoyuan9642 0:6cb2eaf8b133 495 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 496 goto emerge_stop2;
caoyuan9642 0:6cb2eaf8b133 497 }
caoyuan9642 0:6cb2eaf8b133 498
caoyuan9642 0:6cb2eaf8b133 499 angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 500 diff = remainder(angleDeg - dest, 360.0);
caoyuan9642 0:6cb2eaf8b133 501 }
caoyuan9642 0:6cb2eaf8b133 502
caoyuan9642 0:6cb2eaf8b133 503 if (!nTry)
caoyuan9642 0:6cb2eaf8b133 504 {
caoyuan9642 0:6cb2eaf8b133 505 debug("%s: correction failed. Check hardware configuration.\n",
caoyuan9642 0:6cb2eaf8b133 506 axisName);
caoyuan9642 0:6cb2eaf8b133 507 }
caoyuan9642 0:6cb2eaf8b133 508
caoyuan9642 0:6cb2eaf8b133 509 debug_if(AXIS_DEBUG, "%s: correction finished: %f deg\n", axisName,
caoyuan9642 0:6cb2eaf8b133 510 angleDeg); //TODO:DEBUG
caoyuan9642 0:6cb2eaf8b133 511 }
caoyuan9642 0:6cb2eaf8b133 512 emerge_stop2:
caoyuan9642 0:6cb2eaf8b133 513 // Set status to stopped
caoyuan9642 0:6cb2eaf8b133 514 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 515 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 516 idle_mode();
caoyuan9642 0:6cb2eaf8b133 517 }
caoyuan9642 0:6cb2eaf8b133 518
caoyuan9642 0:6cb2eaf8b133 519 void Axis::track(axisrotdir_t dir)
caoyuan9642 0:6cb2eaf8b133 520 {
caoyuan9642 0:6cb2eaf8b133 521 track_mode();
caoyuan9642 0:6cb2eaf8b133 522 if (trackSpeed != 0 && dir != AXIS_ROTATE_STOP)
caoyuan9642 0:6cb2eaf8b133 523 {
caoyuan9642 0:6cb2eaf8b133 524 stepdir_t sd =
caoyuan9642 0:6cb2eaf8b133 525 (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
caoyuan9642 0:6cb2eaf8b133 526 currentSpeed = stepper->setFrequency(trackSpeed * stepsPerDeg)
caoyuan9642 0:6cb2eaf8b133 527 / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 528 currentDirection = dir;
caoyuan9642 0:6cb2eaf8b133 529 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 530 }
caoyuan9642 0:6cb2eaf8b133 531 else
caoyuan9642 0:6cb2eaf8b133 532 {
caoyuan9642 0:6cb2eaf8b133 533 // For DEC axis
caoyuan9642 0:6cb2eaf8b133 534 dir = AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 535 trackSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 536 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 537 currentDirection = AXIS_ROTATE_POSITIVE;
caoyuan9642 0:6cb2eaf8b133 538 }
caoyuan9642 0:6cb2eaf8b133 539 status = AXIS_TRACKING;
caoyuan9642 0:6cb2eaf8b133 540 Thread::signal_clr(
caoyuan9642 0:6cb2eaf8b133 541 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_GUIDE_SIGNAL);
caoyuan9642 0:6cb2eaf8b133 542 // Empty the guide queue
caoyuan9642 0:6cb2eaf8b133 543 while (!guide_queue.empty())
caoyuan9642 0:6cb2eaf8b133 544 guide_queue.get();
caoyuan9642 0:6cb2eaf8b133 545
caoyuan9642 0:6cb2eaf8b133 546 while (true)
caoyuan9642 0:6cb2eaf8b133 547 {
caoyuan9642 0:6cb2eaf8b133 548 // Now we wait for SOMETHING to happen - either STOP, EMERGE_STOP or GUIDE
caoyuan9642 0:6cb2eaf8b133 549 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 550 AXIS_GUIDE_SIGNAL | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 551 osFlagsWaitAny, osWaitForever);
caoyuan9642 0:6cb2eaf8b133 552 if ((flags & osFlagsError) == 0) // has flag
caoyuan9642 0:6cb2eaf8b133 553 {
caoyuan9642 0:6cb2eaf8b133 554 if (flags & (AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL))
caoyuan9642 0:6cb2eaf8b133 555 {
caoyuan9642 0:6cb2eaf8b133 556 // We stop tracking
caoyuan9642 0:6cb2eaf8b133 557 break;
caoyuan9642 0:6cb2eaf8b133 558 }
caoyuan9642 0:6cb2eaf8b133 559 else if (flags & AXIS_GUIDE_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 560 {
caoyuan9642 0:6cb2eaf8b133 561 bool stopped = false;
caoyuan9642 2:2ee28add0821 562 guiding = true;
caoyuan9642 0:6cb2eaf8b133 563 // Guide. Process all commands in the queue
caoyuan9642 0:6cb2eaf8b133 564 while (true)
caoyuan9642 0:6cb2eaf8b133 565 {
caoyuan9642 0:6cb2eaf8b133 566 osEvent evt = guide_queue.get(0); // try to get a message
caoyuan9642 0:6cb2eaf8b133 567 if (evt.status == osEventMessage)
caoyuan9642 0:6cb2eaf8b133 568 {
caoyuan9642 0:6cb2eaf8b133 569 int guideTime_ms = (int) evt.value.p;
caoyuan9642 0:6cb2eaf8b133 570 if (guideTime_ms == 0)
caoyuan9642 0:6cb2eaf8b133 571 continue; // Nothing to guide
caoyuan9642 0:6cb2eaf8b133 572 // Determine guide direction
caoyuan9642 0:6cb2eaf8b133 573 axisrotdir_t guide_dir =
caoyuan9642 0:6cb2eaf8b133 574 (guideTime_ms > 0) ?
caoyuan9642 0:6cb2eaf8b133 575 AXIS_ROTATE_POSITIVE :
caoyuan9642 0:6cb2eaf8b133 576 AXIS_ROTATE_NEGATIVE;
caoyuan9642 0:6cb2eaf8b133 577
caoyuan9642 0:6cb2eaf8b133 578 double newSpeed =
caoyuan9642 0:6cb2eaf8b133 579 (guide_dir == currentDirection) ?
caoyuan9642 0:6cb2eaf8b133 580 trackSpeed + guideSpeed :
caoyuan9642 0:6cb2eaf8b133 581 trackSpeed - guideSpeed; /*Determine speed in the original direction (currentDirection)*/
caoyuan9642 0:6cb2eaf8b133 582
caoyuan9642 0:6cb2eaf8b133 583 // Clamp to maximum guide time
caoyuan9642 0:6cb2eaf8b133 584 guideTime_ms = abs(guideTime_ms);
caoyuan9642 0:6cb2eaf8b133 585 if (guideTime_ms
caoyuan9642 0:6cb2eaf8b133 586 > TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 587 "max_guide_time"))
caoyuan9642 0:6cb2eaf8b133 588 {
caoyuan9642 0:6cb2eaf8b133 589 debug("Axis: Guiding time too long: %d ms\n",
caoyuan9642 0:6cb2eaf8b133 590 abs(guideTime_ms));
caoyuan9642 0:6cb2eaf8b133 591 guideTime_ms = TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 592 "max_guide_time");
caoyuan9642 0:6cb2eaf8b133 593 }
caoyuan9642 0:6cb2eaf8b133 594
caoyuan9642 0:6cb2eaf8b133 595 bool dirswitch = false;
caoyuan9642 0:6cb2eaf8b133 596 if (newSpeed > 0)
caoyuan9642 0:6cb2eaf8b133 597 {
caoyuan9642 0:6cb2eaf8b133 598 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 599 newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 600 if (trackSpeed == 0)
caoyuan9642 0:6cb2eaf8b133 601 {
caoyuan9642 0:6cb2eaf8b133 602 // For DEC, we also need to start the motor
caoyuan9642 0:6cb2eaf8b133 603 stepper->start(STEP_FORWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 604 }
caoyuan9642 0:6cb2eaf8b133 605 }
caoyuan9642 0:6cb2eaf8b133 606 else if (newSpeed < 0)
caoyuan9642 0:6cb2eaf8b133 607 {
caoyuan9642 0:6cb2eaf8b133 608 //
caoyuan9642 0:6cb2eaf8b133 609 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 610 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 611 -newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 612 stepper->start(
caoyuan9642 0:6cb2eaf8b133 613 (currentDirection == AXIS_ROTATE_POSITIVE) ?
caoyuan9642 0:6cb2eaf8b133 614 STEP_BACKWARD : STEP_FORWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 615 dirswitch = true;
caoyuan9642 0:6cb2eaf8b133 616 }
caoyuan9642 0:6cb2eaf8b133 617 else
caoyuan9642 0:6cb2eaf8b133 618 {
caoyuan9642 0:6cb2eaf8b133 619 // newSpeed == 0, just stop the motor
caoyuan9642 0:6cb2eaf8b133 620 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 621 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 622 dirswitch = true; // Make sure to recover the original speed
caoyuan9642 0:6cb2eaf8b133 623 }
caoyuan9642 0:6cb2eaf8b133 624
caoyuan9642 0:6cb2eaf8b133 625 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 626 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 627 osFlagsWaitAny, guideTime_ms);
caoyuan9642 0:6cb2eaf8b133 628 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 629 {
caoyuan9642 0:6cb2eaf8b133 630 //break and stop;
caoyuan9642 0:6cb2eaf8b133 631 stopped = true;
caoyuan9642 0:6cb2eaf8b133 632 break;
caoyuan9642 0:6cb2eaf8b133 633 }
caoyuan9642 0:6cb2eaf8b133 634 if (dirswitch || trackSpeed == 0)
caoyuan9642 0:6cb2eaf8b133 635 {
caoyuan9642 0:6cb2eaf8b133 636 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 637 // Restart the motor in original direction
caoyuan9642 0:6cb2eaf8b133 638 if (trackSpeed != 0)
caoyuan9642 0:6cb2eaf8b133 639 {
caoyuan9642 0:6cb2eaf8b133 640 stepper->start(
caoyuan9642 0:6cb2eaf8b133 641 (currentDirection
caoyuan9642 0:6cb2eaf8b133 642 == AXIS_ROTATE_POSITIVE) ?
caoyuan9642 0:6cb2eaf8b133 643 STEP_FORWARD : STEP_BACKWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 644 }
caoyuan9642 0:6cb2eaf8b133 645 }
caoyuan9642 0:6cb2eaf8b133 646 // Restore to normal speed
caoyuan9642 0:6cb2eaf8b133 647 if (trackSpeed != 0)
caoyuan9642 0:6cb2eaf8b133 648 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 649 trackSpeed * stepsPerDeg) / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 650 else
caoyuan9642 0:6cb2eaf8b133 651 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 652
caoyuan9642 0:6cb2eaf8b133 653 // End guiding
caoyuan9642 0:6cb2eaf8b133 654 }
caoyuan9642 0:6cb2eaf8b133 655 else
caoyuan9642 0:6cb2eaf8b133 656 {
caoyuan9642 0:6cb2eaf8b133 657 // No more message to get. Break out
caoyuan9642 0:6cb2eaf8b133 658 break;
caoyuan9642 0:6cb2eaf8b133 659 }
caoyuan9642 0:6cb2eaf8b133 660 }
caoyuan9642 2:2ee28add0821 661 guiding = false;
caoyuan9642 0:6cb2eaf8b133 662 if (stopped)
caoyuan9642 0:6cb2eaf8b133 663 {
caoyuan9642 0:6cb2eaf8b133 664 //Complete break out
caoyuan9642 0:6cb2eaf8b133 665 break;
caoyuan9642 0:6cb2eaf8b133 666 }
caoyuan9642 0:6cb2eaf8b133 667 }
caoyuan9642 0:6cb2eaf8b133 668 }
caoyuan9642 0:6cb2eaf8b133 669 }
caoyuan9642 0:6cb2eaf8b133 670
caoyuan9642 0:6cb2eaf8b133 671 // Stop
caoyuan9642 0:6cb2eaf8b133 672 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 673 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 674 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 675 idle_mode();
caoyuan9642 0:6cb2eaf8b133 676 }
caoyuan9642 0:6cb2eaf8b133 677
caoyuan9642 0:6cb2eaf8b133 678