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