Mike Gardner / Mbed 2 deprecated LPC1768Template

Template for LPC1768

Dependencies:   Gimbal MLX90620 Socket lwip-eth lwip-sys lwip mbed-rtos mbed

Fork of EkkoEye by EkkoSense

CGimbal.cpp@53:72f350a6d09c, 2016-04-14 (annotated)

Committer:
Mike
Date:
Thu Apr 14 13:45:38 2016 +0100
Revision:
53:72f350a6d09c
DCW

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mike 53:72f350a6d09c 1 /*
Mike 53:72f350a6d09c 2 * CGimbal.cpp
Mike 53:72f350a6d09c 3 *
Mike 53:72f350a6d09c 4 * Created on: 15 Feb 2016
Mike 53:72f350a6d09c 5 * Author: mike
Mike 53:72f350a6d09c 6 */
Mike 53:72f350a6d09c 7 #include "base.h"
Mike 53:72f350a6d09c 8 #include <CGimbal.h>
Mike 53:72f350a6d09c 9 #include <math.h>
Mike 53:72f350a6d09c 10 int debug = 0;
Mike 53:72f350a6d09c 11
Mike 53:72f350a6d09c 12 extern Serial pc;
Mike 53:72f350a6d09c 13
Mike 53:72f350a6d09c 14 /*
Mike 53:72f350a6d09c 15 * 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
Mike 53:72f350a6d09c 16 * Then moves at 60 degree increments to the right-most position
Mike 53:72f350a6d09c 17 * Then moves down at 15 degree increments
Mike 53:72f350a6d09c 18 * Uses meander-scan technique rather than raster-scan to reduce motor loading and risk of skipping steps
Mike 53:72f350a6d09c 19 */
Mike 53:72f350a6d09c 20 static const float ScanArray[36][2] =
Mike 53:72f350a6d09c 21 {
Mike 53:72f350a6d09c 22 {30, 7.5}, {90, 7.5}, {150, 7.5}, {150, 22.5}, {90, 22.5}, {30, 22.5},
Mike 53:72f350a6d09c 23 {30, 37.5}, {90, 37.5}, {150, 37.5}, {150, 52.5}, {90, 52.5}, {30, 52.5},
Mike 53:72f350a6d09c 24 {30, 67.5}, {90, 67.5}, {150, 67.5}, {150, 82.5}, {90, 82.5}, {30, 82.5},
Mike 53:72f350a6d09c 25 {30, 97.5}, {90, 97.5}, {150, 97.5}, {150, 112.5}, {90, 112.5}, {30, 112.5},
Mike 53:72f350a6d09c 26 {30, 127.5}, {90, 127.5}, {150, 127.5}, {150, 142.5}, {90, 142.5}, {30, 142.5},
Mike 53:72f350a6d09c 27 {30, 157.5}, {90, 157.5}, {150, 157.5}, {150, 172.5}, {90, 172.5}, {30, 172.5}
Mike 53:72f350a6d09c 28 };
Mike 53:72f350a6d09c 29
Mike 53:72f350a6d09c 30
Mike 53:72f350a6d09c 31
Mike 53:72f350a6d09c 32 CGimbal::CGimbal(DigitalOut * pXStepPin, DigitalOut * pXDirPin, DigitalOut * pYStepPin, DigitalOut * pYDirPin)
Mike 53:72f350a6d09c 33 {
Mike 53:72f350a6d09c 34 // Zero the tracking parameters
Mike 53:72f350a6d09c 35
Mike 53:72f350a6d09c 36 m_StepInterval = DFT_STEP_INTERVAL; // Determines the default motor speed (10us Ticks between each step pulse)
Mike 53:72f350a6d09c 37 m_AbsoluteXPos = 0; // Always starts at 0 point
Mike 53:72f350a6d09c 38 m_AbsoluteXAngle = 0.0; // Ditto
Mike 53:72f350a6d09c 39 m_AbsoluteYPos = 0;
Mike 53:72f350a6d09c 40 m_AbsoluteYAngle = 0.0;
Mike 53:72f350a6d09c 41 m_xDir = FORWARD; // Always initialised in forward direction
Mike 53:72f350a6d09c 42 m_yDir = FORWARD; // Ditto
Mike 53:72f350a6d09c 43
Mike 53:72f350a6d09c 44 m_xSteps=0; // Nothing to do immediately after being constructed
Mike 53:72f350a6d09c 45 m_ySteps=0;
Mike 53:72f350a6d09c 46
Mike 53:72f350a6d09c 47 m_pXStepPin = pXStepPin; // Initialise Pointers to each pin
Mike 53:72f350a6d09c 48 m_pXDirPin = pXDirPin;
Mike 53:72f350a6d09c 49 m_pYStepPin = pYStepPin;
Mike 53:72f350a6d09c 50 m_pYDirPin = pYDirPin;
Mike 53:72f350a6d09c 51
Mike 53:72f350a6d09c 52 m_pXStepPin->write(0); // Set all motor driver signals to a known state (LOW)
Mike 53:72f350a6d09c 53 m_pXDirPin->write(0);
Mike 53:72f350a6d09c 54 m_pYStepPin->write(0);
Mike 53:72f350a6d09c 55 m_pYDirPin->write(0);
Mike 53:72f350a6d09c 56
Mike 53:72f350a6d09c 57 m_NumWaypoints = 2; // Start with 2 waypoints
Mike 53:72f350a6d09c 58
Mike 53:72f350a6d09c 59 m_TickCount=0; // Start at 0
Mike 53:72f350a6d09c 60 }
Mike 53:72f350a6d09c 61
Mike 53:72f350a6d09c 62 // Destructor
Mike 53:72f350a6d09c 63 CGimbal::~CGimbal()
Mike 53:72f350a6d09c 64 {
Mike 53:72f350a6d09c 65
Mike 53:72f350a6d09c 66 }
Mike 53:72f350a6d09c 67
Mike 53:72f350a6d09c 68 /*
Mike 53:72f350a6d09c 69 * Perform a vectored move, relative to the current x,y position and update internal tracking parameters
Mike 53:72f350a6d09c 70 */
Mike 53:72f350a6d09c 71 int CGimbal::MoveRel()
Mike 53:72f350a6d09c 72 {
Mike 53:72f350a6d09c 73
Mike 53:72f350a6d09c 74 if (m_xSteps > 0) // Determine which direction the motors needs to move
Mike 53:72f350a6d09c 75 m_xDir = FORWARD;
Mike 53:72f350a6d09c 76 else
Mike 53:72f350a6d09c 77 m_xDir = REVERSE;
Mike 53:72f350a6d09c 78 if (m_ySteps > 0)
Mike 53:72f350a6d09c 79 m_yDir = 1;
Mike 53:72f350a6d09c 80 else
Mike 53:72f350a6d09c 81 m_yDir = 0;
Mike 53:72f350a6d09c 82 m_pXDirPin->write(m_xDir);
Mike 53:72f350a6d09c 83 m_pYDirPin->write(m_yDir);
Mike 53:72f350a6d09c 84
Mike 53:72f350a6d09c 85 if (m_xSteps > 0) // Absolute number of pulses is always positive regardless of directiong
Mike 53:72f350a6d09c 86 m_xPulses = m_xSteps;
Mike 53:72f350a6d09c 87 else
Mike 53:72f350a6d09c 88 if ((m_xSteps < 0))
Mike 53:72f350a6d09c 89 m_xPulses = -m_xSteps;
Mike 53:72f350a6d09c 90 else
Mike 53:72f350a6d09c 91 m_xPulses = 0; // If no steps, do nothing
Mike 53:72f350a6d09c 92
Mike 53:72f350a6d09c 93 if (m_ySteps > 0) // Absolute number of pulses is always positive regardless of directiong
Mike 53:72f350a6d09c 94 m_yPulses = m_ySteps;
Mike 53:72f350a6d09c 95 else
Mike 53:72f350a6d09c 96 if ((m_ySteps < 0))
Mike 53:72f350a6d09c 97 m_yPulses = -m_ySteps;
Mike 53:72f350a6d09c 98 else
Mike 53:72f350a6d09c 99 m_yPulses = 0; // If no steps, do nothing
Mike 53:72f350a6d09c 100
Mike 53:72f350a6d09c 101 // Wait for the Ticker to complete this move
Mike 53:72f350a6d09c 102 while (m_xPulses > 0 || m_yPulses > 0);
Mike 53:72f350a6d09c 103
Mike 53:72f350a6d09c 104 m_AbsoluteXPos += m_xSteps; // Track the new absolute x position
Mike 53:72f350a6d09c 105 m_AbsoluteYPos += m_ySteps; // Track the new absolute y position
Mike 53:72f350a6d09c 106 m_AbsoluteXAngle = (float) ( (float) m_AbsoluteXPos * DEGREES_PER_STEP); // Track the new absolute x angle
Mike 53:72f350a6d09c 107 m_AbsoluteYAngle = (float) ( (float) m_AbsoluteYPos * DEGREES_PER_STEP); // Track the new absolute y angle
Mike 53:72f350a6d09c 108
Mike 53:72f350a6d09c 109 return(0);
Mike 53:72f350a6d09c 110
Mike 53:72f350a6d09c 111 }
Mike 53:72f350a6d09c 112
Mike 53:72f350a6d09c 113 /*
Mike 53:72f350a6d09c 114 * Work through the pre-defined tour points to conduct a sweep of the scene in 3 60-degree steps by 12 15-degree steps
Mike 53:72f350a6d09c 115 * Uses a meander-scan rather than raster-scan technique to reduce motor loading
Mike 53:72f350a6d09c 116 * Covers a total scene angle of 180 x 180 (Presently, some points may be occluded by the gimbal housing)
Mike 53:72f350a6d09c 117 */
Mike 53:72f350a6d09c 118 void CGimbal::MeanderScan(void)
Mike 53:72f350a6d09c 119 {
Mike 53:72f350a6d09c 120 int i;
Mike 53:72f350a6d09c 121
Mike 53:72f350a6d09c 122 for (i=0; i<36; i++)
Mike 53:72f350a6d09c 123 {
Mike 53:72f350a6d09c 124 RotateAbs(ScanArray[i][X_AXIS], ScanArray[i][Y_AXIS] );
Mike 53:72f350a6d09c 125 }
Mike 53:72f350a6d09c 126 }
Mike 53:72f350a6d09c 127
Mike 53:72f350a6d09c 128 /*
Mike 53:72f350a6d09c 129 * Move to absolute x,y position in steps
Mike 53:72f350a6d09c 130 */
Mike 53:72f350a6d09c 131 int CGimbal::MoveAbs(float x, float y)
Mike 53:72f350a6d09c 132 {
Mike 53:72f350a6d09c 133 m_xSteps = x - m_AbsoluteXPos;
Mike 53:72f350a6d09c 134 m_ySteps = y - m_AbsoluteYPos;
Mike 53:72f350a6d09c 135
Mike 53:72f350a6d09c 136 MoveRel();
Mike 53:72f350a6d09c 137
Mike 53:72f350a6d09c 138 return(0);
Mike 53:72f350a6d09c 139 }
Mike 53:72f350a6d09c 140
Mike 53:72f350a6d09c 141 /*
Mike 53:72f350a6d09c 142 * Move to absolute x,y angle in degrees
Mike 53:72f350a6d09c 143 */
Mike 53:72f350a6d09c 144 int CGimbal::RotateAbs(float xAngle, float yAngle)
Mike 53:72f350a6d09c 145 {
Mike 53:72f350a6d09c 146 m_xSteps = (float) ((float) ((xAngle - m_AbsoluteXAngle) / (float) DEGREES_PER_STEP));
Mike 53:72f350a6d09c 147 m_ySteps = (float) ((float) ((yAngle - m_AbsoluteYAngle) / (float) DEGREES_PER_STEP));
Mike 53:72f350a6d09c 148
Mike 53:72f350a6d09c 149 MoveRel();
Mike 53:72f350a6d09c 150
Mike 53:72f350a6d09c 151 return(0);
Mike 53:72f350a6d09c 152 }
Mike 53:72f350a6d09c 153
Mike 53:72f350a6d09c 154 /*
Mike 53:72f350a6d09c 155 * Calculate how far away from the home position
Mike 53:72f350a6d09c 156 * Then move there. Note speed is reduced during homing
Mike 53:72f350a6d09c 157 */
Mike 53:72f350a6d09c 158 void CGimbal::Home(void)
Mike 53:72f350a6d09c 159 {
Mike 53:72f350a6d09c 160 int oldSpeed = m_StepInterval;
Mike 53:72f350a6d09c 161
Mike 53:72f350a6d09c 162 m_StepInterval = HOME_STEP_INTERVAL;
Mike 53:72f350a6d09c 163
Mike 53:72f350a6d09c 164 m_xSteps = -m_AbsoluteXPos;
Mike 53:72f350a6d09c 165 m_ySteps = -m_AbsoluteYPos;
Mike 53:72f350a6d09c 166 MoveRel();
Mike 53:72f350a6d09c 167
Mike 53:72f350a6d09c 168 m_StepInterval = oldSpeed;
Mike 53:72f350a6d09c 169
Mike 53:72f350a6d09c 170 }
Mike 53:72f350a6d09c 171
Mike 53:72f350a6d09c 172 /*
Mike 53:72f350a6d09c 173 * Rotate to waypoint n. x,y angle parameters are stored in the member array
Mike 53:72f350a6d09c 174 */
Mike 53:72f350a6d09c 175 int CGimbal::TourPoint(int n)
Mike 53:72f350a6d09c 176 {
Mike 53:72f350a6d09c 177 RotateAbs(m_TourPoints[X_AXIS][n], m_TourPoints[Y_AXIS][n]);
Mike 53:72f350a6d09c 178 return(0);
Mike 53:72f350a6d09c 179 }
Mike 53:72f350a6d09c 180
Mike 53:72f350a6d09c 181 /*
Mike 53:72f350a6d09c 182 * Define the current point as the home reference
Mike 53:72f350a6d09c 183 */
Mike 53:72f350a6d09c 184 void CGimbal::Zero(void)
Mike 53:72f350a6d09c 185 {
Mike 53:72f350a6d09c 186 int oldSpeed = m_StepInterval;
Mike 53:72f350a6d09c 187 int timeout = 0;
Mike 53:72f350a6d09c 188
Mike 53:72f350a6d09c 189
Mike 53:72f350a6d09c 190 // Move at normal speed until 20 steps away from opto switch
Mike 53:72f350a6d09c 191 m_xSteps = (-m_AbsoluteXPos) - 20;
Mike 53:72f350a6d09c 192 m_ySteps = (-m_AbsoluteYPos) -20;
Mike 53:72f350a6d09c 193 MoveRel();
Mike 53:72f350a6d09c 194
Mike 53:72f350a6d09c 195 // Now move at Homing (slow) speed until X opto reached
Mike 53:72f350a6d09c 196 m_StepInterval = HOME_STEP_INTERVAL;
Mike 53:72f350a6d09c 197
Mike 53:72f350a6d09c 198 for (timeout = 0; timeout < 10; timeout++)
Mike 53:72f350a6d09c 199 {
Mike 53:72f350a6d09c 200
Mike 53:72f350a6d09c 201 }
Mike 53:72f350a6d09c 202 // Move at normal speed until 20 steps away from opto switch
Mike 53:72f350a6d09c 203 //m_xSteps = -(sgn(m_AbsoluteXPos) - 20);
Mike 53:72f350a6d09c 204
Mike 53:72f350a6d09c 205
Mike 53:72f350a6d09c 206 m_StepInterval = oldSpeed;
Mike 53:72f350a6d09c 207
Mike 53:72f350a6d09c 208 }
Mike 53:72f350a6d09c 209
Mike 53:72f350a6d09c 210