NagaokaRoboticsClub_mbedTeam
未完成
Dependencies: linemiconget petbottle_Loadin5port Kinect OmniPosition PID QEI led R1307 S-ShapeModel SerialMultiByte TFmini fep2 ikarashiMDC linesSnsor omni_wheel solenoid_valve
Diff: gakuBot/gakubot.cpp
- Revision:
- 0:7822f4172b0c
- Child:
- 2:c501f6845500
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/gakuBot/gakubot.cpp Fri Oct 12 04:40:56 2018 +0000
@@ -0,0 +1,1265 @@
+#include "gakubot.h"
+
+GakuBot::GakuBot():
+ led(LED2),
+ kinect(fepTX, fepRX,151,115200),
+ lmicon(LineTX, LineRX),
+ getposition(measuringTX, measuringRX),
+ valveFire1(solenoidValve1_1, solenoidValve1_2),
+ valveFire2(solenoidValve3_1, solenoidValve3_2),
+ omni(4),
+ anglePID(angleKP, angleKI, angleKD, 0.001),
+ fire1Pid(fire1KP, fire1KI, fire1KD, RATE),
+ fire2Pid(fire2KP, fire2KI, fire2KD, RATE),
+ XPid(XKP, XIP, XDP, RATE),
+ YPid(YKP, YIP, YDP, RATE),
+ distanceFrontPid(frontKP, frontKP, frontKD, RATE),
+ distanceBehindPid(behindKP, behindKI, behindKD, RATE),
+ RS485control(PA_4),
+ RS485(MDTX,MDRX,115200),
+ debugpc(USBTX,USBRX,115200),
+ receiveSuccessed(0),
+ limit1(limitswich2_1),
+ limit2(limitswich2_2),
+ startSW(start)
+
+{
+ for(int i = 0; i < 4; i++)wheels[i] = new ikarashiMDC(&RS485control,0,i,SM,&RS485), wheels[i]->braking = true;
+ for(int i = 0; i < 2; i++)fire[i] = new ikarashiMDC(&RS485control,2,i,SM,&RS485), fire[i]->braking = false;
+ conposition = new PositionController(500.0,3000.0,0.35,-0.2,0.7);
+ for(int i = 0; i < 4; i++)omni.wheel[i].setRadian(PI / 4.0 * (2.0*i+1.0));
+ Xdemo[0] = 0, Xdemo[1] = 3912, Xdemo[2] = 3921, Xdemo[3] = 3912, Xdemo[4] = -1000;
+ Ydemo[0] = 0, Ydemo[1] = -836, Ydemo[2] = -4112, Ydemo[3] = -836, Ydemo[4] = 0;
+
+ suzuki[0] = 4700;
+ suzuki[1] = 3000;
+ suzuki[2] = 2000;
+ startSW.mode(PullUp);
+ limit1.mode(PullUp);
+ limit2.mode(PullUp);
+
+ fire1Pid.setMode(AUTO_MODE);
+ fire1Pid.setInputLimits(-100, 2300);
+ fire1Pid.setOutputLimits(-0.3, 1.0);
+ fire1Pid.setBias(0.0);
+
+ fire2Pid.setMode(AUTO_MODE);
+ fire2Pid.setInputLimits(-100, 2300);
+ fire2Pid.setOutputLimits(-0.3, 1.0);
+ fire2Pid.setSetPoint(fireDistance2);
+
+ XPid.setBias(0.0);
+ XPid.setMode(AUTO_MODE);
+ XPid.setInputLimits(-10000, 10000);
+ XPid.setOutputLimits(-0.8, 0.8);
+
+ YPid.setBias(0.0);
+ YPid.setMode(AUTO_MODE);
+ YPid.setInputLimits(-10000, 10000);
+ YPid.setOutputLimits(-0.8, 0.8);
+
+ anglePID.setMode(AUTO_MODE);
+ anglePID.setInputLimits(-360, 360);
+ anglePID.setOutputLimits(-1.0, 1.0);
+ anglePID.setSetPoint(0.0);
+ anglePID.setBias(0.0);
+
+ distanceFrontPid.setBias(0.0);
+ distanceFrontPid.setMode(AUTO_MODE);
+ distanceFrontPid.setInputLimits(30.0, 35.0);
+ distanceFrontPid.setOutputLimits(-0.5, 0.0);
+ distanceFrontPid.setSetPoint(40);
+
+ distanceBehindPid.setBias(0.0);
+ distanceBehindPid.setMode(AUTO_MODE);
+ distanceBehindPid.setInputLimits(30.0, 70.0);
+ distanceBehindPid.setOutputLimits(-0.7, 0.7);
+ distanceBehindPid.setSetPoint(40);
+
+// conposition->targetXY(2000.0, 1000.0);
+
+ pt.start();
+ confirmT.start();
+ fireFrag1time.start();
+ pidT.start();
+ t.start();
+ getx = 0;
+
+ gety = 0;
+ swflag = 1;
+ airFlag = 1;
+
+}
+
+void GakuBot::botConfirm()
+{
+ //if(con.getButton2(2)==0) fireDistance1 += 10, fireDistance2 += 10;
+// if(con.getButton2(3)==0) fireDistance1 -= 10, fireDistance2 -= 10;
+// receiveSuccessed = con.receiveState();
+ kinect.newdata();
+ kinectDistance = kinect.get_distance();
+ kinectmode = kinect.get_mode();
+ lmicon.receiveState();
+ nowPals = -1*lmicon.getEncoder(2);
+ nowPals2 = lmicon.getEncoder(0);
+ fire1Pid.setSetPoint(fireDistance1);
+ fire1Pid.setProcessValue(nowPals - distanceOfset);
+ Output_PID = -1*fire1Pid.compute();
+ fire2Pid.setSetPoint(fireDistance2);
+ fire2Pid.setProcessValue(nowPals2 - distanceOfset2);
+ Output_PID2 = -1*fire2Pid.compute();
+
+ yawdegree = getposition.getTheta();
+ if((yawdegree - beforeYaw) > 350)yawMode--;
+ else if((yawdegree - beforeYaw) < -350)yawMode++;
+ beforeYaw = yawdegree;
+ nowAngle = 360*yawMode + yawdegree;
+ anglePID.setSetPoint(attachAngle);
+ anglePID.setProcessValue(nowAngle - ofsetNowAngle);
+ distanceBehindPid.setProcessValue(lmicon.getDistance(1));
+
+ if((int)startSW==0 && (pdt > 0.5)) {
+ //getx = getposition.getX();
+// gety = getposition.getY();
+// pt.reset();
+ //demomode++;
+// if(demomode > 3)demomode = 0;
+ }
+ //getx = Xdemo[demomode];
+// gety = Ydemo[demomode];
+
+
+// debugpc.printf("xVector = %f, yVector = %f\r\n", conposition->getVelocityX(), conposition->getVelocityY());
+ //debugpc.printf("XPid.compute()<%f>, YPid.compute()<%f>\r\n", XPid.compute(), YPid.compute());
+ debugpc.printf("x = %d, y = %d\r\n", getposition.getX(), getposition.getY());
+// debugpc.printf("limit1 = %d, limit2 = %d", (int)limit1, (int)limit2);
+// debugpc.printf("rad<%f>", targetradians*180/PI);
+// debugpc.printf("getx<%d>, gety<%d>\r\n", getx, gety);
+
+
+
+}
+
+//void GakuBot::controllerMode1()
+//{
+//// if(receiveSuccessed) {
+// for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
+// if(con.getButton2(1)==0) {
+//
+//// omni.computeXY(conposition->getVelocityX(), conposition->getVelocityY(), -1*anglePID.compute());
+//// omni.computeXY(X, Y, -1*anglePID.compute());
+//
+//
+//
+//// debugpc.printf("X<%f>, Y<%f>\r\n", X, Y);
+// omni.computeXY(X, Y, -1*anglePID.compute());
+//// debugpc.printf("con.getButton1(4) = %d\r\n", con.getButton1(4));
+//
+// //if((limit1 == 1) && (limit2 == 1)) {
+//// omni.computeXY(0.5, 0.0, -1*anglePID.compute());
+//// } else if((limit1 != 1) && (limit2 == 1)) {
+//// omni.computeXY(0.5, -0.08, -1*anglePID.compute()+0.025);
+//// } else if((limit1 == 1) && (limit2 != 1)) {
+//// omni.computeXY(0.5, -0.08, -1*anglePID.compute()-0.025);
+//// } else {
+//// omni.computeXY(0.5, -0.15, -1*anglePID.compute());
+//// }
+// } else if((-0.1 < stick[2] )&& (stick[2] < 0.1)) {
+// omni.computeXY(stick[0]/2,stick[1]/2, -1*anglePID.compute());
+//// debugpc.printf("- distanceBehindPid.compute()<%f>", distanceBehindPid.compute());
+// Xlopassb = 0;
+// Ylopassb = 0;
+// } else {
+// omni.computeXY(stick[0]/3.0,stick[1]/3.0, -1*stick[2]/6.0);
+// ofsetNowAngle = nowAngle;
+// attachAngle = 0;
+// }
+//
+// confirmDt = confirmT.read();
+// //if((con.getButton2(0)==0) && (confirmDt >= 0.5))attachAngle += 10, confirmT.reset();
+//// if((con.getButton2(1)==0) && (confirmDt >= 0.5))attachAngle -= 10, confirmT.reset();
+//// debugpc.printf("attach = %f\r\n", attachAngle);
+//
+// for (int i = 0; i < 4; i++) {
+// speed[i] = omni.wheel[i];
+// wheels[i]->setSpeed(speed[i]);
+// }
+// //} else {
+//// debugpc.printf("error\r\n");
+//// for(int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
+////
+//// }
+//}
+//
+//void GakuBot::controllerMode2()
+//{
+// if(receiveSuccessed) {
+// for(int i = 0; i < 4; i++) stick[i] = con.getStick(i);
+// omni.computeXY(stick[0]/3.0 ,stick[1]/3.0 , -1*stick[2]/5.0);
+// for (int i = 0; i < 4; i++) {
+// speed[i] = omni.wheel[i];
+// wheels[i]->setSpeed(speed[i]);
+// }
+// } else {
+// debugpc.printf("error\r\n");
+// for (int i = 0; i < 4; i++)wheels[i]->setSpeed(0);
+// }
+//}
+//
+//void GakuBot::controllMech1()
+//{
+//// if(receiveSuccessed) {
+// pdt = pt.read();
+// if((con.getButton1(1) == 0) && (pdt > 0.5)) loadingmode++, pt.reset();
+// if(loadingmode == 2 ) loadingmode++;
+// if(loadingmode > 3) loadingmode = 0;
+// loading.petbottlemode(loadingmode);
+// debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
+//// debugpc.printf("pals2 = %d,fireDistance2 = %d, distanceOfset2 = %d", nowPals2 - distanceOfset2,fireDistance2,distanceOfset2);
+// if((con.getButton1(5)==0)&&(airFlag == 0) && (pdt > 0.5))airFlag = 1, pt.reset();
+// pdt = pt.read();
+// if((con.getButton1(5)==0)&&(airFlag == 1) && (pdt > 0.5))airFlag = 0, pt.reset();
+//
+// //if(airFlag == 1) {
+//// led = 1;
+//// valveFire1.inputState(1);
+//// if(con.getButton1(3)==0) {
+//// if(sw3flag == 1) {
+//// fireDistance1 = 750;
+//// sw3flag = 0;
+//// } else firePwm[0] = Output_PID;
+//// } else if((con.getButton1(0) == 0)) {
+////// fireDistance1 = 1928;
+//// fireDistance1 = 1847;
+//// firePwm[0] = Output_PID;
+//// sw3flag = 1;
+//// } else {
+//// if(con.getButton1(2)==0) firePwm[0] = 0.9;
+//// if(con.getButton1(6)==0) firePwm[0] = -0.9;
+//// if(con.getButton1(2) && con.getButton1(6)) firePwm[0] = 0.0;
+//// }
+//// } else {
+//// sw3flag = 1;
+//// valveFire1.inputState(0);
+//// distanceOfset = nowPals;
+//// }
+//
+// if(airFlag == 1) {
+// led = 1;
+// valveFire2.inputState(1);
+// if(con.getButton1(3)==0) {
+// if(sw3flag == 1) {
+// fireDistance2 = 810;
+// sw3flag = 0;
+// } else firePwm[1] = Output_PID2;
+// } else if((con.getButton1(0) == 0)) {
+// // fireDistance1 = 1928;
+// fireDistance2 = 1847;
+// firePwm[1] = Output_PID2;
+// sw3flag = 1;
+// } else {
+// if(con.getButton1(2)==0) firePwm[1] = 0.9;
+// if(con.getButton1(6)==0) firePwm[1] = -0.9;
+// if(con.getButton1(2) && con.getButton1(6)) firePwm[1] = 0.0;
+// }
+// } else {
+// sw3flag = 1;
+// valveFire2.inputState(0);
+// distanceOfset2 = nowPals2;
+// }
+//
+//
+// //debugpc.printf("Output_PID:%f",Output_PID);
+// //for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine1(i));
+//// for(int i = 0; i < 5; i++)debugpc.printf("<%4d>",lmicon.getLine2(i));
+//// for(int i = 0; i < 2; i++)debugpc.printf("<%4d>",lmicon.getDistance(i));
+// for(int i = 0; i < 3; i++)debugpc.printf("<%4d>",lmicon.getEncoder(i));
+//// debugpc.printf("sw<%2d>",(int)startSW);
+//
+// debugpc.printf("\r\n");
+// fire[0]->setSpeed(firePwm[0]);
+// fire[1]->setSpeed(firePwm[1]);
+//// } else for(int i = 0; i < 0; i++) fire[i]->setSpeed(0.0);
+//}
+
+
+
+void GakuBot::autoMode1()
+{
+ piddt = pidT.read();
+ XPid.setSetPoint(getx);
+ XPid.setProcessValue(getposition.getX());
+ YPid.setSetPoint(gety);
+ YPid.setProcessValue(getposition.getY());
+ conposition->targetXY(getx, gety);
+ conposition->compute(getposition.getX(), getposition.getY());
+ targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
+ if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
+ else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
+ if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
+ else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
+
+
+ if((getx - getposition.getX()) >=0 ) {
+ if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
+ else Xlopassb = Xlopass;
+ } else {
+ if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
+ else Xlopassb = Xlopass;
+ }
+ if((gety - getposition.getY()) >=0) {
+ if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
+ else Ylopassb = Ylopass;
+ } else {
+
+ if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
+ else Ylopassb = Ylopass;
+ }
+ X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
+ Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
+
+ dt = t.read();
+// debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
+ debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
+// debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
+ if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
+ if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
+ if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
+//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+ if((int)startSW == 0)start_ = 2;
+
+ if(start_ == 2){
+
+ if(airFlag == 1 && firecount <= 2) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = loadingParamator, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 1120;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+ }
+
+
+// if((int)startSW == 0)start_ = 1;
+ if((start_ == 0)) {
+
+ getx =0;
+ gety =0;
+ }
+ if((mode == 0) && (start_ == 1)) {
+
+ mode++;
+ getx = dash1X;
+ gety = dash1Y;
+ }
+ if(mode == 1 && getposition.getX() >= dash1X && getposition.getY() <= dash1Y) {
+
+ mode++;
+ getx = dash2X;
+ gety = dash2Y;
+ }
+ if(mode == 2 && getposition.getX() >= dash2X && getposition.getY() <= dash2Y) {
+
+ mode++;
+ getx = dash3X;
+ gety = dash3Y;
+ }
+ if(mode == 3 && getposition.getX() >= dash3X-200 && getposition.getY() <= dash3Y+200) {
+ //if(firecount > 3 && (dt >= 1.0)) {
+// mode++;
+// }
+
+ if(airFlag == 1 && firecount <= 2) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 1120;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+// mode++;
+ }
+ if(mode == 4 && kinectFlag == 1) {
+ if(getposition.getX() > table1800X - 200) {
+
+ mode++;
+ getx = table1800X;
+ gety = -1*suzuki[0]-1000;
+ kabeFlag = 0;
+ swflag = 1;
+ airFlag = 1;
+ } else {
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(0.3, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0.3, -0.15, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ }
+
+ if(mode == 5) {
+ if(kinectmode == 3){
+ mode++;
+ }else{
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 1010;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+ }
+ //if(((int)startSW == 0) && (dt >= 0.5)) {
+// mode++;
+// t.reset();
+// }
+
+ /*発射機構動く*/
+
+}
+
+if(mode == 6 && (-1*suzuki[1]-1000 < wallmodeY) && kinectFlag == 2) {
+// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
+ if(getposition.getY() < walldashY) {
+ if(getposition.getX() < table1500X + 400) {
+
+ mode++;
+ getx = table1500X;
+ gety = -1*suzuki[1]-1000;
+ kabeFlag = 0;swflag = 1;
+ airFlag = 1;
+ } else {
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0, -0.18, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ } else {
+ gety = wallmodeY;
+ }
+ }
+ if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
+swflag = 1;
+ airFlag = 1;
+ if(suzuki[0] < suzuki[1]) {
+ gety = -1*suzuki[1]-1000;
+ if(getposition.getY() < -1*suzuki[1]-1000) {
+ mode++;
+ getx = table1500X;
+ }
+ } else {
+ getx = table1500X;
+ if(getposition.getX() < table1500X ) {
+ mode++;
+ gety = -1*suzuki[1]-1000;
+ }
+ }
+
+ }
+ if(mode == 7) {
+if(kinectmode == 5){
+ mode++;
+ }else{
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 820;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+}
+}
+if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
+// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
+swflag = 1;
+ airFlag = 1;
+ if(getposition.getY() < walldashY) {
+ if(getposition.getX() < table1200X + 200) {
+
+ mode++;
+ getx = table1200X;
+ gety = -1*suzuki[2]-1200;
+ kabeFlag = 0;
+ } else {
+
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0, -0.18, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ } else {
+ gety = walldashY;
+ }
+ }
+ if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
+swflag = 1;
+ airFlag = 1;
+ if(suzuki[1] < suzuki[2]) {
+ gety = -1*suzuki[2]-1200;
+ if(getposition.getY() < -1*suzuki[2]-800) {
+ mode++;
+ getx = table1200X;
+ }
+ } else {
+ getx = table1200X;
+ if(getposition.getX() < table1200X) {
+ mode++;
+ gety = -1*suzuki[2]-1200;
+ }
+ }
+
+ }
+
+
+ if(mode == 9) {
+if(kinectmode == 7){
+
+ }else{
+
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 870;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+}
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+ }
+
+
+
+
+// if(con.getButton2(1)==0) {
+ if(kabeFlag == 1) {
+ for (int i = 0; i < 4; i++) {
+ speed[i] = omni.wheel[i];
+ wheels[i]->setSpeed(speed[i]);
+ }
+ } else {
+ for (int i = 0; i < 4; i++) {
+ omni.computeXY(X, Y, -1*anglePID.compute());
+ speed[i] = omni.wheel[i];
+ wheels[i]->setSpeed(speed[i]);
+ }
+
+
+ }
+ for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
+ valveFire1.inputState(solenoidValve1is);
+ valveFire2.inputState(solenoidValve2is);
+ loading.petbottlemode(loadingmode);
+
+
+// if(mode > 37) mode = 0;
+// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
+
+}
+void GakuBot::autoMode2()
+{
+ piddt = pidT.read();
+ XPid.setSetPoint(-1.0*getx);
+ XPid.setProcessValue(getposition.getX());
+ YPid.setSetPoint(-1.0*gety);
+ YPid.setProcessValue(getposition.getY());
+ conposition->compute(getposition.getX(), getposition.getY());
+ targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
+ if(cos(targetradians) < 0)Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*(-1*cos(targetradians)));
+ else Xlopass = (KA * Xlopassb + (1 - KA) * XPid.compute()*cos(targetradians));
+ if(sin(targetradians) < 0) Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*(-1*sin(targetradians)));
+ else Ylopass = (KA * Ylopassb + (1 - KA) * YPid.compute()*sin(targetradians));
+
+
+ if((getx - getposition.getX()) >=0 ) {
+ if(Xlopassb > Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
+ else Xlopassb = Xlopass;
+ } else {
+ if(Xlopassb < Xlopass)Xlopass = XPid.compute(), Xlopassb = 0;
+ else Xlopassb = Xlopass;
+ }
+ if((gety - getposition.getY()) >=0) {
+ if(Ylopassb > Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
+ else Ylopassb = Ylopass;
+ } else {
+
+ if(Ylopassb < Ylopass)Ylopass = YPid.compute(), Ylopassb = 0;
+ else Ylopassb = Ylopass;
+ }
+ X = (Xlopass*cos((float)nowAngle*PI/180.0) - Ylopass*sin((float)nowAngle*PI/180.0));
+ Y = (Xlopass*sin((float)nowAngle*PI/180.0) + Ylopass*cos((float)nowAngle*PI/180.0));
+
+ dt = t.read();
+// debugpc.printf("piddt <%f>dpetbotle<%d>\r\n",piddt, dpetbotle);
+ debugpc.printf("pals = %d,fireDistance1 = %d, distanceOfset = %d", nowPals - distanceOfset,fireDistance1,distanceOfset);
+// debugpc.printf("kinectDistance<%d>,kinectmode<%d>\n\r",kinectDistance,kinectmode);
+ if((kinectmode == 2) && (kinectFlag == 0))kinectFlag = 1,suzuki[0] = kinectDistance ;
+ if((kinectmode == 4) && (kinectFlag == 1))kinectFlag = 2, suzuki[1] = kinectDistance;
+ if((kinectmode == 6) && (kinectFlag == 2))kinectFlag = 3, suzuki[2] = kinectDistance;
+//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+//debugpc.printf("loading.LoadingEncoder()<%d>\r\n",loading.LoadingEncoder());
+
+ if((int)startSW == 0)start_ = 1;
+ if((start_ == 0)) {
+
+ getx =0;
+ gety =0;
+ }
+ if((mode == 0) && (start_ == 1)) {
+
+ mode++;
+ getx = dash1X;
+ gety = dash1Y;
+ }
+ if(mode == 1 && getposition.getX() <= dash1X && getposition.getY() <= dash1Y) {
+
+ mode++;
+ getx = dash2X;
+ gety = dash2Y;
+ }
+ if(mode == 2 && getposition.getX() <= dash2X && getposition.getY() <= dash2Y) {
+
+ mode++;
+ getx = dash3X;
+ gety = dash3Y;
+ }
+ if(mode == 3 && getposition.getX() <= dash3X+200 && getposition.getY() <= dash3Y+200) {
+ //if(firecount > 3 && (dt >= 1.0)) {
+// mode++;
+// }
+
+ if(airFlag == 1 && firecount <= 2) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 1120;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+// mode++;
+ }
+ if(mode == 4 && kinectFlag == 1) {
+ if(getposition.getX() < table1800X + 200) {
+
+ mode++;
+ getx = table1800X;
+ gety = -1*suzuki[0]-1000;
+ kabeFlag = 0;
+ swflag = 1;
+ airFlag = 1;
+ } else {
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(0.3, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0.3, -0.15, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ }
+
+ if(mode == 5) {
+ if(kinectmode == 3){
+ mode++;
+ }else{
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 1010;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+ }
+ //if(((int)startSW == 0) && (dt >= 0.5)) {
+// mode++;
+// t.reset();
+// }
+
+ /*発射機構動く*/
+
+}
+
+if(mode == 6 && (-1*suzuki[1]+1000 > wallmodeY) && kinectFlag == 2) {
+// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
+ if(getposition.getY() < walldashY) {
+ if(getposition.getX() > table1500X + 400) {
+
+ mode++;
+ getx = table1500X;
+ gety = -1*suzuki[1]-1000;
+ kabeFlag = 0;swflag = 1;
+ airFlag = 1;
+ } else {
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0, -0.18, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ } else {
+ gety = wallmodeY;
+ }
+ }
+ if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
+swflag = 1;
+ airFlag = 1;
+ if(suzuki[0] < suzuki[1]) {
+ gety = -1*suzuki[1]-1000;
+ if(getposition.getY() < -1*suzuki[1]-1000) {
+ mode++;
+ getx = table1500X;
+ }
+ } else {
+ getx = table1500X;
+ if(getposition.getX() > table1500X ) {
+ mode++;
+ gety = -1*suzuki[1]-1000;
+ }
+ }
+
+ }
+ if(mode == 7) {
+if(kinectmode == 5){
+ mode++;
+ }else{
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 820;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+}
+}
+if(mode == 8 && (-1*suzuki[2]-1000 < wallmodeY) && kinectFlag == 3) {
+// debugpc.printf("-1*suzuki[1]-1000 < -3500<%d>",-1*suzuki[1]-1000 );
+swflag = 1;
+ airFlag = 1;
+ if(getposition.getY() < walldashY) {
+ if(getposition.getX() > table1200X - 200) {
+
+ mode++;
+ getx = table1200X;
+ gety = -1*suzuki[2]-1200;
+ kabeFlag = 0;
+ } else {
+
+ if((limit1 == 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ } else if((limit1 != 1) && (limit2 == 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ } else if((limit1 == 1) && (limit2 != 1)) {
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
+ } else {
+ omni.computeXY(0, -0.18, -1*anglePID.compute());
+ }
+ kabeFlag = 1;
+ }
+ } else {
+ gety = walldashY;
+ }
+ }
+ if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
+swflag = 1;
+ airFlag = 1;
+ if(suzuki[1] < suzuki[2]) {
+ gety = -1*suzuki[2]-1200;
+ if(getposition.getY() < -1*suzuki[2]-800) {
+ mode++;
+ getx = table1200X;
+ }
+ } else {
+ getx = table1200X;
+ if(getposition.getX() > table1200X) {
+ mode++;
+ gety = -1*suzuki[2]-1200;
+ }
+ }
+
+ }
+
+ if(mode == 9) {
+if(kinectmode == 7){
+
+ }else{
+
+ if(airFlag == 1) {
+ led = 1;
+ solenoidValve1is = 1;
+
+
+
+ if(((nowPals - distanceOfset) > 1800) && (dpetbotle == 0)) {
+ pidT.reset();
+ dpetbotle = 1;
+
+ }
+ piddt = pidT.read();
+ if((piddt > 4.0) && (dpetbotle == 1)) {
+ loadingmode = 3;
+ firePwm[0] = 0.0;
+
+ }
+ if(loadingmode != 3)fireDistance1 = 1955, firePwm[0] = Output_PID, loadingmode = 1;
+ if((loadingmode == 3) && (loading.LoadingEncoder() < 700)) {
+ if(swflag == 1) {
+ fireDistance1 = 870;
+ swflag = 0;
+ pidT.reset();
+ }
+ if((loading.LoadingEncoder() < 40) && (piddt > 4.0)) {
+ loadingmode = 0;
+ if(piddt > 8.0)airFlag = 0;
+ }
+
+ firePwm[0] = Output_PID;
+ }
+
+
+ } else {
+ if(swflag == 0){
+ pidT.reset();
+ swflag = 1;
+ firecount++;
+ }
+ piddt = pidT.read();
+ if((piddt > 2.0))airFlag = 1;
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+}
+ //if(/*suzuki倒立mode == */) {
+// mode++;
+// }
+
+ /*発射機構動く*/
+ }
+
+
+
+
+// if(con.getButton2(1)==0) {
+ if(kabeFlag == 1) {
+ for (int i = 0; i < 4; i++) {
+ speed[i] = omni.wheel[i];
+ wheels[i]->setSpeed(speed[i]);
+ }
+ } else {
+ for (int i = 0; i < 4; i++) {
+ omni.computeXY(X, Y, -1*anglePID.compute());
+ speed[i] = omni.wheel[i];
+ wheels[i]->setSpeed(speed[i]);
+ }
+
+
+ }
+ for(int m = 0; m < 2; m++)fire[m]->setSpeed(firePwm[m]);
+ valveFire1.inputState(solenoidValve1is);
+ valveFire2.inputState(solenoidValve2is);
+ loading.petbottlemode(loadingmode);
+
+
+// if(mode > 37) mode = 0;
+// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
+
+}
\ No newline at end of file