2015_robocon_bteam
/
2015robot_main
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.
Fork of
2015robot_main
by 
Naoto Deguchi
Diff: main_ps3.cpp
- Revision:
- 78:abb760e0a935
- Parent:
- 68:2b2b88ecdcce
- Parent:
- 77:7dd154b5ace3
- Child:
- 79:7f86e18f40ef
--- a/main_ps3.cpp Sat Oct 10 06:01:19 2015 +0000
+++ b/main_ps3.cpp Sat Oct 10 09:53:52 2015 +0000
@@ -5,10 +5,14 @@
//速度コントローラと向きコントローラはそのまま
//大和田にPIC1のシリンダの信号をタイマーで書かせる
//回転機構wait PI/4 追加で待つ
+//射出をスキップする機能を追加
+//スタートゾーンにマシンを戻した後に暴走しないようにする
+//手動時の操作性の改善
+//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
/***コース選択***/
-#define BLUE
-//#define RED
+//#define BLUE
+#define RED
/***コントローラ選択***/
//#define IM920
@@ -26,6 +30,11 @@
Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");
+Timeout MStop;
+
+void Restart(){
+ if(step==10) step=11;
+}
int main() {
#ifdef IM920
@@ -59,68 +68,162 @@
#endif
#ifdef BLUE
//Blue
-// if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
-// if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.015,step=1; /*nomal*/
- if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.015,step=1; /*nomal*/
+ /********************************Nomal Mode*********************************/
+ if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.03,step=1; /*nomal*/
if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
- if((step==2)&&(x<1950.0)) {
+ if((step==2)&&(x<1700.0)) {
targ_sita=PI/4;
step=3;
sendData(7,255);
}
- if((step==3)&&(x<70.0)) targ_velocity=0.0,step=8;
-
- if((step==4)&&((5700.0>x)&&(x>900.0))) targ_sita=0.015,step=5; /*middle*/
- if((step==5)&&(x>5000.0)) { /*middle shoot*/
+ if((step==3)&&(x<-100.0)) {
+ targ_velocity=0.0;
+ step=114;-
+ autoflag = 0;
+ }
+ /***Cylinder***/
+ if((x>2900.0)&&(CStep==1)) { /*nomal*/
+ if(!skip) sendData(1,1);
+ CStep=2;
+ }
+ if((x>5850.0)&&(CStep==2)) {
+ if(!skip) sendData(1,2);
+ CStep=3;
+ }
+ if((x>7600.0)&&(CStep==3)) {
+ if(!skip) sendData(1,3);
+ CStep=4;
+ }
+ if((x<6600.0)&&(CStep==4)) {
+ if(!skip) sendData(1,5);
+ CStep=5;
+ }
+ if((x<6100.0)&&(CStep==5)) {
+ if(!skip) sendData(1,4);
+ CStep=6;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /******************************Middle Mode******************************/
+ if((step==4)&&((5700.0>x)&&(x>600.0))) {/*middle*/
+ targ_sita=0.03;
+ step=5;
+ }
+ if((step==5)&&(x>5000.0)) {
targ_velocity=-speed;
step=6;
sendData(1,5);
flagf=0;
}
- if((step==6)&&(x<1750.0)) {
+ if((step==6)&&(x<1600.0)) {
targ_sita=PI/4;
step=7;
sendData(7,255);
}
- if((step==7)&&(x<70.0)) targ_velocity=0.0,step=8;
-
- if((x>2900.0)&&(CStep==1)) CStep=2,sendData(1,1); /*nomal*/
- if((x>5850.0)&&(CStep==2)) CStep=3,sendData(1,2);
- if((x>7600.0)&&(CStep==3)) CStep=4,sendData(1,3);
- if((x<6500.0)&&(CStep==4)) CStep=5,sendData(1,5);
- if((x<6100.0)&&(CStep==5)) CStep=6,sendData(1,4);
-
- /*if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
- if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
- if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
- if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
- if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);*/
-
-#else
+ if((step==7)&&(x<-100.0)) {
+ targ_velocity=0.0;
+ step=114;
+ autoflag = 0;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /*****************************Opponents Mode****************************/
+ if((step==8)&&(x>1500.0)) {
+ targ_sita=-PI/2.0;
+ step=9;
+ }
+ if((step==9)&&(y<-1000.0)) {
+ targ_velocity = 0.0;
+ targ_sita = 4.0 * ( PI / 9.0 );
+ step=10;
+ }
+ if((step==10)&&(abs(sita-targ_sita)<0.05)) {
+ sendData(1,4);
+ MStop.attach(Restart,0.5);
+ }
+ if(step==11){
+ targ_sita = 0.0;
+ step = 12;
+ }
+ if((step==12)&&(abs(sita-targ_sita)<0.05)){
+ targ_velocity = -speed;
+ flagf = 0;
+ step = 13;
+ }
+ if((step==13)&&(y>0.0)){
+ targ_sita = 0.0;
+ step = 14;
+ }
+ if((step==14)&&(x<-100.0)){
+ targ_velocity = 0.0;
+ targ_sita = PI/4.0;
+ step = 114;
+ }
+ /////////////////////////////////////////////////////////////////////////
+#else
//Red
- if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=-0.015,step=1;
- if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
- if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
- if((step==3)&&(x<10.0)) targ_velocity=0.0,step=8;
-
- if((step==4)&&((5700.0>x)&&(x>950.0))) targ_sita=-0.015,step=5; /*middle*/
- if((step==5)&&(x>5300.0)) { /*middle shoot*/
+ /********************************Nomal Mode*********************************/
+ if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1; /*nomal*/
+ if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
+ if((step==2)&&(x<1700.0)) {
+ targ_sita=-PI/4;
+ step=3;
+ sendData(7,31);
+ }
+ if((step==3)&&(x<-100.0)) {
+ targ_velocity=0.0;
+ step=8;
+ autoflag=0;
+ }
+ /***Cylinder***/
+ if((x>2900.0)&&(CStep==1)) { /*nomal*/
+ if(!skip) sendData(1,1);
+ CStep=2;
+ }
+ if((x>5850.0)&&(CStep==2)) {
+ if(!skip) sendData(1,3);
+ CStep=3;
+ }
+ if((x>7600.0)&&(CStep==3)) {
+ if(!skip) sendData(1,2);
+ CStep=6;
+ }
+ if((x<6600.0)&&(CStep==4)) {
+ if(!skip) sendData(1,4);
+ CStep=5;
+ }
+ if((x<6100.0)&&(CStep==5)) {
+ if(!skip) sendData(1,5);
+ CStep=6;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /******************************Middle Mode******************************/
+ if((step==4)&&((5700.0>x)&&(x>600.0))) { /*middle*/
+ targ_sita=-0.03;
+ step=5;
+ medge=1;
+ }
+ if((step==5)&&(x>5000.0)) {
targ_velocity=-speed;
step=6;
sendData(1,4);
+ flagf=0;
}
- if((step==6)&&(x<2000.0)) targ_sita=-PI/4,step=7;
- if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
-
- if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
- if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
- if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
- if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
- if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
+ if((step==6)&&(x<2000.0)) {
+ targ_sita=-PI/4;
+ step=7;
+ sendData(7,255);
+ }
+ if((step==7)&&(x<-100.0)) {
+ targ_velocity=0.0;
+ step=8;
+ autoflag=0;
+ }
+ /////////////////////////////////////////////////////////////////////////
#endif
- move_following();
+ mesure_state(); /*位置測定*/
+ move_following(); /*移動制御*/
}
else if(!autoflag) {
+ resetState();
#ifdef IM920
manualMoveIM920(); /*analogStick*/
manualIM920(); /*IM920 button*/
@@ -141,7 +244,6 @@
#ifdef IM920
readIM920();
#endif
- mesure_state();
mesureSwing();
wait(RATE);
