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