File content as of revision 0:603f46a29b61:

#include "mbed.h"
#include "threeAxis.h"
#include "rtos.h"

threeAxis::threeAxis(PinName xStep, PinName yStep, PinName zStep, PinName xDir, PinName yDir, PinName zDir, PinName xEnable, PinName yEnable, PinName zEnable, float stepRatio, float xBound, float yBound, float zBound, PinName xLimit, PinName yLimit, PinName zLimit)
: _xStep(xStep), _yStep(yStep), _zStep(zStep), _xDir(xDir), _yDir(yDir), _zDir(zDir), _xEnable(xEnable), _yEnable(yEnable), _zEnable(zEnable), _xLimit(xLimit), _yLimit(yLimit), _zLimit(zLimit)
{
    _stepRatio = stepRatio;
    _xMax = xBound;
    _yMax = yBound;
    _zMax = zBound;


    _xEnable = 1;
    _yEnable = 1;
    _zEnable = 1;

    wait = 5;

    defaultXdir = 0;
    defaultYdir = 0;
    defaultZdir = 0;

    _xDir = defaultXdir;
    _yDir = defaultYdir;
    _zDir = defaultZdir;

    limitsEnabled = false;
}

void threeAxis::invertX(){
    defaultXdir = !defaultXdir;
    _xDir = defaultXdir;
}

void threeAxis::invertY(){
    defaultYdir = !defaultYdir;
    _yDir = defaultYdir;
}

void threeAxis::invertZ(){
    defaultZdir = !defaultZdir;
    _zDir = defaultZdir;
}

void threeAxis::setZeroX(){
    toX = 0;
    currX = 0;
}

void threeAxis::setZeroY(){
    toY = 0;
    currY = 0;
}

void threeAxis::setZeroZ(){
    toZ = 0;
    currZ = 0;
}

void threeAxis::setZero(){
    this->setZeroX();
    this->setZeroY();
    this->setZeroZ();
}

void threeAxis::setWait(int waitPer){
    wait = waitPer;
}

void threeAxis::setXdir(bool val){
    defaultXdir = val;
    _xDir = val;
}

void threeAxis::setYdir(bool val){
    defaultYdir = val;
    _yDir = val;
}

void threeAxis::setZdir(bool val){
    defaultZdir = val;
    _zDir = val;
}


void threeAxis::zeroX(){
    _xEnable = 0;
    _xDir = !defaultXdir;
    
    while(!_xLimit){
        _xStep = 1;
        Thread::wait(wait);
        _xStep = 0;
        Thread::wait(wait);
    }
    
    this->setZeroX();
    _xEnable = 1;

}



void threeAxis::zeroY(){
    _yEnable = 0;
    _yDir = !defaultYdir;
    
    while(!_yLimit){
        _yStep = 1;
        Thread::wait(wait);
        _yStep = 0;
        Thread::wait(wait);
    }
    
    this->setZeroY();
    _yEnable = 1;

}

void threeAxis::zeroZ(){
    _zEnable = 0;
    _zDir = !defaultZdir;
    
    while(!_zLimit){
        _zStep = 1;
        Thread::wait(wait);
        _zStep = 0;
        Thread::wait(wait);
    }
    
    this->setZeroX();
    _zEnable = 1;

}

void threeAxis::setLimits(bool val){
    limitsEnabled = val;
}

int threeAxis::goTo(float xVal, float yVal, float zVal){
    int xSteps = (int)xVal*_stepRatio;
    int ySteps = (int)yVal*_stepRatio;
    int zSteps = (int)zVal*_stepRatio;
    
    return this->goToRaw(xSteps, ySteps, zSteps);
}

int threeAxis::goToRaw(int xVal, int yVal, int zVal){
    toX = xVal;
    toY = yVal;
    toZ = zVal;
    
    
    if (toX > _xMax * _stepRatio){
        toX = _xMax * _stepRatio;
    }
    
    if (toY > _yMax * _stepRatio){
        toY = _yMax * _stepRatio;
    }
    
    if (toZ > _zMax * _stepRatio){
        toZ = _zMax * _stepRatio;
    }
    
    int xStepVal = 1;
    int yStepVal = 1;
    int zStepVal = 1;
    
    _xDir = defaultXdir;
    _yDir = defaultYdir;
    _zDir = defaultZdir;
    
    int x = toX - currX;
    int y = toY - currY;
    int z = toZ - currZ;
        
        
        
    if (x != 0 || y != 0 || z != 0){
         if (x != 0){
             _xEnable = 0;
             if (x < 0){
                 _xDir = !defaultXdir;
                 x = x* -1;
                 xStepVal = xStepVal * -1;
             }
         }
            
         if (y != 0){
             _yEnable = 0;
             if (y < 0){
                 _yDir = !defaultYdir;
                 y = y* -1;
                 yStepVal = yStepVal * -1;
             }
         }
            
         if (z != 0){
             _zEnable = 0;
             if (z < 0){
                 _zDir = !defaultZdir;
                 z = z* -1;
                 zStepVal = zStepVal * -1;
             }
         }
            
         while (x > 0 || y > 0 || z > 0){
             if (x>0){
                 _xStep = 1;
                 x=x-1;
                 currX += xStepVal;
             }
             if (y>0){
                 _yStep = 1;
                 y=y-1;
                 currY += yStepVal;
             }
             if (z>0){
                 _zStep = 1;
                 z=z-1;
                 currZ += zStepVal;
             }
             Thread::wait(wait);
             _xStep = 0;
             _yStep = 0;
             _zStep = 0;
             Thread::wait(wait);
         }
     }

     _xEnable = 1;
     _yEnable = 1;
     _zEnable = 1;
     _xDir = defaultXdir;
     _yDir = defaultYdir;
     _zDir = defaultZdir;
     
     return 0;
}