2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

Revision:
79:7f86e18f40ef
Parent:
78:abb760e0a935
Child:
81:fee3a0a05d51
Child:
82:1bbe138c0b76
Child:
83:e1638c58e1f1
--- a/main_ps3.cpp	Sat Oct 10 09:53:52 2015 +0000
+++ b/main_ps3.cpp	Sun Oct 11 07:34:51 2015 +0000
@@ -11,12 +11,12 @@
 //一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
 
 /***コース選択***/
-//#define BLUE
-#define RED
+#define BLUE
+//#define RED
 
 /***コントローラ選択***/
-//#define IM920
-#define PS3
+#define IM920
+//#define PS3
 
 #if defined(IM920) && defined(PS3)
 #error Caution, You should define either IM920 or PS3
@@ -70,16 +70,19 @@
             //Blue            
             /********************************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)) {
+            if((step==1)&&(x>8650.0)) {
+                step=2;
+                flagf=0;
+                targ_sita=0.05;
+            }
+            if((step==2)&&(x<1950.0)) {
                 targ_sita=PI/4;
                 step=3;
                 sendData(7,255);
             }
             if((step==3)&&(x<-100.0)) {
                 targ_velocity=0.0;
-                step=114;-
-                autoflag = 0;
+                step=114;
             }
             /***Cylinder***/
             if((x>2900.0)&&(CStep==1)) { /*nomal*/
@@ -108,11 +111,11 @@
                 targ_sita=0.03;
                 step=5;
             }
-            if((step==5)&&(x>5000.0)) { 
-                targ_velocity=-speed;
+            if((step==5)&&(x>4900.0)) {
                 step=6;
                 sendData(1,5);
                 flagf=0;
+                targ_sita=0.05;
             }
             if((step==6)&&(x<1600.0)) {
                 targ_sita=PI/4;
@@ -122,48 +125,25 @@
             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;
+            if((step==8)&&(y<-1700.0)) {
+                targ_velocity=0.0;
+                targ_sita=0.0;
+                step=114;
             }
             /////////////////////////////////////////////////////////////////////////
 #else       
             //Red
             /********************************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)) {
+            if((step==0)&&((8650.0>x)&&(x>800.0))) targ_sita=-0.03,step=1; /*nomal*/
+            if((step==1)&&(x>8650.0)) {
+                step=2;
+                flagf=0;
+                targ_sita=-0.05;
+            }
+            if((step==2)&&(x<1900.0)) {
                 targ_sita=-PI/4;
                 step=3;
                 sendData(7,31);
@@ -171,7 +151,6 @@
             if((step==3)&&(x<-100.0)) {
                 targ_velocity=0.0;
                 step=8;
-                autoflag=0;
             }
             /***Cylinder***/
             if((x>2900.0)&&(CStep==1)) { /*nomal*/
@@ -196,16 +175,16 @@
             }
             /////////////////////////////////////////////////////////////////////////
             /******************************Middle Mode******************************/
-            if((step==4)&&((5700.0>x)&&(x>600.0))) { /*middle*/
+            if((step==4)&&((5700.0>x)&&(x>800.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;
+                targ_sita=-0.05;
             }
             if((step==6)&&(x<2000.0)) {
                 targ_sita=-PI/4;