2015_robocon_bteam
/
2015robot_main_zenkoku
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main_ps3.cpp
- Revision:
- 36:b8954b13a6d5
- Parent:
- 35:7b6786193aa2
- Child:
- 37:75fcd28f48c7
diff -r 7b6786193aa2 -r b8954b13a6d5 main_ps3.cpp
--- a/main_ps3.cpp Sat Nov 14 04:05:13 2015 +0000
+++ b/main_ps3.cpp Sun Nov 15 00:30:44 2015 +0000
@@ -1,5 +1,5 @@
/**
- * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
+ * This program is written for 2015 NHK Robot Contest (Bteam).
*/
/***コース選択***/
@@ -44,7 +44,8 @@
autoIM920(); /*IM920 button*/
#ifdef BLUE
/********************************Own & Middle Mode*********************************/
- if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+// if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+ if((step==0)&&((10000.0>x)&&(x>940.0))) {
targ_sita=-0.03;
// targ_sita=0.0;
step=1;
@@ -73,7 +74,7 @@
}
//Cylinder
- if((x>3200.0)&&(CStep==0)) {
+ if((x>3250.0)&&(CStep==0)) {
if(!skip) sendData(1,1);
CStep=1;
}
@@ -85,11 +86,24 @@
if(!skip) sendData(1,3);
CStep=3;
}
- if((x<7200.0)&&(CStep==3)) {
- if(!skip) sendData(1,4); //over
+ if((x>8000.0)&&(CStep==3)) {
+ if(!skip) sendData(1,8);
CStep=4;
}
- if((x<6950.0)&&(CStep==4)) {
+ /*if((x<7200.0)&&(CStep==4)) {
+ if(!skip) sendData(1,4); //over
+ CStep=5;
+ }
+ if((x<6950.0)&&(CStep==5)) {
+ if(!skip) sendData(1,6); //fornt
+ CStep=6;
+ }
+ if((x<6700.0)&&(CStep==6)) {
+ if(!skip) sendData(1,5); //middle
+ CStep=7;
+ }*/
+ /////////////////////////////////////////
+ if((x<7850.0)&&(CStep==4)) {
if(!skip) sendData(1,6); //front
CStep=5;
}
@@ -97,7 +111,12 @@
if(!skip) sendData(1,5); //middle
CStep=6;
}
- if((x<3000.0)&&(CStep==6)){
+ if((x<5600.0)&&(CStep==6)) {
+ if(!skip) sendData(1,4); //over
+ CStep=7;
+ }
+ /////////////////////////////////////////
+ if((x<3000.0)&&(CStep==7)){
sendData(7,0);
CStep=114;
}
@@ -144,7 +163,10 @@
if(!skip) sendData(1,3);
CStep=13;
}
- if((x>9500.0)&&(CStep==13)) CStep=14;
+ if((x>9500.0)&&(CStep==13)) {
+ if(!skip) sendData(1,8);
+ CStep=14;
+ }
if((x<9400.0)&&(CStep==14)) {
if(!skip) sendData(1,4);
CStep=15;
@@ -164,7 +186,7 @@
}
#else
/********************************Own & Middle Mode*********************************/
- if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+ if((step==0)&&((10000.0>x)&&(x>800.0))) {
targ_sita=0.03;
// targ_sita=0.0;
step=1;
@@ -181,7 +203,7 @@
spcount=0.0;
flagf=0;
// targ_sita=0.0;
- targ_sita=-0.03;
+ targ_sita=0.03;
}
if((step==3)&&(x<1700.0)) {
targ_sita=-PI/4;
@@ -193,31 +215,99 @@
}
//Cylinder
- if((x>3250.0)&&(CStep==0)) {
+ if((x>3100.0)&&(CStep==0)) {
if(!skip) sendData(1,1);
CStep=1;
}
- if((x>6200.0)&&(CStep==1)) {
+ if((x>6050.0)&&(CStep==1)) {
if(!skip) sendData(1,3);
CStep=2;
}
- if((x>7800.0)&&(CStep==2)) {
+ if((x>7650.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
- if((x<7000.0)&&(CStep==3)) {
- if(!skip) sendData(1,6); //over
+ if((x>8000.0)&&(CStep==3)) {
+ if(!skip) sendData(1,8);
CStep=4;
}
- if((x<6920.0)&&(CStep==4)) {
- if(!skip) sendData(1,5); //middle
+ if((x<7200.0)&&(CStep==4)) {
+ if(!skip) sendData(1,6); //over
CStep=5;
}
- if((x<6900.0)&&(CStep==5)) {
+ if((x<6950.0)&&(CStep==5)) {
if(!skip) sendData(1,4); //front
CStep=6;
}
- if((x<3000.0)&&(CStep==6)){
+ if((x<6700.0)&&(CStep==6)) {
+ if(!skip) sendData(1,5); //middle
+ CStep=7;
+ }
+ if((x<3000.0)&&(CStep==7)){
+ sendData(7,0);
+ CStep=114;
+ }
+
+ /********************************Own & Opponent Mode*********************************/
+ if((step==10)&&((9800.0>x)&&(x>700.0))) {
+ targ_sita=-0.03;
+// targ_sita=0.0;
+ step=11;
+ }
+ if((step==11)&&(x>10000.0)) {
+ targ_velocity=0.0;
+ velocity_controller.setBias(0.0);
+ velocity_controller.reset();
+ step=12;
+ }
+ if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
+ step=13;
+ spcount=0.0;
+ flagf=0;
+// flagf=3;
+// targ_sita=0.0;
+ targ_sita=-0.03;
+ }
+ if((step==13)&&(x<1400.0)) {
+ targ_sita=PI/4;
+ step=14;
+ }
+ if((step==14)&&(x<600.0)) {
+ dpcount=speed;
+ step=114;
+ }
+
+ //Cylinderd
+ if((x>3200.0)&&(CStep==10)) {
+ if(!skip) sendData(1,1);
+ CStep=11;
+ }
+ if((x>6050.0)&&(CStep==11)) {
+ if(!skip) sendData(1,3);
+ CStep=12;
+ }
+ if((x>7700.0)&&(CStep==12)) {
+ if(!skip) sendData(1,2);
+ CStep=13;
+ }
+ if((x>9500.0)&&(CStep==13)) {
+ if(!skip) sendData(1,8);
+ CStep=14;
+ }
+ if((x<9400.0)&&(CStep==14)) {
+ if(!skip) sendData(1,6); //over
+ CStep=15;
+ }
+ if((x<7250.0)&&(CStep==15)) {
+ if(!skip) sendData(1,5); //middle
+ CStep=16;
+ }
+ if((x<4850.0)&&(CStep==16)) {
+// if((x<5450.0)&&(CStep==16)) {
+ if(!skip) sendData(1,4); //front
+ CStep=17;
+ }
+ if((x<1000.0)&&(CStep==17)){
sendData(7,0);
CStep=114;
}
@@ -227,46 +317,91 @@
else if(!autoflag){
manualIM920(); /*IM920 button*/
#ifdef BLUE
- /********************************Swing Mode*********************************/
- if((mstep==0)&&((10000.0>x)&&(x>1700.0))) {
+ /********************************Swing Mode(middle)*********************************/
+ if((mstep==0)&&((10000.0>x)&&(x>1500.0))) {
dpcount=speed;
mstep=1;
}
if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
targ_sita=PI/4.0;
+ swingmoved=1;
+ mstep=114;
+ }
+ if(mstep==2){
+ mstep=3;
+ targ_sita=3.0*PI/4.0;
+ }
+ if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
+ mstep=4;
+ }
+ if((mstep==4)&&(x<400.0)){
+ targ_sita=PI;
+ mstep=5;
+ }
+ if((mstep==5)&&(x<0.0)){
+ targ_velocity=0.0;
mstep=114;
}
- /********************************Opponent Struct Mode*********************************/
- if((mstep==10)&&((10000.0>x)&&(x>1150.0))) {
- targ_sita=-0.03;
-// targ_sita=0.0;
+
+ /********************************Swing Mode(over)*********************************/
+ if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
+ dpcount=speed;
mstep=11;
}
- /*if((mstep==11)&&(x>8000.0)) {
- targ_velocity=0.0;
+ if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
+ targ_sita=PI/4.0+PI/16.0;
+ swingmoved=2;
+ mstep=114;
+ }
+ if(mstep==12){
mstep=13;
- }*/
- if((mstep==11)&&(x>8000.0)) {
- targ_sita=-PI/4.0;
- mstep=12;
+ targ_sita=3.0*PI/4.0;
}
- if((mstep==12)&&(x>8700.0)) {
- targ_velocity=0.0;
- mstep=13;
- }
- if((mstep==13)&&((velocity<5.0)&&(velocity>-5.0))){
- targ_sita=0.0;
+ if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
mstep=14;
}
- /*if((mstep==14)&&((sita<0.01)&&(sita>-0.01))){
- sendData(1,6);
+ if((mstep==14)&&(x<400.0)){
+ targ_sita=PI;
+ mstep=15;
+ }
+ if((mstep==15)&&(x<0.0)){
+ targ_velocity=0.0;
+ mstep=114;
+ }
+
+ /********************************Swing Mode(over)*********************************/
+ if((mstep==20)&&((10000.0>x)&&(x>1500.0))) {
+ dpcount=speed;
+ mstep=21;
+ }
+ if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
+ targ_sita=PI/4.0+PI/16.0;
+ swingmoved=3;
mstep=114;
- }*/
- /*if((mCStep==0)&&(x>7100.0)){
- sendData(1,4);
- mCStep=114;
- }*/
+ }
+ if(mstep==22){
+ mstep=23;
+ targ_sita=3.0*PI/4.0;
+ }
+ if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
+ mstep=24;
+ }
+ if((mstep==24)&&(x<400.0)){
+ targ_sita=PI;
+ mstep=25;
+ }
+ if((mstep==25)&&(x<0.0)){
+ targ_velocity=0.0;
+ mstep=114;
+ }
+
#else
#endif
}