NagaokaRoboticsClub_mbedTeam
未完成
Dependencies: linemiconget petbottle_Loadin5port Kinect OmniPosition PID QEI led R1307 S-ShapeModel SerialMultiByte TFmini fep2 ikarashiMDC linesSnsor omni_wheel solenoid_valve
Revision 2:c501f6845500, committed 2018-10-16
- Comitter:
- tanabe2000
- Date:
- Tue Oct 16 03:32:37 2018 +0000
- Parent:
- 1:98b9d307510e
- Commit message:
- 2018 NHK auto A team
Changed in this revision
--- 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
--- a/gakuBot/gakubot.h Fri Oct 12 04:48:22 2018 +0000 +++ b/gakuBot/gakubot.h Tue Oct 16 03:32:37 2018 +0000 @@ -78,7 +78,7 @@ #define wallmodeY -4300 #define walldashY -4700 -#define wallslide 0.5 +#define wallslide 0.3
--- a/gakuBot/petbottle_fire.lib Fri Oct 12 04:48:22 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/tanabe2000/code/petbottle_fire/#2cab7399fe0a
--- a/main.cpp Fri Oct 12 04:48:22 2018 +0000
+++ b/main.cpp Tue Oct 16 03:32:37 2018 +0000
@@ -7,22 +7,18 @@
//petbottleLoading loading;
Serial pc(USBTX, USBRX, 115200);
Timer mainT;
+DigitalIn toggle(PC_3);
int main()
{
-// mainT.start();
+ toggle.mode(PullUp);
while(1) {
-// mainT.reset();
-// pc.printf("%f\r\n",mainT.read());
- gakugaku.botConfirm();
-// gakugaku.controllerMode1();
-// gakugaku.controllMech1();
- gakugaku.autoMode1();
-// wait(0.01);
-
+ gakugaku.botConfirm();//いじるな
-// loading.petbottlemode();
+// gakugaku.autoMode1();//赤
+ gakugaku.autoMode2();//青
+
- }
+}
}
\ No newline at end of file