Telescope Control Library
Axis.cpp@2:2ee28add0821, 2018-08-20 (annotated)
- Committer:
- caoyuan9642
- Date:
- Mon Aug 20 23:42:21 2018 +0000
- Revision:
- 2:2ee28add0821
- Parent:
- 0:6cb2eaf8b133
- Child:
- 9:d0413a9b1386
Guiding status
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, |
caoyuan9642 | 2:2ee28add0821 | 25 | 1), slew_finish_state(FINISH_COMPLETE), guiding(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 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 183 | // Ensure that delta is more than the minimum slewing angle, calculate the correct endSpeed and waitTime |
caoyuan9642 | 0:6cb2eaf8b133 | 184 | if (delta > TelescopeConfiguration::getDouble("min_slew_angle")) |
caoyuan9642 | 0:6cb2eaf8b133 | 185 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 186 | /*The motion angle is decreased to ensure the correction step is in the same direction*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 187 | delta = delta |
caoyuan9642 | 0:6cb2eaf8b133 | 188 | - 0.5 * TelescopeConfiguration::getDouble("min_slew_angle"); |
caoyuan9642 | 0:6cb2eaf8b133 | 189 | |
caoyuan9642 | 0:6cb2eaf8b133 | 190 | double angleRotatedDuringAcceleration; |
caoyuan9642 | 0:6cb2eaf8b133 | 191 | |
caoyuan9642 | 0:6cb2eaf8b133 | 192 | /* The actual endSpeed we get might be different than this due to the finite time resolution of the stepper driver |
caoyuan9642 | 0:6cb2eaf8b133 | 193 | * 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 | 194 | */ |
caoyuan9642 | 0:6cb2eaf8b133 | 195 | |
caoyuan9642 | 0:6cb2eaf8b133 | 196 | // Calculate the desired endSpeed. If delta is small, then endSpeed will correspondingly be reduced |
caoyuan9642 | 0:6cb2eaf8b133 | 197 | endSpeed = min(sqrt(delta * acceleration), |
caoyuan9642 | 0:6cb2eaf8b133 | 198 | stepper->setFrequency(stepsPerDeg * endSpeed) |
caoyuan9642 | 0:6cb2eaf8b133 | 199 | / stepsPerDeg); |
caoyuan9642 | 0:6cb2eaf8b133 | 200 | ramp_steps = (unsigned int) (endSpeed |
caoyuan9642 | 0:6cb2eaf8b133 | 201 | / (TelescopeConfiguration::getInt("acceleration_step_time") |
caoyuan9642 | 0:6cb2eaf8b133 | 202 | / 1000.0) / acceleration); |
caoyuan9642 | 0:6cb2eaf8b133 | 203 | if (ramp_steps < 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 204 | ramp_steps = 1; |
caoyuan9642 | 0:6cb2eaf8b133 | 205 | |
caoyuan9642 | 0:6cb2eaf8b133 | 206 | angleRotatedDuringAcceleration = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 207 | /*Simulate the slewing process to get an accurate estimate of the actual angle that will be slewed*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 208 | for (unsigned int i = 1; i <= ramp_steps; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 209 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 210 | double speed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 211 | stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg; |
caoyuan9642 | 0:6cb2eaf8b133 | 212 | angleRotatedDuringAcceleration += speed |
caoyuan9642 | 0:6cb2eaf8b133 | 213 | * (TelescopeConfiguration::getInt( |
caoyuan9642 | 0:6cb2eaf8b133 | 214 | "acceleration_step_time") / 1000.0) |
caoyuan9642 | 0:6cb2eaf8b133 | 215 | * (i == ramp_steps ? 1 : 2); // Count both acceleration and deceleration |
caoyuan9642 | 0:6cb2eaf8b133 | 216 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 217 | |
caoyuan9642 | 0:6cb2eaf8b133 | 218 | waitTime = (delta - angleRotatedDuringAcceleration) / endSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 219 | if (waitTime < 0.0) |
caoyuan9642 | 0:6cb2eaf8b133 | 220 | 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 | 221 | |
caoyuan9642 | 0:6cb2eaf8b133 | 222 | debug_if(AXIS_DEBUG, "%s: endspeed = %f deg/s, time=%f, acc=%f\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 223 | axisName, endSpeed, waitTime, acceleration); |
caoyuan9642 | 0:6cb2eaf8b133 | 224 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 225 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 226 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 227 | // Angle difference is too small, skip slewing |
caoyuan9642 | 0:6cb2eaf8b133 | 228 | skip_slew = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 229 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 230 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 231 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 232 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 233 | // Indefinite slewing mode |
caoyuan9642 | 0:6cb2eaf8b133 | 234 | waitTime = INFINITY; |
caoyuan9642 | 0:6cb2eaf8b133 | 235 | // If was in inertial mode, use startSpeed |
caoyuan9642 | 0:6cb2eaf8b133 | 236 | if (isInertial) |
caoyuan9642 | 0:6cb2eaf8b133 | 237 | startSpeed = currentSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 238 | // No need for correction |
caoyuan9642 | 0:6cb2eaf8b133 | 239 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 240 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 241 | |
caoyuan9642 | 0:6cb2eaf8b133 | 242 | /*Slewing -> accel, wait, decel*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 243 | if (!skip_slew) |
caoyuan9642 | 0:6cb2eaf8b133 | 244 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 245 | int wait_ms; |
caoyuan9642 | 0:6cb2eaf8b133 | 246 | uint32_t flags; |
caoyuan9642 | 0:6cb2eaf8b133 | 247 | /*Acceleration*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 248 | slewState = AXIS_SLEW_ACCELERATING; |
caoyuan9642 | 0:6cb2eaf8b133 | 249 | ramp_steps = (unsigned int) ((endSpeed - startSpeed) |
caoyuan9642 | 0:6cb2eaf8b133 | 250 | / (TelescopeConfiguration::getInt("acceleration_step_time") |
caoyuan9642 | 0:6cb2eaf8b133 | 251 | / 1000.0) / acceleration); |
caoyuan9642 | 0:6cb2eaf8b133 | 252 | |
caoyuan9642 | 0:6cb2eaf8b133 | 253 | if (ramp_steps < 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 254 | ramp_steps = 1; |
caoyuan9642 | 0:6cb2eaf8b133 | 255 | |
caoyuan9642 | 0:6cb2eaf8b133 | 256 | debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n", axisName, |
caoyuan9642 | 0:6cb2eaf8b133 | 257 | ramp_steps); // TODO: DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 258 | |
caoyuan9642 | 0:6cb2eaf8b133 | 259 | for (unsigned int i = 1; i <= ramp_steps; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 260 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 261 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 262 | stepsPerDeg |
caoyuan9642 | 0:6cb2eaf8b133 | 263 | * ((endSpeed - startSpeed) / ramp_steps * i |
caoyuan9642 | 0:6cb2eaf8b133 | 264 | + startSpeed)) / stepsPerDeg; // Set and update currentSpeed with actual speed |
caoyuan9642 | 0:6cb2eaf8b133 | 265 | |
caoyuan9642 | 0:6cb2eaf8b133 | 266 | if (i == 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 267 | stepper->start(sd); |
caoyuan9642 | 0:6cb2eaf8b133 | 268 | |
caoyuan9642 | 0:6cb2eaf8b133 | 269 | /*Monitor whether there is a stop/emerge stop signal*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 270 | uint32_t flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 271 | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, osFlagsWaitAny, |
caoyuan9642 | 0:6cb2eaf8b133 | 272 | TelescopeConfiguration::getInt("acceleration_step_time")); |
caoyuan9642 | 0:6cb2eaf8b133 | 273 | |
caoyuan9642 | 0:6cb2eaf8b133 | 274 | if (flags == osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 275 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 276 | /*Nothing happened, we're good*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 277 | continue; |
caoyuan9642 | 0:6cb2eaf8b133 | 278 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 279 | else if ((flags & osFlagsError) == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 280 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 281 | // We're stopped! |
caoyuan9642 | 0:6cb2eaf8b133 | 282 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 283 | if (flags & AXIS_EMERGE_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 284 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 285 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 286 | goto emerge_stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 287 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 288 | else if (flags & AXIS_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 289 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 290 | slew_finish_state = FINISH_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 291 | goto stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 292 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 293 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 294 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 295 | |
caoyuan9642 | 0:6cb2eaf8b133 | 296 | /*Keep slewing and wait*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 297 | slewState = AXIS_SLEW_CONSTANT_SPEED; |
caoyuan9642 | 0:6cb2eaf8b133 | 298 | debug_if(AXIS_DEBUG, "%s: wait for %f\n", axisName, waitTime); // TODO |
caoyuan9642 | 0:6cb2eaf8b133 | 299 | wait_ms = (isinf(waitTime)) ? osWaitForever : (int) (waitTime * 1000); |
caoyuan9642 | 0:6cb2eaf8b133 | 300 | |
caoyuan9642 | 0:6cb2eaf8b133 | 301 | tim.reset(); |
caoyuan9642 | 0:6cb2eaf8b133 | 302 | |
caoyuan9642 | 0:6cb2eaf8b133 | 303 | while (wait_ms) |
caoyuan9642 | 0:6cb2eaf8b133 | 304 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 305 | flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 306 | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL |
caoyuan9642 | 0:6cb2eaf8b133 | 307 | | (indefinite ? AXIS_SPEEDCHANGE_SIGNAL : 0), |
caoyuan9642 | 0:6cb2eaf8b133 | 308 | osFlagsWaitAny, wait_ms); /*Wait the remaining time*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 309 | if (flags != osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 310 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 311 | if (flags & AXIS_EMERGE_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 312 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 313 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 314 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 315 | goto emerge_stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 316 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 317 | else if (flags & AXIS_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 318 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 319 | slew_finish_state = FINISH_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 320 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 321 | goto stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 322 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 323 | else if (flags & AXIS_SPEEDCHANGE_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 324 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 325 | // Change speed, therefore also changing waittime. Only applies to indefinite slew |
caoyuan9642 | 0:6cb2eaf8b133 | 326 | double newSpeed = slewSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 327 | startSpeed = currentSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 328 | endSpeed = newSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 329 | |
caoyuan9642 | 0:6cb2eaf8b133 | 330 | ramp_steps = (unsigned int) (fabs(endSpeed - startSpeed) |
caoyuan9642 | 0:6cb2eaf8b133 | 331 | / (TelescopeConfiguration::getInt( |
caoyuan9642 | 0:6cb2eaf8b133 | 332 | "acceleration_step_time") / 1000.0) |
caoyuan9642 | 0:6cb2eaf8b133 | 333 | / acceleration); |
caoyuan9642 | 0:6cb2eaf8b133 | 334 | |
caoyuan9642 | 0:6cb2eaf8b133 | 335 | if (ramp_steps < 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 336 | ramp_steps = 1; |
caoyuan9642 | 0:6cb2eaf8b133 | 337 | |
caoyuan9642 | 0:6cb2eaf8b133 | 338 | debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 339 | axisName, ramp_steps); // TODO: DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 340 | |
caoyuan9642 | 0:6cb2eaf8b133 | 341 | for (unsigned int i = 1; i <= ramp_steps; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 342 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 343 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 344 | stepsPerDeg |
caoyuan9642 | 0:6cb2eaf8b133 | 345 | * ((endSpeed - startSpeed) / ramp_steps |
caoyuan9642 | 0:6cb2eaf8b133 | 346 | * i + startSpeed)) |
caoyuan9642 | 0:6cb2eaf8b133 | 347 | / stepsPerDeg; // Set and update currentSpeed with actual speed |
caoyuan9642 | 0:6cb2eaf8b133 | 348 | |
caoyuan9642 | 0:6cb2eaf8b133 | 349 | /*Monitor whether there is a stop/emerge stop signal*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 350 | uint32_t flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 351 | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, |
caoyuan9642 | 0:6cb2eaf8b133 | 352 | osFlagsWaitAny, |
caoyuan9642 | 0:6cb2eaf8b133 | 353 | TelescopeConfiguration::getInt( |
caoyuan9642 | 0:6cb2eaf8b133 | 354 | "acceleration_step_time")); |
caoyuan9642 | 0:6cb2eaf8b133 | 355 | |
caoyuan9642 | 0:6cb2eaf8b133 | 356 | if (flags == osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 357 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 358 | /*Nothing happened, we're good*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 359 | continue; |
caoyuan9642 | 0:6cb2eaf8b133 | 360 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 361 | else if ((flags & osFlagsError) == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 362 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 363 | // We're stopped! |
caoyuan9642 | 0:6cb2eaf8b133 | 364 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 365 | if (flags & AXIS_EMERGE_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 366 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 367 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 368 | goto emerge_stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 369 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 370 | else if (flags & AXIS_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 371 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 372 | slew_finish_state = FINISH_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 373 | goto stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 374 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 375 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 376 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 377 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 378 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 379 | if (!indefinite) |
caoyuan9642 | 0:6cb2eaf8b133 | 380 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 381 | wait_ms -= tim.read_ms(); |
caoyuan9642 | 0:6cb2eaf8b133 | 382 | tim.reset(); |
caoyuan9642 | 0:6cb2eaf8b133 | 383 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 384 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 385 | |
caoyuan9642 | 0:6cb2eaf8b133 | 386 | stop: |
caoyuan9642 | 0:6cb2eaf8b133 | 387 | /*Now deceleration*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 388 | slewState = AXIS_SLEW_DECELERATING; |
caoyuan9642 | 0:6cb2eaf8b133 | 389 | endSpeed = currentSpeed; |
caoyuan9642 | 0:6cb2eaf8b133 | 390 | ramp_steps = (unsigned int) (currentSpeed |
caoyuan9642 | 0:6cb2eaf8b133 | 391 | / (TelescopeConfiguration::getInt("acceleration_step_time") |
caoyuan9642 | 0:6cb2eaf8b133 | 392 | / 1000.0) / acceleration); |
caoyuan9642 | 0:6cb2eaf8b133 | 393 | |
caoyuan9642 | 0:6cb2eaf8b133 | 394 | if (ramp_steps < 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 395 | ramp_steps = 1; |
caoyuan9642 | 0:6cb2eaf8b133 | 396 | |
caoyuan9642 | 0:6cb2eaf8b133 | 397 | debug_if(AXIS_DEBUG, "%s: decelerate in %d steps from %f\n", axisName, |
caoyuan9642 | 0:6cb2eaf8b133 | 398 | ramp_steps, endSpeed); // TODO: DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 399 | |
caoyuan9642 | 0:6cb2eaf8b133 | 400 | for (unsigned int i = ramp_steps - 1; i >= 1; i--) |
caoyuan9642 | 0:6cb2eaf8b133 | 401 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 402 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 403 | stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg; // set and update accurate speed |
caoyuan9642 | 0:6cb2eaf8b133 | 404 | // Wait. Now we only handle EMERGENCY STOP signal, since stop has been handled already |
caoyuan9642 | 0:6cb2eaf8b133 | 405 | flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 406 | AXIS_EMERGE_STOP_SIGNAL | AXIS_STOP_KEEPSPEED_SIGNAL, |
caoyuan9642 | 0:6cb2eaf8b133 | 407 | osFlagsWaitAny, |
caoyuan9642 | 0:6cb2eaf8b133 | 408 | TelescopeConfiguration::getInt("acceleration_step_time")); |
caoyuan9642 | 0:6cb2eaf8b133 | 409 | |
caoyuan9642 | 0:6cb2eaf8b133 | 410 | if (flags != osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 411 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 412 | if (flags & AXIS_EMERGE_STOP_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 413 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 414 | // We're stopped! |
caoyuan9642 | 0:6cb2eaf8b133 | 415 | useCorrection = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 416 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 417 | goto emerge_stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 418 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 419 | else if (flags & AXIS_STOP_KEEPSPEED_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 420 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 421 | // Keep current speed |
caoyuan9642 | 0:6cb2eaf8b133 | 422 | status = AXIS_INERTIAL; |
caoyuan9642 | 0:6cb2eaf8b133 | 423 | slewState = AXIS_NOT_SLEWING; |
caoyuan9642 | 0:6cb2eaf8b133 | 424 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 425 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 426 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 427 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 428 | |
caoyuan9642 | 0:6cb2eaf8b133 | 429 | emerge_stop: |
caoyuan9642 | 0:6cb2eaf8b133 | 430 | /*Fully pull-over*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 431 | slewState = AXIS_NOT_SLEWING; |
caoyuan9642 | 0:6cb2eaf8b133 | 432 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 433 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 434 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 435 | |
caoyuan9642 | 0:6cb2eaf8b133 | 436 | if (useCorrection) |
caoyuan9642 | 0:6cb2eaf8b133 | 437 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 438 | // Switch mode |
caoyuan9642 | 0:6cb2eaf8b133 | 439 | correction_mode(); |
caoyuan9642 | 0:6cb2eaf8b133 | 440 | double correctionSpeed = TelescopeConfiguration::getDouble( |
caoyuan9642 | 0:6cb2eaf8b133 | 441 | "correction_speed_sidereal") * sidereal_speed; |
caoyuan9642 | 0:6cb2eaf8b133 | 442 | /*Use correction to goto the final angle with high resolution*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 443 | angleDeg = getAngleDeg(); |
caoyuan9642 | 0:6cb2eaf8b133 | 444 | debug_if(AXIS_DEBUG, "%s: correct from %f to %f deg\n", axisName, |
caoyuan9642 | 0:6cb2eaf8b133 | 445 | angleDeg, dest); // TODO: DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 446 | |
caoyuan9642 | 0:6cb2eaf8b133 | 447 | double diff = remainder(angleDeg - dest, 360.0); |
caoyuan9642 | 0:6cb2eaf8b133 | 448 | if (diff > TelescopeConfiguration::getDouble("max_correction_angle")) |
caoyuan9642 | 0:6cb2eaf8b133 | 449 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 450 | debug( |
caoyuan9642 | 0:6cb2eaf8b133 | 451 | "%s: correction too large: %f. Check hardware configuration.\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 452 | axisName, diff); |
caoyuan9642 | 0:6cb2eaf8b133 | 453 | stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 454 | idle_mode(); |
caoyuan9642 | 0:6cb2eaf8b133 | 455 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 456 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 457 | status = AXIS_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 458 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 459 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 460 | |
caoyuan9642 | 0:6cb2eaf8b133 | 461 | int nTry = 3; // Try 3 corrections at most |
caoyuan9642 | 0:6cb2eaf8b133 | 462 | while (--nTry |
caoyuan9642 | 0:6cb2eaf8b133 | 463 | && fabsf(diff) |
caoyuan9642 | 0:6cb2eaf8b133 | 464 | > TelescopeConfiguration::getDouble( |
caoyuan9642 | 0:6cb2eaf8b133 | 465 | "correction_tolerance")) |
caoyuan9642 | 0:6cb2eaf8b133 | 466 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 467 | /*Determine correction direction and time*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 468 | sd = (diff > 0.0) ? STEP_BACKWARD : STEP_FORWARD; |
caoyuan9642 | 0:6cb2eaf8b133 | 469 | |
caoyuan9642 | 0:6cb2eaf8b133 | 470 | /*Perform correction*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 471 | currentSpeed = stepper->setFrequency(stepsPerDeg * correctionSpeed) |
caoyuan9642 | 0:6cb2eaf8b133 | 472 | / stepsPerDeg; // Set and update actual speed |
caoyuan9642 | 0:6cb2eaf8b133 | 473 | |
caoyuan9642 | 0:6cb2eaf8b133 | 474 | int correctionTime_ms = (int) (fabs(diff) / currentSpeed * 1000); // Use the accurate speed for calculating time |
caoyuan9642 | 0:6cb2eaf8b133 | 475 | |
caoyuan9642 | 0:6cb2eaf8b133 | 476 | debug_if(AXIS_DEBUG, |
caoyuan9642 | 0:6cb2eaf8b133 | 477 | "%s: correction: from %f to %f deg. time=%d ms\n", axisName, |
caoyuan9642 | 0:6cb2eaf8b133 | 478 | angleDeg, dest, correctionTime_ms); //TODO: DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 479 | if (correctionTime_ms |
caoyuan9642 | 0:6cb2eaf8b133 | 480 | < TelescopeConfiguration::getInt("min_correction_time")) |
caoyuan9642 | 0:6cb2eaf8b133 | 481 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 482 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 483 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 484 | |
caoyuan9642 | 0:6cb2eaf8b133 | 485 | /*Start, wait, stop*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 486 | stepper->start(sd); |
caoyuan9642 | 0:6cb2eaf8b133 | 487 | uint32_t flags = osThreadFlagsWait(AXIS_EMERGE_STOP_SIGNAL, |
caoyuan9642 | 0:6cb2eaf8b133 | 488 | osFlagsWaitAny, correctionTime_ms); |
caoyuan9642 | 0:6cb2eaf8b133 | 489 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 490 | if (flags != osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 491 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 492 | // Emergency stop! |
caoyuan9642 | 0:6cb2eaf8b133 | 493 | slew_finish_state = FINISH_EMERG_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 494 | goto emerge_stop2; |
caoyuan9642 | 0:6cb2eaf8b133 | 495 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 496 | |
caoyuan9642 | 0:6cb2eaf8b133 | 497 | angleDeg = getAngleDeg(); |
caoyuan9642 | 0:6cb2eaf8b133 | 498 | diff = remainder(angleDeg - dest, 360.0); |
caoyuan9642 | 0:6cb2eaf8b133 | 499 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 500 | |
caoyuan9642 | 0:6cb2eaf8b133 | 501 | if (!nTry) |
caoyuan9642 | 0:6cb2eaf8b133 | 502 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 503 | debug("%s: correction failed. Check hardware configuration.\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 504 | axisName); |
caoyuan9642 | 0:6cb2eaf8b133 | 505 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 506 | |
caoyuan9642 | 0:6cb2eaf8b133 | 507 | debug_if(AXIS_DEBUG, "%s: correction finished: %f deg\n", axisName, |
caoyuan9642 | 0:6cb2eaf8b133 | 508 | angleDeg); //TODO:DEBUG |
caoyuan9642 | 0:6cb2eaf8b133 | 509 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 510 | emerge_stop2: |
caoyuan9642 | 0:6cb2eaf8b133 | 511 | // Set status to stopped |
caoyuan9642 | 0:6cb2eaf8b133 | 512 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 513 | status = AXIS_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 514 | idle_mode(); |
caoyuan9642 | 0:6cb2eaf8b133 | 515 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 516 | |
caoyuan9642 | 0:6cb2eaf8b133 | 517 | void Axis::track(axisrotdir_t dir) |
caoyuan9642 | 0:6cb2eaf8b133 | 518 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 519 | track_mode(); |
caoyuan9642 | 0:6cb2eaf8b133 | 520 | if (trackSpeed != 0 && dir != AXIS_ROTATE_STOP) |
caoyuan9642 | 0:6cb2eaf8b133 | 521 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 522 | stepdir_t sd = |
caoyuan9642 | 0:6cb2eaf8b133 | 523 | (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD; |
caoyuan9642 | 0:6cb2eaf8b133 | 524 | currentSpeed = stepper->setFrequency(trackSpeed * stepsPerDeg) |
caoyuan9642 | 0:6cb2eaf8b133 | 525 | / stepsPerDeg; |
caoyuan9642 | 0:6cb2eaf8b133 | 526 | currentDirection = dir; |
caoyuan9642 | 0:6cb2eaf8b133 | 527 | stepper->start(sd); |
caoyuan9642 | 0:6cb2eaf8b133 | 528 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 529 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 530 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 531 | // For DEC axis |
caoyuan9642 | 0:6cb2eaf8b133 | 532 | dir = AXIS_ROTATE_STOP; |
caoyuan9642 | 0:6cb2eaf8b133 | 533 | trackSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 534 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 535 | currentDirection = AXIS_ROTATE_POSITIVE; |
caoyuan9642 | 0:6cb2eaf8b133 | 536 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 537 | status = AXIS_TRACKING; |
caoyuan9642 | 0:6cb2eaf8b133 | 538 | Thread::signal_clr( |
caoyuan9642 | 0:6cb2eaf8b133 | 539 | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_GUIDE_SIGNAL); |
caoyuan9642 | 0:6cb2eaf8b133 | 540 | // Empty the guide queue |
caoyuan9642 | 0:6cb2eaf8b133 | 541 | while (!guide_queue.empty()) |
caoyuan9642 | 0:6cb2eaf8b133 | 542 | guide_queue.get(); |
caoyuan9642 | 0:6cb2eaf8b133 | 543 | |
caoyuan9642 | 0:6cb2eaf8b133 | 544 | while (true) |
caoyuan9642 | 0:6cb2eaf8b133 | 545 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 546 | // Now we wait for SOMETHING to happen - either STOP, EMERGE_STOP or GUIDE |
caoyuan9642 | 0:6cb2eaf8b133 | 547 | uint32_t flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 548 | AXIS_GUIDE_SIGNAL | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, |
caoyuan9642 | 0:6cb2eaf8b133 | 549 | osFlagsWaitAny, osWaitForever); |
caoyuan9642 | 0:6cb2eaf8b133 | 550 | if ((flags & osFlagsError) == 0) // has flag |
caoyuan9642 | 0:6cb2eaf8b133 | 551 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 552 | if (flags & (AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL)) |
caoyuan9642 | 0:6cb2eaf8b133 | 553 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 554 | // We stop tracking |
caoyuan9642 | 0:6cb2eaf8b133 | 555 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 556 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 557 | else if (flags & AXIS_GUIDE_SIGNAL) |
caoyuan9642 | 0:6cb2eaf8b133 | 558 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 559 | bool stopped = false; |
caoyuan9642 | 2:2ee28add0821 | 560 | guiding = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 561 | // Guide. Process all commands in the queue |
caoyuan9642 | 0:6cb2eaf8b133 | 562 | while (true) |
caoyuan9642 | 0:6cb2eaf8b133 | 563 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 564 | osEvent evt = guide_queue.get(0); // try to get a message |
caoyuan9642 | 0:6cb2eaf8b133 | 565 | if (evt.status == osEventMessage) |
caoyuan9642 | 0:6cb2eaf8b133 | 566 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 567 | int guideTime_ms = (int) evt.value.p; |
caoyuan9642 | 0:6cb2eaf8b133 | 568 | if (guideTime_ms == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 569 | continue; // Nothing to guide |
caoyuan9642 | 0:6cb2eaf8b133 | 570 | // Determine guide direction |
caoyuan9642 | 0:6cb2eaf8b133 | 571 | axisrotdir_t guide_dir = |
caoyuan9642 | 0:6cb2eaf8b133 | 572 | (guideTime_ms > 0) ? |
caoyuan9642 | 0:6cb2eaf8b133 | 573 | AXIS_ROTATE_POSITIVE : |
caoyuan9642 | 0:6cb2eaf8b133 | 574 | AXIS_ROTATE_NEGATIVE; |
caoyuan9642 | 0:6cb2eaf8b133 | 575 | |
caoyuan9642 | 0:6cb2eaf8b133 | 576 | double newSpeed = |
caoyuan9642 | 0:6cb2eaf8b133 | 577 | (guide_dir == currentDirection) ? |
caoyuan9642 | 0:6cb2eaf8b133 | 578 | trackSpeed + guideSpeed : |
caoyuan9642 | 0:6cb2eaf8b133 | 579 | trackSpeed - guideSpeed; /*Determine speed in the original direction (currentDirection)*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 580 | |
caoyuan9642 | 0:6cb2eaf8b133 | 581 | // Clamp to maximum guide time |
caoyuan9642 | 0:6cb2eaf8b133 | 582 | guideTime_ms = abs(guideTime_ms); |
caoyuan9642 | 0:6cb2eaf8b133 | 583 | if (guideTime_ms |
caoyuan9642 | 0:6cb2eaf8b133 | 584 | > TelescopeConfiguration::getInt( |
caoyuan9642 | 0:6cb2eaf8b133 | 585 | "max_guide_time")) |
caoyuan9642 | 0:6cb2eaf8b133 | 586 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 587 | debug("Axis: Guiding time too long: %d ms\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 588 | abs(guideTime_ms)); |
caoyuan9642 | 0:6cb2eaf8b133 | 589 | guideTime_ms = TelescopeConfiguration::getInt( |
caoyuan9642 | 0:6cb2eaf8b133 | 590 | "max_guide_time"); |
caoyuan9642 | 0:6cb2eaf8b133 | 591 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 592 | |
caoyuan9642 | 0:6cb2eaf8b133 | 593 | bool dirswitch = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 594 | if (newSpeed > 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 595 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 596 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 597 | newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed |
caoyuan9642 | 0:6cb2eaf8b133 | 598 | if (trackSpeed == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 599 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 600 | // For DEC, we also need to start the motor |
caoyuan9642 | 0:6cb2eaf8b133 | 601 | stepper->start(STEP_FORWARD); // Reverse direction |
caoyuan9642 | 0:6cb2eaf8b133 | 602 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 603 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 604 | else if (newSpeed < 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 605 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 606 | // |
caoyuan9642 | 0:6cb2eaf8b133 | 607 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 608 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 609 | -newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed |
caoyuan9642 | 0:6cb2eaf8b133 | 610 | stepper->start( |
caoyuan9642 | 0:6cb2eaf8b133 | 611 | (currentDirection == AXIS_ROTATE_POSITIVE) ? |
caoyuan9642 | 0:6cb2eaf8b133 | 612 | STEP_BACKWARD : STEP_FORWARD); // Reverse direction |
caoyuan9642 | 0:6cb2eaf8b133 | 613 | dirswitch = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 614 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 615 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 616 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 617 | // newSpeed == 0, just stop the motor |
caoyuan9642 | 0:6cb2eaf8b133 | 618 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 619 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 620 | dirswitch = true; // Make sure to recover the original speed |
caoyuan9642 | 0:6cb2eaf8b133 | 621 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 622 | |
caoyuan9642 | 0:6cb2eaf8b133 | 623 | uint32_t flags = osThreadFlagsWait( |
caoyuan9642 | 0:6cb2eaf8b133 | 624 | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, |
caoyuan9642 | 0:6cb2eaf8b133 | 625 | osFlagsWaitAny, guideTime_ms); |
caoyuan9642 | 0:6cb2eaf8b133 | 626 | if (flags != osFlagsErrorTimeout) |
caoyuan9642 | 0:6cb2eaf8b133 | 627 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 628 | //break and stop; |
caoyuan9642 | 0:6cb2eaf8b133 | 629 | stopped = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 630 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 631 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 632 | if (dirswitch || trackSpeed == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 633 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 634 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 635 | // Restart the motor in original direction |
caoyuan9642 | 0:6cb2eaf8b133 | 636 | if (trackSpeed != 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 637 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 638 | stepper->start( |
caoyuan9642 | 0:6cb2eaf8b133 | 639 | (currentDirection |
caoyuan9642 | 0:6cb2eaf8b133 | 640 | == AXIS_ROTATE_POSITIVE) ? |
caoyuan9642 | 0:6cb2eaf8b133 | 641 | STEP_FORWARD : STEP_BACKWARD); // Reverse direction |
caoyuan9642 | 0:6cb2eaf8b133 | 642 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 643 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 644 | // Restore to normal speed |
caoyuan9642 | 0:6cb2eaf8b133 | 645 | if (trackSpeed != 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 646 | currentSpeed = stepper->setFrequency( |
caoyuan9642 | 0:6cb2eaf8b133 | 647 | trackSpeed * stepsPerDeg) / stepsPerDeg; |
caoyuan9642 | 0:6cb2eaf8b133 | 648 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 649 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 650 | |
caoyuan9642 | 0:6cb2eaf8b133 | 651 | // End guiding |
caoyuan9642 | 0:6cb2eaf8b133 | 652 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 653 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 654 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 655 | // No more message to get. Break out |
caoyuan9642 | 0:6cb2eaf8b133 | 656 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 657 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 658 | } |
caoyuan9642 | 2:2ee28add0821 | 659 | guiding = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 660 | if (stopped) |
caoyuan9642 | 0:6cb2eaf8b133 | 661 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 662 | //Complete break out |
caoyuan9642 | 0:6cb2eaf8b133 | 663 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 664 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 665 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 666 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 667 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 668 | |
caoyuan9642 | 0:6cb2eaf8b133 | 669 | // Stop |
caoyuan9642 | 0:6cb2eaf8b133 | 670 | currentSpeed = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 671 | stepper->stop(); |
caoyuan9642 | 0:6cb2eaf8b133 | 672 | status = AXIS_STOPPED; |
caoyuan9642 | 0:6cb2eaf8b133 | 673 | idle_mode(); |
caoyuan9642 | 0:6cb2eaf8b133 | 674 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 675 | |
caoyuan9642 | 0:6cb2eaf8b133 | 676 |