File content as of revision 3:63bb3e19c7eb:

#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::drop()
{
    timer.start();
    time = timer.read();
    if(time <= 5) {
        pid.setSetPoint(-800);
    } else {
        pid.setSetPoint(0);
        timer.stop();
        timer.reset();
    }
}

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);
}