パラメータ調整
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 19 2022 00:09:21 by
1.7.2
naoki tanabe