パラメータ調整

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers petbottleLoading.cpp Source File

petbottleLoading.cpp

00001 #include "petbottleLoading.h"
00002 
00003 petbottleLoading::petbottleLoading():
00004     loadingPid(loadingKP, loadingKI, loadingKD, RATE),
00005     loadingencoder(loadingencoder1_1, loadingencoder1_2 ,NC, 2048, QEI::X4_ENCODING),
00006     RS485control(PA_4),
00007     RS485(MDTX,MDRX,115200),
00008 //    con(fepTX, fepRX, ADDR),
00009     debug(USBTX, USBRX),
00010 
00011     limitSW1_(limitswich1_1),
00012     limitSW2_(limitswich1_2),
00013     limitSW3_(limitswich1_3)
00014 {
00015     petbottledrop  = new ikarashiMDC(&RS485control,1,0,SM,&RS485);
00016     petbotleslider = new ikarashiMDC(&RS485control,1,1,SM,&RS485);
00017     petbottleIn    = new ikarashiMDC(&RS485control,1,2,SM,&RS485);
00018     petbottledrop->braking = true;/*変更*/
00019     petbotleslider->braking = true;/*変更*/
00020     petbottleIn->braking = true;/*変更*/
00021     limitSW1_.mode(PullUp);
00022     limitSW2_.mode(PullUp);
00023     limitSW3_.mode(PullUp);
00024 
00025     loadingPid.setMode(AUTO_MODE);
00026     loadingPid.setInputLimits(-10, 2200);
00027     loadingPid.setOutputLimits(-0.4, 0.4);
00028     loadingPid.setBias(0.0);   // outputの変わり目
00029     limitSwFlag[2] = 0;
00030     limitSwFlag2[2] = 0;
00031     limitSwFlag[0] = 0;
00032     limitSwFlag2[0] = 0;
00033     sw5flag = 0;
00034     pt.start();
00035 
00036 }
00037 
00038 
00039 void petbottleLoading::petbottlemode(int loadingmode)
00040 {
00041     nowPals = loadingencoder.getPulses();
00042     loadingPid.setSetPoint(loadingDistance);// 目標値
00043     loadingPid.setProcessValue(nowPals);
00044     Output_PID = -1.0*loadingPid.compute();
00045     pdt = pt.read();
00046     //if((con.getButton1(1) == 0) && (pdt > 0.5)&& receiveSuccessed) loadingmode++, pt.reset();
00047 //        if(loadingmode == 2) loadingmode++;
00048 //        if(loadingmode > 3) loadingmode = 0;
00049     limitSW1 = limitSW1_.read();
00050     limitSW2 = limitSW2_.read();
00051     limitSW3 = limitSW3_.read();
00052     //debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3);
00053 
00054     if(loadingmode == 0) {
00055         loadingDistance = 0;
00056         petbottledropPwm = 0.0;
00057         petbottleInPwm = 0.0;
00058     } else if(loadingmode == 1) {
00059         limitSwFlag[2] = 0;
00060         limitSwFlag2[2] = 0;
00061         loadingDistance = slidorparamotor;
00062         if((nowPals - pullDistanceOfset) > 700) {
00063             if(limitSW1 == 1 && limitSwFlag[0] == 0) {
00064                 limitSwFlag[0] = 1;
00065                 petbottledropPwm = 0.5;
00066             }
00067             if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
00068                 limitSwFlag[0] = 1;
00069                 limitSwFlag2[0] = 1;
00070                 petbottledropPwm = 0.5;
00071             }
00072             if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
00073             if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
00074         }
00075     } else if(loadingmode == 2) {
00076         limitSwFlag[2] = 0;
00077         limitSwFlag2[2] = 0;
00078         loadingDistance = 2050;
00079         if((nowPals - pullDistanceOfset) > 700) {
00080             if(limitSW1 == 1 && limitSwFlag[0] == 0) {
00081                 limitSwFlag[0] = 1;
00082                 petbottledropPwm = 0.5;
00083             }
00084             if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
00085                 limitSwFlag[0] = 1;
00086                 limitSwFlag2[0] = 1;
00087                 petbottledropPwm = 0.5;
00088             }
00089             if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
00090             if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
00091         }
00092     } else if(loadingmode == 3) {
00093         if(limitSW3 ==1 && limitSwFlag[2] == 0) limitSwFlag[2] = 1,limitSwFlag2[2] = 0 ;
00094         if(limitSW3 ==0 && limitSwFlag[2] == 1) limitSwFlag2[2] = 1;
00095         if(limitSwFlag[2] == 1 && limitSwFlag2[2] == 1) {
00096             if(limitSW3 ==1) {
00097                 petbottleInPwm = 0.0;
00098                 loadingDistance = 0;
00099                 petbottleInPwm = 0.0;
00100                 if((nowPals - pullDistanceOfset) < 10) {
00101                     if(limitSW1 == 1 && limitSwFlag[0] == 1) {
00102                         limitSwFlag[0] = 0;
00103                         petbottledropPwm = 0.5;
00104                     }
00105                     if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) {
00106                         limitSwFlag[0] = 0;
00107                         limitSwFlag2[0] = 0;
00108                         petbottledropPwm = 0.5;
00109                     }
00110                     if(limitSW1 ==0 && limitSwFlag[0] == 0) limitSwFlag2[0] = 0;
00111                     if((limitSwFlag[0] == 0) && (limitSwFlag2[0] == 0) && (limitSW1 == 1))petbottledropPwm = 0.0;
00112                 }
00113             }
00114         } else {
00115             if((nowPals - pullDistanceOfset) > 700)petbottleInPwm = -0.45;
00116         }
00117     } else {
00118         petbottledropPwm = 0.0;
00119         petbottleInPwm = 0.0;
00120     }
00121     petbottledrop->setSpeed(petbottledropPwm);
00122     petbotleslider->setSpeed(Output_PID);
00123     petbottleIn->setSpeed(petbottleInPwm);
00124 }
00125 
00126 int petbottleLoading::LoadingEncoder()
00127 {
00128     return nowPals - pullDistanceOfset;
00129 }
00130 
00131 int petbottleLoading::state()
00132 {
00133 
00134 }