Template for LPC1768
Dependencies: Gimbal MLX90620 Socket lwip-eth lwip-sys lwip mbed-rtos mbed
Fork of EkkoEye by
Diff: CGimbal.cpp
- Revision:
- 54:aaf6b5ceedd8
- Parent:
- 53:72f350a6d09c
diff -r 72f350a6d09c -r aaf6b5ceedd8 CGimbal.cpp --- a/CGimbal.cpp Thu Apr 14 13:45:38 2016 +0100 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,210 +0,0 @@ -/* - * 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; - -} - -