Diff: Axis.cpp

Revision:

0:6cb2eaf8b133

Child:

2:2ee28add0821

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Axis.cpp	Sun Aug 19 05:21:20 2018 +0000
@@ -0,0 +1,674 @@
+/*
+ * Axis.cpp
+ *
+ *  Created on: 2018Äê2ÔÂ24ÈÕ
+ *      Author: caoyuan9642
+ */
+
+#include <Axis.h>
+
+#define AXIS_DEBUG 1
+
+static inline double min(double x, double y)
+{
+	return (x < y) ? x : y;
+}
+Axis::Axis(double stepsPerDeg, StepperMotor *stepper, const char *name) :
+		stepsPerDeg(stepsPerDeg), stepper(stepper), axisName(name), currentSpeed(
+				0), currentDirection(AXIS_ROTATE_POSITIVE), slewSpeed(
+				TelescopeConfiguration::getDouble("default_slew_speed")), trackSpeed(
+				TelescopeConfiguration::getDouble(
+						"default_track_speed_sidereal") * sidereal_speed), guideSpeed(
+				TelescopeConfiguration::getDouble(
+						"default_guide_speed_sidereal") * sidereal_speed), status(
+				AXIS_STOPPED), slewState(AXIS_NOT_SLEWING), slew_finish_sem(0,
+				1), slew_finish_state(FINISH_COMPLETE)
+{
+	if (stepsPerDeg <= 0)
+		error("Axis: steps per degree must be > 0");
+
+	if (!stepper)
+		error("Axis: stepper must be defined");
+
+	taskName = new char[strlen(name) + 10];
+	strcpy(taskName, name);
+	strcat(taskName, " task");
+	/*Start the task-handling thread*/
+	task_thread = new Thread(osPriorityAboveNormal,
+	OS_STACK_SIZE, NULL, taskName);
+	task_thread->start(callback(this, &Axis::task));
+	tim.start();
+}
+
+Axis::~Axis()
+{
+// Wait until the axis is stopped
+	if (status != AXIS_STOPPED)
+	{
+		stop();
+		while (status != AXIS_STOPPED)
+		{
+			Thread::wait(100);
+		}
+	}
+
+// Terminate the task thread to prevent illegal access to destroyed objects.
+	task_thread->terminate();
+	delete task_thread;
+	delete taskName;
+}
+
+void Axis::task()
+{
+
+	/*Main loop of RotationAxis*/
+	while (true)
+	{
+		/*Get next message*/
+		msg_t *message;
+		enum msg_t::sig_t signal;
+		float value;
+		axisrotdir_t dir;
+		bool wc;
+
+		// Wait for next message
+		osEvent evt = task_queue.get();
+		if (evt.status == osEventMessage)
+		{
+			/*New message arrived. Copy the data and free is asap*/
+			message = (msg_t*) evt.value.p;
+			signal = message->signal;
+			value = message->value;
+			dir = message->dir;
+			wc = message->withCorrection;
+			task_pool.free(message);
+		}
+		else
+		{
+			/*Error*/
+			debug("%s: Error fetching the task queue.\n", axisName);
+			continue;
+		}
+		debug_if(AXIS_DEBUG, "%s: MSG %d %f %d 0x%8x\n", axisName, signal,
+				value, dir);
+
+		/*Check the type of the signal, and start corresponding operations*/
+		switch (signal)
+		{
+		case msg_t::SIGNAL_SLEW_TO:
+			if (status == AXIS_STOPPED)
+			{
+				slew(dir, value, false, wc);
+			}
+			else
+			{
+				debug("%s: being slewed while not in STOPPED mode.\n",
+						axisName);
+			}
+			debug_if(0, "%s: SIG SLEW 0x%08x\n", axisName, Thread::gettid());
+			slew_finish_sem.release(); /*Send a signal so that the caller is free to run*/
+			break;
+		case msg_t::SIGNAL_SLEW_INDEFINITE:
+			if (status == AXIS_STOPPED || status == AXIS_INERTIAL)
+			{
+				slew(dir, 0.0, true, false);
+			}
+			else
+			{
+				debug("%s: being slewed while not in STOPPED mode.\n",
+						axisName);
+			}
+			break;
+		case msg_t::SIGNAL_TRACK:
+			if (status == AXIS_STOPPED)
+			{
+				track(dir);
+			}
+			else
+			{
+				debug("%s: trying to track while not in STOPPED mode.\n",
+						axisName);
+			}
+			break;
+		default:
+			debug("%s: undefined signal %d\n", axisName, message->signal);
+		}
+	}
+}
+
+void Axis::slew(axisrotdir_t dir, double dest, bool indefinite,
+bool useCorrection)
+{
+	if (!indefinite && (isnan(dest) || isinf(dest)))
+	{
+		debug("%s: invalid angle.\n", axisName);
+		return;
+	}
+	if (dir == AXIS_ROTATE_STOP)
+	{
+		debug("%s: skipped.\n", axisName);
+		return;
+	}
+
+	slew_mode(); // Switch to slew mode
+	Thread::signal_clr(
+	AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_SPEEDCHANGE_SIGNAL); // Clear flags
+	bool isInertial = (status == AXIS_INERTIAL);
+	status = AXIS_SLEWING;
+	slewState = AXIS_NOT_SLEWING;
+	slew_finish_state = FINISH_COMPLETE;
+	currentDirection = dir;
+	stepdir_t sd = (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
+
+	/* Calculate the angle to rotate*/
+	bool skip_slew = false;
+	double angleDeg = getAngleDeg();
+	double delta;
+	delta = (dest - angleDeg) * (dir == AXIS_ROTATE_POSITIVE ? 1 : -1); /*delta is the actual angle to rotate*/
+	if (fabs(delta) < 1e-5)
+	{ // FIX: delta 0->360degrees when angleDeg is essentially equal to dest
+		delta = 0;
+	}
+	delta = remainder(delta - 180.0, 360.0) + 180.0; /*Shift to 0-360 deg*/
+	debug_if(AXIS_DEBUG, "%s: start=%f, end=%f, delta=%f\n", axisName, angleDeg,
+			dest, delta);
+
+	double startSpeed = 0;
+	double endSpeed = slewSpeed, waitTime;
+	unsigned int ramp_steps;
+	double acceleration = TelescopeConfiguration::getDouble("acceleration");
+
+	if (!indefinite)
+	{
+		// Ensure that delta is more than the minimum slewing angle, calculate the correct endSpeed and waitTime
+		if (delta > TelescopeConfiguration::getDouble("min_slew_angle"))
+		{
+			/*The motion angle is decreased to ensure the correction step is in the same direction*/
+			delta = delta
+					- 0.5 * TelescopeConfiguration::getDouble("min_slew_angle");
+
+			double angleRotatedDuringAcceleration;
+
+			/* The actual endSpeed we get might be different than this due to the finite time resolution of the stepper driver
+			 * 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
+			 */
+
+			// Calculate the desired endSpeed. If delta is small, then endSpeed will correspondingly be reduced
+			endSpeed = min(sqrt(delta * acceleration),
+					stepper->setFrequency(stepsPerDeg * endSpeed)
+							/ stepsPerDeg);
+			ramp_steps = (unsigned int) (endSpeed
+					/ (TelescopeConfiguration::getInt("acceleration_step_time")
+							/ 1000.0) / acceleration);
+			if (ramp_steps < 1)
+				ramp_steps = 1;
+
+			angleRotatedDuringAcceleration = 0;
+			/*Simulate the slewing process to get an accurate estimate of the actual angle that will be slewed*/
+			for (unsigned int i = 1; i <= ramp_steps; i++)
+			{
+				double speed = stepper->setFrequency(
+						stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg;
+				angleRotatedDuringAcceleration += speed
+						* (TelescopeConfiguration::getInt(
+								"acceleration_step_time") / 1000.0)
+						* (i == ramp_steps ? 1 : 2); // Count both acceleration and deceleration
+			}
+
+			waitTime = (delta - angleRotatedDuringAcceleration) / endSpeed;
+			if (waitTime < 0.0)
+				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
+
+			debug_if(AXIS_DEBUG, "%s: endspeed = %f deg/s, time=%f, acc=%f\n",
+					axisName, endSpeed, waitTime, acceleration);
+		}
+		else
+		{
+			// Angle difference is too small, skip slewing
+			skip_slew = true;
+		}
+	}
+	else
+	{
+		// Indefinite slewing mode
+		waitTime = INFINITY;
+		// If was in inertial mode, use startSpeed
+		if (isInertial)
+			startSpeed = currentSpeed;
+		// No need for correction
+		useCorrection = false;
+	}
+
+	/*Slewing -> accel, wait, decel*/
+	if (!skip_slew)
+	{
+		int wait_ms;
+		uint32_t flags;
+		/*Acceleration*/
+		slewState = AXIS_SLEW_ACCELERATING;
+		ramp_steps = (unsigned int) ((endSpeed - startSpeed)
+				/ (TelescopeConfiguration::getInt("acceleration_step_time")
+						/ 1000.0) / acceleration);
+
+		if (ramp_steps < 1)
+			ramp_steps = 1;
+
+		debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n", axisName,
+				ramp_steps); // TODO: DEBUG
+
+		for (unsigned int i = 1; i <= ramp_steps; i++)
+		{
+			currentSpeed = stepper->setFrequency(
+					stepsPerDeg
+							* ((endSpeed - startSpeed) / ramp_steps * i
+									+ startSpeed)) / stepsPerDeg; // Set and update currentSpeed with actual speed
+
+			if (i == 1)
+				stepper->start(sd);
+
+			/*Monitor whether there is a stop/emerge stop signal*/
+			uint32_t flags = osThreadFlagsWait(
+			AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, osFlagsWaitAny,
+					TelescopeConfiguration::getInt("acceleration_step_time"));
+
+			if (flags == osFlagsErrorTimeout)
+			{
+				/*Nothing happened, we're good*/
+				continue;
+			}
+			else if ((flags & osFlagsError) == 0)
+			{
+				// We're stopped!
+				useCorrection = false;
+				if (flags & AXIS_EMERGE_STOP_SIGNAL)
+				{
+					slew_finish_state = FINISH_EMERG_STOPPED;
+					goto emerge_stop;
+				}
+				else if (flags & AXIS_STOP_SIGNAL)
+				{
+					slew_finish_state = FINISH_STOPPED;
+					goto stop;
+				}
+			}
+		}
+
+		/*Keep slewing and wait*/
+		slewState = AXIS_SLEW_CONSTANT_SPEED;
+		debug_if(AXIS_DEBUG, "%s: wait for %f\n", axisName, waitTime); // TODO
+		wait_ms = (isinf(waitTime)) ? osWaitForever : (int) (waitTime * 1000);
+
+		tim.reset();
+
+		while (wait_ms)
+		{
+			flags = osThreadFlagsWait(
+					AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL
+							| (indefinite ? AXIS_SPEEDCHANGE_SIGNAL : 0),
+					osFlagsWaitAny, wait_ms); /*Wait the remaining time*/
+			if (flags != osFlagsErrorTimeout)
+			{
+				if (flags & AXIS_EMERGE_STOP_SIGNAL)
+				{
+					slew_finish_state = FINISH_EMERG_STOPPED;
+					useCorrection = false;
+					goto emerge_stop;
+				}
+				else if (flags & AXIS_STOP_SIGNAL)
+				{
+					slew_finish_state = FINISH_STOPPED;
+					useCorrection = false;
+					goto stop;
+				}
+				else if (flags & AXIS_SPEEDCHANGE_SIGNAL)
+				{
+					// Change speed, therefore also changing waittime. Only applies to indefinite slew
+					double newSpeed = slewSpeed;
+					startSpeed = currentSpeed;
+					endSpeed = newSpeed;
+
+					ramp_steps = (unsigned int) (fabs(endSpeed - startSpeed)
+							/ (TelescopeConfiguration::getInt(
+									"acceleration_step_time") / 1000.0)
+							/ acceleration);
+
+					if (ramp_steps < 1)
+						ramp_steps = 1;
+
+					debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n",
+							axisName, ramp_steps); // TODO: DEBUG
+
+					for (unsigned int i = 1; i <= ramp_steps; i++)
+					{
+						currentSpeed = stepper->setFrequency(
+								stepsPerDeg
+										* ((endSpeed - startSpeed) / ramp_steps
+												* i + startSpeed))
+								/ stepsPerDeg; // Set and update currentSpeed with actual speed
+
+						/*Monitor whether there is a stop/emerge stop signal*/
+						uint32_t flags = osThreadFlagsWait(
+						AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
+						osFlagsWaitAny,
+								TelescopeConfiguration::getInt(
+										"acceleration_step_time"));
+
+						if (flags == osFlagsErrorTimeout)
+						{
+							/*Nothing happened, we're good*/
+							continue;
+						}
+						else if ((flags & osFlagsError) == 0)
+						{
+							// We're stopped!
+							useCorrection = false;
+							if (flags & AXIS_EMERGE_STOP_SIGNAL)
+							{
+								slew_finish_state = FINISH_EMERG_STOPPED;
+								goto emerge_stop;
+							}
+							else if (flags & AXIS_STOP_SIGNAL)
+							{
+								slew_finish_state = FINISH_STOPPED;
+								goto stop;
+							}
+						}
+					}
+				}
+			}
+			if (!indefinite)
+			{
+				wait_ms -= tim.read_ms();
+				tim.reset();
+			}
+		}
+
+		stop:
+		/*Now deceleration*/
+		slewState = AXIS_SLEW_DECELERATING;
+		endSpeed = currentSpeed;
+		ramp_steps = (unsigned int) (currentSpeed
+				/ (TelescopeConfiguration::getInt("acceleration_step_time")
+						/ 1000.0) / acceleration);
+
+		if (ramp_steps < 1)
+			ramp_steps = 1;
+
+		debug_if(AXIS_DEBUG, "%s: decelerate in %d steps from %f\n", axisName,
+				ramp_steps, endSpeed); // TODO: DEBUG
+
+		for (unsigned int i = ramp_steps - 1; i >= 1; i--)
+		{
+			currentSpeed = stepper->setFrequency(
+					stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg; // set and update accurate speed
+			// Wait. Now we only handle EMERGENCY STOP signal, since stop has been handled already
+			flags = osThreadFlagsWait(
+			AXIS_EMERGE_STOP_SIGNAL | AXIS_STOP_KEEPSPEED_SIGNAL,
+			osFlagsWaitAny,
+					TelescopeConfiguration::getInt("acceleration_step_time"));
+
+			if (flags != osFlagsErrorTimeout)
+			{
+				if (flags & AXIS_EMERGE_STOP_SIGNAL)
+				{
+					// We're stopped!
+					useCorrection = false;
+					slew_finish_state = FINISH_EMERG_STOPPED;
+					goto emerge_stop;
+				}
+				else if (flags & AXIS_STOP_KEEPSPEED_SIGNAL)
+				{
+					// Keep current speed
+					status = AXIS_INERTIAL;
+					slewState = AXIS_NOT_SLEWING;
+					return;
+				}
+			}
+		}
+
+		emerge_stop:
+		/*Fully pull-over*/
+		slewState = AXIS_NOT_SLEWING;
+		stepper->stop();
+		currentSpeed = 0;
+	}
+
+	if (useCorrection)
+	{
+		// Switch mode
+		correction_mode();
+		double correctionSpeed = TelescopeConfiguration::getDouble(
+				"correction_speed_sidereal") * sidereal_speed;
+		/*Use correction to goto the final angle with high resolution*/
+		angleDeg = getAngleDeg();
+		debug_if(AXIS_DEBUG, "%s: correct from %f to %f deg\n", axisName,
+				angleDeg, dest); // TODO: DEBUG
+
+		double diff = remainder(angleDeg - dest, 360.0);
+		if (diff > TelescopeConfiguration::getDouble("max_correction_angle"))
+		{
+			debug(
+					"%s: correction too large: %f. Check hardware configuration.\n",
+					axisName, diff);
+			stop();
+			idle_mode();
+			currentSpeed = 0;
+			slew_finish_state = FINISH_EMERG_STOPPED;
+			status = AXIS_STOPPED;
+			return;
+		}
+
+		int nTry = 3; // Try 3 corrections at most
+		while (--nTry
+				&& fabsf(diff)
+						> TelescopeConfiguration::getDouble(
+								"correction_tolerance"))
+		{
+			/*Determine correction direction and time*/
+			sd = (diff > 0.0) ? STEP_BACKWARD : STEP_FORWARD;
+
+			/*Perform correction*/
+			currentSpeed = stepper->setFrequency(stepsPerDeg * correctionSpeed)
+					/ stepsPerDeg; // Set and update actual speed
+
+			int correctionTime_ms = (int) (fabs(diff) / currentSpeed * 1000); // Use the accurate speed for calculating time
+
+			debug_if(AXIS_DEBUG,
+					"%s: correction: from %f to %f deg. time=%d ms\n", axisName,
+					angleDeg, dest, correctionTime_ms); //TODO: DEBUG
+			if (correctionTime_ms
+					< TelescopeConfiguration::getInt("min_correction_time"))
+			{
+				break;
+			}
+
+			/*Start, wait, stop*/
+			stepper->start(sd);
+			uint32_t flags = osThreadFlagsWait(AXIS_EMERGE_STOP_SIGNAL,
+			osFlagsWaitAny, correctionTime_ms);
+			stepper->stop();
+			if (flags != osFlagsErrorTimeout)
+			{
+				// Emergency stop!
+				slew_finish_state = FINISH_EMERG_STOPPED;
+				goto emerge_stop2;
+			}
+
+			angleDeg = getAngleDeg();
+			diff = remainder(angleDeg - dest, 360.0);
+		}
+
+		if (!nTry)
+		{
+			debug("%s: correction failed. Check hardware configuration.\n",
+					axisName);
+		}
+
+		debug_if(AXIS_DEBUG, "%s: correction finished: %f deg\n", axisName,
+				angleDeg); //TODO:DEBUG
+	}
+	emerge_stop2:
+// Set status to stopped
+	currentSpeed = 0;
+	status = AXIS_STOPPED;
+	idle_mode();
+}
+
+void Axis::track(axisrotdir_t dir)
+{
+	track_mode();
+	if (trackSpeed != 0 && dir != AXIS_ROTATE_STOP)
+	{
+		stepdir_t sd =
+				(dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
+		currentSpeed = stepper->setFrequency(trackSpeed * stepsPerDeg)
+				/ stepsPerDeg;
+		currentDirection = dir;
+		stepper->start(sd);
+	}
+	else
+	{
+		// For DEC axis
+		dir = AXIS_ROTATE_STOP;
+		trackSpeed = 0;
+		currentSpeed = 0;
+		currentDirection = AXIS_ROTATE_POSITIVE;
+	}
+	status = AXIS_TRACKING;
+	Thread::signal_clr(
+	AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_GUIDE_SIGNAL);
+	// Empty the guide queue
+	while (!guide_queue.empty())
+		guide_queue.get();
+
+	while (true)
+	{
+		// Now we wait for SOMETHING to happen - either STOP, EMERGE_STOP or GUIDE
+		uint32_t flags = osThreadFlagsWait(
+		AXIS_GUIDE_SIGNAL | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
+		osFlagsWaitAny, osWaitForever);
+		if ((flags & osFlagsError) == 0) // has flag
+		{
+			if (flags & (AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL))
+			{
+				// We stop tracking
+				break;
+			}
+			else if (flags & AXIS_GUIDE_SIGNAL)
+			{
+				bool stopped = false;
+				// Guide. Process all commands in the queue
+				while (true)
+				{
+					osEvent evt = guide_queue.get(0); // try to get a message
+					if (evt.status == osEventMessage)
+					{
+						int guideTime_ms = (int) evt.value.p;
+						if (guideTime_ms == 0)
+							continue; // Nothing to guide
+						// Determine guide direction
+						axisrotdir_t guide_dir =
+								(guideTime_ms > 0) ?
+										AXIS_ROTATE_POSITIVE :
+										AXIS_ROTATE_NEGATIVE;
+
+						double newSpeed =
+								(guide_dir == currentDirection) ?
+										trackSpeed + guideSpeed :
+										trackSpeed - guideSpeed; /*Determine speed in the original direction (currentDirection)*/
+
+						// Clamp to maximum guide time
+						guideTime_ms = abs(guideTime_ms);
+						if (guideTime_ms
+								> TelescopeConfiguration::getInt(
+										"max_guide_time"))
+						{
+							debug("Axis: Guiding time too long: %d ms\n",
+									abs(guideTime_ms));
+							guideTime_ms = TelescopeConfiguration::getInt(
+									"max_guide_time");
+						}
+
+						bool dirswitch = false;
+						if (newSpeed > 0)
+						{
+							currentSpeed = stepper->setFrequency(
+									newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
+							if (trackSpeed == 0)
+							{
+								// For DEC, we also need to start the motor
+								stepper->start(STEP_FORWARD); // Reverse direction
+							}
+						}
+						else if (newSpeed < 0)
+						{
+							//
+							stepper->stop();
+							currentSpeed = stepper->setFrequency(
+									-newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
+							stepper->start(
+									(currentDirection == AXIS_ROTATE_POSITIVE) ?
+											STEP_BACKWARD : STEP_FORWARD); // Reverse direction
+							dirswitch = true;
+						}
+						else
+						{
+							// newSpeed == 0, just stop the motor
+							currentSpeed = 0;
+							stepper->stop();
+							dirswitch = true; // Make sure to recover the original speed
+						}
+
+						uint32_t flags = osThreadFlagsWait(
+						AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
+						osFlagsWaitAny, guideTime_ms);
+						if (flags != osFlagsErrorTimeout)
+						{
+							//break and stop;
+							stopped = true;
+							break;
+						}
+						if (dirswitch || trackSpeed == 0)
+						{
+							stepper->stop();
+							// Restart the motor in original direction
+							if (trackSpeed != 0)
+							{
+								stepper->start(
+										(currentDirection
+												== AXIS_ROTATE_POSITIVE) ?
+												STEP_FORWARD : STEP_BACKWARD); // Reverse direction
+							}
+						}
+						// Restore to normal speed
+						if (trackSpeed != 0)
+							currentSpeed = stepper->setFrequency(
+									trackSpeed * stepsPerDeg) / stepsPerDeg;
+						else
+							currentSpeed = 0;
+
+						// End guiding
+					}
+					else
+					{
+						// No more message to get. Break out
+						break;
+					}
+				}
+				if (stopped)
+				{
+					//Complete break out
+					break;
+				}
+			}
+		}
+	}
+
+// Stop
+	currentSpeed = 0;
+	stepper->stop();
+	status = AXIS_STOPPED;
+	idle_mode();
+}
+
+