Diff: CGimbal.cpp

Revision:

53:72f350a6d09c

diff -r 53b1625b8035 -r 72f350a6d09c CGimbal.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CGimbal.cpp	Thu Apr 14 13:45:38 2016 +0100
@@ -0,0 +1,210 @@
+/*
+ * CGimbal.cpp
+ *
+ *  Created on: 15 Feb 2016
+ *      Author: mike
+ */
+#include "base.h"
+#include <CGimbal.h>
+#include <math.h>
+int debug = 0;
+
+extern Serial pc;
+
+/*
+ * The way-points required when performing a sweep of the room, moves from Home to a mid-point of the left-most x,y coords
+ * Then moves at 60 degree increments to the right-most position
+ * Then moves down at 15 degree increments
+ * Uses meander-scan technique rather than raster-scan to reduce motor loading and risk of skipping steps
+ */
+static const float ScanArray[36][2] =
+{
+	{30, 7.5}, {90, 7.5}, {150, 7.5}, {150, 22.5}, {90, 22.5}, {30, 22.5},
+	{30, 37.5}, {90, 37.5}, {150, 37.5}, {150, 52.5}, {90, 52.5}, {30, 52.5},
+	{30, 67.5}, {90, 67.5}, {150, 67.5}, {150, 82.5}, {90, 82.5}, {30, 82.5},
+	{30, 97.5}, {90, 97.5}, {150, 97.5}, {150, 112.5}, {90, 112.5}, {30, 112.5},
+	{30, 127.5}, {90, 127.5}, {150, 127.5}, {150, 142.5}, {90, 142.5}, {30, 142.5},
+	{30, 157.5}, {90, 157.5}, {150, 157.5}, {150, 172.5}, {90, 172.5}, {30, 172.5}
+};
+
+
+
+CGimbal::CGimbal(DigitalOut * pXStepPin, DigitalOut * pXDirPin, DigitalOut * pYStepPin, DigitalOut * pYDirPin)
+{
+	//	Zero the tracking parameters
+
+	m_StepInterval = DFT_STEP_INTERVAL;		//	Determines the default motor speed (10us Ticks between each step pulse)
+	m_AbsoluteXPos = 0;						//	Always starts at 0 point
+	m_AbsoluteXAngle = 0.0;					//	Ditto
+	m_AbsoluteYPos = 0;
+	m_AbsoluteYAngle = 0.0;
+	m_xDir = FORWARD;						//	Always initialised in forward direction
+	m_yDir = FORWARD;						//	Ditto
+
+	m_xSteps=0;								//	Nothing to do immediately after being constructed
+	m_ySteps=0;
+
+	m_pXStepPin = pXStepPin;				//	Initialise Pointers to each pin
+	m_pXDirPin = pXDirPin;
+	m_pYStepPin = pYStepPin;
+	m_pYDirPin = pYDirPin;
+
+	m_pXStepPin->write(0);					//	Set all motor driver signals to a known state (LOW)
+	m_pXDirPin->write(0);
+	m_pYStepPin->write(0);
+	m_pYDirPin->write(0);
+
+	m_NumWaypoints = 2;						//	Start with 2 waypoints
+
+	m_TickCount=0;							//	Start at 0
+}
+
+//	Destructor
+CGimbal::~CGimbal()
+{
+
+}
+
+/*
+ *	Perform a vectored move, relative to the current x,y position and update internal tracking parameters
+ */
+int	CGimbal::MoveRel()
+{
+
+	if (m_xSteps > 0)					//	Determine which direction the motors needs to move
+		m_xDir = FORWARD;
+	else
+		m_xDir = REVERSE;
+	if (m_ySteps > 0)
+		m_yDir = 1;
+	else
+		m_yDir = 0;
+	m_pXDirPin->write(m_xDir);
+	m_pYDirPin->write(m_yDir);
+
+	if (m_xSteps > 0)					//	Absolute number of pulses is always positive regardless of directiong
+		m_xPulses = m_xSteps;
+	else
+		if ((m_xSteps < 0))
+			m_xPulses = -m_xSteps;
+		else
+			m_xPulses = 0;				//	If no steps, do nothing
+
+	if (m_ySteps > 0)					//	Absolute number of pulses is always positive regardless of directiong
+		m_yPulses = m_ySteps;
+	else
+		if ((m_ySteps < 0))
+			m_yPulses = -m_ySteps;
+		else
+			m_yPulses = 0;				//	If no steps, do nothing
+
+	//	Wait for the Ticker to complete this move
+	while (m_xPulses > 0 || m_yPulses > 0);
+
+	m_AbsoluteXPos += m_xSteps;			//	Track the new absolute x position
+	m_AbsoluteYPos += m_ySteps;			//	Track the new absolute y position
+	m_AbsoluteXAngle = (float) ( (float) m_AbsoluteXPos * DEGREES_PER_STEP);	//	Track the new absolute x angle
+	m_AbsoluteYAngle = (float) ( (float) m_AbsoluteYPos * DEGREES_PER_STEP);	//	Track the new absolute y angle
+
+	return(0);
+
+}
+
+/*
+ *	Work through the pre-defined tour points to conduct a sweep of the scene in 3 60-degree steps by 12 15-degree steps
+ *	Uses a meander-scan rather than raster-scan technique to reduce motor loading
+ *	Covers a total scene angle of 180 x 180 (Presently, some points may be occluded by the gimbal housing)
+ */
+void CGimbal::MeanderScan(void)
+{
+	int i;
+
+		for (i=0; i<36; i++)
+		{
+			RotateAbs(ScanArray[i][X_AXIS], ScanArray[i][Y_AXIS] );
+		}
+}
+
+/*
+ *	Move to absolute x,y position in steps
+ */
+int	CGimbal::MoveAbs(float x, float y)
+{
+	m_xSteps = x - m_AbsoluteXPos;
+	m_ySteps = y - m_AbsoluteYPos;
+
+	MoveRel();
+
+	return(0);
+}
+
+/*
+ *	Move to absolute x,y angle in degrees
+ */
+int	CGimbal::RotateAbs(float xAngle, float yAngle)
+{
+	m_xSteps = (float) ((float) ((xAngle - m_AbsoluteXAngle) / (float) DEGREES_PER_STEP));
+	m_ySteps = (float) ((float) ((yAngle - m_AbsoluteYAngle) / (float) DEGREES_PER_STEP));
+
+	MoveRel();
+
+	return(0);
+}
+
+/*
+ *	Calculate how far away from the home position
+ *	Then move there.  Note speed is reduced during homing
+ */
+void	CGimbal::Home(void)
+{
+	int	oldSpeed = m_StepInterval;
+
+	m_StepInterval = HOME_STEP_INTERVAL;
+
+	m_xSteps = -m_AbsoluteXPos;
+	m_ySteps = -m_AbsoluteYPos;
+	MoveRel();
+
+	m_StepInterval = oldSpeed;
+
+}
+
+/*
+ * Rotate to waypoint n.  x,y angle parameters are stored in the member array
+ */
+int CGimbal::TourPoint(int n)
+{
+	RotateAbs(m_TourPoints[X_AXIS][n], m_TourPoints[Y_AXIS][n]);
+	return(0);
+}
+
+/*
+ * Define the current point as the home reference
+ */
+void	CGimbal::Zero(void)
+{
+	int	oldSpeed = m_StepInterval;
+	int timeout = 0;
+
+
+	//	Move at normal speed until 20 steps away from opto switch
+	m_xSteps = (-m_AbsoluteXPos) - 20;
+	m_ySteps = (-m_AbsoluteYPos) -20;
+	MoveRel();
+
+	// Now move at Homing (slow) speed until X opto reached
+	m_StepInterval = HOME_STEP_INTERVAL;
+
+	for (timeout = 0; timeout < 10; timeout++)
+	{
+
+	}
+	//	Move at normal speed until 20 steps away from opto switch
+	//m_xSteps = -(sgn(m_AbsoluteXPos) - 20);
+
+
+	m_StepInterval = oldSpeed;
+
+}
+
+