File content as of revision 1:6cdfc10c7b1d:

#include "petbottleLoading.h"

petbottleLoading::petbottleLoading():
    loadingPid(loadingKP, loadingKI, loadingKD, RATE),
    loadingencoder(loadingencoder1_1, loadingencoder1_2 ,NC, 2048, QEI::X4_ENCODING),
    RS485control(PA_4),
    RS485(MDC_TX,MDC_RX,115200),
//    debug(USBTX, USBRX),
    limitSW1_(limitswich1_1),
    limitSW2_(limitswich1_2),
    limitSW3_(limitswich1_3)
{
    petbottledrop  = new ikarashiMDC(&RS485control,1,0,SM,&RS485);
    petbotleslider = new ikarashiMDC(&RS485control,1,1,SM,&RS485);
    petbottleIn    = new ikarashiMDC(&RS485control,1,2,SM,&RS485);
    petbottledrop->braking = true;/*変更*/
    petbotleslider->braking = true;/*変更*/
    petbottleIn->braking = true;/*変更*/
    limitSW1_.mode(PullUp);
    limitSW2_.mode(PullUp);
    limitSW3_.mode(PullUp);

    loadingPid.setMode(AUTO_MODE);
    loadingPid.setInputLimits(-10, 2200);
    loadingPid.setOutputLimits(-0.9, 0.9);
    loadingPid.setBias(0.0);   // outputの変わり目
    limitSwFlag[2] = 0;
    limitSwFlag2[2] = 0;
    limitSwFlag[0] = 0;
    limitSwFlag2[0] = 0;
    sw5flag = 0;
    pt.start();

}


void petbottleLoading::petbottlemode(int loadingmode)
{
    nowPals = loadingencoder.getPulses();
    loadingPid.setSetPoint(loadingDistance);// 目標値
    loadingPid.setProcessValue(nowPals);
    Output_PID = -1.0*loadingPid.compute();
    pdt = pt.read();
    limitSW1 = limitSW1_.read();
    limitSW2 = limitSW2_.read();
    limitSW3 = limitSW3_.read();
//    debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3);
//    debug.printf("nowPals<%d>\r\n", nowPals);

    if(loadingmode == 0) {
        loadingDistance = 0;
        petbottledropPwm = 0.0;
        petbottleInPwm = 0.0;
    } else if(loadingmode == 1) {
        limitSwFlag[2] = 0;
        limitSwFlag2[2] = 0;
        loadingDistance = 875;
        if((nowPals - pullDistanceOfset) > 700) {
            if(limitSW1 == 1 && limitSwFlag[0] == 0) {
                limitSwFlag[0] = 1;
                petbottledropPwm = 0.5;
            }
            if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
                limitSwFlag[0] = 1;
                limitSwFlag2[0] = 1;
                petbottledropPwm = 0.5
                ;
            }
            if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
            if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
        }
    } else if(loadingmode == 2) {
        limitSwFlag[2] = 0;
        limitSwFlag2[2] = 0;
        loadingDistance = 1854;
        if((nowPals - pullDistanceOfset) > 700) {
            if(limitSW1 == 1 && limitSwFlag[0] == 0) {
                limitSwFlag[0] = 1;

                petbottledropPwm = 0.5;
            }
            if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
                limitSwFlag[0] = 1;
                limitSwFlag2[0] = 1;
                petbottledropPwm = 0.5;
            }
            if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
            if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
        }
    } else if(loadingmode == 3) {
        if(limitSW3 ==1 && limitSwFlag[2] == 0) limitSwFlag[2] = 1,limitSwFlag2[2] = 0 ;
        if(limitSW3 ==0 && limitSwFlag[2] == 1) limitSwFlag2[2] = 1;
        if(limitSwFlag[2] == 1 && limitSwFlag2[2] == 1) {
            if(limitSW3 ==1) {
                petbottleInPwm = 0.0;
                loadingDistance = 0;
                petbottleInPwm = 0.0;
                if((nowPals - pullDistanceOfset) < 15) {
                    if(limitSW1 == 1 && limitSwFlag[0] == 1) {
                        limitSwFlag[0] = 0;
                        petbottledropPwm = 0.5;
                    }
                    if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) {
                        limitSwFlag[0] = 0;
                        limitSwFlag2[0] = 0;
                        petbottledropPwm = 0.45;
                    }
                    if(limitSW1 ==0 && limitSwFlag[0] == 0) limitSwFlag2[0] = 0;
                    if((limitSwFlag[0] == 0) && (limitSwFlag2[0] == 0) && (limitSW1 == 1))petbottledropPwm = 0.0;
                }
            }
        } else {
            if((nowPals - pullDistanceOfset) > 700)petbottleInPwm = -0.5;
        }
    } else {
        petbottledropPwm = 0.0;
        petbottleInPwm = 0.0;
    }
    petbottledrop->setSpeed(petbottledropPwm);
    petbotleslider->setSpeed(Output_PID);
    petbottleIn->setSpeed(petbottleInPwm);
}

int petbottleLoading::LoadingEncoder()
{
    return nowPals - pullDistanceOfset;
}

int petbottleLoading::state()
{

}