2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

Revision:
92:3a12444b5192
Parent:
90:9fe39ebe721f
Child:
93:9cacc1763463
diff -r 9fe39ebe721f -r 3a12444b5192 main_ps3.cpp
--- a/main_ps3.cpp	Thu Oct 15 09:44:09 2015 +0000
+++ b/main_ps3.cpp	Fri Oct 16 06:02:29 2015 +0000
@@ -15,8 +15,8 @@
 //モード切替時に時間を待つ
 
 /***コース選択***/
-#define BLUE
-//#define RED
+//#define BLUE
+#define RED
 
 /***コントローラ選択***/
 #define IM920
@@ -84,7 +84,7 @@
                 targ_velocity=spcount;
             }
             /********************************Nomal Mode*********************************/
-            if((step==0)&&((8650.0>x)&&(x>850.0))) {
+            if((step==0)&&((8650.0>x)&&(x>800.0))) {
                 targ_sita=0.035;
                 step=1;
             }
@@ -154,7 +154,7 @@
                 step=114;
             }
             
-            if((x>4650.0)&&(CStep==6)){
+            if((x>4620.0)&&(CStep==6)){
                 sendData(1,5);
                 CStep=7;
             }
@@ -195,27 +195,31 @@
             /////////////////////////////////////////////////////////////////////////
 #else       
             //Red
+            if(spcount<speed){
+                spcount+=speed/100.0;
+                targ_velocity=spcount;
+            }
             /********************************Nomal Mode*********************************/
-            if((step==0)&&((8650.0>x)&&(x>800.0))) {
-                targ_sita=-0.05;
+            if((step==0)&&((8650.0>x)&&(x>850.0))) {
+                targ_sita=-0.035;
                 step=1; 
             }
             if((step==1)&&(x>8600.0)) {
                 targ_velocity=0.0;
                 step=2;
             }
-            if((step==2)&&(abs(velocity)<1000.0)) {
+            if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
                 step=3;
-                targ_velocity=speed;
+                spcount=0.0;
                 flagf=0;
-                targ_sita=-0.07;
+                targ_sita=-0.045;
             }
-            if((step==3)&&(x<1950.0)) {
+            if((step==3)&&(x<2000.0)) {
                 targ_sita=-PI/4;
                 step=4;
                 sendData(7,255);
             }
-            if((step==4)&&(x<-100.0)) {
+            if((step==4)&&(x<0.0)) {
                 targ_velocity=0.0;
                 step=114;
             }
@@ -242,18 +246,18 @@
             }
             /////////////////////////////////////////////////////////////////////////
             /******************************Middle Mode******************************/
-            if((step==5)&&((5700.0>x)&&(x>800.0))) { 
-                targ_sita=-0.08;
-                step=6; 
+            if((step==5)&&((5700.0>x)&&(x>800.0))) {
+                targ_sita=-0.035;
+                step=6;
             }
             if((step==6)&&(x>7000.0)){
                 targ_velocity=0.0;
                 step=7;
             }
-            if((step==7)&&(abs(velocity)<1000.0)) {
+            if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
                 flagf=0;
-                targ_velocity=speed;
-                targ_sita=-0.07;
+                spcount=0.0;
+                targ_sita=-0.045;
                 step=8;
             }
             if((step==8)&&(x<1900.0)) {
@@ -261,36 +265,39 @@
                 step=9;
                 sendData(7,255);
             }
-            if((step==9)&&(x<-100.0)) {
+            if((step==9)&&(x<-10.0)) {
                 targ_velocity=0.0;
                 step=114;
-                autoflag=0;
             }
             
-            if((x>4850.0)&&(CStep==6)){
+            if((x>4620.0)&&(CStep==6)){
                 sendData(1,4);
                 CStep=7;
             }
+            if((x>5720.0)&&(CStep==7)){
+                sendData(1,5);
+                CStep=114;
+            }
             /////////////////////////////////////////////////////////////////////////
             /*****************************Opponents Mode****************************/
-            if((step==15)&&((5700.0>x)&&(x>50.0))) {
-                targ_sita=-0.08;
+            if((step==15)&&((5700.0>x)&&(x>70.0))) {
+                targ_sita=-0.035;
                 step=16;
             }
             if((step==16)&&(x>2800.0)){
                 targ_velocity=0.0;
                 step=17;
             }
-            if((step==17)&&(abs(velocity)<1000.0)){
+            if((step==17)&&((velocity<500.0)&&(velocity>-500.0))){
                 flagf=0;
-                targ_vellocity=speed;
-                targ_sita=-0.07;
+                spcount=0.0;
+                targ_sita=-0.045;
                 step=18;
             }
             if((step==18)&&(x<-100.0)){
                 targ_velocity=0.0;
+                targ_sita=-PI/4.0;
                 step=114;
-                autoflag=0;
             }
             
             if((x>2700.0)&&(CStep==15)){