Kouki Suzuki / petbottle_Loading

gein_bitton_change

Fork of petbottle_Loading by naoki tanabe

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?

UserRevisionLine numberNew 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 }