|
@@ -85,9 +85,9 @@ double gfBrakeReq = 0.0;
|
|
static bool gbSendBrake = false;
|
|
static bool gbSendBrake = false;
|
|
|
|
|
|
|
|
|
|
-static double kp = 0.1;
|
|
|
|
|
|
+static double kp = 0.5;
|
|
static double ki = 0.0;
|
|
static double ki = 0.0;
|
|
-static double kd = 0.0;
|
|
|
|
|
|
+static double kd = 0.0;
|
|
static double MAXACC = 3.0;
|
|
static double MAXACC = 3.0;
|
|
static int64_t gnLastAccUptime = 0;
|
|
static int64_t gnLastAccUptime = 0;
|
|
static bool gbUsePID = true;
|
|
static bool gbUsePID = true;
|
|
@@ -121,6 +121,7 @@ double PIDTorque(double torque)
|
|
if((nnow - gnLastAccUptime)>1e9)return torque;
|
|
if((nnow - gnLastAccUptime)>1e9)return torque;
|
|
|
|
|
|
static double error_previous = 0.0;
|
|
static double error_previous = 0.0;
|
|
|
|
+ static double integral = 0.0;
|
|
double fVehWeight = 1800;
|
|
double fVehWeight = 1800;
|
|
double fRollForce = 50;
|
|
double fRollForce = 50;
|
|
const double fRatio = 2.5;
|
|
const double fRatio = 2.5;
|
|
@@ -131,7 +132,7 @@ double PIDTorque(double torque)
|
|
double error = accAim - gfVehAcc;
|
|
double error = accAim - gfVehAcc;
|
|
double dt = 0.01; //10ms
|
|
double dt = 0.01; //10ms
|
|
double diff = (error - error_previous)/dt;
|
|
double diff = (error - error_previous)/dt;
|
|
- double integral = integral + error * dt * ki;
|
|
|
|
|
|
+ integral = integral + error * dt * ki;
|
|
double output = error * kp + integral + diff * kd;
|
|
double output = error * kp + integral + diff * kd;
|
|
|
|
|
|
double fAccAjust = accAim + output;
|
|
double fAccAjust = accAim + output;
|
|
@@ -140,13 +141,15 @@ double PIDTorque(double torque)
|
|
|
|
|
|
double ftorqueAjust = acctotorque(fAccAjust);
|
|
double ftorqueAjust = acctotorque(fAccAjust);
|
|
|
|
|
|
|
|
+ error_previous = error;
|
|
|
|
+
|
|
if(gbPrintPIDOut)
|
|
if(gbPrintPIDOut)
|
|
{
|
|
{
|
|
int64_t timesecond = nnow/1e9;
|
|
int64_t timesecond = nnow/1e9;
|
|
int64_t timepart = nnow - timesecond * 1e9;
|
|
int64_t timepart = nnow - timesecond * 1e9;
|
|
double fnow = timepart; fnow = fnow/1e9;
|
|
double fnow = timepart; fnow = fnow/1e9;
|
|
fnow = fnow + timesecond;
|
|
fnow = fnow + timesecond;
|
|
- std::cout<<fnow<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
|
|
|
|
|
|
+ std::cout<<fnow<<"\t"<<gfVehSpd<<"\t"<<accAim<<"\t"<<fAccAjust<<"\t"<<error<<"\t"<<torque<<"\t"<<ftorqueAjust<<std::endl;
|
|
}
|
|
}
|
|
|
|
|
|
return ftorqueAjust;
|
|
return ftorqueAjust;
|
|
@@ -166,7 +169,6 @@ double fpidtestmaxvel = 15.0;
|
|
|
|
|
|
void executeDecition(const iv::brain::decition &decition)
|
|
void executeDecition(const iv::brain::decition &decition)
|
|
{
|
|
{
|
|
-
|
|
|
|
double fwheel = decition.wheelangle()*0.9;
|
|
double fwheel = decition.wheelangle()*0.9;
|
|
if(fwheel<-430)fwheel = 430;
|
|
if(fwheel<-430)fwheel = 430;
|
|
if(fwheel>380)fwheel = 380;
|
|
if(fwheel>380)fwheel = 380;
|
|
@@ -189,9 +191,12 @@ void executeDecition(const iv::brain::decition &decition)
|
|
if(pidtestnstep == 0)
|
|
if(pidtestnstep == 0)
|
|
{
|
|
{
|
|
ftorque = PIDTorque(acctotorque(fpidtestacc));
|
|
ftorque = PIDTorque(acctotorque(fpidtestacc));
|
|
|
|
+ fbrake = 0;
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
|
|
|
|
|
|
+
|
|
|
|
+
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",fwheel);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",fwheel);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
|
|
@@ -327,13 +332,29 @@ void Activate()
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",gfWheelReq);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",gfWheelReq);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",1.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",1.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",2.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",2.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);
|
|
|
|
+
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);
|
|
|
|
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
|
|
SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
|
|
SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
|
|
|
|
|
|
- SetMsgSignal("ADS_COM_3","FCM_2_SysStatus",0.0);
|
|
|
|
|
|
+ SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",0.0);
|
|
|
|
|
|
SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
|
|
SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
|
|
- SetMsgSignal("ADS_COM_2","ADS_3_ACCSts",3.0);
|
|
|
|
|
|
+ SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",3.0);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@@ -360,13 +381,31 @@ void UnAcitvate()
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",0.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgReq",0.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",0.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerAgVld",0.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",0.0);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_SteerPilotAgEna",0.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_LdwWarningCmdVld",2.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_SteerMaxTqReq",3.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_SteerMinTqReq",-3.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_ADSHealthSts",1.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","CutOutFreshvalues_2CB_S",1.0);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","CutOutMAC_2CB_S",1.0);
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDriOffReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotDec2StpReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotkBrkDecTar",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarVld",1.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotCtrlType",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotBrkDecTarReq",0.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_3","FCM_3_PilotParkCtrlRepSta",0.0);
|
|
|
|
+
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDecVld",1.0);
|
|
|
|
+ SetMsgSignal("ADS_ONEBOX_2","FCM_2_AebTarDec",1.0);
|
|
|
|
|
|
SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
|
|
SetMsgSignal("ADS_ONEBOX_2","FCM_2_Avl",1.0);
|
|
|
|
|
|
- SetMsgSignal("ADS_COM_3","FCM_2_SysStatus",1.0);
|
|
|
|
|
|
+ SetMsgSignal("ADS_COM_2","FCM_2_SysStatus",1.0);
|
|
|
|
|
|
SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
|
|
SetMsgSignal("ADS_COM_2","ADS_2_AEBStatus",0.0);
|
|
- SetMsgSignal("ADS_COM_2","ADS_3_ACCSts",0.0);
|
|
|
|
|
|
+ SetMsgSignal("ADS_COM_3","ADS_3_ACCSts",0.0);
|
|
|
|
|
|
|
|
|
|
ExecSend();
|
|
ExecSend();
|
|
@@ -458,6 +497,7 @@ void PrepareMsg()
|
|
static int rollcouter = 0;
|
|
static int rollcouter = 0;
|
|
// std::cout<<" roll count:: "<<rollcouter<<std::endl;
|
|
// std::cout<<" roll count:: "<<rollcouter<<std::endl;
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr1",rollcouter);
|
|
SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr1",rollcouter);
|
|
|
|
+ SetMsgSignal("ADS_EPS_1","ADS_1_RollgCntr2",rollcouter);
|
|
// set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
|
|
// set_EPS1_signal("CutOutFreshvalues_2CB_S",1.0);
|
|
// set_EPS1_signal("CutOutMAC_2CB_S",1.0);
|
|
// set_EPS1_signal("CutOutMAC_2CB_S",1.0);
|
|
gpsterraes->GetEPS1Data(ADS_EPS_1);
|
|
gpsterraes->GetEPS1Data(ADS_EPS_1);
|