File content as of revision 1:43d856fad23a:

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

extern uLCD_4DGL uLCD;
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;
}

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

void threeAxis::goTo(float xVal, float yVal, float zVal){
    
    int xSteps = (int)xVal/_stepRatio;
    int ySteps = (int)yVal/_stepRatio;
    int zSteps = (int)zVal/_stepRatio;
    
    //uLCD.printf("\n%.2f, %.2f, %.2f, %d, %d, %d", xVal, yVal, zVal, xSteps, ySteps, zSteps);
    
    toX = xSteps;
    toY = ySteps;
    toZ = zSteps;
    
    
    
    if (toX > _xMax / _stepRatio){
        toX = _xMax / _stepRatio;
    }
    
    if (toY > _yMax / _stepRatio){
        toY = _yMax / _stepRatio;
    }
    
    if (toZ > _zMax / _stepRatio){
        toZ = _zMax / _stepRatio;
    }
    
    //uLCD.printf("\nTo: %d, %d, %d", toX, toY, toZ);
    //uLCD.printf("\nCur: %d, %d, %d", currX, currY, currZ);
    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;
    
    //uLCD.printf("\n%d,%d,%d", x,y,z);
        
        
        
    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;
     
}