File content as of revision 0:9156e6b6bf46:

/*
############################################
##           motorControl v0.1 Library    ##
##          created by Peter Ilsoee       ##
############################################
        ---- piniels@gmail.com -----
This library was made for 4-Phase Stepper Motors
I don't take any resposability for the damage caused to your equipment.

*/

#include "motorControl.h"

#include "mbed.h"
#include "globalt.h"


SPI spi(PTE1, PTE3, PTE2); // mosi, miso, sclk
DigitalOut cs(PTE4);
//struct for holding run register information

//GPIO
DigitalOut valve(PTD0);
DigitalIn centerInput(PTD4);

//Ticker for running at a specified speed
Ticker runTickMotorControl;





/**
 * ! @brief Construct including the pins for "manual(not SPI cmd)" control of A4980
 * @param step Step logic input. Motor advances on rising edge. Filtered input with hysteresis.
 * @param dir Direction logic input. Direction changes on the next STEP rising edge. When high, the Phase Angle Number is increased
 *            on the rising edge of STEP. Has no effect when using the serial
 *            interface. Filtered input with hysteresis.
 * @param ms1 Microstep resolution select input.
 * @param ms0 Microstep resolution select input.
 * @param enable Controls activity of bridge outputs. When held low, deactivates the outputs, that is, turns off all output bridge FETs.
 *               Internal logic continues to follow input commands.
 * @param reset Resets faults when pulsed low. Forces low-power shutdown(sleep) when held low for more than the Reset Shutdown
 *              Width, tRSD . Can be pulled to VBB with 30 kΩ resistor.
 * @param diag  Diagnostic output. Function selected via the serial interface,
 *              setting Configuration Register 1. Default is Fault output.
 */
motorControl::motorControl(int numberOfSteps, PinName step, PinName dir, PinName ms1, PinName ms0, PinName enable, PinName reset, PinName diag): _Number_OF_STEPS(numberOfSteps), _STEP(step), _DIR(dir), _MS1(ms1), _MS0(ms0), _ENABLE(enable), _RESET(reset), _DIAG(diag){

    this->_RESET = 0;
    this->_STEP = 0; // duty cycle on 50 %
    this->_DIR = 0;
    this->_MS1 = 0;
    this->_MS0 = 0;
    this->_ENABLE = 0;
    //Setting default values to run regi
    this->_REGI_RUN.B.sc = 0x1; //1 steps positive value
    this->_REGI_RUN.B.dcy = 0x1; // bit7-6 Decay mode selection 00=Slow, *01=Mixed-PFD fixed, 10=Mixed-PFD auto, 11=Fast
    this->_REGI_RUN.B.brk = 0x0; // bit8 Brake enable *0=Normal operation 1=Brake active
    this->_REGI_RUN.B.slew = 0x1; // bit9 Slew rate control 0=Disable *1=Enable
    this->_REGI_RUN.B.hlr = 0x0; // bit10. Selects slow decay and brake recirculation path *0=High side, 1=Low side
    this->_REGI_RUN.B.ol = 0x1; // bit12-11. Open load current threshold as a percentage of maximum current defined by ISMAX and MXI[1..0] 00=20%, *01=30%, 10=40%, 11=50%
    this->_REGI_RUN.B.en = 1; // bit13. Phase current enable OR with ENABLE pin *0=Output bridges disabled if ENABLE pin = 0, 1=Output bridges enabled
    this->_REGI_RUN.B.addr = 0x2; // bit15-14 Address run = 0b10
    //Setting default value to control regi. 0
    this->_REGI_0.B.pwm = 0x0;
    this->_REGI_0.B.tofFrq = 0x6;
    this->_REGI_0.B.tbk = 0x1;
    this->_REGI_0.B.pfd = 0x4;
    this->_REGI_0.B.mx = 0x03; // bit10-9. Max phase current as a percentage of I(SMAX) 00=25% 01=50% 10=75% *11=100%
    this->_REGI_0.B.ms = 0x0;
    this->_REGI_0.B.syr = 0x1;
    this->_REGI_0.B.addr = 0x0;
    //Setting default value to control regi. 1
    this->_REGI_1.B.diag = 0x0;
    this->_REGI_1.B.cd = 0x8;
    this->_REGI_1.B.notInUse = 0x0;
    this->_REGI_1.B.tsc = 0x2;
    this->_REGI_1.B.osc = 0x0;
    this->_REGI_1.B.addr = 0x1;
    wait(0.1);
    this->_RESET = 1;
    this->_Step_Counter = 0;
    
    

}


void motorControl::initSpi() { // rotate the motor 1 step anticlockwise
cs = 1;
spi.format(8,3);
spi.frequency(1000000);

}

/**
 * ! @brief set the step counter
 * @param value an int counting the steps
 */
