File content as of revision 53:72f350a6d09c:

/*
 * 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;

}