NagaokaRoboticsClub_mbedTeam / Mbed OS 2018NHK_gaku_ver2

未完成

Dependencies:   linemiconget petbottle_Loadin5port Kinect OmniPosition PID QEI　led R1307 S-ShapeModel SerialMultiByte TFmini fep2 ikarashiMDC linesSnsor omni_wheel solenoid_valve

gakuBot/gakubot.cpp@2:c501f6845500, 2018-10-16 (annotated)

Committer:
tanabe2000
Date:
Tue Oct 16 03:32:37 2018 +0000
Revision:
2:c501f6845500
Parent:
0:7822f4172b0c 
2018 NHK auto A team

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tanabe2000 0:7822f4172b0c 1 #include "gakubot.h"
tanabe2000 0:7822f4172b0c 2
tanabe2000 0:7822f4172b0c 3 GakuBot::GakuBot():
tanabe2000 0:7822f4172b0c 4 led(LED2),
tanabe2000 0:7822f4172b0c 5 kinect(fepTX, fepRX,151,115200),
tanabe2000 0:7822f4172b0c 6 lmicon(LineTX, LineRX),
tanabe2000 0:7822f4172b0c 7 getposition(measuringTX, measuringRX),
tanabe2000 0:7822f4172b0c 8 valveFire1(solenoidValve1_1, solenoidValve1_2),
tanabe2000 0:7822f4172b0c 9 valveFire2(solenoidValve3_1, solenoidValve3_2),
tanabe2000 0:7822f4172b0c 10 omni(4),
tanabe2000 0:7822f4172b0c 11 anglePID(angleKP, angleKI, angleKD, 0.001),
tanabe2000 0:7822f4172b0c 12 fire1Pid(fire1KP, fire1KI, fire1KD, RATE),
tanabe2000 0:7822f4172b0c 13 fire2Pid(fire2KP, fire2KI, fire2KD, RATE),
tanabe2000 0:7822f4172b0c 14 XPid(XKP, XIP, XDP, RATE),
tanabe2000 0:7822f4172b0c 15 YPid(YKP, YIP, YDP, RATE),
tanabe2000 0:7822f4172b0c 16 distanceFrontPid(frontKP, frontKP, frontKD, RATE),
tanabe2000 0:7822f4172b0c 17 distanceBehindPid(behindKP, behindKI, behindKD, RATE),
tanabe2000 0:7822f4172b0c 18 RS485control(PA_4),
tanabe2000 0:7822f4172b0c 19 RS485(MDTX,MDRX,115200),
tanabe2000 0:7822f4172b0c 20 debugpc(USBTX,USBRX,115200),
tanabe2000 0:7822f4172b0c 21 receiveSuccessed(0),
tanabe2000 0:7822f4172b0c 22 limit1(limitswich2_1),
tanabe2000 0:7822f4172b0c 23 limit2(limitswich2_2),
tanabe2000 0:7822f4172b0c 24 startSW(start)
tanabe2000 0:7822f4172b0c 25
tanabe2000 0:7822f4172b0c 26 {
tanabe2000 0:7822f4172b0c 27 for(int i = 0; i < 4; i++)wheels[i] = new ikarashiMDC(&RS485control,0,i,SM,&RS485), wheels[i]->braking = true;
tanabe2000 0:7822f4172b0c 28 for(int i = 0; i < 2; i++)fire[i] = new ikarashiMDC(&RS485control,2,i,SM,&RS485), fire[i]->braking = false;
tanabe2000 0:7822f4172b0c 29 conposition = new PositionController(500.0,3000.0,0.35,-0.2,0.7);
tanabe2000 0:7822f4172b0c 30 for(int i = 0; i < 4; i++)omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
tanabe2000 0:7822f4172b0c 31 Xdemo[0] = 0, Xdemo[1] = 3912, Xdemo[2] = 3921, Xdemo[3] = 3912, Xdemo[4] = -1000;
tanabe2000 0:7822f4172b0c 32 Ydemo[0] = 0, Ydemo[1] = -836, Ydemo[2] = -4112, Ydemo[3] = -836, Ydemo[4] = 0;
tanabe2000 0:7822f4172b0c 33
tanabe2000 0:7822f4172b0c 34 suzuki[0] = 4700;
tanabe2000 0:7822f4172b0c 35 suzuki[1] = 3000;
tanabe2000 0:7822f4172b0c 36 suzuki[2] = 2000;
tanabe2000 0:7822f4172b0c 37 startSW.mode(PullUp);
tanabe2000 0:7822f4172b0c 38 limit1.mode(PullUp);
tanabe2000 0:7822f4172b0c 39 limit2.mode(PullUp);
tanabe2000 0:7822f4172b0c 40
tanabe2000 0:7822f4172b0c 41 fire1Pid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 42 fire1Pid.setInputLimits(-100, 2300);
tanabe2000 0:7822f4172b0c 43 fire1Pid.setOutputLimits(-0.3, 1.0);
tanabe2000 0:7822f4172b0c 44 fire1Pid.setBias(0.0);
tanabe2000 0:7822f4172b0c 45
tanabe2000 0:7822f4172b0c 46 fire2Pid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 47 fire2Pid.setInputLimits(-100, 2300);
tanabe2000 0:7822f4172b0c 48 fire2Pid.setOutputLimits(-0.3, 1.0);
tanabe2000 0:7822f4172b0c 49 fire2Pid.setSetPoint(fireDistance2);
tanabe2000 0:7822f4172b0c 50
tanabe2000 0:7822f4172b0c 51 XPid.setBias(0.0);
tanabe2000 0:7822f4172b0c 52 XPid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 53 XPid.setInputLimits(-10000, 10000);
tanabe2000 0:7822f4172b0c 54 XPid.setOutputLimits(-0.8, 0.8);
tanabe2000 0:7822f4172b0c 55
tanabe2000 0:7822f4172b0c 56 YPid.setBias(0.0);
tanabe2000 0:7822f4172b0c 57 YPid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 58 YPid.setInputLimits(-10000, 10000);
tanabe2000 0:7822f4172b0c 59 YPid.setOutputLimits(-0.8, 0.8);
tanabe2000 0:7822f4172b0c 60
tanabe2000 0:7822f4172b0c 61 anglePID.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 62 anglePID.setInputLimits(-360, 360);
tanabe2000 0:7822f4172b0c 63 anglePID.setOutputLimits(-1.0, 1.0);
tanabe2000 0:7822f4172b0c 64 anglePID.setSetPoint(0.0);
tanabe2000 0:7822f4172b0c 65 anglePID.setBias(0.0);
tanabe2000 0:7822f4172b0c 66
tanabe2000 0:7822f4172b0c 67 distanceFrontPid.setBias(0.0);
tanabe2000 0:7822f4172b0c 68 distanceFrontPid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 69 distanceFrontPid.setInputLimits(30.0, 35.0);
tanabe2000 0:7822f4172b0c 70 distanceFrontPid.setOutputLimits(-0.5, 0.0);
tanabe2000 0:7822f4172b0c 71 distanceFrontPid.setSetPoint(40);
tanabe2000 0:7822f4172b0c 72
tanabe2000 0:7822f4172b0c 73 distanceBehindPid.setBias(0.0);
tanabe2000 0:7822f4172b0c 74 distanceBehindPid.setMode(AUTO_MODE);
tanabe2000 0:7822f4172b0c 75 distanceBehindPid.setInputLimits(30.0, 70.0);
tanabe2000 0:7822f4172b0c 76 distanceBehindPid.setOutputLimits(-0.7, 0.7);
tanabe2000 0:7822f4172b0c 77 distanceBehindPid.setSetPoint(40);
tanabe2000 0:7822f4172b0c 78
tanabe2000 0:7822f4172b0c 79 // conposition->targetXY(2000.0, 1000.0);
tanabe2000 0:7822f4172b0c 80
tanabe2000 0:7822f4172b0c 81 pt.start();
tanabe2000 0:7822f4172b0c 82 confirmT.start();
tanabe2000 0:7822f4172b0c 83 fireFrag1time.start();
tanabe2000 0:7822f4172b0c 84 pidT.start();
tanabe2000 0:7822f4172b0c 85 t.start();
tanabe2000 0:7822f4172b0c 86 getx = 0;
tanabe2000 0:7822f4172b0c 87
tanabe2000 0:7822f4172b0c 88 gety = 0;
tanabe2000 0:7822f4172b0c 89 swflag = 1;
tanabe2000 0:7822f4172b0c 90 airFlag = 1;
tanabe2000 0:7822f4172b0c 91
tanabe2000 0:7822f4172b0c 92 }
tanabe2000 0:7822f4172b0c 93
tanabe2000 0:7822f4172b0c 94 void GakuBot::botConfirm()
tanabe2000 0:7822f4172b0c 95 {
tanabe2000 0:7822f4172b0c 96 //if(con.getButton2(2)==0) fireDistance1 += 10, fireDistance2 += 10;
tanabe2000 0:7822f4172b0c 97 // if(con.getButton2(3)==0) fireDistance1 -= 10, fireDistance2 -= 10;
tanabe2000 0:7822f4172b0c 98 // receiveSuccessed = con.receiveState();
tanabe2000 0:7822f4172b0c 99 kinect.newdata();
tanabe2000 0:7822f4172b0c 100 kinectDistance = kinect.get_distance();
tanabe2000 0:7822f4172b0c 101 kinectmode = kinect.get_mode();
tanabe2000 0:7822f4172b0c 102 lmicon.receiveState();
tanabe2000 0:7822f4172b0c 103 nowPals = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 104 nowPals2 = lmicon.getEncoder(0);
tanabe2000 0:7822f4172b0c 105 fire1Pid.setSetPoint(fireDistance1);
tanabe2000 0:7822f4172b0c 106 fire1Pid.setProcessValue(nowPals - distanceOfset);
tanabe2000 0:7822f4172b0c 107 Output_PID = -1*fire1Pid.compute();
tanabe2000 0:7822f4172b0c 108 fire2Pid.setSetPoint(fireDistance2);
tanabe2000 0:7822f4172b0c 109 fire2Pid.setProcessValue(nowPals2 - distanceOfset2);
tanabe2000 0:7822f4172b0c 110 Output_PID2 = -1*fire2Pid.compute();
tanabe2000 0:7822f4172b0c 111
tanabe2000 0:7822f4172b0c 112 yawdegree = getposition.getTheta();
tanabe2000 0:7822f4172b0c 113 if((yawdegree - beforeYaw) > 350)yawMode--;
tanabe2000 0:7822f4172b0c 114 else if((yawdegree - beforeYaw) < -350)yawMode++;
tanabe2000 0:7822f4172b0c 115 beforeYaw = yawdegree;
tanabe2000 0:7822f4172b0c 116 nowAngle = 360*yawMode + yawdegree;
tanabe2000 0:7822f4172b0c 117 anglePID.setSetPoint(attachAngle);
tanabe2000 0:7822f4172b0c 118 anglePID.setProcessValue(nowAngle - ofsetNowAngle);
tanabe2000 0:7822f4172b0c 119 distanceBehindPid.setProcessValue(lmicon.getDistance(1));
tanabe2000 0:7822f4172b0c 120
tanabe2000 0:7822f4172b0c 121 if((int)startSW==0 && (pdt > 0.5)) {
tanabe2000 0:7822f4172b0c 122 //getx = getposition.getX();
tanabe2000 0:7822f4172b0c 123 // gety = getposition.getY();
tanabe2000 0:7822f4172b0c 124 // pt.reset();
tanabe2000 0:7822f4172b0c 125 //demomode++;
tanabe2000 0:7822f4172b0c 126 // if(demomode > 3)demomode = 0;
tanabe2000 0:7822f4172b0c 127 }
tanabe2000 0:7822f4172b0c 128 //getx = Xdemo[demomode];
tanabe2000 0:7822f4172b0c 129 // gety = Ydemo[demomode];
tanabe2000 0:7822f4172b0c 130
tanabe2000 2:c501f6845500 131
tanabe2000 0:7822f4172b0c 132 // debugpc.printf("xVector = %f, yVector = %f\r\n", conposition->getVelocityX(), conposition->getVelocityY());
tanabe2000 0:7822f4172b0c 133 //debugpc.printf("XPid.compute()<%f>, YPid.compute()<%f>\r\n", XPid.compute(), YPid.compute());
tanabe2000 0:7822f4172b0c 134 debugpc.printf("x = %d, y = %d\r\n", getposition.getX(), getposition.getY());
tanabe2000 0:7822f4172b0c 135 // debugpc.printf("limit1 = %d, limit2 = %d", (int)limit1, (int)limit2);
tanabe2000 0:7822f4172b0c 136 // debugpc.printf("rad<%f>", targetradians*180/PI);
tanabe2000 0:7822f4172b0c 137 // debugpc.printf("getx<%d>, gety<%d>\r\n", getx, gety);
tanabe2000 0:7822f4172b0c 138
tanabe2000 0:7822f4172b0c 139
tanabe2000 0:7822f4172b0c 140
tanabe2000 0:7822f4172b0c 141 }
tanabe2000 0:7822f4172b0c 142
tanabe2000 0:7822f4172b0c 143 //void GakuBot::controllerMode1()
tanabe2000 0:7822f4172b0c 144 //{
tanabe2000 0:7822f4172b0c 145 //// if(receiveSuccessed) {
tanabe2000 0:7822f4172b0c 146 // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
tanabe2000 0:7822f4172b0c 147 // if(con.getButton2(1)==0) {
tanabe2000 0:7822f4172b0c 148 //
tanabe2000 0:7822f4172b0c 149 //// omni.computeXY(conposition->getVelocityX(), conposition->getVelocityY(), -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 150 //// omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 151 //
tanabe2000 0:7822f4172b0c 152 //
tanabe2000 0:7822f4172b0c 153 //
tanabe2000 0:7822f4172b0c 154 //// debugpc.printf("X<%f>, Y<%f>\r\n", X, Y);
tanabe2000 0:7822f4172b0c 155 // omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 156 //// debugpc.printf("con.getButton1(4) = %d\r\n", con.getButton1(4));
tanabe2000 0:7822f4172b0c 157 //
tanabe2000 0:7822f4172b0c 158 // //if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 159 //// omni.computeXY(0.5, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 160 //// } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 161 //// omni.computeXY(0.5, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 162 //// } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 163 //// omni.computeXY(0.5, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 164 //// } else {
tanabe2000 0:7822f4172b0c 165 //// omni.computeXY(0.5, -0.15, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 166 //// }
tanabe2000 0:7822f4172b0c 167 // } else if((-0.1 < stick[2] )&& (stick[2] < 0.1)) {
tanabe2000 0:7822f4172b0c 168 // omni.computeXY(stick[0]/2,stick[1]/2, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 169 //// debugpc.printf("- distanceBehindPid.compute()<%f>", distanceBehindPid.compute());
tanabe2000 0:7822f4172b0c 170 // Xlopassb = 0;
tanabe2000 0:7822f4172b0c 171 // Ylopassb = 0;
tanabe2000 0:7822f4172b0c 172 // } else {
tanabe2000 0:7822f4172b0c 173 // omni.computeXY(stick[0]/3.0,stick[1]/3.0, -1*stick[2]/6.0);
tanabe2000 0:7822f4172b0c 174 // ofsetNowAngle = nowAngle;
tanabe2000 0:7822f4172b0c 175 // attachAngle = 0;
tanabe2000 0:7822f4172b0c 176 // }
tanabe2000 0:7822f4172b0c 177 //
tanabe2000 0:7822f4172b0c 178 // confirmDt = confirmT.read();
tanabe2000 0:7822f4172b0c 179 // //if((con.getButton2(0)==0) && (confirmDt >= 0.5))attachAngle += 10, confirmT.reset();
tanabe2000 0:7822f4172b0c 180 //// if((con.getButton2(1)==0) && (confirmDt >= 0.5))attachAngle -= 10, confirmT.reset();
tanabe2000 0:7822f4172b0c 181 //// debugpc.printf("attach = %f\r\n", attachAngle);
tanabe2000 0:7822f4172b0c 182 //
tanabe2000 0:7822f4172b0c 183 // for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 184 // speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 185 // wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 186 // }
tanabe2000 0:7822f4172b0c 187 // //} else {
tanabe2000 0:7822f4172b0c 188 //// debugpc.printf("error\r\n");
tanabe2000 0:7822f4172b0c 189 //// for(int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
tanabe2000 0:7822f4172b0c 190 ////
tanabe2000 0:7822f4172b0c 191 //// }
tanabe2000 0:7822f4172b0c 192 //}
tanabe2000 0:7822f4172b0c 193 //
tanabe2000 0:7822f4172b0c 194 //void GakuBot::controllerMode2()
tanabe2000 0:7822f4172b0c 195 //{
tanabe2000 0:7822f4172b0c 196 // if(receiveSuccessed) {
tanabe2000 0:7822f4172b0c 197 // for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
tanabe2000 0:7822f4172b0c 198 // omni.computeXY(stick[0]/3.0 ,stick[1]/3.0 , -1*stick[2]/5.0);
tanabe2000 0:7822f4172b0c 199 // for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 200 // speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 201 // wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 202 // }
tanabe2000 0:7822f4172b0c 203 // } else {
tanabe2000 0:7822f4172b0c 204 // debugpc.printf("error\r\n");
tanabe2000 0:7822f4172b0c 205 // for (int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
tanabe2000 0:7822f4172b0c 206 // }
tanabe2000 0:7822f4172b0c 207 //}
tanabe2000 0:7822f4172b0c 208 //
tanabe2000 0:7822f4172b0c 209 //void GakuBot::controllMech1()
tanabe2000 0:7822f4172b0c 210 //{
tanabe2000 0:7822f4172b0c 211 //// if(receiveSuccessed) {
tanabe2000 0:7822f4172b0c 212 // pdt = pt.read();
tanabe2000 0:7822f4172b0c 213 // if((con.getButton1(1) == 0) && (pdt > 0.5)) loadingmode++, pt.reset();
tanabe2000 0:7822f4172b0c 214 // if(loadingmode == 2 ) loadingmode++;
tanabe2000 0:7822f4172b0c 215 // if(loadingmode > 3) loadingmode = 0;
tanabe2000 0:7822f4172b0c 216 // loading.petbottlemode(loadingmode);
tanabe2000 0:7822f4172b0c 217 // debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
tanabe2000 0:7822f4172b0c 218 //// debugpc.printf("pals2 = %d,fireDistance2 = %d, distanceOfset2 = %d", nowPals2 - distanceOfset2,fireDistance2,distanceOfset2);
tanabe2000 0:7822f4172b0c 219 // if((con.getButton1(5)==0)&&(airFlag == 0) && (pdt > 0.5))airFlag = 1, pt.reset();
tanabe2000 0:7822f4172b0c 220 // pdt = pt.read();
tanabe2000 0:7822f4172b0c 221 // if((con.getButton1(5)==0)&&(airFlag == 1) && (pdt > 0.5))airFlag = 0, pt.reset();
tanabe2000 0:7822f4172b0c 222 //
tanabe2000 0:7822f4172b0c 223 // //if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 224 //// led = 1;
tanabe2000 0:7822f4172b0c 225 //// valveFire1.inputState(1);
tanabe2000 0:7822f4172b0c 226 //// if(con.getButton1(3)==0) {
tanabe2000 0:7822f4172b0c 227 //// if(sw3flag == 1) {
tanabe2000 0:7822f4172b0c 228 //// fireDistance1 = 750;
tanabe2000 0:7822f4172b0c 229 //// sw3flag = 0;
tanabe2000 0:7822f4172b0c 230 //// } else firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 231 //// } else if((con.getButton1(0) == 0)) {
tanabe2000 0:7822f4172b0c 232 ////// fireDistance1 = 1928;
tanabe2000 0:7822f4172b0c 233 //// fireDistance1 = 1847;
tanabe2000 0:7822f4172b0c 234 //// firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 235 //// sw3flag = 1;
tanabe2000 0:7822f4172b0c 236 //// } else {
tanabe2000 0:7822f4172b0c 237 //// if(con.getButton1(2)==0) firePwm[0] = 0.9;
tanabe2000 0:7822f4172b0c 238 //// if(con.getButton1(6)==0) firePwm[0] = -0.9;
tanabe2000 0:7822f4172b0c 239 //// if(con.getButton1(2) && con.getButton1(6)) firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 240 //// }
tanabe2000 0:7822f4172b0c 241 //// } else {
tanabe2000 0:7822f4172b0c 242 //// sw3flag = 1;
tanabe2000 0:7822f4172b0c 243 //// valveFire1.inputState(0);
tanabe2000 0:7822f4172b0c 244 //// distanceOfset = nowPals;
tanabe2000 0:7822f4172b0c 245 //// }
tanabe2000 0:7822f4172b0c 246 //
tanabe2000 0:7822f4172b0c 247 // if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 248 // led = 1;
tanabe2000 0:7822f4172b0c 249 // valveFire2.inputState(1);
tanabe2000 0:7822f4172b0c 250 // if(con.getButton1(3)==0) {
tanabe2000 0:7822f4172b0c 251 // if(sw3flag == 1) {
tanabe2000 0:7822f4172b0c 252 // fireDistance2 = 810;
tanabe2000 0:7822f4172b0c 253 // sw3flag = 0;
tanabe2000 0:7822f4172b0c 254 // } else firePwm[1] = Output_PID2;
tanabe2000 0:7822f4172b0c 255 // } else if((con.getButton1(0) == 0)) {
tanabe2000 0:7822f4172b0c 256 // // fireDistance1 = 1928;
tanabe2000 0:7822f4172b0c 257 // fireDistance2 = 1847;
tanabe2000 0:7822f4172b0c 258 // firePwm[1] = Output_PID2;
tanabe2000 0:7822f4172b0c 259 // sw3flag = 1;
tanabe2000 0:7822f4172b0c 260 // } else {
tanabe2000 0:7822f4172b0c 261 // if(con.getButton1(2)==0) firePwm[1] = 0.9;
tanabe2000 0:7822f4172b0c 262 // if(con.getButton1(6)==0) firePwm[1] = -0.9;
tanabe2000 0:7822f4172b0c 263 // if(con.getButton1(2) && con.getButton1(6)) firePwm[1] = 0.0;
tanabe2000 0:7822f4172b0c 264 // }
tanabe2000 0:7822f4172b0c 265 // } else {
tanabe2000 0:7822f4172b0c 266 // sw3flag = 1;
tanabe2000 0:7822f4172b0c 267 // valveFire2.inputState(0);
tanabe2000 0:7822f4172b0c 268 // distanceOfset2 = nowPals2;
tanabe2000 0:7822f4172b0c 269 // }
tanabe2000 0:7822f4172b0c 270 //
tanabe2000 0:7822f4172b0c 271 //
tanabe2000 0:7822f4172b0c 272 // //debugpc.printf("Output_PID:%f",Output_PID);
tanabe2000 0:7822f4172b0c 273 // //for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine1(i));
tanabe2000 0:7822f4172b0c 274 //// for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine2(i));
tanabe2000 0:7822f4172b0c 275 //// for(int i = 0; i < 2; i++)debugpc.printf("<%4d>",lmicon.getDistance(i));
tanabe2000 0:7822f4172b0c 276 // for(int i = 0; i < 3; i++)debugpc.printf("<%4d>",lmicon.getEncoder(i));
tanabe2000 0:7822f4172b0c 277 //// debugpc.printf("sw<%2d>",(int)startSW);
tanabe2000 0:7822f4172b0c 278 //
tanabe2000 0:7822f4172b0c 279 // debugpc.printf("\r\n");
tanabe2000 0:7822f4172b0c 280 // fire[0]->setSpeed(firePwm[0]);
tanabe2000 0:7822f4172b0c 281 // fire[1]->setSpeed(firePwm[1]);
tanabe2000 0:7822f4172b0c 282 //// } else for(int i = 0; i < 0; i++) fire[i]->setSpeed(0.0);
tanabe2000 0:7822f4172b0c 283 //}
tanabe2000 0:7822f4172b0c 284
tanabe2000 0:7822f4172b0c 285
tanabe2000 0:7822f4172b0c 286
tanabe2000 0:7822f4172b0c 287 void GakuBot::autoMode1()
tanabe2000 0:7822f4172b0c 288 {
tanabe2000 0:7822f4172b0c 289 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 290 XPid.setSetPoint(getx);
tanabe2000 0:7822f4172b0c 291 XPid.setProcessValue(getposition.getX());
tanabe2000 0:7822f4172b0c 292 YPid.setSetPoint(gety);
tanabe2000 0:7822f4172b0c 293 YPid.setProcessValue(getposition.getY());
tanabe2000 0:7822f4172b0c 294 conposition->targetXY(getx, gety);
tanabe2000 0:7822f4172b0c 295 conposition->compute(getposition.getX(), getposition.getY());
tanabe2000 0:7822f4172b0c 296 targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
tanabe2000 0:7822f4172b0c 297 if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
tanabe2000 0:7822f4172b0c 298 else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
tanabe2000 0:7822f4172b0c 299 if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
tanabe2000 0:7822f4172b0c 300 else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
tanabe2000 0:7822f4172b0c 301
tanabe2000 0:7822f4172b0c 302
tanabe2000 0:7822f4172b0c 303 if((getx - getposition.getX()) >=0 ) {
tanabe2000 0:7822f4172b0c 304 if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 305 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 306 } else {
tanabe2000 0:7822f4172b0c 307 if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 308 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 309 }
tanabe2000 0:7822f4172b0c 310 if((gety - getposition.getY()) >=0) {
tanabe2000 0:7822f4172b0c 311 if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 312 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 313 } else {
tanabe2000 0:7822f4172b0c 314
tanabe2000 0:7822f4172b0c 315 if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 316 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 317 }
tanabe2000 0:7822f4172b0c 318 X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 319 Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 320
tanabe2000 0:7822f4172b0c 321 dt = t.read();
tanabe2000 0:7822f4172b0c 322 // debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
tanabe2000 2:c501f6845500 323 // debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
tanabe2000 0:7822f4172b0c 324 // debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
tanabe2000 0:7822f4172b0c 325 if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
tanabe2000 0:7822f4172b0c 326 if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
tanabe2000 0:7822f4172b0c 327 if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
tanabe2000 0:7822f4172b0c 328 //debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 329
tanabe2000 2:c501f6845500 330 if((int)startSW == 0)start_ = 1;
tanabe2000 0:7822f4172b0c 331
tanabe2000 2:c501f6845500 332 if((start_ == 0)) {
tanabe2000 0:7822f4172b0c 333
tanabe2000 2:c501f6845500 334 getx =0;
tanabe2000 2:c501f6845500 335 gety =0;
tanabe2000 2:c501f6845500 336 }
tanabe2000 0:7822f4172b0c 337 if((mode == 0) && (start_ == 1)) {
tanabe2000 0:7822f4172b0c 338
tanabe2000 0:7822f4172b0c 339 mode++;
tanabe2000 0:7822f4172b0c 340 getx = dash1X;
tanabe2000 0:7822f4172b0c 341 gety = dash1Y;
tanabe2000 0:7822f4172b0c 342 }
tanabe2000 0:7822f4172b0c 343 if(mode == 1 && getposition.getX() >= dash1X && getposition.getY() <= dash1Y) {
tanabe2000 0:7822f4172b0c 344
tanabe2000 0:7822f4172b0c 345 mode++;
tanabe2000 0:7822f4172b0c 346 getx = dash2X;
tanabe2000 0:7822f4172b0c 347 gety = dash2Y;
tanabe2000 0:7822f4172b0c 348 }
tanabe2000 0:7822f4172b0c 349 if(mode == 2 && getposition.getX() >= dash2X && getposition.getY() <= dash2Y) {
tanabe2000 0:7822f4172b0c 350
tanabe2000 0:7822f4172b0c 351 mode++;
tanabe2000 0:7822f4172b0c 352 getx = dash3X;
tanabe2000 0:7822f4172b0c 353 gety = dash3Y;
tanabe2000 0:7822f4172b0c 354 }
tanabe2000 2:c501f6845500 355 if(mode == 3 && getposition.getX() >= dash3X-400 && getposition.getY() <= dash3Y+400) {
tanabe2000 2:c501f6845500 356 if(firecount > 2 && (dt >= 1.0)) {
tanabe2000 2:c501f6845500 357 mode++;
tanabe2000 2:c501f6845500 358 }
tanabe2000 2:c501f6845500 359
tanabe2000 0:7822f4172b0c 360 if(airFlag == 1 && firecount <= 2) {
tanabe2000 0:7822f4172b0c 361 led = 1;
tanabe2000 0:7822f4172b0c 362 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 363
tanabe2000 0:7822f4172b0c 364
tanabe2000 2:c501f6845500 365
tanabe2000 0:7822f4172b0c 366 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 367 pidT.reset();
tanabe2000 0:7822f4172b0c 368 dpetbotle = 1;
tanabe2000 2:c501f6845500 369
tanabe2000 0:7822f4172b0c 370 }
tanabe2000 0:7822f4172b0c 371 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 372 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 373 loadingmode = 3;
tanabe2000 0:7822f4172b0c 374 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 375
tanabe2000 2:c501f6845500 376 }
tanabe2000 0:7822f4172b0c 377 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 378 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 379 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 380 fireDistance1 = 1120;
tanabe2000 0:7822f4172b0c 381 swflag = 0;
tanabe2000 0:7822f4172b0c 382 pidT.reset();
tanabe2000 0:7822f4172b0c 383 }
tanabe2000 0:7822f4172b0c 384 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 385 loadingmode = 0;
tanabe2000 0:7822f4172b0c 386 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 387 }
tanabe2000 2:c501f6845500 388
tanabe2000 0:7822f4172b0c 389 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 390 }
tanabe2000 2:c501f6845500 391
tanabe2000 0:7822f4172b0c 392
tanabe2000 0:7822f4172b0c 393 } else {
tanabe2000 2:c501f6845500 394 if(swflag == 0) {
tanabe2000 2:c501f6845500 395 pidT.reset();
tanabe2000 0:7822f4172b0c 396 swflag = 1;
tanabe2000 0:7822f4172b0c 397 firecount++;
tanabe2000 2:c501f6845500 398 }
tanabe2000 2:c501f6845500 399 piddt = pidT.read();
tanabe2000 2:c501f6845500 400 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 401 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 402 led = 0;
tanabe2000 2:c501f6845500 403
tanabe2000 0:7822f4172b0c 404 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 405 loadingmode = 0;
tanabe2000 0:7822f4172b0c 406 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 407
tanabe2000 0:7822f4172b0c 408 }
tanabe2000 0:7822f4172b0c 409
tanabe2000 0:7822f4172b0c 410 // mode++;
tanabe2000 0:7822f4172b0c 411 }
tanabe2000 0:7822f4172b0c 412 if(mode == 4 && kinectFlag == 1) {
tanabe2000 2:c501f6845500 413 if(getposition.getX() > table1800X) {
tanabe2000 0:7822f4172b0c 414
tanabe2000 0:7822f4172b0c 415 mode++;
tanabe2000 0:7822f4172b0c 416 getx = table1800X;
tanabe2000 0:7822f4172b0c 417 gety = -1*suzuki[0]-1000;
tanabe2000 0:7822f4172b0c 418 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 419 swflag = 1;
tanabe2000 2:c501f6845500 420 airFlag = 1;
tanabe2000 0:7822f4172b0c 421 } else {
tanabe2000 0:7822f4172b0c 422 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 423 omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 424 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 425 omni.computeXY(wallslide, -0.05, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 426 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 2:c501f6845500 427 omni.computeXY(wallslide, -0.05, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 428 } else {
tanabe2000 2:c501f6845500 429 omni.computeXY(wallslide, -0.15, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 430 }
tanabe2000 0:7822f4172b0c 431 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 432 }
tanabe2000 0:7822f4172b0c 433 }
tanabe2000 0:7822f4172b0c 434
tanabe2000 0:7822f4172b0c 435 if(mode == 5) {
tanabe2000 2:c501f6845500 436 if(kinectmode == 3) {
tanabe2000 2:c501f6845500 437 mode++;
tanabe2000 2:c501f6845500 438 } else {
tanabe2000 0:7822f4172b0c 439 if(airFlag == 1) {
tanabe2000 2:c501f6845500 440 led = 1;
tanabe2000 2:c501f6845500 441 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 442
tanabe2000 2:c501f6845500 443
tanabe2000 0:7822f4172b0c 444
tanabe2000 2:c501f6845500 445 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 446 pidT.reset();
tanabe2000 2:c501f6845500 447 dpetbotle = 1;
tanabe2000 2:c501f6845500 448
tanabe2000 2:c501f6845500 449 }
tanabe2000 2:c501f6845500 450 piddt = pidT.read();
tanabe2000 2:c501f6845500 451 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 452 loadingmode = 3;
tanabe2000 2:c501f6845500 453 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 454
tanabe2000 0:7822f4172b0c 455 }
tanabe2000 2:c501f6845500 456 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 457 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 458 if(swflag == 1) {
tanabe2000 2:c501f6845500 459 fireDistance1 = 1010;
tanabe2000 2:c501f6845500 460 swflag = 0;
tanabe2000 2:c501f6845500 461 pidT.reset();
tanabe2000 2:c501f6845500 462 }
tanabe2000 2:c501f6845500 463 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 464 loadingmode = 0;
tanabe2000 2:c501f6845500 465 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 466 }
tanabe2000 2:c501f6845500 467
tanabe2000 2:c501f6845500 468 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 469 }
tanabe2000 2:c501f6845500 470
tanabe2000 0:7822f4172b0c 471
tanabe2000 2:c501f6845500 472 } else {
tanabe2000 2:c501f6845500 473 if(swflag == 0) {
tanabe2000 2:c501f6845500 474 pidT.reset();
tanabe2000 2:c501f6845500 475 swflag = 1;
tanabe2000 2:c501f6845500 476 firecount++;
tanabe2000 0:7822f4172b0c 477 }
tanabe2000 0:7822f4172b0c 478 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 479 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 480 dpetbotle = 0;
tanabe2000 2:c501f6845500 481 led = 0;
tanabe2000 2:c501f6845500 482
tanabe2000 2:c501f6845500 483 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 484 loadingmode = 0;
tanabe2000 2:c501f6845500 485 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 486
tanabe2000 2:c501f6845500 487 }
tanabe2000 0:7822f4172b0c 488 }
tanabe2000 0:7822f4172b0c 489 //if(((int)startSW == 0) && (dt >= 0.5)) {
tanabe2000 0:7822f4172b0c 490 // mode++;
tanabe2000 0:7822f4172b0c 491 // t.reset();
tanabe2000 0:7822f4172b0c 492 // }
tanabe2000 0:7822f4172b0c 493
tanabe2000 2:c501f6845500 494 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 495
tanabe2000 2:c501f6845500 496 }
tanabe2000 0:7822f4172b0c 497
tanabe2000 2:c501f6845500 498 if(mode == 6 && (-1*suzuki[1]-1000 < wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 499 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 500 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 501 if(getposition.getX() < table1500X + 400) {
tanabe2000 0:7822f4172b0c 502
tanabe2000 0:7822f4172b0c 503 mode++;
tanabe2000 0:7822f4172b0c 504 getx = table1500X;
tanabe2000 0:7822f4172b0c 505 gety = -1*suzuki[1]-1000;
tanabe2000 2:c501f6845500 506 kabeFlag = 0;
tanabe2000 2:c501f6845500 507 swflag = 1;
tanabe2000 2:c501f6845500 508 airFlag = 1;
tanabe2000 0:7822f4172b0c 509 } else {
tanabe2000 0:7822f4172b0c 510 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 511 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 512 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 513 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 514 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 515 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 516 } else {
tanabe2000 0:7822f4172b0c 517 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 518 }
tanabe2000 0:7822f4172b0c 519 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 520 }
tanabe2000 0:7822f4172b0c 521 } else {
tanabe2000 0:7822f4172b0c 522 gety = wallmodeY;
tanabe2000 0:7822f4172b0c 523 }
tanabe2000 0:7822f4172b0c 524 }
tanabe2000 0:7822f4172b0c 525 if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 2:c501f6845500 526 swflag = 1;
tanabe2000 2:c501f6845500 527 airFlag = 1;
tanabe2000 0:7822f4172b0c 528 if(suzuki[0] < suzuki[1]) {
tanabe2000 0:7822f4172b0c 529 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 530 if(getposition.getY() < -1*suzuki[1]-1000) {
tanabe2000 0:7822f4172b0c 531 mode++;
tanabe2000 0:7822f4172b0c 532 getx = table1500X;
tanabe2000 0:7822f4172b0c 533 }
tanabe2000 0:7822f4172b0c 534 } else {
tanabe2000 0:7822f4172b0c 535 getx = table1500X;
tanabe2000 0:7822f4172b0c 536 if(getposition.getX() < table1500X ) {
tanabe2000 0:7822f4172b0c 537 mode++;
tanabe2000 0:7822f4172b0c 538 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 539 }
tanabe2000 0:7822f4172b0c 540 }
tanabe2000 0:7822f4172b0c 541
tanabe2000 0:7822f4172b0c 542 }
tanabe2000 0:7822f4172b0c 543 if(mode == 7) {
tanabe2000 2:c501f6845500 544 if(kinectmode == 5) {
tanabe2000 2:c501f6845500 545 mode++;
tanabe2000 2:c501f6845500 546 } else {
tanabe2000 2:c501f6845500 547 if(airFlag == 1) {
tanabe2000 2:c501f6845500 548 led = 1;
tanabe2000 2:c501f6845500 549 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 550
tanabe2000 2:c501f6845500 551
tanabe2000 0:7822f4172b0c 552
tanabe2000 2:c501f6845500 553 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 554 pidT.reset();
tanabe2000 2:c501f6845500 555 dpetbotle = 1;
tanabe2000 2:c501f6845500 556
tanabe2000 2:c501f6845500 557 }
tanabe2000 2:c501f6845500 558 piddt = pidT.read();
tanabe2000 2:c501f6845500 559 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 560 loadingmode = 3;
tanabe2000 2:c501f6845500 561 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 562
tanabe2000 0:7822f4172b0c 563 }
tanabe2000 2:c501f6845500 564 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 565 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 566 if(swflag == 1) {
tanabe2000 2:c501f6845500 567 fireDistance1 = 820;
tanabe2000 2:c501f6845500 568 swflag = 0;
tanabe2000 2:c501f6845500 569 pidT.reset();
tanabe2000 2:c501f6845500 570 }
tanabe2000 2:c501f6845500 571 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 572 loadingmode = 0;
tanabe2000 2:c501f6845500 573 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 574 }
tanabe2000 2:c501f6845500 575
tanabe2000 2:c501f6845500 576 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 577 }
tanabe2000 2:c501f6845500 578
tanabe2000 0:7822f4172b0c 579
tanabe2000 2:c501f6845500 580 } else {
tanabe2000 2:c501f6845500 581 if(swflag == 0) {
tanabe2000 2:c501f6845500 582 pidT.reset();
tanabe2000 2:c501f6845500 583 swflag = 1;
tanabe2000 2:c501f6845500 584 firecount++;
tanabe2000 0:7822f4172b0c 585 }
tanabe2000 0:7822f4172b0c 586 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 587 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 588 dpetbotle = 0;
tanabe2000 2:c501f6845500 589 led = 0;
tanabe2000 2:c501f6845500 590
tanabe2000 2:c501f6845500 591 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 592 loadingmode = 0;
tanabe2000 2:c501f6845500 593 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 594
tanabe2000 2:c501f6845500 595 }
tanabe2000 2:c501f6845500 596
tanabe2000 2:c501f6845500 597
tanabe2000 2:c501f6845500 598 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 599 // mode++;
tanabe2000 0:7822f4172b0c 600 // }
tanabe2000 0:7822f4172b0c 601
tanabe2000 2:c501f6845500 602 /*発射機構動く*/
tanabe2000 2:c501f6845500 603 }
tanabe2000 2:c501f6845500 604 }
tanabe2000 2:c501f6845500 605 if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
tanabe2000 0:7822f4172b0c 606 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 2:c501f6845500 607 swflag = 1;
tanabe2000 2:c501f6845500 608 airFlag = 1;
tanabe2000 0:7822f4172b0c 609 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 610 if(getposition.getX() < table1200X + 200) {
tanabe2000 0:7822f4172b0c 611
tanabe2000 0:7822f4172b0c 612 mode++;
tanabe2000 0:7822f4172b0c 613 getx = table1200X;
tanabe2000 0:7822f4172b0c 614 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 615 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 616 } else {
tanabe2000 0:7822f4172b0c 617
tanabe2000 0:7822f4172b0c 618 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 619 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 620 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 621 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 622 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 2:c501f6845500 623 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 624 } else {
tanabe2000 0:7822f4172b0c 625 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 626 }
tanabe2000 0:7822f4172b0c 627 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 628 }
tanabe2000 0:7822f4172b0c 629 } else {
tanabe2000 0:7822f4172b0c 630 gety = walldashY;
tanabe2000 0:7822f4172b0c 631 }
tanabe2000 0:7822f4172b0c 632 }
tanabe2000 0:7822f4172b0c 633 if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
tanabe2000 2:c501f6845500 634 swflag = 1;
tanabe2000 2:c501f6845500 635 airFlag = 1;
tanabe2000 0:7822f4172b0c 636 if(suzuki[1] < suzuki[2]) {
tanabe2000 0:7822f4172b0c 637 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 638 if(getposition.getY() < -1*suzuki[2]-800) {
tanabe2000 0:7822f4172b0c 639 mode++;
tanabe2000 0:7822f4172b0c 640 getx = table1200X;
tanabe2000 0:7822f4172b0c 641 }
tanabe2000 0:7822f4172b0c 642 } else {
tanabe2000 0:7822f4172b0c 643 getx = table1200X;
tanabe2000 0:7822f4172b0c 644 if(getposition.getX() < table1200X) {
tanabe2000 0:7822f4172b0c 645 mode++;
tanabe2000 0:7822f4172b0c 646 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 647 }
tanabe2000 0:7822f4172b0c 648 }
tanabe2000 0:7822f4172b0c 649
tanabe2000 0:7822f4172b0c 650 }
tanabe2000 0:7822f4172b0c 651
tanabe2000 0:7822f4172b0c 652
tanabe2000 0:7822f4172b0c 653 if(mode == 9) {
tanabe2000 2:c501f6845500 654 if(kinectmode == 7) {
tanabe2000 0:7822f4172b0c 655
tanabe2000 0:7822f4172b0c 656 } else {
tanabe2000 2:c501f6845500 657
tanabe2000 2:c501f6845500 658 if(airFlag == 1) {
tanabe2000 2:c501f6845500 659 led = 1;
tanabe2000 2:c501f6845500 660 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 661
tanabe2000 2:c501f6845500 662
tanabe2000 2:c501f6845500 663
tanabe2000 2:c501f6845500 664 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 665 pidT.reset();
tanabe2000 2:c501f6845500 666 dpetbotle = 1;
tanabe2000 2:c501f6845500 667
tanabe2000 2:c501f6845500 668 }
tanabe2000 2:c501f6845500 669 piddt = pidT.read();
tanabe2000 2:c501f6845500 670 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 671 loadingmode = 3;
tanabe2000 2:c501f6845500 672 firePwm[0] = 0.0;
tanabe2000 2:c501f6845500 673
tanabe2000 2:c501f6845500 674 }
tanabe2000 2:c501f6845500 675 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 676 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 677 if(swflag == 1) {
tanabe2000 2:c501f6845500 678 fireDistance1 = 870;
tanabe2000 2:c501f6845500 679 swflag = 0;
tanabe2000 2:c501f6845500 680 pidT.reset();
tanabe2000 2:c501f6845500 681 }
tanabe2000 2:c501f6845500 682 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 683 loadingmode = 0;
tanabe2000 2:c501f6845500 684 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 685 }
tanabe2000 2:c501f6845500 686
tanabe2000 2:c501f6845500 687 firePwm[0] = Output_PID;
tanabe2000 2:c501f6845500 688 }
tanabe2000 2:c501f6845500 689
tanabe2000 2:c501f6845500 690
tanabe2000 2:c501f6845500 691 } else {
tanabe2000 2:c501f6845500 692 if(swflag == 0) {
tanabe2000 2:c501f6845500 693 pidT.reset();
tanabe2000 2:c501f6845500 694 swflag = 1;
tanabe2000 2:c501f6845500 695 firecount++;
tanabe2000 0:7822f4172b0c 696 }
tanabe2000 0:7822f4172b0c 697 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 698 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 699 dpetbotle = 0;
tanabe2000 2:c501f6845500 700 led = 0;
tanabe2000 2:c501f6845500 701
tanabe2000 2:c501f6845500 702 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 703 loadingmode = 0;
tanabe2000 2:c501f6845500 704 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 705
tanabe2000 2:c501f6845500 706 }
tanabe2000 2:c501f6845500 707
tanabe2000 2:c501f6845500 708
tanabe2000 2:c501f6845500 709 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 710 // mode++;
tanabe2000 0:7822f4172b0c 711 // }
tanabe2000 0:7822f4172b0c 712
tanabe2000 2:c501f6845500 713 /*発射機構動く*/
tanabe2000 2:c501f6845500 714 }
tanabe2000 0:7822f4172b0c 715 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 716 // mode++;
tanabe2000 0:7822f4172b0c 717 // }
tanabe2000 0:7822f4172b0c 718
tanabe2000 0:7822f4172b0c 719 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 720 }
tanabe2000 0:7822f4172b0c 721
tanabe2000 0:7822f4172b0c 722
tanabe2000 0:7822f4172b0c 723
tanabe2000 0:7822f4172b0c 724
tanabe2000 0:7822f4172b0c 725 // if(con.getButton2(1)==0) {
tanabe2000 2:c501f6845500 726 if(kabeFlag == 1) {
tanabe2000 2:c501f6845500 727 for (int i = 0; i < 4; i++) {
tanabe2000 2:c501f6845500 728 speed[i] = omni.wheel[i];
tanabe2000 2:c501f6845500 729 wheels[i]->setSpeed(speed[i]);
tanabe2000 2:c501f6845500 730 }
tanabe2000 2:c501f6845500 731 } else {
tanabe2000 2:c501f6845500 732 for (int i = 0; i < 4; i++) {
tanabe2000 2:c501f6845500 733 omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 2:c501f6845500 734 speed[i] = omni.wheel[i];
tanabe2000 2:c501f6845500 735 wheels[i]->setSpeed(speed[i]);
tanabe2000 2:c501f6845500 736 }
tanabe2000 0:7822f4172b0c 737
tanabe2000 0:7822f4172b0c 738
tanabe2000 0:7822f4172b0c 739 }
tanabe2000 0:7822f4172b0c 740 for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
tanabe2000 0:7822f4172b0c 741 valveFire1.inputState(solenoidValve1is);
tanabe2000 0:7822f4172b0c 742 valveFire2.inputState(solenoidValve2is);
tanabe2000 0:7822f4172b0c 743 loading.petbottlemode(loadingmode);
tanabe2000 0:7822f4172b0c 744
tanabe2000 0:7822f4172b0c 745
tanabe2000 0:7822f4172b0c 746 // if(mode > 37) mode = 0;
tanabe2000 0:7822f4172b0c 747 // debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
tanabe2000 2:c501f6845500 748 // debugpc.printf("\r\n");
tanabe2000 0:7822f4172b0c 749 }
tanabe2000 2:c501f6845500 750
tanabe2000 2:c501f6845500 751
tanabe2000 2:c501f6845500 752 /******************************************************************************************************************************/
tanabe2000 2:c501f6845500 753 void GakuBot::autoMode2()//青ゾーン
tanabe2000 0:7822f4172b0c 754 {
tanabe2000 0:7822f4172b0c 755 piddt = pidT.read();
tanabe2000 2:c501f6845500 756 XPid.setSetPoint(getx);
tanabe2000 0:7822f4172b0c 757 XPid.setProcessValue(getposition.getX());
tanabe2000 2:c501f6845500 758 YPid.setSetPoint(gety);
tanabe2000 0:7822f4172b0c 759 YPid.setProcessValue(getposition.getY());
tanabe2000 0:7822f4172b0c 760 conposition->compute(getposition.getX(), getposition.getY());
tanabe2000 0:7822f4172b0c 761 targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
tanabe2000 0:7822f4172b0c 762 if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
tanabe2000 0:7822f4172b0c 763 else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
tanabe2000 0:7822f4172b0c 764 if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
tanabe2000 0:7822f4172b0c 765 else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
tanabe2000 0:7822f4172b0c 766
tanabe2000 0:7822f4172b0c 767
tanabe2000 0:7822f4172b0c 768 if((getx - getposition.getX()) >=0 ) {
tanabe2000 0:7822f4172b0c 769 if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 770 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 771 } else {
tanabe2000 0:7822f4172b0c 772 if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 773 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 774 }
tanabe2000 0:7822f4172b0c 775 if((gety - getposition.getY()) >=0) {
tanabe2000 0:7822f4172b0c 776 if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 777 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 778 } else {
tanabe2000 0:7822f4172b0c 779
tanabe2000 0:7822f4172b0c 780 if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 781 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 782 }
tanabe2000 0:7822f4172b0c 783 X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 784 Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 785
tanabe2000 0:7822f4172b0c 786 dt = t.read();
tanabe2000 0:7822f4172b0c 787 // debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
tanabe2000 0:7822f4172b0c 788 debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
tanabe2000 0:7822f4172b0c 789 // debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
tanabe2000 0:7822f4172b0c 790 if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
tanabe2000 0:7822f4172b0c 791 if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
tanabe2000 0:7822f4172b0c 792 if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
tanabe2000 0:7822f4172b0c 793 debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 794
tanabe2000 0:7822f4172b0c 795 if((int)startSW == 0)start_ = 1;
tanabe2000 2:c501f6845500 796 if((start_ == 0)) {
tanabe2000 0:7822f4172b0c 797
tanabe2000 2:c501f6845500 798 getx =0;
tanabe2000 2:c501f6845500 799 gety =0;
tanabe2000 2:c501f6845500 800 }
tanabe2000 0:7822f4172b0c 801 if((mode == 0) && (start_ == 1)) {
tanabe2000 0:7822f4172b0c 802
tanabe2000 0:7822f4172b0c 803 mode++;
tanabe2000 2:c501f6845500 804 getx = -1.0*dash1X;
tanabe2000 0:7822f4172b0c 805 gety = dash1Y;
tanabe2000 0:7822f4172b0c 806 }
tanabe2000 2:c501f6845500 807 if(mode == 1 && getposition.getX() <= -1.0*dash1X+200 && getposition.getY() <= dash1Y) {
tanabe2000 0:7822f4172b0c 808
tanabe2000 0:7822f4172b0c 809 mode++;
tanabe2000 2:c501f6845500 810 getx = -1.0*dash2X;
tanabe2000 0:7822f4172b0c 811 gety = dash2Y;
tanabe2000 0:7822f4172b0c 812 }
tanabe2000 2:c501f6845500 813 if(mode == 2 && getposition.getX() <= -1.0*dash2X+200 && getposition.getY() <= dash2Y) {
tanabe2000 0:7822f4172b0c 814
tanabe2000 0:7822f4172b0c 815 mode++;
tanabe2000 2:c501f6845500 816 getx = -1.0*dash3X;
tanabe2000 0:7822f4172b0c 817 gety = dash3Y;
tanabe2000 0:7822f4172b0c 818 }
tanabe2000 2:c501f6845500 819 if(mode == 3 && getposition.getX() <= -1.0*dash3X+200 && getposition.getY() <= dash3Y+200) {
tanabe2000 2:c501f6845500 820 if(firecount > 2 && (dt >= 1.0)) {
tanabe2000 2:c501f6845500 821 mode++;
tanabe2000 2:c501f6845500 822 }
tanabe2000 2:c501f6845500 823
tanabe2000 0:7822f4172b0c 824 if(airFlag == 1 && firecount <= 2) {
tanabe2000 0:7822f4172b0c 825 led = 1;
tanabe2000 0:7822f4172b0c 826 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 827
tanabe2000 0:7822f4172b0c 828
tanabe2000 2:c501f6845500 829
tanabe2000 0:7822f4172b0c 830 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 831 pidT.reset();
tanabe2000 0:7822f4172b0c 832 dpetbotle = 1;
tanabe2000 2:c501f6845500 833
tanabe2000 0:7822f4172b0c 834 }
tanabe2000 0:7822f4172b0c 835 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 836 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 837 loadingmode = 3;
tanabe2000 0:7822f4172b0c 838 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 839
tanabe2000 0:7822f4172b0c 840 }
tanabe2000 0:7822f4172b0c 841 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 842 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 843 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 844 fireDistance1 = 1120;
tanabe2000 0:7822f4172b0c 845 swflag = 0;
tanabe2000 0:7822f4172b0c 846 pidT.reset();
tanabe2000 0:7822f4172b0c 847 }
tanabe2000 0:7822f4172b0c 848 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 849 loadingmode = 0;
tanabe2000 0:7822f4172b0c 850 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 851 }
tanabe2000 2:c501f6845500 852
tanabe2000 0:7822f4172b0c 853 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 854 }
tanabe2000 2:c501f6845500 855
tanabe2000 0:7822f4172b0c 856
tanabe2000 0:7822f4172b0c 857 } else {
tanabe2000 2:c501f6845500 858 if(swflag == 0) {
tanabe2000 2:c501f6845500 859 pidT.reset();
tanabe2000 0:7822f4172b0c 860 swflag = 1;
tanabe2000 0:7822f4172b0c 861 firecount++;
tanabe2000 2:c501f6845500 862 }
tanabe2000 2:c501f6845500 863 piddt = pidT.read();
tanabe2000 2:c501f6845500 864 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 865 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 866 led = 0;
tanabe2000 2:c501f6845500 867
tanabe2000 0:7822f4172b0c 868 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 869 loadingmode = 0;
tanabe2000 0:7822f4172b0c 870 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 871
tanabe2000 0:7822f4172b0c 872 }
tanabe2000 0:7822f4172b0c 873
tanabe2000 0:7822f4172b0c 874 // mode++;
tanabe2000 0:7822f4172b0c 875 }
tanabe2000 0:7822f4172b0c 876 if(mode == 4 && kinectFlag == 1) {
tanabe2000 2:c501f6845500 877 if(getposition.getX() < -1.0*table1800X) {
tanabe2000 0:7822f4172b0c 878
tanabe2000 0:7822f4172b0c 879 mode++;
tanabe2000 2:c501f6845500 880 getx = -1.0*table1800X;
tanabe2000 0:7822f4172b0c 881 gety = -1*suzuki[0]-1000;
tanabe2000 0:7822f4172b0c 882 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 883 swflag = 1;
tanabe2000 2:c501f6845500 884 airFlag = 1;
tanabe2000 0:7822f4172b0c 885 } else {
tanabe2000 0:7822f4172b0c 886 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 887 omni.computeXY(-wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 888 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 889 omni.computeXY(-wallslide, -0.05, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 890 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 2:c501f6845500 891 omni.computeXY(-wallslide, -0.05, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 892 } else {
tanabe2000 2:c501f6845500 893 omni.computeXY(-wallslide, -0.15, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 894 }
tanabe2000 0:7822f4172b0c 895 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 896 }
tanabe2000 0:7822f4172b0c 897 }
tanabe2000 0:7822f4172b0c 898
tanabe2000 0:7822f4172b0c 899 if(mode == 5) {
tanabe2000 2:c501f6845500 900 if(kinectmode == 3) {
tanabe2000 2:c501f6845500 901 mode++;
tanabe2000 2:c501f6845500 902 } else {
tanabe2000 0:7822f4172b0c 903 if(airFlag == 1) {
tanabe2000 2:c501f6845500 904 led = 1;
tanabe2000 2:c501f6845500 905 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 906
tanabe2000 2:c501f6845500 907
tanabe2000 0:7822f4172b0c 908
tanabe2000 2:c501f6845500 909 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 910 pidT.reset();
tanabe2000 2:c501f6845500 911 dpetbotle = 1;
tanabe2000 2:c501f6845500 912
tanabe2000 2:c501f6845500 913 }
tanabe2000 2:c501f6845500 914 piddt = pidT.read();
tanabe2000 2:c501f6845500 915 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 916 loadingmode = 3;
tanabe2000 2:c501f6845500 917 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 918
tanabe2000 0:7822f4172b0c 919 }
tanabe2000 2:c501f6845500 920 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 921 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 922 if(swflag == 1) {
tanabe2000 2:c501f6845500 923 fireDistance1 = 1010;
tanabe2000 2:c501f6845500 924 swflag = 0;
tanabe2000 2:c501f6845500 925 pidT.reset();
tanabe2000 2:c501f6845500 926 }
tanabe2000 2:c501f6845500 927 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 928 loadingmode = 0;
tanabe2000 2:c501f6845500 929 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 930 }
tanabe2000 2:c501f6845500 931
tanabe2000 2:c501f6845500 932 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 933 }
tanabe2000 2:c501f6845500 934
tanabe2000 0:7822f4172b0c 935
tanabe2000 2:c501f6845500 936 } else {
tanabe2000 2:c501f6845500 937 if(swflag == 0) {
tanabe2000 2:c501f6845500 938 pidT.reset();
tanabe2000 2:c501f6845500 939 swflag = 1;
tanabe2000 2:c501f6845500 940 firecount++;
tanabe2000 0:7822f4172b0c 941 }
tanabe2000 0:7822f4172b0c 942 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 943 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 944 dpetbotle = 0;
tanabe2000 2:c501f6845500 945 led = 0;
tanabe2000 2:c501f6845500 946
tanabe2000 2:c501f6845500 947 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 948 loadingmode = 0;
tanabe2000 2:c501f6845500 949 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 950
tanabe2000 2:c501f6845500 951 }
tanabe2000 0:7822f4172b0c 952 }
tanabe2000 0:7822f4172b0c 953 //if(((int)startSW == 0) && (dt >= 0.5)) {
tanabe2000 0:7822f4172b0c 954 // mode++;
tanabe2000 0:7822f4172b0c 955 // t.reset();
tanabe2000 0:7822f4172b0c 956 // }
tanabe2000 0:7822f4172b0c 957
tanabe2000 2:c501f6845500 958 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 959
tanabe2000 2:c501f6845500 960 }
tanabe2000 0:7822f4172b0c 961
tanabe2000 2:c501f6845500 962 if(mode == 6 && (-1*suzuki[1]+1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 963 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 964 if(getposition.getY() < walldashY) {
tanabe2000 2:c501f6845500 965 if(getposition.getX() < -1.0*table1500X + 200) {
tanabe2000 0:7822f4172b0c 966
tanabe2000 0:7822f4172b0c 967 mode++;
tanabe2000 2:c501f6845500 968 getx = -1.0*table1500X;
tanabe2000 0:7822f4172b0c 969 gety = -1*suzuki[1]-1000;
tanabe2000 2:c501f6845500 970 kabeFlag = 0;
tanabe2000 2:c501f6845500 971 swflag = 1;
tanabe2000 2:c501f6845500 972 airFlag = 1;
tanabe2000 0:7822f4172b0c 973 } else {
tanabe2000 0:7822f4172b0c 974 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 975 omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 976 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 977 omni.computeXY(wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 978 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 2:c501f6845500 979 omni.computeXY(wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 980 } else {
tanabe2000 0:7822f4172b0c 981 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 982 }
tanabe2000 0:7822f4172b0c 983 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 984 }
tanabe2000 0:7822f4172b0c 985 } else {
tanabe2000 0:7822f4172b0c 986 gety = wallmodeY;
tanabe2000 0:7822f4172b0c 987 }
tanabe2000 0:7822f4172b0c 988 }
tanabe2000 0:7822f4172b0c 989 if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 2:c501f6845500 990 swflag = 1;
tanabe2000 2:c501f6845500 991 airFlag = 1;
tanabe2000 0:7822f4172b0c 992 if(suzuki[0] < suzuki[1]) {
tanabe2000 0:7822f4172b0c 993 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 994 if(getposition.getY() < -1*suzuki[1]-1000) {
tanabe2000 0:7822f4172b0c 995 mode++;
tanabe2000 2:c501f6845500 996 getx = -1.0*table1500X;
tanabe2000 0:7822f4172b0c 997 }
tanabe2000 0:7822f4172b0c 998 } else {
tanabe2000 2:c501f6845500 999 getx = -1.0*table1500X;
tanabe2000 2:c501f6845500 1000 if(getposition.getX() < -1.0*table1500X ) {
tanabe2000 0:7822f4172b0c 1001 mode++;
tanabe2000 0:7822f4172b0c 1002 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 1003 }
tanabe2000 0:7822f4172b0c 1004 }
tanabe2000 0:7822f4172b0c 1005
tanabe2000 0:7822f4172b0c 1006 }
tanabe2000 0:7822f4172b0c 1007 if(mode == 7) {
tanabe2000 2:c501f6845500 1008 if(kinectmode == 5) {
tanabe2000 2:c501f6845500 1009 mode++;
tanabe2000 2:c501f6845500 1010 } else {
tanabe2000 2:c501f6845500 1011 if(airFlag == 1) {
tanabe2000 2:c501f6845500 1012 led = 1;
tanabe2000 2:c501f6845500 1013 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 1014
tanabe2000 2:c501f6845500 1015
tanabe2000 0:7822f4172b0c 1016
tanabe2000 2:c501f6845500 1017 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 1018 pidT.reset();
tanabe2000 2:c501f6845500 1019 dpetbotle = 1;
tanabe2000 2:c501f6845500 1020
tanabe2000 2:c501f6845500 1021 }
tanabe2000 2:c501f6845500 1022 piddt = pidT.read();
tanabe2000 2:c501f6845500 1023 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 1024 loadingmode = 3;
tanabe2000 2:c501f6845500 1025 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 1026
tanabe2000 0:7822f4172b0c 1027 }
tanabe2000 2:c501f6845500 1028 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 1029 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 1030 if(swflag == 1) {
tanabe2000 2:c501f6845500 1031 fireDistance1 = 820;
tanabe2000 2:c501f6845500 1032 swflag = 0;
tanabe2000 2:c501f6845500 1033 pidT.reset();
tanabe2000 2:c501f6845500 1034 }
tanabe2000 2:c501f6845500 1035 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 1036 loadingmode = 0;
tanabe2000 2:c501f6845500 1037 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 1038 }
tanabe2000 2:c501f6845500 1039
tanabe2000 2:c501f6845500 1040 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 1041 }
tanabe2000 2:c501f6845500 1042
tanabe2000 0:7822f4172b0c 1043
tanabe2000 2:c501f6845500 1044 } else {
tanabe2000 2:c501f6845500 1045 if(swflag == 0) {
tanabe2000 2:c501f6845500 1046 pidT.reset();
tanabe2000 2:c501f6845500 1047 swflag = 1;
tanabe2000 2:c501f6845500 1048 firecount++;
tanabe2000 0:7822f4172b0c 1049 }
tanabe2000 0:7822f4172b0c 1050 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1051 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 1052 dpetbotle = 0;
tanabe2000 2:c501f6845500 1053 led = 0;
tanabe2000 2:c501f6845500 1054
tanabe2000 2:c501f6845500 1055 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 1056 loadingmode = 0;
tanabe2000 2:c501f6845500 1057 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 1058
tanabe2000 2:c501f6845500 1059 }
tanabe2000 2:c501f6845500 1060
tanabe2000 2:c501f6845500 1061
tanabe2000 2:c501f6845500 1062 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1063 // mode++;
tanabe2000 0:7822f4172b0c 1064 // }
tanabe2000 0:7822f4172b0c 1065
tanabe2000 2:c501f6845500 1066 /*発射機構動く*/
tanabe2000 2:c501f6845500 1067 }
tanabe2000 2:c501f6845500 1068 }
tanabe2000 2:c501f6845500 1069 if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
tanabe2000 0:7822f4172b0c 1070 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 2:c501f6845500 1071 swflag = 1;
tanabe2000 2:c501f6845500 1072 airFlag = 1;
tanabe2000 0:7822f4172b0c 1073 if(getposition.getY() < walldashY) {
tanabe2000 2:c501f6845500 1074 if(getposition.getX() < -1.0*table1200X) {
tanabe2000 0:7822f4172b0c 1075
tanabe2000 0:7822f4172b0c 1076 mode++;
tanabe2000 2:c501f6845500 1077 getx = -1.0*table1200X;
tanabe2000 0:7822f4172b0c 1078 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1079 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 1080 } else {
tanabe2000 0:7822f4172b0c 1081
tanabe2000 0:7822f4172b0c 1082 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 1083 omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1084 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 2:c501f6845500 1085 omni.computeXY(wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 1086 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 2:c501f6845500 1087 omni.computeXY(wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 1088 } else {
tanabe2000 0:7822f4172b0c 1089 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1090 }
tanabe2000 0:7822f4172b0c 1091 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 1092 }
tanabe2000 0:7822f4172b0c 1093 } else {
tanabe2000 0:7822f4172b0c 1094 gety = walldashY;
tanabe2000 0:7822f4172b0c 1095 }
tanabe2000 0:7822f4172b0c 1096 }
tanabe2000 0:7822f4172b0c 1097 if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
tanabe2000 2:c501f6845500 1098 swflag = 1;
tanabe2000 2:c501f6845500 1099 airFlag = 1;
tanabe2000 0:7822f4172b0c 1100 if(suzuki[1] < suzuki[2]) {
tanabe2000 0:7822f4172b0c 1101 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1102 if(getposition.getY() < -1*suzuki[2]-800) {
tanabe2000 0:7822f4172b0c 1103 mode++;
tanabe2000 2:c501f6845500 1104 getx = -1.0*table1200X;
tanabe2000 0:7822f4172b0c 1105 }
tanabe2000 0:7822f4172b0c 1106 } else {
tanabe2000 2:c501f6845500 1107 getx = -1.0*table1200X;
tanabe2000 2:c501f6845500 1108 if(getposition.getX() < -1.0*table1200X) {
tanabe2000 0:7822f4172b0c 1109 mode++;
tanabe2000 0:7822f4172b0c 1110 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1111 }
tanabe2000 0:7822f4172b0c 1112 }
tanabe2000 0:7822f4172b0c 1113
tanabe2000 0:7822f4172b0c 1114 }
tanabe2000 0:7822f4172b0c 1115
tanabe2000 0:7822f4172b0c 1116 if(mode == 9) {
tanabe2000 2:c501f6845500 1117 if(kinectmode == 7) {
tanabe2000 0:7822f4172b0c 1118
tanabe2000 0:7822f4172b0c 1119 } else {
tanabe2000 2:c501f6845500 1120
tanabe2000 2:c501f6845500 1121 if(airFlag == 1) {
tanabe2000 2:c501f6845500 1122 led = 1;
tanabe2000 2:c501f6845500 1123 solenoidValve1is = 1;
tanabe2000 2:c501f6845500 1124
tanabe2000 2:c501f6845500 1125
tanabe2000 2:c501f6845500 1126
tanabe2000 2:c501f6845500 1127 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 2:c501f6845500 1128 pidT.reset();
tanabe2000 2:c501f6845500 1129 dpetbotle = 1;
tanabe2000 2:c501f6845500 1130
tanabe2000 2:c501f6845500 1131 }
tanabe2000 2:c501f6845500 1132 piddt = pidT.read();
tanabe2000 2:c501f6845500 1133 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 2:c501f6845500 1134 loadingmode = 3;
tanabe2000 2:c501f6845500 1135 firePwm[0] = 0.0;
tanabe2000 2:c501f6845500 1136
tanabe2000 2:c501f6845500 1137 }
tanabe2000 2:c501f6845500 1138 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 2:c501f6845500 1139 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 2:c501f6845500 1140 if(swflag == 1) {
tanabe2000 2:c501f6845500 1141 fireDistance1 = 870;
tanabe2000 2:c501f6845500 1142 swflag = 0;
tanabe2000 2:c501f6845500 1143 pidT.reset();
tanabe2000 2:c501f6845500 1144 }
tanabe2000 2:c501f6845500 1145 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 2:c501f6845500 1146 loadingmode = 0;
tanabe2000 2:c501f6845500 1147 if(piddt > 8.0)airFlag = 0;
tanabe2000 2:c501f6845500 1148 }
tanabe2000 2:c501f6845500 1149
tanabe2000 2:c501f6845500 1150 firePwm[0] = Output_PID;
tanabe2000 2:c501f6845500 1151 }
tanabe2000 2:c501f6845500 1152
tanabe2000 2:c501f6845500 1153
tanabe2000 2:c501f6845500 1154 } else {
tanabe2000 2:c501f6845500 1155 if(swflag == 0) {
tanabe2000 2:c501f6845500 1156 pidT.reset();
tanabe2000 2:c501f6845500 1157 swflag = 1;
tanabe2000 2:c501f6845500 1158 firecount++;
tanabe2000 0:7822f4172b0c 1159 }
tanabe2000 0:7822f4172b0c 1160 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1161 if((piddt > 2.0))airFlag = 1;
tanabe2000 2:c501f6845500 1162 dpetbotle = 0;
tanabe2000 2:c501f6845500 1163 led = 0;
tanabe2000 2:c501f6845500 1164
tanabe2000 2:c501f6845500 1165 solenoidValve1is = 0;
tanabe2000 2:c501f6845500 1166 loadingmode = 0;
tanabe2000 2:c501f6845500 1167 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 2:c501f6845500 1168
tanabe2000 2:c501f6845500 1169 }
tanabe2000 2:c501f6845500 1170
tanabe2000 2:c501f6845500 1171
tanabe2000 2:c501f6845500 1172 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1173 // mode++;
tanabe2000 0:7822f4172b0c 1174 // }
tanabe2000 0:7822f4172b0c 1175
tanabe2000 2:c501f6845500 1176 /*発射機構動く*/
tanabe2000 2:c501f6845500 1177 }
tanabe2000 0:7822f4172b0c 1178 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1179 // mode++;
tanabe2000 0:7822f4172b0c 1180 // }
tanabe2000 0:7822f4172b0c 1181
tanabe2000 0:7822f4172b0c 1182 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 1183 }
tanabe2000 0:7822f4172b0c 1184
tanabe2000 0:7822f4172b0c 1185
tanabe2000 0:7822f4172b0c 1186
tanabe2000 0:7822f4172b0c 1187
tanabe2000 0:7822f4172b0c 1188 // if(con.getButton2(1)==0) {
tanabe2000 2:c501f6845500 1189 if(kabeFlag == 1) {
tanabe2000 2:c501f6845500 1190 for (int i = 0; i < 4; i++) {
tanabe2000 2:c501f6845500 1191 speed[i] = omni.wheel[i];
tanabe2000 2:c501f6845500 1192 wheels[i]->setSpeed(speed[i]);
tanabe2000 2:c501f6845500 1193 }
tanabe2000 2:c501f6845500 1194 } else {
tanabe2000 2:c501f6845500 1195 for (int i = 0; i < 4; i++) {
tanabe2000 2:c501f6845500 1196 omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 2:c501f6845500 1197 speed[i] = omni.wheel[i];
tanabe2000 2:c501f6845500 1198 wheels[i]->setSpeed(speed[i]);
tanabe2000 2:c501f6845500 1199 }
tanabe2000 0:7822f4172b0c 1200
tanabe2000 0:7822f4172b0c 1201
tanabe2000 0:7822f4172b0c 1202 }
tanabe2000 0:7822f4172b0c 1203 for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
tanabe2000 0:7822f4172b0c 1204 valveFire1.inputState(solenoidValve1is);
tanabe2000 0:7822f4172b0c 1205 valveFire2.inputState(solenoidValve2is);
tanabe2000 0:7822f4172b0c 1206 loading.petbottlemode(loadingmode);
tanabe2000 0:7822f4172b0c 1207
tanabe2000 0:7822f4172b0c 1208
tanabe2000 0:7822f4172b0c 1209 // if(mode > 37) mode = 0;
tanabe2000 0:7822f4172b0c 1210 // debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
tanabe2000 0:7822f4172b0c 1211
tanabe2000 2:c501f6845500 1212
tanabe2000 0:7822f4172b0c 1213 }