Telescope Control Library
Axis.cpp@10:e356188d208e, 2018-09-14 (annotated)
- Committer:
- caoyu@caoyuan9642-desktop.MIT.EDU
- Date:
- Fri Sep 14 01:30:55 2018 -0400
- Revision:
- 10:e356188d208e
- Parent:
- 9:d0413a9b1386
- Child:
- 15:0bdda8dadafe
Resolved circular dependency. Slewing rate now only applies to indefinite slews. GOTO use goto_slew_speed in the configuration.
Who changed what in which revision?
User | Revision | Line number | New 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 | 10:e356188d208e | 184 | slewSpeed = 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 |