File content as of revision 4:416d194973a3:

#include "towelest.h"

towelest::towelest(Serial* RS485,int address,PinName Limit0,PinName Limit1,PinName Limit2):
pid(4.0, 0, 0.001, 0.01),
LimitSW0(Limit0),
LimitSW1(Limit1),
LimitSW2(Limit2)
{
    Motor = new ikarashiMDC*[3];
    Motor[0] = new ikarashiMDC(address,0,SM,RS485);
    Motor[1] = new ikarashiMDC(address,1,SM,RS485);
    Motor[2] = new ikarashiMDC(address,2,SM,RS485);      
      
    Motor[0]->braking = true;
    Motor[1]->braking = true;
    Motor[2]->braking = true;
    
    pid.setInputLimits(-1800, 1800);
    pid.setOutputLimits(-0.6,0.6);
    pid.setBias(0.0);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(0);

}

void towelest::open()
{
    if(LimitSW0.read() == 1){
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0);
    }else{
        Motor[0]->setSpeed(-0.2);
        Motor[1]->setSpeed(0);
    }
}
void towelest::close()
{
    Motor[0]->setSpeed(0.2);
    Motor[1]->setSpeed(0);
}
    
void towelest::lift()
{
    if(LimitSW1.read() == 1){
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0);
    }else{
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0.8);
    }
}

void towelest::descent()
{
    if(LimitSW2.read() == 1){
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0);
    }else{
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(-0.8);
    }
}

void towelest::setPoint(int target)
{
    pid.setSetPoint(target);
}

void towelest::setPulse(int Loli)
{
    loli = Loli;
}

void towelest::pid_compute()
{
    pid.setProcessValue(loli);
    compute = pid.compute();
    Motor[2]->setSpeed(compute);
        //pc.printf("%d  %d  %f \n\r",Limit1,Limit2,Limit3,RotationDistance);
}

void towelest::allstop()
{
    Motor[0]->setSpeed(0);
    Motor[1]->setSpeed(0);
    Motor[2]->setSpeed(0);
}