void motorControl::setStepCount(int value)
{
    this->_Step_Counter = value;
    
}

/**
 * ! @brief get the step counter
 * @return an int counting the steps
 */
int motorControl::getStepCount()
{
    return this->_Step_Counter;
    
}

/**
 * ! @brief set configuration and control register 0
 * @param t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
 */
int motorControl::setConfigRegi0(t_config_control_regi_0 value)
{
    this->_REGI_0 = value;
    cs = 0;
    int response = (spi.write(value.I >> 8)) << 8;
    response |= spi.write(value.I & 0xff);
    cs = 1;
    return response;
}

/**
 * ! @brief get configuration and control register 0
 * @return t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
 */
t_config_control_regi_0 motorControl::getConfigRegi0()
{
    return this->_REGI_0;
}

/**
 * ! @brief open or close the GPIO controlling the valve
 * @parm openTrue Boolean true for open
 */
void motorControl::openCloseValve(bool openTrue)
{
    if(openTrue)
    {
        valve = 0;
    }
    else
    {
        valve = 1;
    }
}

/**
 * ! @brief set configuration and control register 1
 * @param t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
 */
int motorControl::setConfigRegi1(t_config_control_regi_1 value)
{
    this->_REGI_1 = value;
    cs = 0;
    int response = (spi.write(value.I >> 8)) << 8;
    response |= spi.write(value.I & 0xff);
    cs = 1;
    return response;
}

/**
 * ! @brief set micro step bits
 * @param int from 0 to 3
 */
void motorControl::setMicroSteps(int value)
{
    printf("Value for microsteps: %i",value);
    printf("\r\n");
    this->_MS1 = (value & 0x2) >> 1;
    this->_MS0 = value & 0x1;
    
}

/**
 * ! @brief get micro step bits
 * @param int from 0 to 3
 */
int motorControl::getMicroSteps()
{
    return this->_MS1 << 1 | this->_MS0;
    
}


/**
 * ! @brief get configuration and control register 1
 * @return t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
 */
t_config_control_regi_1 motorControl::getConfigRegi1()
{
    return this->_REGI_1;
}

/**
 * ! @brief set configuration and run register
 * @param t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
 */
int motorControl::setRunRegi(t_run_regi value)
{
    this->_REGI_RUN = value;
    cs = 0;
    int response = (spi.write(value.I >> 8)) << 8;
    response |= spi.write(value.I & 0xff);
    cs = 1;
    return response;
}

/**
 * ! @brief get run control register
 * @return t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
 */
t_run_regi motorControl::getRunRegi()
{
    return this->_REGI_RUN;
}

/**
 * ! @brief Run at a specified speed, none blocking
 * @param startStop If true start moter if false stop motor
 * @param whatSpeed speed in RPM
 * @param direction if 1 then postive phase, if 0 negative phase
 */
// 
void motorControl::runStepperAtSpeed(bool startStop, int whatSpeed, int direction)
{
    this->_DIR = direction;
    this->step_delay = calculateStepDelay(whatSpeed);
    
    if(this->step_delay > MAX_STEP_DELAY)
    {
        this->step_delay = MAX_STEP_DELAY;
    }
    else if(this->step_delay < MIN_STEP_DELAY)
    {
        this->step_delay = MIN_STEP_DELAY;
    }
    
    
    
    if(startStop)
    {
        this->_ENABLE = 1;
        _STEP = 0.5; //duty cycle 50%
        _STEP.period_us((int) this->step_delay);
        
    }
    else
    {
        _STEP = 0; //Duty-cycle 0
        _ENABLE = 0;
        _STEP = 0;
        
    }
}

void motorControl::doRamp(int numberOfRampsteps,float endDelay)
{
    //Test do ramping
    
    int delay_us_start = 10000;
    int diff_delay_us = delay_us_start - endDelay;
    int ramping_step_us = diff_delay_us/numberOfRampsteps;
    int delay_ramp = 0;
     
    
    for(int i = 0;i < numberOfRampsteps; i++)
    {
           delay_ramp = delay_us_start - (i*ramping_step_us);
           printf("Ramping Delay is in  us: %i", delay_ramp);
           printf("\r\n");
           _STEP.period_us(delay_ramp);
           wait_us(delay_ramp);
    }
}

float motorControl::calculateStepDelay(int whatSpeed)
{
   if(whatSpeed != 0)
    {
        this->step_delay = 60L * 1000000L / ((float)this->_Number_OF_STEPS * (float)whatSpeed);
        int microStep = getMicroSteps();
        if(microStep > 0)
        {
           if(microStep == 1)
           {
            this->step_delay = this->step_delay/2;  
            }
            else if(microStep == 2)
            {
                this->step_delay = this->step_delay/4;  
            }
            else
            {
                this->step_delay = this->step_delay/16;  
            }
           
        }
        
    }
    else
    {
        this->step_delay = 60L * 1000L / (float) this->_Number_OF_STEPS / (float) 1;
    }
    
    return this->step_delay;
}

