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:
- 2:c501f6845500
- Parent:
- 0:7822f4172b0c
--- a/gakuBot/gakubot.cpp Fri Oct 12 04:48:22 2018 +0000
+++ b/gakuBot/gakubot.cpp Tue Oct 16 03:32:37 2018 +0000
@@ -128,7 +128,7 @@
//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());
@@ -320,76 +320,20 @@
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("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((int)startSW == 0)start_ = 1;
- }
- 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;
- }
-
+ if((start_ == 0)) {
- } 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;
- }
+ getx =0;
+ gety =0;
+ }
if((mode == 0) && (start_ == 1)) {
mode++;
@@ -408,28 +352,28 @@
getx = dash3X;
gety = dash3Y;
}
- if(mode == 3 && getposition.getX() >= dash3X-200 && getposition.getY() <= dash3Y+200) {
- //if(firecount > 3 && (dt >= 1.0)) {
-// mode++;
-// }
-
+ if(mode == 3 && getposition.getX() >= dash3X-400 && getposition.getY() <= dash3Y+400) {
+ if(firecount > 2 && (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) {
@@ -441,117 +385,117 @@
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
-
+
firePwm[0] = Output_PID;
}
-
+
} else {
- if(swflag == 0){
- pidT.reset();
+ if(swflag == 0) {
+ pidT.reset();
swflag = 1;
firecount++;
- }
- piddt = pidT.read();
- if((piddt > 2.0))airFlag = 1;
+ }
+ 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) {
+ if(getposition.getX() > table1800X) {
mode++;
getx = table1800X;
gety = -1*suzuki[0]-1000;
kabeFlag = 0;
swflag = 1;
- airFlag = 1;
+ airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
- omni.computeXY(0.3, 0.0, -1*anglePID.compute());
+ omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
- omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
+ omni.computeXY(wallslide, -0.05, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
- omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
+ omni.computeXY(wallslide, -0.05, -1*anglePID.compute()-0.025);
} else {
- omni.computeXY(0.3, -0.15, -1*anglePID.compute());
+ omni.computeXY(wallslide, -0.15, -1*anglePID.compute());
}
kabeFlag = 1;
}
}
if(mode == 5) {
- if(kinectmode == 3){
- mode++;
- }else{
+ if(kinectmode == 3) {
+ mode++;
+ } else {
if(airFlag == 1) {
- led = 1;
- solenoidValve1is = 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(((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;
+ 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;
}
-
- firePwm[0] = Output_PID;
- }
-
+
- } else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+ } 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);
-
+ 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) {
+ 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) {
@@ -559,8 +503,9 @@
mode++;
getx = table1500X;
gety = -1*suzuki[1]-1000;
- kabeFlag = 0;swflag = 1;
- airFlag = 1;
+ kabeFlag = 0;
+ swflag = 1;
+ airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
@@ -578,8 +523,8 @@
}
}
if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
-swflag = 1;
- airFlag = 1;
+ swflag = 1;
+ airFlag = 1;
if(suzuki[0] < suzuki[1]) {
gety = -1*suzuki[1]-1000;
if(getposition.getY() < -1*suzuki[1]-1000) {
@@ -596,71 +541,71 @@
}
if(mode == 7) {
-if(kinectmode == 5){
- mode++;
- }else{
- if(airFlag == 1) {
- led = 1;
- solenoidValve1is = 1;
-
+ 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(((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;
+ 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;
}
-
- firePwm[0] = Output_PID;
- }
-
+
- } else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+ } 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 == */) {
+ 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) {
+ /*発射機構動く*/
+ }
+ }
+ 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;
+ swflag = 1;
+ airFlag = 1;
if(getposition.getY() < walldashY) {
if(getposition.getX() < table1200X + 200) {
@@ -675,7 +620,7 @@
} 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);
+ omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
@@ -686,8 +631,8 @@
}
}
if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
-swflag = 1;
- airFlag = 1;
+ swflag = 1;
+ airFlag = 1;
if(suzuki[1] < suzuki[2]) {
gety = -1*suzuki[2]-1200;
if(getposition.getY() < -1*suzuki[2]-800) {
@@ -706,67 +651,67 @@
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;
- }
-
+ if(kinectmode == 7) {
} else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+
+ 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 == */) {
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
// mode++;
// }
- /*発射機構動く*/
-}
+ /*発射機構動く*/
+ }
//if(/*suzuki倒立mode == */) {
// mode++;
// }
@@ -778,17 +723,17 @@
// 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]);
- }
+ 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]);
+ }
}
@@ -800,14 +745,17 @@
// if(mode > 37) mode = 0;
// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
-
+// debugpc.printf("\r\n");
}
-void GakuBot::autoMode2()
+
+
+/******************************************************************************************************************************/
+void GakuBot::autoMode2()//青ゾーン
{
piddt = pidT.read();
- XPid.setSetPoint(-1.0*getx);
+ XPid.setSetPoint(getx);
XPid.setProcessValue(getposition.getX());
- YPid.setSetPoint(-1.0*gety);
+ YPid.setSetPoint(gety);
YPid.setProcessValue(getposition.getY());
conposition->compute(getposition.getX(), getposition.getY());
targetradians = atan2((double)(gety - getposition.getY()), (double)(getx - getposition.getX()));
@@ -842,49 +790,47 @@
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)) {
+ if((start_ == 0)) {
- getx =0;
- gety =0;
- }
+ getx =0;
+ gety =0;
+ }
if((mode == 0) && (start_ == 1)) {
mode++;
- getx = dash1X;
+ getx = -1.0*dash1X;
gety = dash1Y;
}
- if(mode == 1 && getposition.getX() <= dash1X && getposition.getY() <= dash1Y) {
+ if(mode == 1 && getposition.getX() <= -1.0*dash1X+200 && getposition.getY() <= dash1Y) {
mode++;
- getx = dash2X;
+ getx = -1.0*dash2X;
gety = dash2Y;
}
- if(mode == 2 && getposition.getX() <= dash2X && getposition.getY() <= dash2Y) {
+ if(mode == 2 && getposition.getX() <= -1.0*dash2X+200 && getposition.getY() <= dash2Y) {
mode++;
- getx = dash3X;
+ getx = -1.0*dash3X;
gety = dash3Y;
}
- if(mode == 3 && getposition.getX() <= dash3X+200 && getposition.getY() <= dash3Y+200) {
- //if(firecount > 3 && (dt >= 1.0)) {
-// mode++;
-// }
-
+ if(mode == 3 && getposition.getX() <= -1.0*dash3X+200 && getposition.getY() <= dash3Y+200) {
+ if(firecount > 2 && (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)) {
@@ -903,133 +849,134 @@
loadingmode = 0;
if(piddt > 8.0)airFlag = 0;
}
-
+
firePwm[0] = Output_PID;
}
-
+
} else {
- if(swflag == 0){
- pidT.reset();
+ if(swflag == 0) {
+ pidT.reset();
swflag = 1;
firecount++;
- }
- piddt = pidT.read();
- if((piddt > 2.0))airFlag = 1;
+ }
+ 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) {
+ if(getposition.getX() < -1.0*table1800X) {
mode++;
- getx = table1800X;
+ getx = -1.0*table1800X;
gety = -1*suzuki[0]-1000;
kabeFlag = 0;
swflag = 1;
- airFlag = 1;
+ airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
- omni.computeXY(0.3, 0.0, -1*anglePID.compute());
+ omni.computeXY(-wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
- omni.computeXY(0.3, -0.05, -1*anglePID.compute()+0.025);
+ omni.computeXY(-wallslide, -0.05, -1*anglePID.compute()+0.025);
} else if((limit1 == 1) && (limit2 != 1)) {
- omni.computeXY(0.3, -0.05, -1*anglePID.compute()-0.025);
+ omni.computeXY(-wallslide, -0.05, -1*anglePID.compute()-0.025);
} else {
- omni.computeXY(0.3, -0.15, -1*anglePID.compute());
+ omni.computeXY(-wallslide, -0.15, -1*anglePID.compute());
}
kabeFlag = 1;
}
}
if(mode == 5) {
- if(kinectmode == 3){
- mode++;
- }else{
+ if(kinectmode == 3) {
+ mode++;
+ } else {
if(airFlag == 1) {
- led = 1;
- solenoidValve1is = 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(((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;
+ 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;
}
-
- firePwm[0] = Output_PID;
- }
-
+
- } else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+ } 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);
-
+ 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) {
+ 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) {
+ if(getposition.getX() < -1.0*table1500X + 200) {
mode++;
- getx = table1500X;
+ getx = -1.0*table1500X;
gety = -1*suzuki[1]-1000;
- kabeFlag = 0;swflag = 1;
- airFlag = 1;
+ kabeFlag = 0;
+ swflag = 1;
+ airFlag = 1;
} else {
if((limit1 == 1) && (limit2 == 1)) {
- omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
- omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ omni.computeXY(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);
+ omni.computeXY(wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
@@ -1040,17 +987,17 @@
}
}
if(mode == 6 && (-1*suzuki[1]-1000 > wallmodeY) && kinectFlag == 2) {
-swflag = 1;
- airFlag = 1;
+ swflag = 1;
+ airFlag = 1;
if(suzuki[0] < suzuki[1]) {
gety = -1*suzuki[1]-1000;
if(getposition.getY() < -1*suzuki[1]-1000) {
mode++;
- getx = table1500X;
+ getx = -1.0*table1500X;
}
} else {
- getx = table1500X;
- if(getposition.getX() > table1500X ) {
+ getx = -1.0*table1500X;
+ if(getposition.getX() < -1.0*table1500X ) {
mode++;
gety = -1*suzuki[1]-1000;
}
@@ -1058,86 +1005,86 @@
}
if(mode == 7) {
-if(kinectmode == 5){
- mode++;
- }else{
- if(airFlag == 1) {
- led = 1;
- solenoidValve1is = 1;
-
+ 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(((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;
+ 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;
}
-
- firePwm[0] = Output_PID;
- }
-
+
- } else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+ } 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 == */) {
+ 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) {
+ /*発射機構動く*/
+ }
+ }
+ 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;
+ swflag = 1;
+ airFlag = 1;
if(getposition.getY() < walldashY) {
- if(getposition.getX() > table1200X - 200) {
+ if(getposition.getX() < -1.0*table1200X) {
mode++;
- getx = table1200X;
+ getx = -1.0*table1200X;
gety = -1*suzuki[2]-1200;
kabeFlag = 0;
} else {
if((limit1 == 1) && (limit2 == 1)) {
- omni.computeXY(-1*wallslide, 0.0, -1*anglePID.compute());
+ omni.computeXY(wallslide, 0.0, -1*anglePID.compute());
} else if((limit1 != 1) && (limit2 == 1)) {
- omni.computeXY(-1*wallslide, -0.08, -1*anglePID.compute()+0.025);
+ omni.computeXY(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);
+ omni.computeXY(wallslide, -0.08, -1*anglePID.compute()-0.025);
} else {
omni.computeXY(0, -0.18, -1*anglePID.compute());
}
@@ -1148,17 +1095,17 @@
}
}
if(mode == 8 && -1*suzuki[1]-1200 > wallmodeY/* && kinectFlag == 2*/) {
-swflag = 1;
- airFlag = 1;
+ swflag = 1;
+ airFlag = 1;
if(suzuki[1] < suzuki[2]) {
gety = -1*suzuki[2]-1200;
if(getposition.getY() < -1*suzuki[2]-800) {
mode++;
- getx = table1200X;
+ getx = -1.0*table1200X;
}
} else {
- getx = table1200X;
- if(getposition.getX() > table1200X) {
+ getx = -1.0*table1200X;
+ if(getposition.getX() < -1.0*table1200X) {
mode++;
gety = -1*suzuki[2]-1200;
}
@@ -1167,67 +1114,67 @@
}
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;
- }
-
+ if(kinectmode == 7) {
} else {
- if(swflag == 0){
- pidT.reset();
- swflag = 1;
- firecount++;
+
+ 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 == */) {
+ dpetbotle = 0;
+ led = 0;
+
+ solenoidValve1is = 0;
+ loadingmode = 0;
+ distanceOfset = -1*lmicon.getEncoder(2);
+
+ }
+
+
+ //if(/*suzuki倒立mode == */) {
// mode++;
// }
- /*発射機構動く*/
-}
+ /*発射機構動く*/
+ }
//if(/*suzuki倒立mode == */) {
// mode++;
// }
@@ -1239,17 +1186,17 @@
// 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]);
- }
+ 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]);
+ }
}
@@ -1262,4 +1209,5 @@
// if(mode > 37) mode = 0;
// debugpc.printf("mode = %d,time = %f, kinectFlag<%d>\r\n",mode,dt, kinectFlag);
+
}
\ No newline at end of file