add park

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -504,12 +504,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }else if(bocheEN==0){
                 ServiceCarStatus.bocheEnable=0;
             }
+            givlog->debug("decition_brainR","bocheEN: %d,ServiceCarStatus.bocheMode: %d",
+                            bocheEN,ServiceCarStatus.bocheMode);
         }else{
             ServiceCarStatus.bocheEnable=2;
         }
 
-
-
         if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect&&  vehState!=reverseArr ){
 
             if(abs(realspeed)>0.1){
@@ -517,6 +517,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 minDecelerate=min(minDecelerate,-0.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
                 gps_decition->wheel_angle=0;
+
+                givlog->debug("decition_brainR","ServiceCarStatus.bocheMode: %d,dSpeed: %f",
+                               ServiceCarStatus.bocheMode,dSpeed);
                 return gps_decition;
             }else{
                 if(trumpet()>3000){
@@ -526,7 +529,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
             }
             //     ServiceCarStatus.bocheMode=false;
-
         }
     }
     // ChuiZhiTingChe
@@ -555,6 +557,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             gps_decition->wheel_angle = 0;
             gps_decition->speed = dSpeed;
             obsDistance=-1;
+            givlog->debug("decition_brainR","vehState: %d,dSpeed: %f",
+                            vehState,dSpeed);
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             //	gps_decition->accelerator = 0;
             return gps_decition;
@@ -597,10 +601,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
 
             }
-
             controlAng = Compute00().bocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*1.05;
+            gps_decition->wheel_angle =controlAng*1.05;
 
+            givlog->debug("decition_brainR","vehState: %d,controlAng: %f,dSpeed: %f",
+                            vehState,controlAng,dSpeed);
             cout<<"farTpointDistance================"<<dis<<endl;
 
             return gps_decition;
@@ -614,31 +619,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         if ((pt.y)<0.5)
         {
-
             qiedianCount=true;
             vehState=reverseArr;
             cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
-            //            gps_decition->accelerator = -3;
-            //            gps_decition->brake = 10;
-            //            gps_decition->torque = 0;
             gps_decition->wheel_angle=0;
             dSpeed=0;
             minDecelerate=min(minDecelerate,-0.5f);
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             return gps_decition;
-
-
-
-
         }
         else {
-
             //if (qiedianCount && trumpet()<0)
             if (qiedianCount && trumpet()<1000)
             {
-
-                //                gps_decition->brake = 10;
-                //                gps_decition->torque = 0;
                 dSpeed=0;
                 minDecelerate=min(minDecelerate,-0.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
@@ -647,19 +640,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             {
                 qiedianCount = false;
                 trumpetFirstCount = true;
-                dSpeed = 1;
+                dSpeed = 2;
                 dSecSpeed = dSpeed / 3.6;
                 gps_decition->speed = dSpeed;
                 obsDistance=-1;
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
 
-
-
             controlAng = 0;
-
             gps_decition->wheel_angle = 0;
 
+            givlog->debug("decition_brainR","vehState: %d,controlAng: %f",
+                            vehState,controlAng);
             return gps_decition;
         }
     }
@@ -673,9 +665,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         ServiceCarStatus.bocheMode=false;
         if (qiedianCount && trumpet()<1500)
         {
-
-            //            gps_decition->brake = 10;
-            //            gps_decition->torque = 0;
             dSpeed=0;
             minDecelerate=min(minDecelerate,-0.5f);
             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
@@ -694,9 +683,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
         gps_decition->wheel_angle = 0 ;
-
-
-
         return gps_decition;
 
     }