Browse Source

change controller_chery_sterra_es. change controller for limit torque for u turn.

yuchuli 4 months ago
parent
commit
73e9143934
1 changed files with 20 additions and 4 deletions
  1. 20 4
      src/controller/controller_chery_sterra_es/main.cpp

+ 20 - 4
src/controller/controller_chery_sterra_es/main.cpp

@@ -130,15 +130,22 @@ void set_VCU1_signal(std::string strsigname,double value){
 void ExecSend();
 
 int testnum = 0;
+
+int testwheel = 0;
 void executeDecition(const iv::brain::decition &decition)
 {
 
 
-    double fwheel = decition.wheelangle();
+    double fwheel = decition.wheelangle()*0.9;
     if(fwheel<-430)fwheel = 430;
     if(fwheel>380)fwheel = 380;
     ADS_1_SteerAgReq =fwheel;
 
+//    if(testwheel<1000)ADS_1_SteerAgReq = 180;
+//    else ADS_1_SteerAgReq = -180;
+//    testwheel++;
+//    if(testwheel > 2000)testwheel = 0;
+
 //    std::cout<<"brake: "<<decition.brake()<<std::endl;
 
 //    if((testnum < 1000) || (testnum > 1500))
@@ -149,7 +156,7 @@ void executeDecition(const iv::brain::decition &decition)
 
         ADS_1_ParkCtrlMod = 1.0;
         ADS_1_PilotParkDriOffReq = 0.0;
-        ADS_1_PilotParkDec2StpReq = 1.0;
+        ADS_1_PilotParkDec2StpReq = 0.0;
         ADS_1_PilotParkBrkDecTar =  decition.brake();
         ADS_1_PilotParkBrkDecTarVld = 1.0;
         ADS_1_PilotParkCtrlType = 0.0;
@@ -190,6 +197,15 @@ void executeDecition(const iv::brain::decition &decition)
         }
 
         ADS_1_DrvTarTq = decition.torque();
+
+        if((fabs(gfVehSpd)>5.0)&&(fabs(ADS_1_SteerAgReq)>180))
+        {
+            ADS_1_DrvTarTq = 0;
+        }
+        if((fabs(gfVehSpd)>3.0)&&(fabs(ADS_1_SteerAgReq)>90))
+        {
+            ADS_1_DrvTarTq = 0;
+        }
 //        if(ADS_1_DrvTarTq > 100)ADS_1_DrvTarTq =100.0;
 
 
@@ -425,8 +441,8 @@ void PrepareMsg()
     set_EPS1_signal("ADS_1_SteerTqEna",1.0);
     set_EPS1_signal("ADS_1_LdwWarningCmd",0.0);
     set_EPS1_signal("ADS_1_LdwWarningCmdVld",2.0);
-    set_EPS1_signal("ADS_1_SteerMaxTqReq",10.0);
-    set_EPS1_signal("ADS_1_SteerMinTqReq",1.0);
+    set_EPS1_signal("ADS_1_SteerMaxTqReq",60.0);
+    set_EPS1_signal("ADS_1_SteerMinTqReq",-60.0);
     set_EPS1_signal("ADS_1_ADSHealthSts",1.0);
     set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
     set_EPS1_signal("CutOutMAC_2CB_S",1.0);