gein_bitton_change
Fork of petbottle_Loading by
petbottleLoading.cpp
- Committer:
- skouki
- Date:
- 2018-10-13
- Revision:
- 2:7fc5c9527580
- Parent:
- 1:6cdfc10c7b1d
File content as of revision 2:7fc5c9527580:
#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(); limitSW1_.rise(callback(this,&petbottleLoading::limit_sw)); limitSW2_.rise(callback(this,&petbottleLoading::limit_sw)); limitSW3_.rise(callback(this,&petbottleLoading::limit_sw)); } void petbottleLoading::limit_sw() { limitSW1 = limitSW1_.read(); limitSW2 = limitSW2_.read(); limitSW3 = limitSW3_.read(); } 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() { }