naoki tanabe / petbottle_Loadin5port

パラメータ調整

petbottleLoading.cpp@0:b412810ae2db, 2018-10-12 (annotated)

Committer:
tanabe2000
Date:
Fri Oct 12 04:38:22 2018 +0000
Revision:
0:b412810ae2db
???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanabe2000 0:b412810ae2db 1 #include "petbottleLoading.h"
tanabe2000 0:b412810ae2db 2
tanabe2000 0:b412810ae2db 3 petbottleLoading::petbottleLoading():
tanabe2000 0:b412810ae2db 4 loadingPid(loadingKP, loadingKI, loadingKD, RATE),
tanabe2000 0:b412810ae2db 5 loadingencoder(loadingencoder1_1, loadingencoder1_2 ,NC, 2048, QEI::X4_ENCODING),
tanabe2000 0:b412810ae2db 6 RS485control(PA_4),
tanabe2000 0:b412810ae2db 7 RS485(MDTX,MDRX,115200),
tanabe2000 0:b412810ae2db 8 // con(fepTX, fepRX, ADDR),
tanabe2000 0:b412810ae2db 9 debug(USBTX, USBRX),
tanabe2000 0:b412810ae2db 10
tanabe2000 0:b412810ae2db 11 limitSW1_(limitswich1_1),
tanabe2000 0:b412810ae2db 12 limitSW2_(limitswich1_2),
tanabe2000 0:b412810ae2db 13 limitSW3_(limitswich1_3)
tanabe2000 0:b412810ae2db 14 {
tanabe2000 0:b412810ae2db 15 petbottledrop = new ikarashiMDC(&RS485control,1,0,SM,&RS485);
tanabe2000 0:b412810ae2db 16 petbotleslider = new ikarashiMDC(&RS485control,1,1,SM,&RS485);
tanabe2000 0:b412810ae2db 17 petbottleIn = new ikarashiMDC(&RS485control,1,2,SM,&RS485);
tanabe2000 0:b412810ae2db 18 petbottledrop->braking = true;/*変更*/
tanabe2000 0:b412810ae2db 19 petbotleslider->braking = true;/*変更*/
tanabe2000 0:b412810ae2db 20 petbottleIn->braking = true;/*変更*/
tanabe2000 0:b412810ae2db 21 limitSW1_.mode(PullUp);
tanabe2000 0:b412810ae2db 22 limitSW2_.mode(PullUp);
tanabe2000 0:b412810ae2db 23 limitSW3_.mode(PullUp);
tanabe2000 0:b412810ae2db 24
tanabe2000 0:b412810ae2db 25 loadingPid.setMode(AUTO_MODE);
tanabe2000 0:b412810ae2db 26 loadingPid.setInputLimits(-10, 2200);
tanabe2000 0:b412810ae2db 27 loadingPid.setOutputLimits(-0.4, 0.4);
tanabe2000 0:b412810ae2db 28 loadingPid.setBias(0.0); // outputの変わり目
tanabe2000 0:b412810ae2db 29 limitSwFlag[2] = 0;
tanabe2000 0:b412810ae2db 30 limitSwFlag2[2] = 0;
tanabe2000 0:b412810ae2db 31 limitSwFlag[0] = 0;
tanabe2000 0:b412810ae2db 32 limitSwFlag2[0] = 0;
tanabe2000 0:b412810ae2db 33 sw5flag = 0;
tanabe2000 0:b412810ae2db 34 pt.start();
tanabe2000 0:b412810ae2db 35
tanabe2000 0:b412810ae2db 36 }
tanabe2000 0:b412810ae2db 37
tanabe2000 0:b412810ae2db 38
tanabe2000 0:b412810ae2db 39 void petbottleLoading::petbottlemode(int loadingmode)
tanabe2000 0:b412810ae2db 40 {
tanabe2000 0:b412810ae2db 41 nowPals = loadingencoder.getPulses();
tanabe2000 0:b412810ae2db 42 loadingPid.setSetPoint(loadingDistance);// 目標値
tanabe2000 0:b412810ae2db 43 loadingPid.setProcessValue(nowPals);
tanabe2000 0:b412810ae2db 44 Output_PID = -1.0*loadingPid.compute();
tanabe2000 0:b412810ae2db 45 pdt = pt.read();
tanabe2000 0:b412810ae2db 46 //if((con.getButton1(1) == 0) && (pdt > 0.5)&& receiveSuccessed) loadingmode++, pt.reset();
tanabe2000 0:b412810ae2db 47 // if(loadingmode == 2) loadingmode++;
tanabe2000 0:b412810ae2db 48 // if(loadingmode > 3) loadingmode = 0;
tanabe2000 0:b412810ae2db 49 limitSW1 = limitSW1_.read();
tanabe2000 0:b412810ae2db 50 limitSW2 = limitSW2_.read();
tanabe2000 0:b412810ae2db 51 limitSW3 = limitSW3_.read();
tanabe2000 0:b412810ae2db 52 //debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3);
tanabe2000 0:b412810ae2db 53
tanabe2000 0:b412810ae2db 54 if(loadingmode == 0) {
tanabe2000 0:b412810ae2db 55 loadingDistance = 0;
tanabe2000 0:b412810ae2db 56 petbottledropPwm = 0.0;
tanabe2000 0:b412810ae2db 57 petbottleInPwm = 0.0;
tanabe2000 0:b412810ae2db 58 } else if(loadingmode == 1) {
tanabe2000 0:b412810ae2db 59 limitSwFlag[2] = 0;
tanabe2000 0:b412810ae2db 60 limitSwFlag2[2] = 0;
tanabe2000 0:b412810ae2db 61 loadingDistance = slidorparamotor;
tanabe2000 0:b412810ae2db 62 if((nowPals - pullDistanceOfset) > 700) {
tanabe2000 0:b412810ae2db 63 if(limitSW1 == 1 && limitSwFlag[0] == 0) {
tanabe2000 0:b412810ae2db 64 limitSwFlag[0] = 1;
tanabe2000 0:b412810ae2db 65 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 66 }
tanabe2000 0:b412810ae2db 67 if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
tanabe2000 0:b412810ae2db 68 limitSwFlag[0] = 1;
tanabe2000 0:b412810ae2db 69 limitSwFlag2[0] = 1;
tanabe2000 0:b412810ae2db 70 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 71 }
tanabe2000 0:b412810ae2db 72 if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
tanabe2000 0:b412810ae2db 73 if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
tanabe2000 0:b412810ae2db 74 }
tanabe2000 0:b412810ae2db 75 } else if(loadingmode == 2) {
tanabe2000 0:b412810ae2db 76 limitSwFlag[2] = 0;
tanabe2000 0:b412810ae2db 77 limitSwFlag2[2] = 0;
tanabe2000 0:b412810ae2db 78 loadingDistance = 2050;
tanabe2000 0:b412810ae2db 79 if((nowPals - pullDistanceOfset) > 700) {
tanabe2000 0:b412810ae2db 80 if(limitSW1 == 1 && limitSwFlag[0] == 0) {
tanabe2000 0:b412810ae2db 81 limitSwFlag[0] = 1;
tanabe2000 0:b412810ae2db 82 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 83 }
tanabe2000 0:b412810ae2db 84 if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) {
tanabe2000 0:b412810ae2db 85 limitSwFlag[0] = 1;
tanabe2000 0:b412810ae2db 86 limitSwFlag2[0] = 1;
tanabe2000 0:b412810ae2db 87 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 88 }
tanabe2000 0:b412810ae2db 89 if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1;
tanabe2000 0:b412810ae2db 90 if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0;
tanabe2000 0:b412810ae2db 91 }
tanabe2000 0:b412810ae2db 92 } else if(loadingmode == 3) {
tanabe2000 0:b412810ae2db 93 if(limitSW3 ==1 && limitSwFlag[2] == 0) limitSwFlag[2] = 1,limitSwFlag2[2] = 0 ;
tanabe2000 0:b412810ae2db 94 if(limitSW3 ==0 && limitSwFlag[2] == 1) limitSwFlag2[2] = 1;
tanabe2000 0:b412810ae2db 95 if(limitSwFlag[2] == 1 && limitSwFlag2[2] == 1) {
tanabe2000 0:b412810ae2db 96 if(limitSW3 ==1) {
tanabe2000 0:b412810ae2db 97 petbottleInPwm = 0.0;
tanabe2000 0:b412810ae2db 98 loadingDistance = 0;
tanabe2000 0:b412810ae2db 99 petbottleInPwm = 0.0;
tanabe2000 0:b412810ae2db 100 if((nowPals - pullDistanceOfset) < 10) {
tanabe2000 0:b412810ae2db 101 if(limitSW1 == 1 && limitSwFlag[0] == 1) {
tanabe2000 0:b412810ae2db 102 limitSwFlag[0] = 0;
tanabe2000 0:b412810ae2db 103 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 104 }
tanabe2000 0:b412810ae2db 105 if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) {
tanabe2000 0:b412810ae2db 106 limitSwFlag[0] = 0;
tanabe2000 0:b412810ae2db 107 limitSwFlag2[0] = 0;
tanabe2000 0:b412810ae2db 108 petbottledropPwm = 0.5;
tanabe2000 0:b412810ae2db 109 }
tanabe2000 0:b412810ae2db 110 if(limitSW1 ==0 && limitSwFlag[0] == 0) limitSwFlag2[0] = 0;
tanabe2000 0:b412810ae2db 111 if((limitSwFlag[0] == 0) && (limitSwFlag2[0] == 0) && (limitSW1 == 1))petbottledropPwm = 0.0;
tanabe2000 0:b412810ae2db 112 }
tanabe2000 0:b412810ae2db 113 }
tanabe2000 0:b412810ae2db 114 } else {
tanabe2000 0:b412810ae2db 115 if((nowPals - pullDistanceOfset) > 700)petbottleInPwm = -0.45;
tanabe2000 0:b412810ae2db 116 }
tanabe2000 0:b412810ae2db 117 } else {
tanabe2000 0:b412810ae2db 118 petbottledropPwm = 0.0;
tanabe2000 0:b412810ae2db 119 petbottleInPwm = 0.0;
tanabe2000 0:b412810ae2db 120 }
tanabe2000 0:b412810ae2db 121 petbottledrop->setSpeed(petbottledropPwm);
tanabe2000 0:b412810ae2db 122 petbotleslider->setSpeed(Output_PID);
tanabe2000 0:b412810ae2db 123 petbottleIn->setSpeed(petbottleInPwm);
tanabe2000 0:b412810ae2db 124 }
tanabe2000 0:b412810ae2db 125
tanabe2000 0:b412810ae2db 126 int petbottleLoading::LoadingEncoder()
tanabe2000 0:b412810ae2db 127 {
tanabe2000 0:b412810ae2db 128 return nowPals - pullDistanceOfset;
tanabe2000 0:b412810ae2db 129 }
tanabe2000 0:b412810ae2db 130
tanabe2000 0:b412810ae2db 131 int petbottleLoading::state()
tanabe2000 0:b412810ae2db 132 {
tanabe2000 0:b412810ae2db 133
tanabe2000 0:b412810ae2db 134 }