File content as of revision 2:623fb33351af:

#include "towelest.h"

towelest::towelest():
    serial(mdTX,mdRX,115200),
    pid(4.0, 0, 0.001, 0.01),
    LimitSW1(schmitt_trigger_0),
    LimitSW2(schmitt_trigger_1)
{
    Motor[0] = new ikarashiMDC(2,0,SM,&serial);
    Motor[1] = new ikarashiMDC(2,2,SM,&serial);
    Motor[2] = new ikarashiMDC(2,1,SM,&serial);

    Motor[0]->braking = true;
    Motor[1]->braking = true;
    Motor[2]->braking = true;

    thread.start(callback(this, &towelest::loop));

    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(LimitSW1.read() == 1) {
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0);
    } else if(LimitSW1.read() == 0) {
        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(LimitSW2.read() == 1) {
        Motor[0]->setSpeed(0);
        Motor[1]->setSpeed(0);
    } else if(LimitSW2.read() == 0) {
        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::loop()
{
    while(true) {
        pid.setProcessValue(loli);
        compute = pid.compute();
        Motor[2]->setSpeed(compute);
        Limit1 = LimitSW1.read();
        Limit2 = LimitSW2.read();
        //pc.printf("%d  %d  %f \n\r",Limit1,Limit2,RotationDistance);
    }
}