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@0:7822f4172b0c, 2018-10-12 (annotated)

Committer:
tanabe2000
Date:
Fri Oct 12 04:40:56 2018 +0000
Revision:
0:7822f4172b0c
Child:
2:c501f6845500
????????;

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 0:7822f4172b0c 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 0:7822f4172b0c 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 debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 330 //debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 331 if((int)startSW == 0)start_ = 2;
tanabe2000 0:7822f4172b0c 332
tanabe2000 0:7822f4172b0c 333 if(start_ == 2){
tanabe2000 0:7822f4172b0c 334
tanabe2000 0:7822f4172b0c 335 if(airFlag == 1 && firecount <= 2) {
tanabe2000 0:7822f4172b0c 336 led = 1;
tanabe2000 0:7822f4172b0c 337 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 338
tanabe2000 0:7822f4172b0c 339
tanabe2000 0:7822f4172b0c 340
tanabe2000 0:7822f4172b0c 341 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 342 pidT.reset();
tanabe2000 0:7822f4172b0c 343 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 344
tanabe2000 0:7822f4172b0c 345 }
tanabe2000 0:7822f4172b0c 346 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 347 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 348 loadingmode = 3;
tanabe2000 0:7822f4172b0c 349 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 350
tanabe2000 0:7822f4172b0c 351 }
tanabe2000 0:7822f4172b0c 352 if(loadingmode != 3)fireDistance1 = loadingParamator, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 353 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 354 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 355 fireDistance1 = 1120;
tanabe2000 0:7822f4172b0c 356 swflag = 0;
tanabe2000 0:7822f4172b0c 357 pidT.reset();
tanabe2000 0:7822f4172b0c 358 }
tanabe2000 0:7822f4172b0c 359 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 360 loadingmode = 0;
tanabe2000 0:7822f4172b0c 361 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 362 }
tanabe2000 0:7822f4172b0c 363
tanabe2000 0:7822f4172b0c 364 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 365 }
tanabe2000 0:7822f4172b0c 366
tanabe2000 0:7822f4172b0c 367
tanabe2000 0:7822f4172b0c 368 } else {
tanabe2000 0:7822f4172b0c 369 if(swflag == 0){
tanabe2000 0:7822f4172b0c 370 pidT.reset();
tanabe2000 0:7822f4172b0c 371 swflag = 1;
tanabe2000 0:7822f4172b0c 372 firecount++;
tanabe2000 0:7822f4172b0c 373 }
tanabe2000 0:7822f4172b0c 374 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 375 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 376 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 377 led = 0;
tanabe2000 0:7822f4172b0c 378
tanabe2000 0:7822f4172b0c 379 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 380 loadingmode = 0;
tanabe2000 0:7822f4172b0c 381 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 382
tanabe2000 0:7822f4172b0c 383 }
tanabe2000 0:7822f4172b0c 384 }
tanabe2000 0:7822f4172b0c 385
tanabe2000 0:7822f4172b0c 386
tanabe2000 0:7822f4172b0c 387 // if((int)startSW == 0)start_ = 1;
tanabe2000 0:7822f4172b0c 388 if((start_ == 0)) {
tanabe2000 0:7822f4172b0c 389
tanabe2000 0:7822f4172b0c 390 getx =0;
tanabe2000 0:7822f4172b0c 391 gety =0;
tanabe2000 0:7822f4172b0c 392 }
tanabe2000 0:7822f4172b0c 393 if((mode == 0) && (start_ == 1)) {
tanabe2000 0:7822f4172b0c 394
tanabe2000 0:7822f4172b0c 395 mode++;
tanabe2000 0:7822f4172b0c 396 getx = dash1X;
tanabe2000 0:7822f4172b0c 397 gety = dash1Y;
tanabe2000 0:7822f4172b0c 398 }
tanabe2000 0:7822f4172b0c 399 if(mode == 1 && getposition.getX() >= dash1X && getposition.getY() <= dash1Y) {
tanabe2000 0:7822f4172b0c 400
tanabe2000 0:7822f4172b0c 401 mode++;
tanabe2000 0:7822f4172b0c 402 getx = dash2X;
tanabe2000 0:7822f4172b0c 403 gety = dash2Y;
tanabe2000 0:7822f4172b0c 404 }
tanabe2000 0:7822f4172b0c 405 if(mode == 2 && getposition.getX() >= dash2X && getposition.getY() <= dash2Y) {
tanabe2000 0:7822f4172b0c 406
tanabe2000 0:7822f4172b0c 407 mode++;
tanabe2000 0:7822f4172b0c 408 getx = dash3X;
tanabe2000 0:7822f4172b0c 409 gety = dash3Y;
tanabe2000 0:7822f4172b0c 410 }
tanabe2000 0:7822f4172b0c 411 if(mode == 3 && getposition.getX() >= dash3X-200 && getposition.getY() <= dash3Y+200) {
tanabe2000 0:7822f4172b0c 412 //if(firecount > 3 && (dt >= 1.0)) {
tanabe2000 0:7822f4172b0c 413 // mode++;
tanabe2000 0:7822f4172b0c 414 // }
tanabe2000 0:7822f4172b0c 415
tanabe2000 0:7822f4172b0c 416 if(airFlag == 1 && firecount <= 2) {
tanabe2000 0:7822f4172b0c 417 led = 1;
tanabe2000 0:7822f4172b0c 418 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 419
tanabe2000 0:7822f4172b0c 420
tanabe2000 0:7822f4172b0c 421
tanabe2000 0:7822f4172b0c 422 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 423 pidT.reset();
tanabe2000 0:7822f4172b0c 424 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 425
tanabe2000 0:7822f4172b0c 426 }
tanabe2000 0:7822f4172b0c 427 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 428 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 429 loadingmode = 3;
tanabe2000 0:7822f4172b0c 430 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 431
tanabe2000 0:7822f4172b0c 432 }
tanabe2000 0:7822f4172b0c 433 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 434 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 435 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 436 fireDistance1 = 1120;
tanabe2000 0:7822f4172b0c 437 swflag = 0;
tanabe2000 0:7822f4172b0c 438 pidT.reset();
tanabe2000 0:7822f4172b0c 439 }
tanabe2000 0:7822f4172b0c 440 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 441 loadingmode = 0;
tanabe2000 0:7822f4172b0c 442 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 443 }
tanabe2000 0:7822f4172b0c 444
tanabe2000 0:7822f4172b0c 445 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 446 }
tanabe2000 0:7822f4172b0c 447
tanabe2000 0:7822f4172b0c 448
tanabe2000 0:7822f4172b0c 449 } else {
tanabe2000 0:7822f4172b0c 450 if(swflag == 0){
tanabe2000 0:7822f4172b0c 451 pidT.reset();
tanabe2000 0:7822f4172b0c 452 swflag = 1;
tanabe2000 0:7822f4172b0c 453 firecount++;
tanabe2000 0:7822f4172b0c 454 }
tanabe2000 0:7822f4172b0c 455 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 456 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 457 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 458 led = 0;
tanabe2000 0:7822f4172b0c 459
tanabe2000 0:7822f4172b0c 460 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 461 loadingmode = 0;
tanabe2000 0:7822f4172b0c 462 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 463
tanabe2000 0:7822f4172b0c 464 }
tanabe2000 0:7822f4172b0c 465
tanabe2000 0:7822f4172b0c 466 // mode++;
tanabe2000 0:7822f4172b0c 467 }
tanabe2000 0:7822f4172b0c 468 if(mode == 4 && kinectFlag == 1) {
tanabe2000 0:7822f4172b0c 469 if(getposition.getX() > table1800X - 200) {
tanabe2000 0:7822f4172b0c 470
tanabe2000 0:7822f4172b0c 471 mode++;
tanabe2000 0:7822f4172b0c 472 getx = table1800X;
tanabe2000 0:7822f4172b0c 473 gety = -1*suzuki[0]-1000;
tanabe2000 0:7822f4172b0c 474 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 475 swflag = 1;
tanabe2000 0:7822f4172b0c 476 airFlag = 1;
tanabe2000 0:7822f4172b0c 477 } else {
tanabe2000 0:7822f4172b0c 478 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 479 omni.computeXY(0.3, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 480 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 481 omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 482 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 483 omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 484 } else {
tanabe2000 0:7822f4172b0c 485 omni.computeXY(0.3, -0.15, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 486 }
tanabe2000 0:7822f4172b0c 487 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 488 }
tanabe2000 0:7822f4172b0c 489 }
tanabe2000 0:7822f4172b0c 490
tanabe2000 0:7822f4172b0c 491 if(mode == 5) {
tanabe2000 0:7822f4172b0c 492 if(kinectmode == 3){
tanabe2000 0:7822f4172b0c 493 mode++;
tanabe2000 0:7822f4172b0c 494 }else{
tanabe2000 0:7822f4172b0c 495 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 496 led = 1;
tanabe2000 0:7822f4172b0c 497 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 498
tanabe2000 0:7822f4172b0c 499
tanabe2000 0:7822f4172b0c 500
tanabe2000 0:7822f4172b0c 501 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 502 pidT.reset();
tanabe2000 0:7822f4172b0c 503 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 504
tanabe2000 0:7822f4172b0c 505 }
tanabe2000 0:7822f4172b0c 506 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 507 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 508 loadingmode = 3;
tanabe2000 0:7822f4172b0c 509 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 510
tanabe2000 0:7822f4172b0c 511 }
tanabe2000 0:7822f4172b0c 512 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 513 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 514 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 515 fireDistance1 = 1010;
tanabe2000 0:7822f4172b0c 516 swflag = 0;
tanabe2000 0:7822f4172b0c 517 pidT.reset();
tanabe2000 0:7822f4172b0c 518 }
tanabe2000 0:7822f4172b0c 519 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 520 loadingmode = 0;
tanabe2000 0:7822f4172b0c 521 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 522 }
tanabe2000 0:7822f4172b0c 523
tanabe2000 0:7822f4172b0c 524 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 525 }
tanabe2000 0:7822f4172b0c 526
tanabe2000 0:7822f4172b0c 527
tanabe2000 0:7822f4172b0c 528 } else {
tanabe2000 0:7822f4172b0c 529 if(swflag == 0){
tanabe2000 0:7822f4172b0c 530 pidT.reset();
tanabe2000 0:7822f4172b0c 531 swflag = 1;
tanabe2000 0:7822f4172b0c 532 firecount++;
tanabe2000 0:7822f4172b0c 533 }
tanabe2000 0:7822f4172b0c 534 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 535 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 536 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 537 led = 0;
tanabe2000 0:7822f4172b0c 538
tanabe2000 0:7822f4172b0c 539 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 540 loadingmode = 0;
tanabe2000 0:7822f4172b0c 541 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 542
tanabe2000 0:7822f4172b0c 543 }
tanabe2000 0:7822f4172b0c 544 }
tanabe2000 0:7822f4172b0c 545 //if(((int)startSW == 0) && (dt >= 0.5)) {
tanabe2000 0:7822f4172b0c 546 // mode++;
tanabe2000 0:7822f4172b0c 547 // t.reset();
tanabe2000 0:7822f4172b0c 548 // }
tanabe2000 0:7822f4172b0c 549
tanabe2000 0:7822f4172b0c 550 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 551
tanabe2000 0:7822f4172b0c 552 }
tanabe2000 0:7822f4172b0c 553
tanabe2000 0:7822f4172b0c 554 if(mode == 6 && (-1*suzuki[1]-1000 < wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 555 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 556 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 557 if(getposition.getX() < table1500X + 400) {
tanabe2000 0:7822f4172b0c 558
tanabe2000 0:7822f4172b0c 559 mode++;
tanabe2000 0:7822f4172b0c 560 getx = table1500X;
tanabe2000 0:7822f4172b0c 561 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 562 kabeFlag = 0;swflag = 1;
tanabe2000 0:7822f4172b0c 563 airFlag = 1;
tanabe2000 0:7822f4172b0c 564 } else {
tanabe2000 0:7822f4172b0c 565 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 566 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 567 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 568 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 569 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 570 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 571 } else {
tanabe2000 0:7822f4172b0c 572 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 573 }
tanabe2000 0:7822f4172b0c 574 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 575 }
tanabe2000 0:7822f4172b0c 576 } else {
tanabe2000 0:7822f4172b0c 577 gety = wallmodeY;
tanabe2000 0:7822f4172b0c 578 }
tanabe2000 0:7822f4172b0c 579 }
tanabe2000 0:7822f4172b0c 580 if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 581 swflag = 1;
tanabe2000 0:7822f4172b0c 582 airFlag = 1;
tanabe2000 0:7822f4172b0c 583 if(suzuki[0] < suzuki[1]) {
tanabe2000 0:7822f4172b0c 584 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 585 if(getposition.getY() < -1*suzuki[1]-1000) {
tanabe2000 0:7822f4172b0c 586 mode++;
tanabe2000 0:7822f4172b0c 587 getx = table1500X;
tanabe2000 0:7822f4172b0c 588 }
tanabe2000 0:7822f4172b0c 589 } else {
tanabe2000 0:7822f4172b0c 590 getx = table1500X;
tanabe2000 0:7822f4172b0c 591 if(getposition.getX() < table1500X ) {
tanabe2000 0:7822f4172b0c 592 mode++;
tanabe2000 0:7822f4172b0c 593 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 594 }
tanabe2000 0:7822f4172b0c 595 }
tanabe2000 0:7822f4172b0c 596
tanabe2000 0:7822f4172b0c 597 }
tanabe2000 0:7822f4172b0c 598 if(mode == 7) {
tanabe2000 0:7822f4172b0c 599 if(kinectmode == 5){
tanabe2000 0:7822f4172b0c 600 mode++;
tanabe2000 0:7822f4172b0c 601 }else{
tanabe2000 0:7822f4172b0c 602 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 603 led = 1;
tanabe2000 0:7822f4172b0c 604 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 605
tanabe2000 0:7822f4172b0c 606
tanabe2000 0:7822f4172b0c 607
tanabe2000 0:7822f4172b0c 608 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 609 pidT.reset();
tanabe2000 0:7822f4172b0c 610 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 611
tanabe2000 0:7822f4172b0c 612 }
tanabe2000 0:7822f4172b0c 613 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 614 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 615 loadingmode = 3;
tanabe2000 0:7822f4172b0c 616 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 617
tanabe2000 0:7822f4172b0c 618 }
tanabe2000 0:7822f4172b0c 619 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 620 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 621 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 622 fireDistance1 = 820;
tanabe2000 0:7822f4172b0c 623 swflag = 0;
tanabe2000 0:7822f4172b0c 624 pidT.reset();
tanabe2000 0:7822f4172b0c 625 }
tanabe2000 0:7822f4172b0c 626 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 627 loadingmode = 0;
tanabe2000 0:7822f4172b0c 628 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 629 }
tanabe2000 0:7822f4172b0c 630
tanabe2000 0:7822f4172b0c 631 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 632 }
tanabe2000 0:7822f4172b0c 633
tanabe2000 0:7822f4172b0c 634
tanabe2000 0:7822f4172b0c 635 } else {
tanabe2000 0:7822f4172b0c 636 if(swflag == 0){
tanabe2000 0:7822f4172b0c 637 pidT.reset();
tanabe2000 0:7822f4172b0c 638 swflag = 1;
tanabe2000 0:7822f4172b0c 639 firecount++;
tanabe2000 0:7822f4172b0c 640 }
tanabe2000 0:7822f4172b0c 641 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 642 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 643 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 644 led = 0;
tanabe2000 0:7822f4172b0c 645
tanabe2000 0:7822f4172b0c 646 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 647 loadingmode = 0;
tanabe2000 0:7822f4172b0c 648 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 649
tanabe2000 0:7822f4172b0c 650 }
tanabe2000 0:7822f4172b0c 651
tanabe2000 0:7822f4172b0c 652
tanabe2000 0:7822f4172b0c 653 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 654 // mode++;
tanabe2000 0:7822f4172b0c 655 // }
tanabe2000 0:7822f4172b0c 656
tanabe2000 0:7822f4172b0c 657 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 658 }
tanabe2000 0:7822f4172b0c 659 }
tanabe2000 0:7822f4172b0c 660 if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
tanabe2000 0:7822f4172b0c 661 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 662 swflag = 1;
tanabe2000 0:7822f4172b0c 663 airFlag = 1;
tanabe2000 0:7822f4172b0c 664 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 665 if(getposition.getX() < table1200X + 200) {
tanabe2000 0:7822f4172b0c 666
tanabe2000 0:7822f4172b0c 667 mode++;
tanabe2000 0:7822f4172b0c 668 getx = table1200X;
tanabe2000 0:7822f4172b0c 669 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 670 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 671 } else {
tanabe2000 0:7822f4172b0c 672
tanabe2000 0:7822f4172b0c 673 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 674 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 675 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 676 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 677 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 678 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 679 } else {
tanabe2000 0:7822f4172b0c 680 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 681 }
tanabe2000 0:7822f4172b0c 682 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 683 }
tanabe2000 0:7822f4172b0c 684 } else {
tanabe2000 0:7822f4172b0c 685 gety = walldashY;
tanabe2000 0:7822f4172b0c 686 }
tanabe2000 0:7822f4172b0c 687 }
tanabe2000 0:7822f4172b0c 688 if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
tanabe2000 0:7822f4172b0c 689 swflag = 1;
tanabe2000 0:7822f4172b0c 690 airFlag = 1;
tanabe2000 0:7822f4172b0c 691 if(suzuki[1] < suzuki[2]) {
tanabe2000 0:7822f4172b0c 692 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 693 if(getposition.getY() < -1*suzuki[2]-800) {
tanabe2000 0:7822f4172b0c 694 mode++;
tanabe2000 0:7822f4172b0c 695 getx = table1200X;
tanabe2000 0:7822f4172b0c 696 }
tanabe2000 0:7822f4172b0c 697 } else {
tanabe2000 0:7822f4172b0c 698 getx = table1200X;
tanabe2000 0:7822f4172b0c 699 if(getposition.getX() < table1200X) {
tanabe2000 0:7822f4172b0c 700 mode++;
tanabe2000 0:7822f4172b0c 701 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 702 }
tanabe2000 0:7822f4172b0c 703 }
tanabe2000 0:7822f4172b0c 704
tanabe2000 0:7822f4172b0c 705 }
tanabe2000 0:7822f4172b0c 706
tanabe2000 0:7822f4172b0c 707
tanabe2000 0:7822f4172b0c 708 if(mode == 9) {
tanabe2000 0:7822f4172b0c 709 if(kinectmode == 7){
tanabe2000 0:7822f4172b0c 710
tanabe2000 0:7822f4172b0c 711 }else{
tanabe2000 0:7822f4172b0c 712
tanabe2000 0:7822f4172b0c 713 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 714 led = 1;
tanabe2000 0:7822f4172b0c 715 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 716
tanabe2000 0:7822f4172b0c 717
tanabe2000 0:7822f4172b0c 718
tanabe2000 0:7822f4172b0c 719 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 720 pidT.reset();
tanabe2000 0:7822f4172b0c 721 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 722
tanabe2000 0:7822f4172b0c 723 }
tanabe2000 0:7822f4172b0c 724 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 725 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 726 loadingmode = 3;
tanabe2000 0:7822f4172b0c 727 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 728
tanabe2000 0:7822f4172b0c 729 }
tanabe2000 0:7822f4172b0c 730 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 731 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 732 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 733 fireDistance1 = 870;
tanabe2000 0:7822f4172b0c 734 swflag = 0;
tanabe2000 0:7822f4172b0c 735 pidT.reset();
tanabe2000 0:7822f4172b0c 736 }
tanabe2000 0:7822f4172b0c 737 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 738 loadingmode = 0;
tanabe2000 0:7822f4172b0c 739 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 740 }
tanabe2000 0:7822f4172b0c 741
tanabe2000 0:7822f4172b0c 742 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 743 }
tanabe2000 0:7822f4172b0c 744
tanabe2000 0:7822f4172b0c 745
tanabe2000 0:7822f4172b0c 746 } else {
tanabe2000 0:7822f4172b0c 747 if(swflag == 0){
tanabe2000 0:7822f4172b0c 748 pidT.reset();
tanabe2000 0:7822f4172b0c 749 swflag = 1;
tanabe2000 0:7822f4172b0c 750 firecount++;
tanabe2000 0:7822f4172b0c 751 }
tanabe2000 0:7822f4172b0c 752 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 753 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 754 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 755 led = 0;
tanabe2000 0:7822f4172b0c 756
tanabe2000 0:7822f4172b0c 757 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 758 loadingmode = 0;
tanabe2000 0:7822f4172b0c 759 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 760
tanabe2000 0:7822f4172b0c 761 }
tanabe2000 0:7822f4172b0c 762
tanabe2000 0:7822f4172b0c 763
tanabe2000 0:7822f4172b0c 764 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 765 // mode++;
tanabe2000 0:7822f4172b0c 766 // }
tanabe2000 0:7822f4172b0c 767
tanabe2000 0:7822f4172b0c 768 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 769 }
tanabe2000 0:7822f4172b0c 770 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 771 // mode++;
tanabe2000 0:7822f4172b0c 772 // }
tanabe2000 0:7822f4172b0c 773
tanabe2000 0:7822f4172b0c 774 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 775 }
tanabe2000 0:7822f4172b0c 776
tanabe2000 0:7822f4172b0c 777
tanabe2000 0:7822f4172b0c 778
tanabe2000 0:7822f4172b0c 779
tanabe2000 0:7822f4172b0c 780 // if(con.getButton2(1)==0) {
tanabe2000 0:7822f4172b0c 781 if(kabeFlag == 1) {
tanabe2000 0:7822f4172b0c 782 for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 783 speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 784 wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 785 }
tanabe2000 0:7822f4172b0c 786 } else {
tanabe2000 0:7822f4172b0c 787 for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 788 omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 789 speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 790 wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 791 }
tanabe2000 0:7822f4172b0c 792
tanabe2000 0:7822f4172b0c 793
tanabe2000 0:7822f4172b0c 794 }
tanabe2000 0:7822f4172b0c 795 for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
tanabe2000 0:7822f4172b0c 796 valveFire1.inputState(solenoidValve1is);
tanabe2000 0:7822f4172b0c 797 valveFire2.inputState(solenoidValve2is);
tanabe2000 0:7822f4172b0c 798 loading.petbottlemode(loadingmode);
tanabe2000 0:7822f4172b0c 799
tanabe2000 0:7822f4172b0c 800
tanabe2000 0:7822f4172b0c 801 // if(mode > 37) mode = 0;
tanabe2000 0:7822f4172b0c 802 // debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
tanabe2000 0:7822f4172b0c 803
tanabe2000 0:7822f4172b0c 804 }
tanabe2000 0:7822f4172b0c 805 void GakuBot::autoMode2()
tanabe2000 0:7822f4172b0c 806 {
tanabe2000 0:7822f4172b0c 807 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 808 XPid.setSetPoint(-1.0*getx);
tanabe2000 0:7822f4172b0c 809 XPid.setProcessValue(getposition.getX());
tanabe2000 0:7822f4172b0c 810 YPid.setSetPoint(-1.0*gety);
tanabe2000 0:7822f4172b0c 811 YPid.setProcessValue(getposition.getY());
tanabe2000 0:7822f4172b0c 812 conposition->compute(getposition.getX(), getposition.getY());
tanabe2000 0:7822f4172b0c 813 targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
tanabe2000 0:7822f4172b0c 814 if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
tanabe2000 0:7822f4172b0c 815 else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
tanabe2000 0:7822f4172b0c 816 if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
tanabe2000 0:7822f4172b0c 817 else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
tanabe2000 0:7822f4172b0c 818
tanabe2000 0:7822f4172b0c 819
tanabe2000 0:7822f4172b0c 820 if((getx - getposition.getX()) >=0 ) {
tanabe2000 0:7822f4172b0c 821 if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 822 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 823 } else {
tanabe2000 0:7822f4172b0c 824 if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
tanabe2000 0:7822f4172b0c 825 else Xlopassb = Xlopass;
tanabe2000 0:7822f4172b0c 826 }
tanabe2000 0:7822f4172b0c 827 if((gety - getposition.getY()) >=0) {
tanabe2000 0:7822f4172b0c 828 if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 829 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 830 } else {
tanabe2000 0:7822f4172b0c 831
tanabe2000 0:7822f4172b0c 832 if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
tanabe2000 0:7822f4172b0c 833 else Ylopassb = Ylopass;
tanabe2000 0:7822f4172b0c 834 }
tanabe2000 0:7822f4172b0c 835 X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 836 Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
tanabe2000 0:7822f4172b0c 837
tanabe2000 0:7822f4172b0c 838 dt = t.read();
tanabe2000 0:7822f4172b0c 839 // debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
tanabe2000 0:7822f4172b0c 840 debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
tanabe2000 0:7822f4172b0c 841 // debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
tanabe2000 0:7822f4172b0c 842 if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
tanabe2000 0:7822f4172b0c 843 if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
tanabe2000 0:7822f4172b0c 844 if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
tanabe2000 0:7822f4172b0c 845 //debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 846 debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 847 //debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
tanabe2000 0:7822f4172b0c 848
tanabe2000 0:7822f4172b0c 849 if((int)startSW == 0)start_ = 1;
tanabe2000 0:7822f4172b0c 850 if((start_ == 0)) {
tanabe2000 0:7822f4172b0c 851
tanabe2000 0:7822f4172b0c 852 getx =0;
tanabe2000 0:7822f4172b0c 853 gety =0;
tanabe2000 0:7822f4172b0c 854 }
tanabe2000 0:7822f4172b0c 855 if((mode == 0) && (start_ == 1)) {
tanabe2000 0:7822f4172b0c 856
tanabe2000 0:7822f4172b0c 857 mode++;
tanabe2000 0:7822f4172b0c 858 getx = dash1X;
tanabe2000 0:7822f4172b0c 859 gety = dash1Y;
tanabe2000 0:7822f4172b0c 860 }
tanabe2000 0:7822f4172b0c 861 if(mode == 1 && getposition.getX() <= dash1X && getposition.getY() <= dash1Y) {
tanabe2000 0:7822f4172b0c 862
tanabe2000 0:7822f4172b0c 863 mode++;
tanabe2000 0:7822f4172b0c 864 getx = dash2X;
tanabe2000 0:7822f4172b0c 865 gety = dash2Y;
tanabe2000 0:7822f4172b0c 866 }
tanabe2000 0:7822f4172b0c 867 if(mode == 2 && getposition.getX() <= dash2X && getposition.getY() <= dash2Y) {
tanabe2000 0:7822f4172b0c 868
tanabe2000 0:7822f4172b0c 869 mode++;
tanabe2000 0:7822f4172b0c 870 getx = dash3X;
tanabe2000 0:7822f4172b0c 871 gety = dash3Y;
tanabe2000 0:7822f4172b0c 872 }
tanabe2000 0:7822f4172b0c 873 if(mode == 3 && getposition.getX() <= dash3X+200 && getposition.getY() <= dash3Y+200) {
tanabe2000 0:7822f4172b0c 874 //if(firecount > 3 && (dt >= 1.0)) {
tanabe2000 0:7822f4172b0c 875 // mode++;
tanabe2000 0:7822f4172b0c 876 // }
tanabe2000 0:7822f4172b0c 877
tanabe2000 0:7822f4172b0c 878 if(airFlag == 1 && firecount <= 2) {
tanabe2000 0:7822f4172b0c 879 led = 1;
tanabe2000 0:7822f4172b0c 880 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 881
tanabe2000 0:7822f4172b0c 882
tanabe2000 0:7822f4172b0c 883
tanabe2000 0:7822f4172b0c 884 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 885 pidT.reset();
tanabe2000 0:7822f4172b0c 886 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 887
tanabe2000 0:7822f4172b0c 888 }
tanabe2000 0:7822f4172b0c 889 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 890 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 891 loadingmode = 3;
tanabe2000 0:7822f4172b0c 892 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 893
tanabe2000 0:7822f4172b0c 894 }
tanabe2000 0:7822f4172b0c 895 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 896 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 897 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 898 fireDistance1 = 1120;
tanabe2000 0:7822f4172b0c 899 swflag = 0;
tanabe2000 0:7822f4172b0c 900 pidT.reset();
tanabe2000 0:7822f4172b0c 901 }
tanabe2000 0:7822f4172b0c 902 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 903 loadingmode = 0;
tanabe2000 0:7822f4172b0c 904 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 905 }
tanabe2000 0:7822f4172b0c 906
tanabe2000 0:7822f4172b0c 907 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 908 }
tanabe2000 0:7822f4172b0c 909
tanabe2000 0:7822f4172b0c 910
tanabe2000 0:7822f4172b0c 911 } else {
tanabe2000 0:7822f4172b0c 912 if(swflag == 0){
tanabe2000 0:7822f4172b0c 913 pidT.reset();
tanabe2000 0:7822f4172b0c 914 swflag = 1;
tanabe2000 0:7822f4172b0c 915 firecount++;
tanabe2000 0:7822f4172b0c 916 }
tanabe2000 0:7822f4172b0c 917 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 918 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 919 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 920 led = 0;
tanabe2000 0:7822f4172b0c 921
tanabe2000 0:7822f4172b0c 922 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 923 loadingmode = 0;
tanabe2000 0:7822f4172b0c 924 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 925
tanabe2000 0:7822f4172b0c 926 }
tanabe2000 0:7822f4172b0c 927
tanabe2000 0:7822f4172b0c 928 // mode++;
tanabe2000 0:7822f4172b0c 929 }
tanabe2000 0:7822f4172b0c 930 if(mode == 4 && kinectFlag == 1) {
tanabe2000 0:7822f4172b0c 931 if(getposition.getX() < table1800X + 200) {
tanabe2000 0:7822f4172b0c 932
tanabe2000 0:7822f4172b0c 933 mode++;
tanabe2000 0:7822f4172b0c 934 getx = table1800X;
tanabe2000 0:7822f4172b0c 935 gety = -1*suzuki[0]-1000;
tanabe2000 0:7822f4172b0c 936 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 937 swflag = 1;
tanabe2000 0:7822f4172b0c 938 airFlag = 1;
tanabe2000 0:7822f4172b0c 939 } else {
tanabe2000 0:7822f4172b0c 940 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 941 omni.computeXY(0.3, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 942 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 943 omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 944 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 945 omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 946 } else {
tanabe2000 0:7822f4172b0c 947 omni.computeXY(0.3, -0.15, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 948 }
tanabe2000 0:7822f4172b0c 949 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 950 }
tanabe2000 0:7822f4172b0c 951 }
tanabe2000 0:7822f4172b0c 952
tanabe2000 0:7822f4172b0c 953 if(mode == 5) {
tanabe2000 0:7822f4172b0c 954 if(kinectmode == 3){
tanabe2000 0:7822f4172b0c 955 mode++;
tanabe2000 0:7822f4172b0c 956 }else{
tanabe2000 0:7822f4172b0c 957 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 958 led = 1;
tanabe2000 0:7822f4172b0c 959 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 960
tanabe2000 0:7822f4172b0c 961
tanabe2000 0:7822f4172b0c 962
tanabe2000 0:7822f4172b0c 963 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 964 pidT.reset();
tanabe2000 0:7822f4172b0c 965 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 966
tanabe2000 0:7822f4172b0c 967 }
tanabe2000 0:7822f4172b0c 968 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 969 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 970 loadingmode = 3;
tanabe2000 0:7822f4172b0c 971 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 972
tanabe2000 0:7822f4172b0c 973 }
tanabe2000 0:7822f4172b0c 974 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 975 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 976 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 977 fireDistance1 = 1010;
tanabe2000 0:7822f4172b0c 978 swflag = 0;
tanabe2000 0:7822f4172b0c 979 pidT.reset();
tanabe2000 0:7822f4172b0c 980 }
tanabe2000 0:7822f4172b0c 981 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 982 loadingmode = 0;
tanabe2000 0:7822f4172b0c 983 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 984 }
tanabe2000 0:7822f4172b0c 985
tanabe2000 0:7822f4172b0c 986 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 987 }
tanabe2000 0:7822f4172b0c 988
tanabe2000 0:7822f4172b0c 989
tanabe2000 0:7822f4172b0c 990 } else {
tanabe2000 0:7822f4172b0c 991 if(swflag == 0){
tanabe2000 0:7822f4172b0c 992 pidT.reset();
tanabe2000 0:7822f4172b0c 993 swflag = 1;
tanabe2000 0:7822f4172b0c 994 firecount++;
tanabe2000 0:7822f4172b0c 995 }
tanabe2000 0:7822f4172b0c 996 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 997 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 998 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 999 led = 0;
tanabe2000 0:7822f4172b0c 1000
tanabe2000 0:7822f4172b0c 1001 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 1002 loadingmode = 0;
tanabe2000 0:7822f4172b0c 1003 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 1004
tanabe2000 0:7822f4172b0c 1005 }
tanabe2000 0:7822f4172b0c 1006 }
tanabe2000 0:7822f4172b0c 1007 //if(((int)startSW == 0) && (dt >= 0.5)) {
tanabe2000 0:7822f4172b0c 1008 // mode++;
tanabe2000 0:7822f4172b0c 1009 // t.reset();
tanabe2000 0:7822f4172b0c 1010 // }
tanabe2000 0:7822f4172b0c 1011
tanabe2000 0:7822f4172b0c 1012 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 1013
tanabe2000 0:7822f4172b0c 1014 }
tanabe2000 0:7822f4172b0c 1015
tanabe2000 0:7822f4172b0c 1016 if(mode == 6 && (-1*suzuki[1]+1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 1017 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 1018 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 1019 if(getposition.getX() > table1500X + 400) {
tanabe2000 0:7822f4172b0c 1020
tanabe2000 0:7822f4172b0c 1021 mode++;
tanabe2000 0:7822f4172b0c 1022 getx = table1500X;
tanabe2000 0:7822f4172b0c 1023 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 1024 kabeFlag = 0;swflag = 1;
tanabe2000 0:7822f4172b0c 1025 airFlag = 1;
tanabe2000 0:7822f4172b0c 1026 } else {
tanabe2000 0:7822f4172b0c 1027 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 1028 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1029 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 1030 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 1031 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 1032 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 1033 } else {
tanabe2000 0:7822f4172b0c 1034 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1035 }
tanabe2000 0:7822f4172b0c 1036 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 1037 }
tanabe2000 0:7822f4172b0c 1038 } else {
tanabe2000 0:7822f4172b0c 1039 gety = wallmodeY;
tanabe2000 0:7822f4172b0c 1040 }
tanabe2000 0:7822f4172b0c 1041 }
tanabe2000 0:7822f4172b0c 1042 if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
tanabe2000 0:7822f4172b0c 1043 swflag = 1;
tanabe2000 0:7822f4172b0c 1044 airFlag = 1;
tanabe2000 0:7822f4172b0c 1045 if(suzuki[0] < suzuki[1]) {
tanabe2000 0:7822f4172b0c 1046 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 1047 if(getposition.getY() < -1*suzuki[1]-1000) {
tanabe2000 0:7822f4172b0c 1048 mode++;
tanabe2000 0:7822f4172b0c 1049 getx = table1500X;
tanabe2000 0:7822f4172b0c 1050 }
tanabe2000 0:7822f4172b0c 1051 } else {
tanabe2000 0:7822f4172b0c 1052 getx = table1500X;
tanabe2000 0:7822f4172b0c 1053 if(getposition.getX() > table1500X ) {
tanabe2000 0:7822f4172b0c 1054 mode++;
tanabe2000 0:7822f4172b0c 1055 gety = -1*suzuki[1]-1000;
tanabe2000 0:7822f4172b0c 1056 }
tanabe2000 0:7822f4172b0c 1057 }
tanabe2000 0:7822f4172b0c 1058
tanabe2000 0:7822f4172b0c 1059 }
tanabe2000 0:7822f4172b0c 1060 if(mode == 7) {
tanabe2000 0:7822f4172b0c 1061 if(kinectmode == 5){
tanabe2000 0:7822f4172b0c 1062 mode++;
tanabe2000 0:7822f4172b0c 1063 }else{
tanabe2000 0:7822f4172b0c 1064 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 1065 led = 1;
tanabe2000 0:7822f4172b0c 1066 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 1067
tanabe2000 0:7822f4172b0c 1068
tanabe2000 0:7822f4172b0c 1069
tanabe2000 0:7822f4172b0c 1070 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 1071 pidT.reset();
tanabe2000 0:7822f4172b0c 1072 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 1073
tanabe2000 0:7822f4172b0c 1074 }
tanabe2000 0:7822f4172b0c 1075 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1076 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 1077 loadingmode = 3;
tanabe2000 0:7822f4172b0c 1078 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 1079
tanabe2000 0:7822f4172b0c 1080 }
tanabe2000 0:7822f4172b0c 1081 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 1082 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 1083 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 1084 fireDistance1 = 820;
tanabe2000 0:7822f4172b0c 1085 swflag = 0;
tanabe2000 0:7822f4172b0c 1086 pidT.reset();
tanabe2000 0:7822f4172b0c 1087 }
tanabe2000 0:7822f4172b0c 1088 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 1089 loadingmode = 0;
tanabe2000 0:7822f4172b0c 1090 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 1091 }
tanabe2000 0:7822f4172b0c 1092
tanabe2000 0:7822f4172b0c 1093 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 1094 }
tanabe2000 0:7822f4172b0c 1095
tanabe2000 0:7822f4172b0c 1096
tanabe2000 0:7822f4172b0c 1097 } else {
tanabe2000 0:7822f4172b0c 1098 if(swflag == 0){
tanabe2000 0:7822f4172b0c 1099 pidT.reset();
tanabe2000 0:7822f4172b0c 1100 swflag = 1;
tanabe2000 0:7822f4172b0c 1101 firecount++;
tanabe2000 0:7822f4172b0c 1102 }
tanabe2000 0:7822f4172b0c 1103 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1104 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 1105 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 1106 led = 0;
tanabe2000 0:7822f4172b0c 1107
tanabe2000 0:7822f4172b0c 1108 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 1109 loadingmode = 0;
tanabe2000 0:7822f4172b0c 1110 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 1111
tanabe2000 0:7822f4172b0c 1112 }
tanabe2000 0:7822f4172b0c 1113
tanabe2000 0:7822f4172b0c 1114
tanabe2000 0:7822f4172b0c 1115 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1116 // mode++;
tanabe2000 0:7822f4172b0c 1117 // }
tanabe2000 0:7822f4172b0c 1118
tanabe2000 0:7822f4172b0c 1119 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 1120 }
tanabe2000 0:7822f4172b0c 1121 }
tanabe2000 0:7822f4172b0c 1122 if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
tanabe2000 0:7822f4172b0c 1123 // debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
tanabe2000 0:7822f4172b0c 1124 swflag = 1;
tanabe2000 0:7822f4172b0c 1125 airFlag = 1;
tanabe2000 0:7822f4172b0c 1126 if(getposition.getY() < walldashY) {
tanabe2000 0:7822f4172b0c 1127 if(getposition.getX() > table1200X - 200) {
tanabe2000 0:7822f4172b0c 1128
tanabe2000 0:7822f4172b0c 1129 mode++;
tanabe2000 0:7822f4172b0c 1130 getx = table1200X;
tanabe2000 0:7822f4172b0c 1131 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1132 kabeFlag = 0;
tanabe2000 0:7822f4172b0c 1133 } else {
tanabe2000 0:7822f4172b0c 1134
tanabe2000 0:7822f4172b0c 1135 if((limit1 == 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 1136 omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1137 } else if((limit1 != 1) && (limit2 == 1)) {
tanabe2000 0:7822f4172b0c 1138 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
tanabe2000 0:7822f4172b0c 1139 } else if((limit1 == 1) && (limit2 != 1)) {
tanabe2000 0:7822f4172b0c 1140 omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
tanabe2000 0:7822f4172b0c 1141 } else {
tanabe2000 0:7822f4172b0c 1142 omni.computeXY(0, -0.18, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1143 }
tanabe2000 0:7822f4172b0c 1144 kabeFlag = 1;
tanabe2000 0:7822f4172b0c 1145 }
tanabe2000 0:7822f4172b0c 1146 } else {
tanabe2000 0:7822f4172b0c 1147 gety = walldashY;
tanabe2000 0:7822f4172b0c 1148 }
tanabe2000 0:7822f4172b0c 1149 }
tanabe2000 0:7822f4172b0c 1150 if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
tanabe2000 0:7822f4172b0c 1151 swflag = 1;
tanabe2000 0:7822f4172b0c 1152 airFlag = 1;
tanabe2000 0:7822f4172b0c 1153 if(suzuki[1] < suzuki[2]) {
tanabe2000 0:7822f4172b0c 1154 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1155 if(getposition.getY() < -1*suzuki[2]-800) {
tanabe2000 0:7822f4172b0c 1156 mode++;
tanabe2000 0:7822f4172b0c 1157 getx = table1200X;
tanabe2000 0:7822f4172b0c 1158 }
tanabe2000 0:7822f4172b0c 1159 } else {
tanabe2000 0:7822f4172b0c 1160 getx = table1200X;
tanabe2000 0:7822f4172b0c 1161 if(getposition.getX() > table1200X) {
tanabe2000 0:7822f4172b0c 1162 mode++;
tanabe2000 0:7822f4172b0c 1163 gety = -1*suzuki[2]-1200;
tanabe2000 0:7822f4172b0c 1164 }
tanabe2000 0:7822f4172b0c 1165 }
tanabe2000 0:7822f4172b0c 1166
tanabe2000 0:7822f4172b0c 1167 }
tanabe2000 0:7822f4172b0c 1168
tanabe2000 0:7822f4172b0c 1169 if(mode == 9) {
tanabe2000 0:7822f4172b0c 1170 if(kinectmode == 7){
tanabe2000 0:7822f4172b0c 1171
tanabe2000 0:7822f4172b0c 1172 }else{
tanabe2000 0:7822f4172b0c 1173
tanabe2000 0:7822f4172b0c 1174 if(airFlag == 1) {
tanabe2000 0:7822f4172b0c 1175 led = 1;
tanabe2000 0:7822f4172b0c 1176 solenoidValve1is = 1;
tanabe2000 0:7822f4172b0c 1177
tanabe2000 0:7822f4172b0c 1178
tanabe2000 0:7822f4172b0c 1179
tanabe2000 0:7822f4172b0c 1180 if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
tanabe2000 0:7822f4172b0c 1181 pidT.reset();
tanabe2000 0:7822f4172b0c 1182 dpetbotle = 1;
tanabe2000 0:7822f4172b0c 1183
tanabe2000 0:7822f4172b0c 1184 }
tanabe2000 0:7822f4172b0c 1185 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1186 if((piddt > 4.0) && (dpetbotle == 1)) {
tanabe2000 0:7822f4172b0c 1187 loadingmode = 3;
tanabe2000 0:7822f4172b0c 1188 firePwm[0] = 0.0;
tanabe2000 0:7822f4172b0c 1189
tanabe2000 0:7822f4172b0c 1190 }
tanabe2000 0:7822f4172b0c 1191 if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
tanabe2000 0:7822f4172b0c 1192 if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
tanabe2000 0:7822f4172b0c 1193 if(swflag == 1) {
tanabe2000 0:7822f4172b0c 1194 fireDistance1 = 870;
tanabe2000 0:7822f4172b0c 1195 swflag = 0;
tanabe2000 0:7822f4172b0c 1196 pidT.reset();
tanabe2000 0:7822f4172b0c 1197 }
tanabe2000 0:7822f4172b0c 1198 if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
tanabe2000 0:7822f4172b0c 1199 loadingmode = 0;
tanabe2000 0:7822f4172b0c 1200 if(piddt > 8.0)airFlag = 0;
tanabe2000 0:7822f4172b0c 1201 }
tanabe2000 0:7822f4172b0c 1202
tanabe2000 0:7822f4172b0c 1203 firePwm[0] = Output_PID;
tanabe2000 0:7822f4172b0c 1204 }
tanabe2000 0:7822f4172b0c 1205
tanabe2000 0:7822f4172b0c 1206
tanabe2000 0:7822f4172b0c 1207 } else {
tanabe2000 0:7822f4172b0c 1208 if(swflag == 0){
tanabe2000 0:7822f4172b0c 1209 pidT.reset();
tanabe2000 0:7822f4172b0c 1210 swflag = 1;
tanabe2000 0:7822f4172b0c 1211 firecount++;
tanabe2000 0:7822f4172b0c 1212 }
tanabe2000 0:7822f4172b0c 1213 piddt = pidT.read();
tanabe2000 0:7822f4172b0c 1214 if((piddt > 2.0))airFlag = 1;
tanabe2000 0:7822f4172b0c 1215 dpetbotle = 0;
tanabe2000 0:7822f4172b0c 1216 led = 0;
tanabe2000 0:7822f4172b0c 1217
tanabe2000 0:7822f4172b0c 1218 solenoidValve1is = 0;
tanabe2000 0:7822f4172b0c 1219 loadingmode = 0;
tanabe2000 0:7822f4172b0c 1220 distanceOfset = -1*lmicon.getEncoder(2);
tanabe2000 0:7822f4172b0c 1221
tanabe2000 0:7822f4172b0c 1222 }
tanabe2000 0:7822f4172b0c 1223
tanabe2000 0:7822f4172b0c 1224
tanabe2000 0:7822f4172b0c 1225 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1226 // mode++;
tanabe2000 0:7822f4172b0c 1227 // }
tanabe2000 0:7822f4172b0c 1228
tanabe2000 0:7822f4172b0c 1229 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 1230 }
tanabe2000 0:7822f4172b0c 1231 //if(/*suzuki倒立mode == */) {
tanabe2000 0:7822f4172b0c 1232 // mode++;
tanabe2000 0:7822f4172b0c 1233 // }
tanabe2000 0:7822f4172b0c 1234
tanabe2000 0:7822f4172b0c 1235 /*発射機構動く*/
tanabe2000 0:7822f4172b0c 1236 }
tanabe2000 0:7822f4172b0c 1237
tanabe2000 0:7822f4172b0c 1238
tanabe2000 0:7822f4172b0c 1239
tanabe2000 0:7822f4172b0c 1240
tanabe2000 0:7822f4172b0c 1241 // if(con.getButton2(1)==0) {
tanabe2000 0:7822f4172b0c 1242 if(kabeFlag == 1) {
tanabe2000 0:7822f4172b0c 1243 for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 1244 speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 1245 wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 1246 }
tanabe2000 0:7822f4172b0c 1247 } else {
tanabe2000 0:7822f4172b0c 1248 for (int i = 0; i < 4; i++) {
tanabe2000 0:7822f4172b0c 1249 omni.computeXY(X, Y, -1*anglePID.compute());
tanabe2000 0:7822f4172b0c 1250 speed[i] = omni.wheel[i];
tanabe2000 0:7822f4172b0c 1251 wheels[i]->setSpeed(speed[i]);
tanabe2000 0:7822f4172b0c 1252 }
tanabe2000 0:7822f4172b0c 1253
tanabe2000 0:7822f4172b0c 1254
tanabe2000 0:7822f4172b0c 1255 }
tanabe2000 0:7822f4172b0c 1256 for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
tanabe2000 0:7822f4172b0c 1257 valveFire1.inputState(solenoidValve1is);
tanabe2000 0:7822f4172b0c 1258 valveFire2.inputState(solenoidValve2is);
tanabe2000 0:7822f4172b0c 1259 loading.petbottlemode(loadingmode);
tanabe2000 0:7822f4172b0c 1260
tanabe2000 0:7822f4172b0c 1261
tanabe2000 0:7822f4172b0c 1262 // if(mode > 37) mode = 0;
tanabe2000 0:7822f4172b0c 1263 // debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
tanabe2000 0:7822f4172b0c 1264
tanabe2000 0:7822f4172b0c 1265 }