Andy K
Provides an API software interface to TIMER2 to control upto four stepper motors.
Diff: src/SimpleSteppers.cpp
- Revision:
- 0:7393c52297ee
- Child:
- 4:7b7940df7865
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/SimpleSteppers.cpp Mon May 02 10:02:18 2011 +0000
@@ -0,0 +1,360 @@
+/*
+ Copyright (c) 2010 Andy Kirkham
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ THE SOFTWARE.
+*/
+
+#include <math.h>
+#include "SimpleSteppers.h"
+#include "IOmacros.h"
+
+#define _PMR *((uint32_t *)_pMR)
+
+namespace AjK {
+
+extern SimpleStepperController __simpleStepperController;
+
+void TIMER2_IRQHandler(void);
+
+SimpleStepper::SimpleStepper(PinName pulse, SimpleStepperOutput *direction)
+{
+ _simpleStepperController = &__simpleStepperController;
+ _pulse = pulse;
+ _direction = direction;
+ init();
+}
+
+SimpleStepper::SimpleStepper(SimpleStepperController *con, PinName pulse, SimpleStepperOutput *direction)
+{
+ _simpleStepperController = con;
+ _pulse = pulse;
+ _direction = direction;
+ init();
+}
+
+void
+SimpleStepper::init(void)
+{
+ _pulseCounter = 0;
+ _pulseWrapPos = 0;
+ _pulseWrapNeg = 0;
+
+ _maintainPositionData = true;
+
+ setPulseSense(1);
+ setDirectionSense(1);
+
+ switch (_pulse) {
+ case P0_6: // Mbed p8
+ _pulseMAT = MAT2_0;
+ _operShift = 4;
+ _pMR = 0x40090018; // T2MR0
+ _match_shift = 0;
+ _EMmask = 1;
+ _EMCshift = 4;
+ _simpleStepperController->_stepper[0] = this;
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_PINCON->PINSEL0 |= (3UL << 12);
+ break;
+ case P0_7: // Mbed p7
+ _pulseMAT = MAT2_1;
+ _operShift = 6;
+ _pMR = 0x4009001C; // T2MR1
+ _match_shift = 3;
+ _EMmask = 2;
+ _EMCshift = 6;
+ _simpleStepperController->_stepper[1] = this;
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_PINCON->PINSEL0 |= (3UL << 14);
+ break;
+ case P0_8: // Mbed p6
+ _pulseMAT = MAT2_2;
+ _operShift = 8;
+ _pMR = 0x40090020; // T2MR2
+ _match_shift = 6;
+ _EMmask = 4;
+ _EMCshift = 8;
+ _simpleStepperController->_stepper[2] = this;
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_PINCON->PINSEL0 |= (3UL << 16);
+ break;
+ case P0_9: // Mbed p5
+ _pulseMAT = MAT2_3;
+ _operShift = 10;
+ _pMR = 0x40090024; // T2MR3
+ _match_shift = 9;
+ _EMmask = 8;
+ _EMCshift = 10;
+ _simpleStepperController->_stepper[3] = this;
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_PINCON->PINSEL0 |= (3UL << 18);
+ break;
+ default:
+ while(1); /* ToDo, proper error reporting. */
+ }
+
+
+ // The default pulse length. 5 * (1/SIMPLESTEPPER_F) = 100us.
+ _pulseLen = 5;
+
+ // Initial state machine conditions.
+ _pulseState = PulseIdle;
+ _stepperState = Stopped;
+}
+
+void
+SimpleStepper::setInterval(int interval, uint32_t raw)
+{
+
+ if (interval == 0) { setSpeed((int)0); }
+ else {
+ int n = interval;
+ if (n < 0) n *= -1;
+ if (_direction) {
+ if (_directionSense) {
+ if (interval >= 0.0) _direction->write(1);
+ if (interval < 0.0) _direction->write(0);
+ }
+ else {
+ if (interval >= 0.0) _direction->write(0);
+ if (interval < 0.0) _direction->write(1);
+ }
+ }
+ _commandSpeed = n - _pulseLen;
+ _pulseState = PulseAssert;
+ _stepperState = ConstantSpeed;
+ if (raw) {
+ _PMR = raw + _commandSpeed;
+ }
+ else {
+ _PMR = LPC_TIM2->TC + _commandSpeed;
+ }
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_TIM2->MCR &= ~(7UL << _match_shift);
+ LPC_TIM2->MCR |= (1UL << _match_shift);
+ }
+}
+
+void
+SimpleStepper::setSpeed(double steps_per_second, uint32_t raw)
+{
+ int i = 0;
+
+ if (steps_per_second == 0.0) { setSpeed(i); }
+ else {
+ double fractpart, intpart;
+ fractpart = modf (steps_per_second , &intpart);
+ if (fractpart >= 0.50) intpart++;
+ double n = intpart;
+ if (n < 0.0) n *= -1.0;
+ if (_direction) {
+ if (_directionSense) {
+ if (steps_per_second >= 0.0) _direction->write(1);
+ if (steps_per_second < 0.0) _direction->write(0);
+ }
+ else {
+ if (steps_per_second >= 0.0) _direction->write(0);
+ if (steps_per_second < 0.0) _direction->write(1);
+ }
+ }
+ _commandSpeed = (int)(((double)SIMPLESTEPPER_F / n)) - _pulseLen;
+ _pulseState = PulseAssert;
+ _stepperState = ConstantSpeed;
+ if (raw) {
+ _PMR = raw + _commandSpeed;
+ }
+ else {
+ _PMR = LPC_TIM2->TC + _commandSpeed;
+ }
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_TIM2->MCR &= ~(7UL << _match_shift);
+ LPC_TIM2->MCR |= (1UL << _match_shift);
+ }
+}
+
+void
+SimpleStepper::setSpeed(int steps_per_second, uint32_t raw)
+{
+ if (steps_per_second == 0) {
+ LPC_TIM2->EMR |= (NothingOnMatch << _EMCshift);
+ LPC_TIM2->MCR &= ~(7UL << _match_shift);
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ _stepperState = Stopped;
+ _pulseState = PulseIdle;
+ _commandSpeed = 0;
+ }
+ else {
+ int n = steps_per_second;
+ if (n < 0) n *= -1;
+ if (_direction) {
+ if (_directionSense) {
+ if (steps_per_second >= 0) _direction->write(1);
+ if (steps_per_second < 0) _direction->write(0);
+ }
+ else {
+ if (steps_per_second >= 0) _direction->write(0);
+ if (steps_per_second < 0) _direction->write(1);
+ }
+ }
+ _commandSpeed = (SIMPLESTEPPER_F / n) - _pulseLen;
+ _pulseState = PulseAssert;
+ _stepperState = ConstantSpeed;
+ if (raw) {
+ _PMR = raw + _commandSpeed;
+ }
+ else {
+ _PMR = LPC_TIM2->TC + _commandSpeed;
+ }
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ LPC_TIM2->MCR &= ~(7UL << _match_shift);
+ LPC_TIM2->MCR |= (1UL << _match_shift);
+ }
+}
+
+void
+SimpleStepper::next_step(void)
+{
+ _PMR += _commandSpeed;
+
+ switch(_pulseSense) {
+ case 1:
+ LPC_TIM2->EMR &= ~(3UL << _EMCshift);
+ LPC_TIM2->EMR |= (SetOnMatch << _EMCshift);
+ break;
+ case 0:
+ LPC_TIM2->EMR &= ~(3UL << _EMCshift);
+ LPC_TIM2->EMR |= (ClrOnMatch << _EMCshift);
+ break;
+ }
+
+ // Next IRQ will automatically assert the pulse.
+ _pulseState = PulseAssert;
+}
+
+void
+SimpleStepper::isr(uint32_t ir)
+{
+ // Test to see if this interrupt is for us.
+ if (ir & _EMmask) {
+
+ if (_stepperState == Stopped) {
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ return;
+ }
+
+ switch(_pulseState) {
+ case PulseIdle:
+ if (_pulseSense) LPC_TIM2->EMR &= ~_EMmask; else LPC_TIM2->EMR |= _EMmask;
+ _currentMatch = _PMR; // Store the current match value for profile().
+ next_step();
+ break;
+
+ case PulseAssert: // Pulse has just been asserted on MAT2_x.
+ _PMR += _pulseLen; // We now program for the deassert IRQ to occur at the required time.
+ LPC_TIM2->EMR &= ~(ToggleOnMatch << _EMCshift); // At next IRQ we want to switch the pulse off...
+ LPC_TIM2->EMR |= (ToggleOnMatch << _EMCshift); // ... we do this by toggling it.
+ _pulseState = PulseDeassert; // Set state for next interrupt.
+
+ if (_maintainPositionData) {
+ if (_directionSense) {
+ if (_commandSpeed > 0) {
+ _pulseCounter++;
+ if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
+ _pulseCounter = _pulseWrapNeg + 1;
+ }
+ }
+ if (_commandSpeed < 0) {
+ _pulseCounter--;
+ if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
+ _pulseCounter = _pulseWrapPos - 1;
+ }
+ }
+ }
+ else {
+ if (_commandSpeed > 0) {
+ _pulseCounter--;
+ if (_pulseWrapNeg && _pulseCounter <= _pulseWrapNeg) {
+ _pulseCounter = _pulseWrapPos - 1;
+ }
+ }
+ if (_commandSpeed < 0) {
+ _pulseCounter++;
+ if (_pulseWrapPos && _pulseCounter >= _pulseWrapPos) {
+ _pulseCounter = _pulseWrapNeg + 1;
+ }
+ }
+ }
+ }
+ break;
+
+ case PulseDeassert: // Pulse has just been deasserted on MAT2_x.
+ LPC_TIM2->EMR &= ~(NothingOnMatch << _EMCshift); // No further action...
+ LPC_TIM2->EMR |= (NothingOnMatch << _EMCshift); // ... just in case.
+ _pulseState = PulseIdle; // Set state for next interrupt (profile() may change this).
+ _currentMatch = _PMR; // Store the current match value for profile().
+ next_step();
+ break;
+ }
+ }
+}
+
+// **************************************************
+// * Controller class SimpleStepperController code. *
+// **************************************************
+
+SimpleStepperController::SimpleStepperController()
+{
+ for (int i = 0; i < 4; i++) _stepper[i] = 0;
+
+ uint32_t pr = (uint32_t)(SystemCoreClock / 8 / SIMPLESTEPPER_F);
+
+ LPC_SC->PCONP |= (1UL << 22); // TIM2 On
+ LPC_SC->PCLKSEL1 |= (3UL << 12); // CCLK/8 = 12MHz
+ LPC_TIM2->PR = pr - 1; // TC clocks at SIMPLESTEPPER_F hz.
+ LPC_TIM2->MR0 = 0;
+ LPC_TIM2->MR1 = 0;
+ LPC_TIM2->MR2 = 0;
+ LPC_TIM2->MR3 = 0;
+ LPC_TIM2->MCR = 0;
+
+ // Enable interrupts
+ NVIC_EnableIRQ(TIMER2_IRQn);
+
+ // Start timer operations.
+ LPC_TIM2->TCR = 2; // reset
+ LPC_TIM2->TCR = 1; // enable
+}
+
+SimpleStepperController::~SimpleStepperController()
+{
+ NVIC_DisableIRQ(TIMER2_IRQn);
+ LPC_SC->PCONP &= ~(1UL << 22); // TIM2 Off.
+}
+
+void
+SimpleStepperController::isr(void)
+{
+ uint32_t ir = LPC_TIM2->IR & 0xF;
+ if (_stepper[0] && ir & 1) _stepper[0]->isr(ir);
+ if (_stepper[1] && ir & 2) _stepper[1]->isr(ir);
+ if (_stepper[2] && ir & 4) _stepper[2]->isr(ir);
+ if (_stepper[3] && ir & 8) _stepper[3]->isr(ir);
+}
+
+}; // namespace AjK ends.