パラメータ調整
petbottleLoading.cpp
- Committer:
- tanabe2000
- Date:
- 2018-10-12
- Revision:
- 0:b412810ae2db
File content as of revision 0:b412810ae2db:
#include "petbottleLoading.h" petbottleLoading::petbottleLoading(): loadingPid(loadingKP, loadingKI, loadingKD, RATE), loadingencoder(loadingencoder1_1, loadingencoder1_2 ,NC, 2048, QEI::X4_ENCODING), RS485control(PA_4), RS485(MDTX,MDRX,115200), // con(fepTX, fepRX, ADDR), 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.4, 0.4); 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(); //if((con.getButton1(1) == 0) && (pdt > 0.5)&& receiveSuccessed) loadingmode++, pt.reset(); // if(loadingmode == 2) loadingmode++; // if(loadingmode > 3) loadingmode = 0; limitSW1 = limitSW1_.read(); limitSW2 = limitSW2_.read(); limitSW3 = limitSW3_.read(); //debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3); if(loadingmode == 0) { loadingDistance = 0; petbottledropPwm = 0.0; petbottleInPwm = 0.0; } else if(loadingmode == 1) { limitSwFlag[2] = 0; limitSwFlag2[2] = 0; loadingDistance = slidorparamotor; 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 = 2050; 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) < 10) { 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.5; } 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.45; } } 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() { }