gein_bitton_change
Fork of petbottle_Loading by
petbottleLoading.cpp@2:7fc5c9527580, 2018-10-13 (annotated)
- Committer:
- skouki
- Date:
- Sat Oct 13 06:33:32 2018 +0000
- Revision:
- 2:7fc5c9527580
- Parent:
- 1:6cdfc10c7b1d
gein change
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 | 1:6cdfc10c7b1d | 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(); |
skouki | 2:7fc5c9527580 | 33 | limitSW1_.rise(callback(this,&petbottleLoading::limit_sw)); |
skouki | 2:7fc5c9527580 | 34 | limitSW2_.rise(callback(this,&petbottleLoading::limit_sw)); |
skouki | 2:7fc5c9527580 | 35 | limitSW3_.rise(callback(this,&petbottleLoading::limit_sw)); |
tanabe2000 | 0:890f901f4898 | 36 | |
tanabe2000 | 0:890f901f4898 | 37 | } |
tanabe2000 | 0:890f901f4898 | 38 | |
skouki | 2:7fc5c9527580 | 39 | void petbottleLoading::limit_sw() |
skouki | 2:7fc5c9527580 | 40 | { |
skouki | 2:7fc5c9527580 | 41 | limitSW1 = limitSW1_.read(); |
skouki | 2:7fc5c9527580 | 42 | limitSW2 = limitSW2_.read(); |
skouki | 2:7fc5c9527580 | 43 | limitSW3 = limitSW3_.read(); |
skouki | 2:7fc5c9527580 | 44 | } |
tanabe2000 | 0:890f901f4898 | 45 | void petbottleLoading::petbottlemode(int loadingmode) |
tanabe2000 | 0:890f901f4898 | 46 | { |
tanabe2000 | 0:890f901f4898 | 47 | nowPals = loadingencoder.getPulses(); |
tanabe2000 | 0:890f901f4898 | 48 | loadingPid.setSetPoint(loadingDistance);// 目標値 |
tanabe2000 | 0:890f901f4898 | 49 | loadingPid.setProcessValue(nowPals); |
tanabe2000 | 0:890f901f4898 | 50 | Output_PID = -1.0*loadingPid.compute(); |
tanabe2000 | 0:890f901f4898 | 51 | pdt = pt.read(); |
tanabe2000 | 0:890f901f4898 | 52 | limitSW1 = limitSW1_.read(); |
tanabe2000 | 0:890f901f4898 | 53 | limitSW2 = limitSW2_.read(); |
tanabe2000 | 0:890f901f4898 | 54 | limitSW3 = limitSW3_.read(); |
tanabe2000 | 0:890f901f4898 | 55 | // debug.printf("L1:<%d>, L2:<%d>, L3:<%d>\r\n", limitSW1, limitSW2, limitSW3); |
tanabe2000 | 1:6cdfc10c7b1d | 56 | // debug.printf("nowPals<%d>\r\n", nowPals); |
tanabe2000 | 0:890f901f4898 | 57 | |
tanabe2000 | 0:890f901f4898 | 58 | if(loadingmode == 0) { |
tanabe2000 | 0:890f901f4898 | 59 | loadingDistance = 0; |
tanabe2000 | 0:890f901f4898 | 60 | petbottledropPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 61 | petbottleInPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 62 | } else if(loadingmode == 1) { |
tanabe2000 | 0:890f901f4898 | 63 | limitSwFlag[2] = 0; |
tanabe2000 | 0:890f901f4898 | 64 | limitSwFlag2[2] = 0; |
tanabe2000 | 1:6cdfc10c7b1d | 65 | loadingDistance = 875; |
tanabe2000 | 0:890f901f4898 | 66 | if((nowPals - pullDistanceOfset) > 700) { |
tanabe2000 | 0:890f901f4898 | 67 | if(limitSW1 == 1 && limitSwFlag[0] == 0) { |
tanabe2000 | 0:890f901f4898 | 68 | limitSwFlag[0] = 1; |
tanabe2000 | 1:6cdfc10c7b1d | 69 | petbottledropPwm = 0.5; |
tanabe2000 | 0:890f901f4898 | 70 | } |
tanabe2000 | 0:890f901f4898 | 71 | if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) { |
tanabe2000 | 0:890f901f4898 | 72 | limitSwFlag[0] = 1; |
tanabe2000 | 0:890f901f4898 | 73 | limitSwFlag2[0] = 1; |
tanabe2000 | 1:6cdfc10c7b1d | 74 | petbottledropPwm = 0.5 |
tanabe2000 | 1:6cdfc10c7b1d | 75 | ; |
tanabe2000 | 0:890f901f4898 | 76 | } |
tanabe2000 | 0:890f901f4898 | 77 | if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1; |
tanabe2000 | 0:890f901f4898 | 78 | if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 79 | } |
tanabe2000 | 0:890f901f4898 | 80 | } else if(loadingmode == 2) { |
tanabe2000 | 0:890f901f4898 | 81 | limitSwFlag[2] = 0; |
tanabe2000 | 0:890f901f4898 | 82 | limitSwFlag2[2] = 0; |
tanabe2000 | 0:890f901f4898 | 83 | loadingDistance = 1854; |
tanabe2000 | 0:890f901f4898 | 84 | if((nowPals - pullDistanceOfset) > 700) { |
tanabe2000 | 0:890f901f4898 | 85 | if(limitSW1 == 1 && limitSwFlag[0] == 0) { |
tanabe2000 | 0:890f901f4898 | 86 | limitSwFlag[0] = 1; |
tanabe2000 | 1:6cdfc10c7b1d | 87 | |
tanabe2000 | 1:6cdfc10c7b1d | 88 | petbottledropPwm = 0.5; |
tanabe2000 | 0:890f901f4898 | 89 | } |
tanabe2000 | 0:890f901f4898 | 90 | if(limitSW1 == 0 && limitSwFlag[0] == 0 &&limitSwFlag2[0] == 0) { |
tanabe2000 | 0:890f901f4898 | 91 | limitSwFlag[0] = 1; |
tanabe2000 | 0:890f901f4898 | 92 | limitSwFlag2[0] = 1; |
tanabe2000 | 1:6cdfc10c7b1d | 93 | petbottledropPwm = 0.5; |
tanabe2000 | 0:890f901f4898 | 94 | } |
tanabe2000 | 0:890f901f4898 | 95 | if(limitSW1 ==0 && limitSwFlag[0] == 1) limitSwFlag2[0] = 1; |
tanabe2000 | 0:890f901f4898 | 96 | if((limitSwFlag[0] == 1) && (limitSwFlag2[0] == 1) && (limitSW1 == 1))petbottledropPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 97 | } |
tanabe2000 | 0:890f901f4898 | 98 | } else if(loadingmode == 3) { |
tanabe2000 | 0:890f901f4898 | 99 | if(limitSW3 ==1 && limitSwFlag[2] == 0) limitSwFlag[2] = 1,limitSwFlag2[2] = 0 ; |
tanabe2000 | 0:890f901f4898 | 100 | if(limitSW3 ==0 && limitSwFlag[2] == 1) limitSwFlag2[2] = 1; |
tanabe2000 | 0:890f901f4898 | 101 | if(limitSwFlag[2] == 1 && limitSwFlag2[2] == 1) { |
tanabe2000 | 0:890f901f4898 | 102 | if(limitSW3 ==1) { |
tanabe2000 | 0:890f901f4898 | 103 | petbottleInPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 104 | loadingDistance = 0; |
tanabe2000 | 0:890f901f4898 | 105 | petbottleInPwm = 0.0; |
tanabe2000 | 1:6cdfc10c7b1d | 106 | if((nowPals - pullDistanceOfset) < 15) { |
tanabe2000 | 0:890f901f4898 | 107 | if(limitSW1 == 1 && limitSwFlag[0] == 1) { |
tanabe2000 | 0:890f901f4898 | 108 | limitSwFlag[0] = 0; |
tanabe2000 | 1:6cdfc10c7b1d | 109 | petbottledropPwm = 0.5; |
tanabe2000 | 0:890f901f4898 | 110 | } |
tanabe2000 | 0:890f901f4898 | 111 | if(limitSW1 == 0 && limitSwFlag[0] == 1 &&limitSwFlag2[0] == 1) { |
tanabe2000 | 0:890f901f4898 | 112 | limitSwFlag[0] = 0; |
tanabe2000 | 0:890f901f4898 | 113 | limitSwFlag2[0] = 0; |
tanabe2000 | 1:6cdfc10c7b1d | 114 | petbottledropPwm = 0.45; |
tanabe2000 | 0:890f901f4898 | 115 | } |
tanabe2000 | 0:890f901f4898 | 116 | if(limitSW1 ==0 && limitSwFlag[0] == 0) limitSwFlag2[0] = 0; |
tanabe2000 | 0:890f901f4898 | 117 | if((limitSwFlag[0] == 0) && (limitSwFlag2[0] == 0) && (limitSW1 == 1))petbottledropPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 118 | } |
tanabe2000 | 0:890f901f4898 | 119 | } |
tanabe2000 | 0:890f901f4898 | 120 | } else { |
tanabe2000 | 1:6cdfc10c7b1d | 121 | if((nowPals - pullDistanceOfset) > 700)petbottleInPwm = -0.5; |
tanabe2000 | 0:890f901f4898 | 122 | } |
tanabe2000 | 0:890f901f4898 | 123 | } else { |
tanabe2000 | 0:890f901f4898 | 124 | petbottledropPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 125 | petbottleInPwm = 0.0; |
tanabe2000 | 0:890f901f4898 | 126 | } |
tanabe2000 | 0:890f901f4898 | 127 | petbottledrop->setSpeed(petbottledropPwm); |
tanabe2000 | 0:890f901f4898 | 128 | petbotleslider->setSpeed(Output_PID); |
tanabe2000 | 0:890f901f4898 | 129 | petbottleIn->setSpeed(petbottleInPwm); |
tanabe2000 | 0:890f901f4898 | 130 | } |
tanabe2000 | 0:890f901f4898 | 131 | |
tanabe2000 | 1:6cdfc10c7b1d | 132 | int petbottleLoading::LoadingEncoder() |
tanabe2000 | 1:6cdfc10c7b1d | 133 | { |
tanabe2000 | 0:890f901f4898 | 134 | return nowPals - pullDistanceOfset; |
tanabe2000 | 1:6cdfc10c7b1d | 135 | } |
tanabe2000 | 0:890f901f4898 | 136 | |
tanabe2000 | 0:890f901f4898 | 137 | int petbottleLoading::state() |
tanabe2000 | 0:890f901f4898 | 138 | { |
tanabe2000 | 0:890f901f4898 | 139 | |
tanabe2000 | 0:890f901f4898 | 140 | } |