// tick event handler for running the motor at a specified speed
void motorControl::runMotor(void)
{
    setRunRegi(this->_REGI_RUN); //1 steps positive value
    if(this->_Bool_Direction)
    {
        this->_Step_Counter++;
    }
    else
    {
        this->_Step_Counter--;
    }
    
    
    
}

 void motorControl::goToZeroPosition(int whatSpeed)
 {
    float step_delay = (60 / (float)this->_Number_OF_STEPS / (float)whatSpeed);
    bool runMotorBool = true;
    bool runMotorBack = false;
    openCloseValve(true);
    
    while(runMotorBool)//this->_Step_Counter != 0 || centerInput )
    {
        this->_REGI_RUN.B.en = 1;
        if(whatSpeed > 0)
        {
            this->_Bool_Direction = true;
            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
            runMotor();
            wait(step_delay);
            if(centerInput)
            {
                runMotorBool = false;
                this->_Step_Counter = 0;
                
            }
        }
        else
        {
            this->_Bool_Direction = false;
            this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
            runMotor();
            wait(-1*step_delay);
            if(centerInput)
            {
                runMotorBack = true;
            }
            else if(runMotorBack && !centerInput)
            {
                runMotorBool = false;
                this->_Step_Counter = 0;
            }
            
        }
       
        
    } 
    this->_REGI_RUN.B.en = 0;    

}

// Run at a specified speed, no blocking
void motorControl::runStepperAtSpeedWithSPI(bool startStop, int whatSpeed, int direction)
{
    
    
    if(whatSpeed != 0)
    {
        this->step_delay = (60L * 1000000L / this->_Number_OF_STEPS / whatSpeed);
        
    }
    else
    {
        this->step_delay = 60L * 1000000L / this->_Number_OF_STEPS / 1;
        
    }
    //pc.printf("Step dealy: %d",this->step_delay);
    //pc.printf("\r\n");
    if(direction)
    {
        this->_Bool_Direction = true;
        if(whatSpeed > 50)//HACK: for testing micro step
        {
            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
        }
        else
        {
            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
            //this->_REGI_RUN.B.sc = 0x4; // 1/8 steps positive value
            //this->step_delay = this->step_delay/4;
            printf("Step dealy micro step: %d",this->step_delay);
            printf("\r\n");
        }
        
    }
    else
    {
        this->_Bool_Direction = false;;
        
        if(whatSpeed > 50) //HACK: for testing micro step
        {
            this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
        }
        else
        {
            this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
            //this->_REGI_RUN.B.sc = 0x3C; // 1/16 steps negative value
            //this->step_delay = this->step_delay/4;
            printf("Step dealy micro step: %d",this->step_delay);
            printf("\r\n");
        }
        //
        
    }

    if(startStop)
    {
        this->_REGI_RUN.B.en = 1;
        runTickMotorControl.attach_us(this, &motorControl::runMotor, (this->step_delay)); // the address of the function to be attached
    }
    else
    {
        this->_REGI_RUN.B.en = 0;
        setRunRegi(this->_REGI_RUN);
        runTickMotorControl.detach();
        
    }
}



void motorControl::step(int num_steps, int delay, bool direction) {// steper function: number of steps, direction (0- right, 1- left), 

    this->_REGI_RUN.B.en = 1;

    
    
    if(direction)
    {
        if(getMicroSteps() == 0)
        {
            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
        }
        else if(getMicroSteps() == 1)
        {
            this->_REGI_RUN.B.sc = 0x8; // 1 steps positive value
        }
        else if(getMicroSteps() == 2)
        {
            this->_REGI_RUN.B.sc = 0x4; // 1 steps positive value
        }
        else if(getMicroSteps() == 3)
        {
            this->_REGI_RUN.B.sc = 0x2; // 1 steps positive value
        }
        
        
    }
    else
    {
        if(getMicroSteps() == 0)
        {
            this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
        }
        else if(getMicroSteps() == 1)
        {
            this->_REGI_RUN.B.sc = 0x38; // 1 steps positive value
        }
        else if(getMicroSteps() == 2)
        {
            this->_REGI_RUN.B.sc = 0x3C; // 1 steps positive value
        }
        else if(getMicroSteps() == 3)
        {
            this->_REGI_RUN.B.sc = 0x3E; // 1 steps positive value
        }
    }

 
    
    for(int i = 0;i < num_steps;i++)
    {
        setRunRegi(this->_REGI_RUN); //1 steps positive value
         wait_ms(delay);
    }
    
    this->_REGI_RUN.B.en = 0;
    setRunRegi(this->_REGI_RUN); //1 steps positive value

}