Microduino

Dependencies:   mbed

Fork of BalanceCar by Li Weiyi

Microduino_Stepper.cpp

Committer:
lixianyu
Date:
2016-06-04
Revision:
0:a4d8f5b3c546
Child:
2:99785a1007a4

File content as of revision 0:a4d8f5b3c546:

// Microduino_Stepper.cpp
//
// Copyright (C) 2009-2013 Shenyang
// $Id: Microduino_Stepper.cpp,v 1.00 2016/04/07 $

#include "Microduino_Stepper.h"
extern Timer g_Timer;
DigitalOut gSteperE(PIN_EN);
Ticker g_Ticker;
static Stepper_t steppers[MAX_STEPPERS];

uint8_t StepperCount = 0;

// Some debugging assistance

void stepperAllEnable()
{
    //digitalWrite(PIN_EN, LOW);
    gSteperE = 0;
}

void stepperAllDisable()
{
    //digitalWrite(PIN_EN, HIGH);
    gSteperE = 1;
}

static inline void handle_interrupts()
{
    for(uint8_t channel=0; channel < MAX_STEPPERS; channel++) {
        if(steppers[channel].isActive)
            steppers[channel].stepper->computeStep();
    }
}

#if 0
static void initISR()
{
#if defined(_useTimer1)
    cli();
    TCCR1A = 0;
    TCCR1B = _BV(WGM12)|_BV(CS11);
    OCR1A = TIMER_COMP;
    TCNT1 = 0;
    TIMSK1 = _BV(OCIE1A);
    sei();
#endif
    pinMode(PIN_EN, OUTPUT);
    stepperAllEnable();
}

#if defined(_useTimer1)
ISR(TIMER1_COMPA_vect)
{
    handle_interrupts();
}
#endif
#endif

static void timerHandle()
{
    for (uint8_t channel=0; channel < MAX_STEPPERS; channel++) {
        if(steppers[channel].isActive)
            steppers[channel].stepper->computeStep();
    }
}

static bool isTimerActive()
{
    for(uint8_t channel=0; channel <MAX_STEPPERS ; channel++) {
        if(steppers[channel].isActive == true)
            return  true;
    }
    return false;
}
/****************** end of static functions ******************************/

Stepper::Stepper(uint8_t _dirPin, uint8_t _stepPin)
{
    if(StepperCount < MAX_STEPPERS) {
        this->stepperIndex = StepperCount++;
    } else {
        this->stepperIndex = INVALID_STEPPER;
    }
    dirPin = _dirPin;
    stepPin = _stepPin;
    speed = 0;
    period = 0;
    counter = 0;
    steppers[this->stepperIndex].isActive = false;
}

uint8_t Stepper::begin()
{
    if (this->stepperIndex < MAX_STEPPERS) {
#if 0
        pinMode(dirPin, OUTPUT);
        pinMode(stepPin, OUTPUT);
#else
        gpio_init_out(&dirOUT, (PinName)dirPin);
        gpio_init_out(&stepOUT, (PinName)stepPin);
#endif
        setMaxAccel(DEFAULT_ACCEL);
        if (isTimerActive() == false) {
            //initISR();
            //pinMode(PIN_EN, OUTPUT);
			g_Ticker.attach_us(&timerHandle, 25);
            stepperAllEnable();
        }
        steppers[this->stepperIndex].isActive = true;
        steppers[this->stepperIndex].stepper = this;
    }
    return this->stepperIndex;
}
#define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
void Stepper::setSpeed(int16_t _speed)
{
    speed += constrain((_speed-speed), -(int16_t)maxAccel, (int16_t)maxAccel);
    if (speed == 0) {
        period = 0;
    } else {
        period = MAX_SPEED_S / abs(speed);
    }
    //(speed>0) ? PIN_CLR(dirPin) : PIN_SET(dirPin);
    if (speed > 0) {
        gpio_write(&dirOUT, 0);
    } else {
        gpio_write(&dirOUT, 1);
    }
}

void Stepper::setMaxAccel(uint16_t _accel)
{
    maxAccel = _accel;
}

int16_t Stepper::getSpeed()
{
    return  speed;
}

uint16_t Stepper::getMaxAccel()
{
    return  maxAccel;
}

void Stepper::computeStep()
{
    counter++;
    if(counter > period) {
        counter = 0;
        if(period > 0) {
            //PIN_SET(stepPin);
            gpio_write(&stepOUT, 1);
            //delayMicroseconds(1);
            wait_us(1);
            //PIN_CLR(stepPin);
            gpio_write(&stepOUT, 0);
        }
    }
}