File content as of revision 1:c6950f45b03c:

#include "towelest.h"

towelest::towelest():
serial(mdTX,mdRX,115200),
enc1(PA_6,PA_7,NC,100, QEI::X4_ENCODING),
pid(4.0, 0, 0.001, 0.01),
LimitSW1(PB_5),
LimitSW2(PB_4)
{
    Motor[0] = new ikarashiMDC(2,0,SM,&serial);
    Motor[1] = new ikarashiMDC(2,1,SM,&serial);
    Motor[2] = new ikarashiMDC(2,2,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::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::loop()
{
    while(true){
        RotationDistance = (float)enc1.getPulses();
        pid.setProcessValue(RotationDistance);
        compute = pid.compute();
        Motor[2]->setSpeed(compute);
        Limit1 = LimitSW1.read();
        Limit2 = LimitSW2.read();
        //pc.printf("%d  %d  %f \n\r",Limit1,Limit2,RotationDistance);
    }
}