装填機構ライブラリ
Diff: petbottleLoading.cpp
- Revision:
- 0:890f901f4898
- Child:
- 1:6cdfc10c7b1d
diff -r 000000000000 -r 890f901f4898 petbottleLoading.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/petbottleLoading.cpp Thu Oct 04 11:43:22 2018 +0000 @@ -0,0 +1,130 @@ +#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 = 954; + if((nowPals - pullDistanceOfset) > 700) { + if(limitSW1 == 1 && limitSwFlag[0] == 0) { + limitSwFlag[0] = 1; + petbottledropPwm = 1.0; + } + if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) { + limitSwFlag[0] = 1; + limitSwFlag2[0] = 1; + petbottledropPwm = 1.0; + } + 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 = 1.0; + } + if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) { + limitSwFlag[0] = 1; + limitSwFlag2[0] = 1; + petbottledropPwm = 1.0; + } + 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) < 25) { + if(limitSW1 == 1 && limitSwFlag[0] == 1) { + limitSwFlag[0] = 0; + petbottledropPwm = 1.0; + } + if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) { + limitSwFlag[0] = 0; + limitSwFlag2[0] = 0; + petbottledropPwm = 1.0; + } + 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 = -1.0; + } + } 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() +{ + +}