|
@@ -102,6 +102,7 @@ int Canbus::Start() {
|
|
canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
|
|
canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
|
|
canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
|
|
canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
|
|
canRackData_pub = node_handle_->advertise<canbus::rackTestReplyMsg>(robot::common::RackReplyData,1);
|
|
canRackData_pub = node_handle_->advertise<canbus::rackTestReplyMsg>(robot::common::RackReplyData,1);
|
|
|
|
+ canRackDetailData_pub = node_handle_->advertise<canbus::rackTestReplyDetailMsg>(robot::common::RackReplyDetailData,1);
|
|
|
|
|
|
ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
|
|
ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
|
|
&Canbus::ActuatorEnableRequest, this);
|
|
&Canbus::ActuatorEnableRequest, this);
|
|
@@ -189,48 +190,6 @@ int Canbus::Start() {
|
|
ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
|
|
ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
|
|
&Canbus::RTKLocationCallback, this);
|
|
&Canbus::RTKLocationCallback, this);
|
|
|
|
|
|
-
|
|
|
|
- string str = "/home/madesheng/angle.txt";
|
|
|
|
- int flag = Canbus::read_txt(str,1);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
- str = "/home/madesheng/psng.txt";
|
|
|
|
- flag = Canbus::read_txt(str,2);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- str = "/home/madesheng/acc.txt";
|
|
|
|
- flag = Canbus::read_txt(str,3);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- str = "/home/madesheng/angleLine.txt";
|
|
|
|
- flag = Canbus::read_txt(str,4);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- str = "/home/madesheng/sft.txt";
|
|
|
|
- flag = Canbus::read_txt(str,5);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- str = "/home/madesheng/NEDC Speed.txt";
|
|
|
|
- flag = Canbus::read_txt(str,6);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
- {
|
|
|
|
- return ROBOT_FAILTURE;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
//data receive
|
|
//data receive
|
|
while(ros::ok())
|
|
while(ros::ok())
|
|
{
|
|
{
|
|
@@ -248,45 +207,45 @@ int Canbus::Start() {
|
|
void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
//LOG(INFO)<< Name()<< " timer start ..";
|
|
//LOG(INFO)<< Name()<< " timer start ..";
|
|
//TODO dakai
|
|
//TODO dakai
|
|
- if(heatbeat > 15)
|
|
|
|
- {
|
|
|
|
- heatbeat = 0;
|
|
|
|
- }
|
|
|
|
- icuMonitor.uiICUAlive = heatbeat++;
|
|
|
|
-
|
|
|
|
- try
|
|
|
|
- {
|
|
|
|
- canObdData_pub.publish(obd);
|
|
|
|
- TPCANMsg canIcuMonitor; //ICUMonitor Can Message
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
|
|
|
|
- write_data(&canIcuMonitor, false);
|
|
|
|
-
|
|
|
|
- if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_VIHICLE_DEPEND)
|
|
|
|
- {
|
|
|
|
- TPCANMsg canStatus1; //VehicleStaus1 Can Message
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
|
|
|
|
- write_data(&canStatus1, false);
|
|
|
|
- TPCANMsg canStatus2; //VehicleStatus2 Can Message
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
|
|
- write_data(&canStatus2, false);
|
|
|
|
- TPCANMsg canStatus3; //VehicleStatus3 Can Message
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
|
|
|
|
- write_data(&canStatus3, false);
|
|
|
|
- }
|
|
|
|
- else if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_STAND_ALONE)
|
|
|
|
- {
|
|
|
|
- status2.iStrAng = 0;
|
|
|
|
- status2.iStrAngGrd = 0;
|
|
|
|
- status2.uiEngSpd = 0;
|
|
|
|
- TPCANMsg canStatus2; //VehicleStatus2 Can Message
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
|
|
- write_data(&canStatus2, false);
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
- catch (ros::Exception ex)
|
|
|
|
- {
|
|
|
|
- LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
|
|
|
|
- }
|
|
|
|
|
|
+// if(heatbeat > 15)
|
|
|
|
+// {
|
|
|
|
+// heatbeat = 0;
|
|
|
|
+// }
|
|
|
|
+// icuMonitor.uiICUAlive = heatbeat++;
|
|
|
|
+
|
|
|
|
+// try
|
|
|
|
+// {
|
|
|
|
+// canObdData_pub.publish(obd);
|
|
|
|
+// TPCANMsg canIcuMonitor; //ICUMonitor Can Message
|
|
|
|
+// MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
|
|
|
|
+// write_data(&canIcuMonitor, false);
|
|
|
|
+
|
|
|
|
+// if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_VIHICLE_DEPEND)
|
|
|
|
+// {
|
|
|
|
+// TPCANMsg canStatus1; //VehicleStaus1 Can Message
|
|
|
|
+// MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
|
|
|
|
+// write_data(&canStatus1, false);
|
|
|
|
+// TPCANMsg canStatus2; //VehicleStatus2 Can Message
|
|
|
|
+// MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
|
|
+// write_data(&canStatus2, false);
|
|
|
|
+// TPCANMsg canStatus3; //VehicleStatus3 Can Message
|
|
|
|
+// MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
|
|
|
|
+// write_data(&canStatus3, false);
|
|
|
|
+// }
|
|
|
|
+// else if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_STAND_ALONE)
|
|
|
|
+// {
|
|
|
|
+// status2.iStrAng = 0;
|
|
|
|
+// status2.iStrAngGrd = 0;
|
|
|
|
+// status2.uiEngSpd = 0;
|
|
|
|
+// TPCANMsg canStatus2; //VehicleStatus2 Can Message
|
|
|
|
+// MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
|
|
+// write_data(&canStatus2, false);
|
|
|
|
+// }
|
|
|
|
+// }
|
|
|
|
+// catch (ros::Exception ex)
|
|
|
|
+// {
|
|
|
|
+// LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
|
|
|
|
+// }
|
|
//TODO
|
|
//TODO
|
|
}
|
|
}
|
|
|
|
|
|
@@ -333,23 +292,23 @@ int Canbus::read_loop()
|
|
TPCANRdMsg msg;
|
|
TPCANRdMsg msg;
|
|
__u32 status;
|
|
__u32 status;
|
|
//TODO dakai
|
|
//TODO dakai
|
|
- if (funLINUX_CAN_Read(pcan_handle, &msg)) {
|
|
|
|
- LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
|
|
|
|
- return errno;
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- read_data(&(msg.Msg));
|
|
|
|
|
|
+// if (funLINUX_CAN_Read(pcan_handle, &msg)) {
|
|
|
|
+// LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
|
|
|
|
+// return errno;
|
|
|
|
+// }
|
|
|
|
|
|
- // deal receive data
|
|
|
|
- // check if a CAN status is pending
|
|
|
|
- if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
|
|
|
|
- status = funCAN_Status(pcan_handle);
|
|
|
|
- if ((int)status < 0) {
|
|
|
|
- errno = funnGetLastError();
|
|
|
|
- LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
|
|
|
|
- return errno;
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
|
|
+// read_data(&(msg.Msg));
|
|
|
|
+
|
|
|
|
+// // deal receive data
|
|
|
|
+// // check if a CAN status is pending
|
|
|
|
+// if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
|
|
|
|
+// status = funCAN_Status(pcan_handle);
|
|
|
|
+// if ((int)status < 0) {
|
|
|
|
+// errno = funnGetLastError();
|
|
|
|
+// LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
|
|
|
|
+// return errno;
|
|
|
|
+// }
|
|
|
|
+// }
|
|
//TODO
|
|
//TODO
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
@@ -1162,6 +1121,13 @@ bool Canbus::StrTxtReq(canbus::StrTxtReq::Request &req,canbus::StrTxtReq::Respon
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ string str = "/home/madesheng/angle.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,1);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
if(strTxtTimerState)
|
|
if(strTxtTimerState)
|
|
{
|
|
{
|
|
str_txt_timer_.stop();
|
|
str_txt_timer_.stop();
|
|
@@ -1238,6 +1204,13 @@ bool Canbus::BrkTxtReq(canbus::BrkTxtReq::Request &req,canbus::BrkTxtReq::Respon
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ string str = "/home/madesheng/psng.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,2);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
if(brkTxtTimerState)
|
|
if(brkTxtTimerState)
|
|
{
|
|
{
|
|
brk_txt_timer_.stop();
|
|
brk_txt_timer_.stop();
|
|
@@ -1309,6 +1282,13 @@ bool Canbus::AccTxtReq(canbus::AccTxtReq::Request &req,canbus::AccTxtReq::Respon
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ string str = "/home/madesheng/acc.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,3);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
if(accTxtTimerState)
|
|
if(accTxtTimerState)
|
|
{
|
|
{
|
|
acc_txt_timer_.stop();
|
|
acc_txt_timer_.stop();
|
|
@@ -1378,6 +1358,13 @@ bool Canbus::StrTxtLineReq(canbus::StrTxtLineReq::Request &req,canbus::StrTxtLin
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ string str = "/home/madesheng/angleLine.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,4);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
if(strTxtLineTimerState)
|
|
if(strTxtLineTimerState)
|
|
{
|
|
{
|
|
str_txt_line_timer_.stop();
|
|
str_txt_line_timer_.stop();
|
|
@@ -1446,6 +1433,13 @@ bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Respon
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ string str = "/home/madesheng/sft.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,5);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
if(sftTxtTimerState)
|
|
if(sftTxtTimerState)
|
|
{
|
|
{
|
|
sft_txt_timer_.stop();
|
|
sft_txt_timer_.stop();
|
|
@@ -1607,7 +1601,7 @@ int Canbus::read_txt(string &path,int flag)
|
|
}
|
|
}
|
|
else if(flag == 7)
|
|
else if(flag == 7)
|
|
{
|
|
{
|
|
- for (int i = 0; i < 3; i++)
|
|
|
|
|
|
+ for (int i = 0; i < 10; i++)
|
|
{
|
|
{
|
|
if(myfile.eof())
|
|
if(myfile.eof())
|
|
{
|
|
{
|
|
@@ -1617,7 +1611,7 @@ int Canbus::read_txt(string &path,int flag)
|
|
myfile >> fuzzy_pid[i];
|
|
myfile >> fuzzy_pid[i];
|
|
|
|
|
|
}
|
|
}
|
|
- cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
|
|
|
|
|
|
+ // cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -2044,20 +2038,31 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
if(rackTestState)
|
|
if(rackTestState)
|
|
{
|
|
{
|
|
//TODO zhushidiao
|
|
//TODO zhushidiao
|
|
- //rack_test_timer_.stop();
|
|
|
|
|
|
+ rack_test_timer_.stop();
|
|
//TODO
|
|
//TODO
|
|
rackTestState = false;
|
|
rackTestState = false;
|
|
}
|
|
}
|
|
if(rackTestSendTimerState)
|
|
if(rackTestSendTimerState)
|
|
{
|
|
{
|
|
rack_reply_timer_.stop();
|
|
rack_reply_timer_.stop();
|
|
|
|
+ rack_reply_detail_timer_.stop();
|
|
rackTestSendTimerState = false;
|
|
rackTestSendTimerState = false;
|
|
rackTestReplyState = 0;
|
|
rackTestReplyState = 0;
|
|
}
|
|
}
|
|
publish_rack_speed();
|
|
publish_rack_speed();
|
|
|
|
+ publis_rack_detail_speed();
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
+ nedc_speeds[1182] = {0};
|
|
|
|
+ string str = "/home/madesheng/NEDC Speed.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,6);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+
|
|
if(rackTestState || rackTestSendTimerState)
|
|
if(rackTestState || rackTestSendTimerState)
|
|
{
|
|
{
|
|
res.uiReturn = 2;
|
|
res.uiReturn = 2;
|
|
@@ -2065,15 +2070,19 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
}
|
|
}
|
|
nedc_speed_count = 0;
|
|
nedc_speed_count = 0;
|
|
rack_test_send_count = 0;
|
|
rack_test_send_count = 0;
|
|
|
|
+ set_speed_last = 0;
|
|
//TODO zhushidiao
|
|
//TODO zhushidiao
|
|
- // rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
|
|
|
|
- // rack_test_timer_.start();
|
|
|
|
|
|
+ rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
|
|
|
|
+ rack_test_timer_.start();
|
|
//TODO
|
|
//TODO
|
|
rackTestState = true;
|
|
rackTestState = true;
|
|
|
|
|
|
rack_reply_timer_ =
|
|
rack_reply_timer_ =
|
|
CreateTimer(ros::Duration(duration_rack_reply), &Canbus::RackTestReplyOnTimer, this,false,false);
|
|
CreateTimer(ros::Duration(duration_rack_reply), &Canbus::RackTestReplyOnTimer, this,false,false);
|
|
rack_reply_timer_.start();
|
|
rack_reply_timer_.start();
|
|
|
|
+ rack_reply_detail_timer_ =
|
|
|
|
+ CreateTimer(ros::Duration(duration_rack_reply_detail), &Canbus::RackTestReplyDetailOnTimer, this,false,false);
|
|
|
|
+ rack_reply_detail_timer_.start();
|
|
rackTestSendTimerState = true;
|
|
rackTestSendTimerState = true;
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
}
|
|
}
|
|
@@ -2089,28 +2098,36 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
|
|
|
|
void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
{
|
|
{
|
|
- PID PidControll;
|
|
|
|
try
|
|
try
|
|
{
|
|
{
|
|
if(nedc_speed_count == nedc_speed_number)
|
|
if(nedc_speed_count == nedc_speed_number)
|
|
{
|
|
{
|
|
- rack_test_timer_.stop();
|
|
|
|
rackTestState = false;
|
|
rackTestState = false;
|
|
rack_reply_timer_.stop();
|
|
rack_reply_timer_.stop();
|
|
|
|
+ rack_reply_detail_timer_.stop();
|
|
rackTestSendTimerState = false;
|
|
rackTestSendTimerState = false;
|
|
rackTestReplyState = 0;
|
|
rackTestReplyState = 0;
|
|
publish_rack_speed();
|
|
publish_rack_speed();
|
|
|
|
+ publis_rack_detail_speed();
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ if(!rackTestState)
|
|
|
|
+ {
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
+ // LOG(INFO)<< "RackTestOnTimer Start FUZZY PID";
|
|
|
|
+ PID PidControll;
|
|
|
|
+
|
|
|
|
|
|
gettimeofday(&now_time, NULL);
|
|
gettimeofday(&now_time, NULL);
|
|
float delta_time = 0.0;
|
|
float delta_time = 0.0;
|
|
if(sizeof(last_time) != 0)
|
|
if(sizeof(last_time) != 0)
|
|
{
|
|
{
|
|
long elapsed = (now_time.tv_sec-last_time.tv_sec) * 1000000 + (now_time.tv_usec - last_time.tv_usec);
|
|
long elapsed = (now_time.tv_sec-last_time.tv_sec) * 1000000 + (now_time.tv_usec - last_time.tv_usec);
|
|
- delta_time = elapsed / 1000000;
|
|
|
|
|
|
+ delta_time = (float)elapsed / 1000000;
|
|
}
|
|
}
|
|
- cout << "time sub" << delta_time <<endl;
|
|
|
|
|
|
+
|
|
|
|
+ // LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
|
|
|
|
|
|
last_time = now_time;
|
|
last_time = now_time;
|
|
|
|
|
|
@@ -2125,80 +2142,135 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
{
|
|
{
|
|
set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
}
|
|
}
|
|
- float set_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
- float actual_speed = status2.uiVehSpd;
|
|
|
|
-
|
|
|
|
- string str = "/home/madesheng/FuzzyPidFactor.txt";
|
|
|
|
|
|
+ float set_speed = 0;
|
|
|
|
+ if(set_speed1 == 0 && set_speed2 ==0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
+ }
|
|
|
|
+ if(set_speed < 0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
|
|
+ float actual_speed = msg_RTK.Speed;
|
|
|
|
+ if(actual_speed - set_speed > 20 || actual_speed - set_speed < -20)
|
|
|
|
+ {
|
|
|
|
+ // LOG(INFO)<< "actual_speed too more";
|
|
|
|
+ // return;
|
|
|
|
+ }
|
|
|
|
+ set_speed_reply = set_speed;
|
|
|
|
+// LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
|
|
+ string str = "/home/madesheng/FuzzyPid.txt";
|
|
int flag = Canbus::read_txt(str,7);
|
|
int flag = Canbus::read_txt(str,7);
|
|
if(flag != ROBOT_SUCCESS)
|
|
if(flag != ROBOT_SUCCESS)
|
|
{
|
|
{
|
|
- LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
|
|
|
|
|
|
+ LOG(INFO)<< "Get FuzzyPID.txt error";
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
float pi_out = 0.01;
|
|
float pi_out = 0.01;
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
- fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
|
|
|
|
|
|
+ fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2],
|
|
|
|
+ fuzzy_pid[3],fuzzy_pid[4],fuzzy_pid[5]);
|
|
|
|
+
|
|
|
|
+// LOG(INFO)<< "P:"<<std::setprecision(10)<<fuzzy_pid[0];
|
|
|
|
+// LOG(INFO)<< "I:"<<std::setprecision(10)<<fuzzy_pid[1];
|
|
|
|
+// LOG(INFO)<< "D:"<<std::setprecision(10)<<fuzzy_pid[2];
|
|
|
|
+
|
|
|
|
+// LOG(INFO)<< "P_F:"<<std::setprecision(10)<<fuzzy_pid[3];
|
|
|
|
+// LOG(INFO)<< "I_F:"<<std::setprecision(10)<<fuzzy_pid[4];
|
|
|
|
+// LOG(INFO)<< "D_F:"<<std::setprecision(10)<<fuzzy_pid[5];
|
|
|
|
|
|
- LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
|
|
|
|
- LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
|
|
|
|
- LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
|
|
|
|
- LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
|
|
|
|
- LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
|
|
|
|
- LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
|
|
|
|
|
|
+// LOG(INFO)<< "Set Speed:"<<std::setprecision(8)<<set_speed;
|
|
|
|
+// LOG(INFO)<< "Actual Speed:"<<std::setprecision(8)<<actual_speed;
|
|
|
|
+// LOG(INFO)<< "pi_out:"<<std::setprecision(8)<<pi_out;
|
|
|
|
|
|
TPCANMsg canBrkPedalActCtrReqMsg;
|
|
TPCANMsg canBrkPedalActCtrReqMsg;
|
|
T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
TPCANMsg canAccPedalActCtrReqMsg;
|
|
TPCANMsg canAccPedalActCtrReqMsg;
|
|
T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
|
|
|
|
- if(set_speed == 0)
|
|
|
|
|
|
+ if(set_speed == 0 && actual_speed < 3 || nedc_speed_count == nedc_speed_number - 1)
|
|
{
|
|
{
|
|
|
|
+ requestAcc.uiAccSpdCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccAccCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccPsngCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccDecCtrReq = 0;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
|
|
+ // write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
|
|
- requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 5;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
- requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 1;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
- requestBrk.uiBrkPsngCtrReq = (50/30) * 5;
|
|
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = 35 * 10;
|
|
|
|
|
|
- LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
|
|
+// LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
- write_data(&canBrkPedalActCtrReqMsg);
|
|
|
|
|
|
+// write_data(&canBrkPedalActCtrReqMsg,true);
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
if(pi_out >= 0)
|
|
if(pi_out >= 0)
|
|
{
|
|
{
|
|
|
|
+
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkAccCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
+ requestBrk.uiBrkDecCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkFocCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = 0;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
|
|
+// write_data(&canBrkPedalActCtrReqMsg,true);
|
|
|
|
+
|
|
requestAcc.uiAccSpdCtrReq = 0;
|
|
requestAcc.uiAccSpdCtrReq = 0;
|
|
requestAcc.uiAccAccCtrReq = 0;
|
|
requestAcc.uiAccAccCtrReq = 0;
|
|
- requestAcc.uiAccPsngCtrReq = pi_out *100;
|
|
|
|
|
|
+ requestAcc.uiAccPsngCtrReq = pi_out *100 * 10;
|
|
requestAcc.uiAccDecCtrReq = 0;
|
|
requestAcc.uiAccDecCtrReq = 0;
|
|
|
|
|
|
- LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
|
|
|
|
|
|
+// LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
- write_data(&canAccPedalActCtrReqMsg);
|
|
|
|
|
|
+// write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
|
|
+
|
|
|
|
+ requestAcc.uiAccSpdCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccAccCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccPsngCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccDecCtrReq = 0;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
|
|
+// write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
+
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 100;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
- requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 1;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
- requestBrk.uiBrkPsngCtrReq = (50/30) * (-pi_out * 30);
|
|
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = (85 * (-pi_out * 30) / 30) * 10;
|
|
|
|
+ if(requestBrk.uiBrkPsngCtrReq > 350)
|
|
|
|
+ {
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = 350;
|
|
|
|
+ }
|
|
|
|
|
|
- LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
|
|
+// LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
- write_data(&canBrkPedalActCtrReqMsg);
|
|
|
|
|
|
+// write_data(&canBrkPedalActCtrReqMsg,true);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
+ set_speed_last = set_speed;
|
|
nedc_speed_count = rack_test_send_count;
|
|
nedc_speed_count = rack_test_send_count;
|
|
-
|
|
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
{
|
|
{
|
|
@@ -2230,6 +2302,27 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
|
|
|
|
+{
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ if(rackTestSendTimerState)
|
|
|
|
+ {
|
|
|
|
+ rackTestReplyState = 1;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ }
|
|
|
|
+ // 发布消息
|
|
|
|
+ publis_rack_detail_speed();
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " RackTestReplyDetailOnTimer error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
//获取当前时间和毫秒
|
|
//获取当前时间和毫秒
|
|
int Canbus::get_datetime_mill(tm *datetime)
|
|
int Canbus::get_datetime_mill(tm *datetime)
|
|
{
|
|
{
|
|
@@ -2269,14 +2362,40 @@ int Canbus::get_datetime_mill(tm *datetime)
|
|
//发布台架测试任务实际车速
|
|
//发布台架测试任务实际车速
|
|
void Canbus::publish_rack_speed()
|
|
void Canbus::publish_rack_speed()
|
|
{
|
|
{
|
|
- canbus::rackTestReplyMsg rackMsg;
|
|
|
|
- //rackMsg.ActualSpeed = nedc_speeds[nedc_speed_count];
|
|
|
|
- rackMsg.ActualSpeed = msg_RTK.Speed;
|
|
|
|
- rackMsg.RackTestReplyState = rackTestReplyState;
|
|
|
|
- canRackData_pub.publish(rackMsg);
|
|
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ canbus::rackTestReplyMsg rackMsg;
|
|
|
|
+ //TODO
|
|
|
|
+ rackMsg.ActualSpeed = nedc_speeds[nedc_speed_count];
|
|
|
|
+ //rackMsg.ActualSpeed = msg_RTK.Speed;
|
|
|
|
+ rackMsg.RackTestReplyState = rackTestReplyState;
|
|
|
|
+ canRackData_pub.publish(rackMsg);
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " publish rackTestReplyMsg error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+void Canbus::publis_rack_detail_speed()
|
|
|
|
+{
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ canbus::rackTestReplyDetailMsg detailMsg;
|
|
|
|
+ //TODO
|
|
|
|
+ //detailMsg.ActualSpeed = msg_RTK.Speed;
|
|
|
|
+ detailMsg.ActualSpeed = set_speed_reply;
|
|
|
|
+ detailMsg.SetSpeed = set_speed_reply;
|
|
|
|
+ detailMsg.RackTestReplyState = rackTestReplyState;
|
|
|
|
+ canRackDetailData_pub.publish(detailMsg);
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " publish rackTestReplyDetailMsg error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
// 接收RTK数据
|
|
// 接收RTK数据
|
|
void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
@@ -2294,6 +2413,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
rackTestSendTimerState = false;
|
|
rackTestSendTimerState = false;
|
|
rackTestReplyState = 0;
|
|
rackTestReplyState = 0;
|
|
publish_rack_speed();
|
|
publish_rack_speed();
|
|
|
|
+ publis_rack_detail_speed();
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
if(!rackTestState)
|
|
if(!rackTestState)
|
|
@@ -2312,8 +2432,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
delta_time = (float)elapsed / 1000000;
|
|
delta_time = (float)elapsed / 1000000;
|
|
}
|
|
}
|
|
|
|
|
|
- LOG(INFO)<< "delta time:"<<std::setprecision(18)<<delta_time<<" s";
|
|
|
|
- // cout << "time sub " << delta_time <<" s"<<endl;
|
|
|
|
|
|
+ LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
|
|
|
|
|
|
last_time = now_time;
|
|
last_time = now_time;
|
|
|
|
|
|
@@ -2328,34 +2447,65 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
}
|
|
}
|
|
- float set_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
|
|
+ float set_speed = 0;
|
|
|
|
+ if(set_speed1 == 0 && set_speed2 ==0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
+ }
|
|
|
|
+ if(set_speed < 0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
float actual_speed = msg_RTK.Speed;
|
|
float actual_speed = msg_RTK.Speed;
|
|
-
|
|
|
|
- string str = "/home/madesheng/FuzzyPidFactor.txt";
|
|
|
|
|
|
+ if(actual_speed - set_speed > 20 || actual_speed - set_speed < -20)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO)<< "actual_speed too more";
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
|
|
+ set_speed_reply = set_speed;
|
|
|
|
+ string str = "/home/madesheng/FuzzyPid.txt";
|
|
int flag = Canbus::read_txt(str,7);
|
|
int flag = Canbus::read_txt(str,7);
|
|
if(flag != ROBOT_SUCCESS)
|
|
if(flag != ROBOT_SUCCESS)
|
|
{
|
|
{
|
|
- LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
|
|
|
|
|
|
+ LOG(INFO)<< "Get FuzzyPID.txt error";
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+// if(nedc_speed_count - 1 >=0 && nedc_speeds[nedc_speed_count - 1] > nedc_speeds[nedc_speed_count])
|
|
|
|
+// {
|
|
|
|
+// fuzzy_pid[3] = 0;
|
|
|
|
+// fuzzy_pid[4] = 0;
|
|
|
|
+// fuzzy_pid[5] = 0;
|
|
|
|
+// }
|
|
|
|
+
|
|
float pi_out = 0.01;
|
|
float pi_out = 0.01;
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
- fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
|
|
|
|
|
|
+ fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2],
|
|
|
|
+ fuzzy_pid[3],fuzzy_pid[4],fuzzy_pid[5]);
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "P:"<<std::setprecision(10)<<fuzzy_pid[0];
|
|
|
|
+ LOG(INFO)<< "I:"<<std::setprecision(10)<<fuzzy_pid[1];
|
|
|
|
+ LOG(INFO)<< "D:"<<std::setprecision(10)<<fuzzy_pid[2];
|
|
|
|
|
|
- LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
|
|
|
|
- LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
|
|
|
|
- LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
|
|
|
|
- LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
|
|
|
|
- LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
|
|
|
|
- LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
|
|
|
|
|
|
+ LOG(INFO)<< "P_F:"<<std::setprecision(10)<<fuzzy_pid[3];
|
|
|
|
+ LOG(INFO)<< "I_F:"<<std::setprecision(10)<<fuzzy_pid[4];
|
|
|
|
+ LOG(INFO)<< "D_F:"<<std::setprecision(10)<<fuzzy_pid[5];
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Set Speed:"<<std::setprecision(8)<<set_speed;
|
|
|
|
+ LOG(INFO)<< "Actual Speed:"<<std::setprecision(8)<<actual_speed;
|
|
|
|
+ LOG(INFO)<< "pi_out:"<<std::setprecision(8)<<pi_out;
|
|
|
|
|
|
TPCANMsg canBrkPedalActCtrReqMsg;
|
|
TPCANMsg canBrkPedalActCtrReqMsg;
|
|
T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
TPCANMsg canAccPedalActCtrReqMsg;
|
|
TPCANMsg canAccPedalActCtrReqMsg;
|
|
T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
|
|
|
|
- if(set_speed == 0)
|
|
|
|
|
|
+ if(set_speed == 0 && actual_speed < 3 || nedc_speed_count == nedc_speed_number - 1)
|
|
{
|
|
{
|
|
requestAcc.uiAccSpdCtrReq = 0;
|
|
requestAcc.uiAccSpdCtrReq = 0;
|
|
requestAcc.uiAccAccCtrReq = 0;
|
|
requestAcc.uiAccAccCtrReq = 0;
|
|
@@ -2365,9 +2515,9 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
write_data(&canAccPedalActCtrReqMsg,true);
|
|
write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
|
|
- requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 5;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
- requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 1;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
requestBrk.uiBrkPsngCtrReq = 35 * 10;
|
|
requestBrk.uiBrkPsngCtrReq = 35 * 10;
|
|
@@ -2414,12 +2564,16 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
write_data(&canAccPedalActCtrReqMsg,true);
|
|
write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
|
|
- requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 100;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
requestBrk.uiBrkAccCtrReq = 0;
|
|
- requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 1;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkDecCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
requestBrk.uiBrkFocCtrReq = 0;
|
|
- requestBrk.uiBrkPsngCtrReq = (80 * (-pi_out * 30) / 30) * 10;
|
|
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = (85 * (-pi_out * 30) / 30) * 10;
|
|
|
|
+ if(requestBrk.uiBrkPsngCtrReq > 350)
|
|
|
|
+ {
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = 350;
|
|
|
|
+ }
|
|
|
|
|
|
LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
|
|
@@ -2427,6 +2581,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
write_data(&canBrkPedalActCtrReqMsg,true);
|
|
write_data(&canBrkPedalActCtrReqMsg,true);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
+ set_speed_last = set_speed;
|
|
nedc_speed_count = rack_test_send_count;
|
|
nedc_speed_count = rack_test_send_count;
|
|
|
|
|
|
}
|
|
}
|