NagaokaRoboticsClub_mbedTeam / Mbed OS Nucleo_NHK_2018_syudo_wheel

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   petbottle_Loading PID PS3 SerialMultiByte ikarashiMDC omni_wheel QEI solenoid_valve

Fork of Nucleo_NHK_2018_syudo_wheel by Kouki Suzuki

main.cpp@7:41b6b3a52c21, 2018-10-12 (annotated)

Committer:
tanabe2000
Date:
Fri Oct 12 02:52:29 2018 +0000
Revision:
7:41b6b3a52c21
Parent:
6:c14b110e6a95 
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kouki Suzuki 2:4a8227afe47b 1 #include"wheel_unit.h"
Kouki Suzuki 2:4a8227afe47b 2 #include"mbed.h"
tanabe2000 4:292ba4425f35 3 #include "petbottleLoading.h"
tanabe2000 4:292ba4425f35 4 #include "PID.h"
tanabe2000 4:292ba4425f35 5 #include "solenoid_valve.h"
tanabe2000 4:292ba4425f35 6 #include"ikarashiMDC.h"
tanabe2000 6:c14b110e6a95 7
tanabe2000 4:292ba4425f35 8 #include"pin_config.h"
tanabe2000 4:292ba4425f35 9
Kouki Suzuki 2:4a8227afe47b 10
Kouki Suzuki 2:4a8227afe47b 11 wheel_unit wheel_unit;
tanabe2000 6:c14b110e6a95 12 Serial debugpc(USBTX, USBRX, 1152000);
tanabe2000 4:292ba4425f35 13 petbottleLoading loading;
tanabe2000 4:292ba4425f35 14 PID fire1Pid(fire1KP, fire1KI, fire1KD, RATE);
tanabe2000 4:292ba4425f35 15 PID fire2Pid(fire2KP, fire2KI, fire2KD, RATE);
tanabe2000 4:292ba4425f35 16 PortValve valveFire1(solenoidValve1_1, solenoidValve1_2);
tanabe2000 4:292ba4425f35 17 PortValve valveFire2(solenoidValve2_1, solenoidValve2_2);
tanabe2000 4:292ba4425f35 18 DigitalOut serialcontrol(SC);
tanabe2000 4:292ba4425f35 19 Serial serial(MDC_TX,MDC_RX,115200);
tanabe2000 4:292ba4425f35 20 DigitalOut startSw(start);
tanabe2000 6:c14b110e6a95 21 Ticker ticker;
tanabe2000 6:c14b110e6a95 22
tanabe2000 6:c14b110e6a95 23 //Serial mi(SERIAL_TX, SERIAL_RX,115200);
tanabe2000 4:292ba4425f35 24 Timer pt;
tanabe2000 4:292ba4425f35 25 DigitalOut led(LED2);
tanabe2000 4:292ba4425f35 26 ikarashiMDC fireMDC[] {
tanabe2000 4:292ba4425f35 27 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
tanabe2000 4:292ba4425f35 28 ikarashiMDC(&serialcontrol,2,1,SM,&serial)
tanabe2000 4:292ba4425f35 29 };
tanabe2000 4:292ba4425f35 30
tanabe2000 6:c14b110e6a95 31 //void recive(){
tanabe2000 6:c14b110e6a95 32 // debugpc.printf("<%c>\r\n", mi.getc());
tanabe2000 6:c14b110e6a95 33 // }
tanabe2000 6:c14b110e6a95 34
tanabe2000 4:292ba4425f35 35 int main()
tanabe2000 4:292ba4425f35 36 {
tanabe2000 4:292ba4425f35 37 int loadingmode;
tanabe2000 4:292ba4425f35 38 int distanceOfset[2],constDistance, fireDistance;
tanabe2000 4:292ba4425f35 39 float fireMDCPwm[2];
tanabe2000 6:c14b110e6a95 40 int getdata;
tanabe2000 6:c14b110e6a95 41
tanabe2000 4:292ba4425f35 42 float pdt;
tanabe2000 6:c14b110e6a95 43 int airFlag, airFlag2, swflag, dpetbotle;
tanabe2000 4:292ba4425f35 44 int solenoidValve1is, solenoidValve2is;
tanabe2000 4:292ba4425f35 45
tanabe2000 4:292ba4425f35 46 fire1Pid.setMode(AUTO_MODE);
tanabe2000 4:292ba4425f35 47 fire1Pid.setInputLimits(0, 2300);
tanabe2000 6:c14b110e6a95 48 fire1Pid.setOutputLimits(-1.0, 1.0);
tanabe2000 4:292ba4425f35 49 fire1Pid.setBias(0.0);
tanabe2000 4:292ba4425f35 50
tanabe2000 4:292ba4425f35 51 fire2Pid.setMode(AUTO_MODE);
tanabe2000 4:292ba4425f35 52 fire2Pid.setInputLimits(0, 2300);
tanabe2000 6:c14b110e6a95 53 fire2Pid.setOutputLimits(-1.0, 1.0);
tanabe2000 4:292ba4425f35 54 fire2Pid.setBias(0.0);
tanabe2000 4:292ba4425f35 55 pt.start();
tanabe2000 6:c14b110e6a95 56
tanabe2000 6:c14b110e6a95 57 // mi.attach(recive);
tanabe2000 4:292ba4425f35 58 while(true) {
tanabe2000 6:c14b110e6a95 59 // debugpc.printf("<%c>\r\n", mi.getc());
tanabe2000 6:c14b110e6a95 60 //// debugpc.printf("test\r\n");
tanabe2000 6:c14b110e6a95 61 //// getdata = mi.readable();
tanabe2000 6:c14b110e6a95 62 //
tanabe2000 6:c14b110e6a95 63 //
tanabe2000 4:292ba4425f35 64 startSw = 1;
tanabe2000 4:292ba4425f35 65 wheel_unit.main_do();
tanabe2000 6:c14b110e6a95 66
tanabe2000 4:292ba4425f35 67 fire1Pid.setSetPoint(fireDistance);
tanabe2000 4:292ba4425f35 68 fire1Pid.setProcessValue(wheel_unit.encoder2Get() - distanceOfset[0]);
tanabe2000 4:292ba4425f35 69 fire2Pid.setSetPoint(fireDistance);
tanabe2000 4:292ba4425f35 70 fire2Pid.setProcessValue(wheel_unit.encoder1Get() - distanceOfset[1]);
tanabe2000 6:c14b110e6a95 71
tanabe2000 6:c14b110e6a95 72 //
tanabe2000 6:c14b110e6a95 73 //if(wheel_unit.getbutton(6)==0) fireDistance -= 10;
tanabe2000 6:c14b110e6a95 74 // if(wheel_unit.getbutton(7)==0) fireDistance += 10;
tanabe2000 6:c14b110e6a95 75 //if((wheel_unit.getbutton(9) == 0) && (pdt > 0.3)) loadingmode++, pt.reset();
tanabe2000 6:c14b110e6a95 76 // if(loadingmode == 2 ) loadingmode++;
tanabe2000 6:c14b110e6a95 77 // if(loadingmode > 3) loadingmode = 0;
tanabe2000 4:292ba4425f35 78 pdt = pt.read();
tanabe2000 6:c14b110e6a95 79 if((wheel_unit.getbutton(8)==0)&&(airFlag == 0) && (pdt > 0.3))airFlag = 1, pt.reset(), loadingmode = 1, constDistance = Distance1;
tanabe2000 4:292ba4425f35 80 pdt = pt.read();
tanabe2000 4:292ba4425f35 81 if(((wheel_unit.getbutton(8)==0)&&(airFlag == 1)) && (pdt > 0.3)) airFlag = 0, pt.reset();
tanabe2000 4:292ba4425f35 82 pdt = pt.read();
tanabe2000 6:c14b110e6a95 83 if((wheel_unit.getbutton(6)==0)&&(airFlag == 0) && (pdt > 0.3))airFlag = 1, pt.reset(), loadingmode = 1, constDistance = Distance2;
tanabe2000 4:292ba4425f35 84 pdt = pt.read();
tanabe2000 4:292ba4425f35 85 if(((wheel_unit.getbutton(6)==0)&&(airFlag == 1)) && (pdt > 0.3)) airFlag = 0, pt.reset();
tanabe2000 6:c14b110e6a95 86 /* pdt = pt.read();
tanabe2000 6:c14b110e6a95 87 if((wheel_unit.getbutton(8)==0)&&(airFlag == 0) && (pdt > 0.3))airFlag = 1, pt.reset(), constDistance = Distance1;
tanabe2000 4:292ba4425f35 88 pdt = pt.read();
tanabe2000 6:c14b110e6a95 89 if(((wheel_unit.getbutton(8)==0)&&(airFlag == 1)) && (pdt > 0.3)) airFlag = 0, pt.reset();
tanabe2000 6:c14b110e6a95 90 */
tanabe2000 6:c14b110e6a95 91 // if(wheel_unit.getbutton(6)==0) fireMDCPwm[0] = 0.7;
tanabe2000 6:c14b110e6a95 92 // if(wheel_unit.getbutton(7)==0) fireMDCPwm[0] = -0.7;
tanabe2000 6:c14b110e6a95 93 // if((wheel_unit.getbutton(6)==1) && (wheel_unit.getbutton(7)==1)) fireMDCPwm[0] = 0.0;
tanabe2000 4:292ba4425f35 94
tanabe2000 4:292ba4425f35 95 if(airFlag == 1) {
tanabe2000 4:292ba4425f35 96 led = 1;
tanabe2000 4:292ba4425f35 97 solenoidValve1is = 1;
tanabe2000 6:c14b110e6a95 98 if(((wheel_unit.encoder2Get() - distanceOfset[0]) > 1800) && (dpetbotle == 0)) {
tanabe2000 6:c14b110e6a95 99 pt.reset();
tanabe2000 6:c14b110e6a95 100 dpetbotle = 1;
tanabe2000 6:c14b110e6a95 101 }
tanabe2000 6:c14b110e6a95 102 if((pdt > 4.0) && (dpetbotle == 1)) {
tanabe2000 4:292ba4425f35 103 loadingmode = 3;
tanabe2000 6:c14b110e6a95 104 fireMDCPwm[0] = 0.0;
tanabe2000 6:c14b110e6a95 105
tanabe2000 4:292ba4425f35 106 }
tanabe2000 7:41b6b3a52c21 107 if(loadingmode != 3)fireDistance = di, fireMDCPwm[0] = -1*fire1Pid.compute();
tanabe2000 4:292ba4425f35 108 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 4:292ba4425f35 109 if(swflag == 1) {
tanabe2000 4:292ba4425f35 110 fireDistance = constDistance;
tanabe2000 4:292ba4425f35 111 swflag = 0;
tanabe2000 6:c14b110e6a95 112 pt.reset();
tanabe2000 6:c14b110e6a95 113 }
tanabe2000 6:c14b110e6a95 114 if((loading.LoadingEncoder() < 40) && (pdt > 4.0)) {
tanabe2000 6:c14b110e6a95 115 loadingmode = 0;
tanabe2000 6:c14b110e6a95 116 }
tanabe2000 6:c14b110e6a95 117 fireMDCPwm[0] = -1*fire1Pid.compute();
tanabe2000 4:292ba4425f35 118 }
tanabe2000 6:c14b110e6a95 119
tanabe2000 6:c14b110e6a95 120 } else {
tanabe2000 6:c14b110e6a95 121 dpetbotle = 0;
tanabe2000 6:c14b110e6a95 122 led = 0;
tanabe2000 6:c14b110e6a95 123 swflag = 1;
tanabe2000 6:c14b110e6a95 124 solenoidValve1is = 0;
tanabe2000 6:c14b110e6a95 125 loadingmode = 0;
tanabe2000 6:c14b110e6a95 126 distanceOfset[0] = wheel_unit.encoder2Get();
tanabe2000 6:c14b110e6a95 127 if(wheel_unit.getbutton(6)==0) fireMDCPwm[0] = 0.7;
tanabe2000 6:c14b110e6a95 128 if(wheel_unit.getbutton(7)==0) fireMDCPwm[0] = -0.7;
tanabe2000 6:c14b110e6a95 129 if((wheel_unit.getbutton(6)==1) && (wheel_unit.getbutton(7)==1)) fireMDCPwm[0] = 0.0;
tanabe2000 6:c14b110e6a95 130 }
tanabe2000 6:c14b110e6a95 131
tanabe2000 6:c14b110e6a95 132 /*
tanabe2000 6:c14b110e6a95 133 if(airFlag == 1) {
tanabe2000 6:c14b110e6a95 134 led = 1;
tanabe2000 6:c14b110e6a95 135 solenoidValve1is = 1;
tanabe2000 6:c14b110e6a95 136 if(swflag == 1) {
tanabe2000 6:c14b110e6a95 137 fireDistance = Distance1;
tanabe2000 6:c14b110e6a95 138 swflag = 0;
tanabe2000 6:c14b110e6a95 139 } else fireMDCPwm[0] = -1*fire1Pid.compute();
tanabe2000 6:c14b110e6a95 140
tanabe2000 6:c14b110e6a95 141 } else {
tanabe2000 6:c14b110e6a95 142 led = 0;
tanabe2000 6:c14b110e6a95 143 // swflag = 1;
tanabe2000 6:c14b110e6a95 144 solenoidValve1is = 0;
tanabe2000 6:c14b110e6a95 145 distanceOfset[0] = wheel_unit.encoder2Get();
tanabe2000 6:c14b110e6a95 146 fireMDCPwm[0] = 0.0;
tanabe2000 6:c14b110e6a95 147 }
tanabe2000 6:c14b110e6a95 148 */
tanabe2000 6:c14b110e6a95 149 /*
tanabe2000 6:c14b110e6a95 150 pdt = pt.read();
tanabe2000 6:c14b110e6a95 151 if((wheel_unit.getbutton(8)==0)&&(airFlag == 0)&&(airFlag2 == 0) && (pdt > 0.3))airFlag = 1, pt.reset(), constDistance = Distance1, loadingmode = 1;
tanabe2000 6:c14b110e6a95 152 pdt = pt.read();
tanabe2000 6:c14b110e6a95 153 if(((wheel_unit.getbutton(8)==0)&&(airFlag == 1)&&(airFlag2 == 0)) && (pdt > 0.3)) airFlag = 0, pt.reset();
tanabe2000 6:c14b110e6a95 154 pdt = pt.read();
tanabe2000 6:c14b110e6a95 155 if((wheel_unit.getbutton(6)==0)&&(airFlag == 0)&&(airFlag2 == 0) && (pdt > 0.3))airFlag = 1, pt.reset(), constDistance = Distance2, loadingmode = 1;
tanabe2000 6:c14b110e6a95 156 pdt = pt.read();
tanabe2000 6:c14b110e6a95 157 if(((wheel_unit.getbutton(6)==0)&&(airFlag == 1)&&(airFlag2 == 0)) && (pdt > 0.3)) airFlag = 0, pt.reset();
tanabe2000 6:c14b110e6a95 158 pdt = pt.read();
tanabe2000 6:c14b110e6a95 159 if((wheel_unit.getbutton(7)==0)&&(airFlag2 == 0) && (airFlag == 0) && (pdt > 0.3))airFlag2 = 1, pt.reset(), constDistance = Distance3, loadingmode = 2;
tanabe2000 6:c14b110e6a95 160 pdt = pt.read();
tanabe2000 6:c14b110e6a95 161 if(((wheel_unit.getbutton(7)==0)&&(airFlag == 0)&&(airFlag2 == 1)) && (pdt > 0.3)) airFlag2 = 0, pt.reset();
tanabe2000 6:c14b110e6a95 162
tanabe2000 6:c14b110e6a95 163 if(airFlag == 1) {
tanabe2000 6:c14b110e6a95 164 led = 1;
tanabe2000 6:c14b110e6a95 165 solenoidValve1is = 1;
tanabe2000 6:c14b110e6a95 166 if((wheel_unit.encoder2Get() - distanceOfset[0]) > 1900) {
tanabe2000 6:c14b110e6a95 167 loadingmode = 3;
tanabe2000 6:c14b110e6a95 168 fireMDCPwm[0] = 0.0;
tanabe2000 6:c14b110e6a95 169 }
tanabe2000 6:c14b110e6a95 170 if(loadingmode != 3)fireDistance = 1900, fireMDCPwm[0] = -1*fire1Pid.compute();
tanabe2000 6:c14b110e6a95 171 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 6:c14b110e6a95 172 if(swflag == 1) {
tanabe2000 6:c14b110e6a95 173 fireDistance = constDistance;
tanabe2000 6:c14b110e6a95 174 swflag = 0;
tanabe2000 6:c14b110e6a95 175 }// else if(wheel_unit.getbutton(9)==0)fireDistance = 0;
tanabe2000 6:c14b110e6a95 176 fireMDCPwm[0] = -1*fire1Pid.compute();
tanabe2000 6:c14b110e6a95 177 }
tanabe2000 6:c14b110e6a95 178
tanabe2000 4:292ba4425f35 179 } else {
tanabe2000 4:292ba4425f35 180 led = 0;
tanabe2000 4:292ba4425f35 181 swflag = 1;
tanabe2000 4:292ba4425f35 182 solenoidValve1is = 0;
tanabe2000 4:292ba4425f35 183 distanceOfset[0] = wheel_unit.encoder2Get();
tanabe2000 6:c14b110e6a95 184 loadingmode = 0;
tanabe2000 4:292ba4425f35 185 fireMDCPwm[0] = 0.0;
tanabe2000 4:292ba4425f35 186 }
tanabe2000 6:c14b110e6a95 187
tanabe2000 6:c14b110e6a95 188 if(airFlag2 == 1) {
tanabe2000 4:292ba4425f35 189 led = 1;
tanabe2000 4:292ba4425f35 190 solenoidValve2is = 1;
tanabe2000 6:c14b110e6a95 191 if((wheel_unit.encoder1Get() - distanceOfset[1]) > 1900) {
tanabe2000 4:292ba4425f35 192 loadingmode = 3;
tanabe2000 6:c14b110e6a95 193 fireMDCPwm[1] = 0.0;
tanabe2000 4:292ba4425f35 194 }
tanabe2000 6:c14b110e6a95 195 if(loadingmode != 3)fireDistance = 1900, fireMDCPwm[1] = -1*fire1Pid.compute();
tanabe2000 6:c14b110e6a95 196 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 4:292ba4425f35 197 if(swflag == 1) {
tanabe2000 6:c14b110e6a95 198 fireDistance = 100;
tanabe2000 4:292ba4425f35 199 swflag = 0;
tanabe2000 6:c14b110e6a95 200 }// else if(wheel_unit.getbutton(9)==0)fireDistance = 0;
tanabe2000 6:c14b110e6a95 201 fireMDCPwm[1] = -1*fire2Pid.compute();
tanabe2000 4:292ba4425f35 202 }
tanabe2000 6:c14b110e6a95 203
tanabe2000 6:c14b110e6a95 204 } else if(airFlag2 == 1){
tanabe2000 4:292ba4425f35 205 led = 0;
tanabe2000 4:292ba4425f35 206 swflag = 1;
tanabe2000 4:292ba4425f35 207 solenoidValve2is = 0;
tanabe2000 6:c14b110e6a95 208 distanceOfset[0] = wheel_unit.encoder1Get();
tanabe2000 6:c14b110e6a95 209 loadingmode = 0;
tanabe2000 4:292ba4425f35 210 fireMDCPwm[1] = 0.0;
tanabe2000 4:292ba4425f35 211 }
tanabe2000 6:c14b110e6a95 212 */
tanabe2000 4:292ba4425f35 213
tanabe2000 6:c14b110e6a95 214 debugpc.printf("g1<%d>, g2<%d>", wheel_unit.encoder1Get(), wheel_unit.encoder2Get());
tanabe2000 6:c14b110e6a95 215 // debugpc.printf("encoder1<%d>, encoder2<%d>targetValue<%d>", wheel_unit.encoder2Get(),wheel_unit.encoder1Get(),fireDistance);
tanabe2000 4:292ba4425f35 216 // for(int n = 0; n < 12; n++)debugpc.printf("B%d:<%d>", n,wheel_unit.getbutton(n));
tanabe2000 6:c14b110e6a95 217 // debugpc.printf("fireMDCPwm[1]<%f>", fireMDCPwm[1]);
tanabe2000 4:292ba4425f35 218 loading.petbottlemode(loadingmode);
tanabe2000 4:292ba4425f35 219 valveFire1.inputState(solenoidValve1is);
tanabe2000 4:292ba4425f35 220 valveFire2.inputState(solenoidValve2is);
tanabe2000 4:292ba4425f35 221 for(int m = 0; m < 2; m++)fireMDC[m].setSpeed(fireMDCPwm[m]);
tanabe2000 6:c14b110e6a95 222
tanabe2000 4:292ba4425f35 223 debugpc.printf("\r\n");
tanabe2000 4:292ba4425f35 224 }
skouki 3:5c79aea8d9f5 225 }