|
@@ -7,19 +7,22 @@
|
|
#include "canbus.h"
|
|
#include "canbus.h"
|
|
#include"candatatype.h"
|
|
#include"candatatype.h"
|
|
#include"messagecoder.h"
|
|
#include"messagecoder.h"
|
|
|
|
+#include "pid.h"
|
|
#include"../common/message.h"
|
|
#include"../common/message.h"
|
|
#include"../common/canmsgobdmsgid.h"
|
|
#include"../common/canmsgobdmsgid.h"
|
|
#include "std_msgs/String.h"
|
|
#include "std_msgs/String.h"
|
|
#include <canbus/canMsg.h>
|
|
#include <canbus/canMsg.h>
|
|
|
|
+#include <canbus/rackTestReplyMsg.h>
|
|
#include <iostream>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <fstream>
|
|
|
|
+#include <chrono>
|
|
|
|
+#include <sys/time.h>
|
|
|
|
|
|
|
|
|
|
namespace robot{
|
|
namespace robot{
|
|
namespace canbusSpace {
|
|
namespace canbusSpace {
|
|
|
|
|
|
|
|
|
|
-
|
|
|
|
std::string Canbus::Name() const { return "canbus"; }
|
|
std::string Canbus::Name() const { return "canbus"; }
|
|
|
|
|
|
int Canbus::Init() {
|
|
int Canbus::Init() {
|
|
@@ -98,6 +101,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);
|
|
|
|
|
|
ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
|
|
ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
|
|
&Canbus::ActuatorEnableRequest, this);
|
|
&Canbus::ActuatorEnableRequest, this);
|
|
@@ -179,6 +183,12 @@ int Canbus::Start() {
|
|
|
|
|
|
ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
|
|
ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
|
|
&Canbus::SftTxtReq,this);
|
|
&Canbus::SftTxtReq,this);
|
|
|
|
+ ros::ServiceServer RackTestService = node_handle_->advertiseService(robot::common::RackTestService,
|
|
|
|
+ &Canbus::RackTestReq,this);
|
|
|
|
+
|
|
|
|
+ ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
|
|
|
|
+ &Canbus::RTKLocationCallback, this);
|
|
|
|
+
|
|
|
|
|
|
string str = "/home/madesheng/angle.txt";
|
|
string str = "/home/madesheng/angle.txt";
|
|
int flag = Canbus::read_txt(str,1);
|
|
int flag = Canbus::read_txt(str,1);
|
|
@@ -214,6 +224,13 @@ int Canbus::Start() {
|
|
return ROBOT_FAILTURE;
|
|
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())
|
|
{
|
|
{
|
|
@@ -230,6 +247,7 @@ 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
|
|
if(heatbeat > 15)
|
|
if(heatbeat > 15)
|
|
{
|
|
{
|
|
heatbeat = 0;
|
|
heatbeat = 0;
|
|
@@ -269,6 +287,7 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
{
|
|
{
|
|
LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
|
|
LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
|
|
}
|
|
}
|
|
|
|
+ //TODO
|
|
}
|
|
}
|
|
|
|
|
|
void Canbus::Stop() {
|
|
void Canbus::Stop() {
|
|
@@ -313,11 +332,12 @@ int Canbus::read_loop()
|
|
{
|
|
{
|
|
TPCANRdMsg msg;
|
|
TPCANRdMsg msg;
|
|
__u32 status;
|
|
__u32 status;
|
|
-
|
|
|
|
|
|
+ //TODO dakai
|
|
if (funLINUX_CAN_Read(pcan_handle, &msg)) {
|
|
if (funLINUX_CAN_Read(pcan_handle, &msg)) {
|
|
LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
|
|
LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
|
|
return errno;
|
|
return errno;
|
|
}
|
|
}
|
|
|
|
+
|
|
read_data(&(msg.Msg));
|
|
read_data(&(msg.Msg));
|
|
|
|
|
|
// deal receive data
|
|
// deal receive data
|
|
@@ -330,7 +350,7 @@ int Canbus::read_loop()
|
|
return errno;
|
|
return errno;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
-
|
|
|
|
|
|
+//TODO
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -1571,6 +1591,34 @@ int Canbus::read_txt(string &path,int flag)
|
|
}
|
|
}
|
|
cout << "sft NUMBER:" <<sft_psng_number<<endl;
|
|
cout << "sft NUMBER:" <<sft_psng_number<<endl;
|
|
}
|
|
}
|
|
|
|
+ else if(flag == 6)
|
|
|
|
+ {
|
|
|
|
+ for (int i = 0; i < 1190; i++)
|
|
|
|
+ {
|
|
|
|
+ if(myfile.eof())
|
|
|
|
+ {
|
|
|
|
+ nedc_speed_number = i;
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ myfile >> nedc_speeds[i];
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ cout << "nedc speed NUMBER:" <<nedc_speed_number<<endl;
|
|
|
|
+ }
|
|
|
|
+ else if(flag == 7)
|
|
|
|
+ {
|
|
|
|
+ for (int i = 0; i < 3; i++)
|
|
|
|
+ {
|
|
|
|
+ if(myfile.eof())
|
|
|
|
+ {
|
|
|
|
+ fuzzy_pid_number = i;
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ myfile >> fuzzy_pid[i];
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
|
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|
myfile.close();
|
|
myfile.close();
|
|
@@ -1984,7 +2032,410 @@ bool Canbus::IOMode(canbus::IOMode::Request &req,
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
|
|
+ canbus::RackTestReq::Response &res)
|
|
|
|
+{
|
|
|
|
+ LOG(INFO)<< "controlservice start RackTestReq";
|
|
|
|
+
|
|
|
|
+ // RackTestReq request
|
|
|
|
+ try{
|
|
|
|
+ if(req.uiStart == 0)
|
|
|
|
+ {
|
|
|
|
+ if(rackTestState)
|
|
|
|
+ {
|
|
|
|
+ //TODO zhushidiao
|
|
|
|
+ //rack_test_timer_.stop();
|
|
|
|
+ //TODO
|
|
|
|
+ rackTestState = false;
|
|
|
|
+ }
|
|
|
|
+ if(rackTestSendTimerState)
|
|
|
|
+ {
|
|
|
|
+ rack_reply_timer_.stop();
|
|
|
|
+ rackTestSendTimerState = false;
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ }
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ if(rackTestState || rackTestSendTimerState)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 2;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ nedc_speed_count = 0;
|
|
|
|
+ rack_test_send_count = 0;
|
|
|
|
+//TODO zhushidiao
|
|
|
|
+ // rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
|
|
|
|
+ // rack_test_timer_.start();
|
|
|
|
+ //TODO
|
|
|
|
+ rackTestState = true;
|
|
|
|
+
|
|
|
|
+ rack_reply_timer_ =
|
|
|
|
+ CreateTimer(ros::Duration(duration_rack_reply), &Canbus::RackTestReplyOnTimer, this,false,false);
|
|
|
|
+ rack_reply_timer_.start();
|
|
|
|
+ rackTestSendTimerState = true;
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ LOG(ERROR)<< "controlservice RackTestReq failed "<< ex.what();
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
|
|
+{
|
|
|
|
+ PID PidControll;
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number)
|
|
|
|
+ {
|
|
|
|
+ rack_test_timer_.stop();
|
|
|
|
+ rackTestState = false;
|
|
|
|
+ rack_reply_timer_.stop();
|
|
|
|
+ rackTestSendTimerState = false;
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ gettimeofday(&now_time, NULL);
|
|
|
|
+ float delta_time = 0.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);
|
|
|
|
+ delta_time = elapsed / 1000000;
|
|
|
|
+ }
|
|
|
|
+ cout << "time sub" << delta_time <<endl;
|
|
|
|
+
|
|
|
|
+ last_time = now_time;
|
|
|
|
+
|
|
|
|
+ //根据时间差计算速度插值
|
|
|
|
+ float set_speed1 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ float set_speed2 = 0;
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number - 1)
|
|
|
|
+ {
|
|
|
|
+ set_speed2 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ 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";
|
|
|
|
+ int flag = Canbus::read_txt(str,7);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ float pi_out = 0.01;
|
|
|
|
+ pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
|
|
+ fuzzy_pid[0],fuzzy_pid[1],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;
|
|
|
|
+
|
|
|
|
+ TPCANMsg canBrkPedalActCtrReqMsg;
|
|
|
|
+ T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
|
|
+ TPCANMsg canAccPedalActCtrReqMsg;
|
|
|
|
+ T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
|
|
+
|
|
|
|
+ if(set_speed == 0)
|
|
|
|
+ {
|
|
|
|
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkAccCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
+ requestBrk.uiBrkDecCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkFocCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = (50/30) * 5;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canBrkPedalActCtrReqMsg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ if(pi_out >= 0)
|
|
|
|
+ {
|
|
|
|
+ requestAcc.uiAccSpdCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccAccCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccPsngCtrReq = pi_out *100;
|
|
|
|
+ requestAcc.uiAccDecCtrReq = 0;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canAccPedalActCtrReqMsg);
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ requestBrk.uiBrkSpdCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkAccCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
+ requestBrk.uiBrkDecCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkFocCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = (50/30) * (-pi_out * 30);
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canBrkPedalActCtrReqMsg);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ nedc_speed_count = rack_test_send_count;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " RackTestOnTimer error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
|
|
+{
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ if(rackTestSendTimerState)
|
|
|
|
+ {
|
|
|
|
+ rackTestReplyState = 1;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ // 发布消息,rackTestReplyState,actual_speed 1s回复前端一次消息
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ rack_test_send_count ++;
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " RackTestReplyOnTimer error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+//获取当前时间和毫秒
|
|
|
|
+int Canbus::get_datetime_mill(tm *datetime)
|
|
|
|
+{
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ auto time_now = std::chrono::system_clock::now();
|
|
|
|
+
|
|
|
|
+ //通过不同精度获取相差的毫秒数
|
|
|
|
+ uint64_t difference_millseconds =
|
|
|
|
+ std::chrono::duration_cast<std::chrono::milliseconds>(time_now.time_since_epoch()).count()
|
|
|
|
+ - std::chrono::duration_cast<std::chrono::seconds>(time_now.time_since_epoch()).count() * 1000;
|
|
|
|
+ time_t t_time = std::chrono::system_clock::to_time_t(time_now);
|
|
|
|
+ tm *time_date = localtime(&t_time);
|
|
|
|
+ char strTime[25] = { 0 };
|
|
|
|
+ sprintf(strTime, "[%d-%02d-%02d %02d:%02d:%02d.%03d]", time_date->tm_year + 1900,
|
|
|
|
+ time_date->tm_mon + 1, time_date->tm_mday, time_date->tm_hour,
|
|
|
|
+ time_date->tm_min, time_date->tm_sec, (int)difference_millseconds);
|
|
|
|
+ /* 打印时间 */
|
|
|
|
+ // std::cout << strTime << std::endl;
|
|
|
|
+
|
|
|
|
+ datetime->tm_year = time_date->tm_year + 1900;
|
|
|
|
+ datetime->tm_mon = time_date->tm_mon + 1;
|
|
|
|
+ datetime->tm_mday = time_date->tm_mday;
|
|
|
|
+ datetime->tm_hour = time_date->tm_hour;
|
|
|
|
+ datetime->tm_min = time_date->tm_min;
|
|
|
|
+ datetime->tm_sec = time_date->tm_sec;
|
|
|
|
+ return (int)difference_millseconds;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " Get Now DateTime And Millseconds error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+//发布台架测试任务实际车速
|
|
|
|
+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);
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+// 接收RTK数据
|
|
|
|
+void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
|
+{
|
|
|
|
+ msg_RTK.AngleDeviated = msg->AngleDeviated;
|
|
|
|
+ msg_RTK.Height = msg->Height;
|
|
|
|
+ msg_RTK.Latitude = msg->Latitude;
|
|
|
|
+ msg_RTK.Longitude = msg->Longitude;
|
|
|
|
+ msg_RTK.Speed = msg->Speed;
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number)
|
|
|
|
+ {
|
|
|
|
+ rackTestState = false;
|
|
|
|
+ rack_reply_timer_.stop();
|
|
|
|
+ rackTestSendTimerState = false;
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ if(!rackTestState)
|
|
|
|
+ {
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ LOG(INFO)<< "RTKLocationCallback Start FUZZY PID";
|
|
|
|
+ PID PidControll;
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ gettimeofday(&now_time, NULL);
|
|
|
|
+ float delta_time = 0.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);
|
|
|
|
+ delta_time = (float)elapsed / 1000000;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "delta time:"<<std::setprecision(18)<<delta_time<<" s";
|
|
|
|
+ // cout << "time sub " << delta_time <<" s"<<endl;
|
|
|
|
+
|
|
|
|
+ last_time = now_time;
|
|
|
|
+
|
|
|
|
+ //根据时间差计算速度插值
|
|
|
|
+ float set_speed1 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ float set_speed2 = 0;
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number - 1)
|
|
|
|
+ {
|
|
|
|
+ set_speed2 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
|
|
+ }
|
|
|
|
+ float set_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
+ float actual_speed = msg_RTK.Speed;
|
|
|
|
+
|
|
|
|
+ string str = "/home/madesheng/FuzzyPidFactor.txt";
|
|
|
|
+ int flag = Canbus::read_txt(str,7);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ float pi_out = 0.01;
|
|
|
|
+ pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
|
|
+ fuzzy_pid[0],fuzzy_pid[1],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;
|
|
|
|
+
|
|
|
|
+ TPCANMsg canBrkPedalActCtrReqMsg;
|
|
|
|
+ T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
|
|
|
|
+ TPCANMsg canAccPedalActCtrReqMsg;
|
|
|
|
+ T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
|
|
|
|
+
|
|
|
|
+ if(set_speed == 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 = 0;
|
|
|
|
+ requestBrk.uiBrkAccCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
+ requestBrk.uiBrkDecCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkFocCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = 35 * 10;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canBrkPedalActCtrReqMsg,true);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ 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.uiAccAccCtrReq = 0;
|
|
|
|
+ requestAcc.uiAccPsngCtrReq = pi_out *100 * 10;
|
|
|
|
+ requestAcc.uiAccDecCtrReq = 0;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canAccPedalActCtrReqMsg,true);
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ 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.uiBrkAccCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkCtrMdReq = 0;
|
|
|
|
+ requestBrk.uiBrkDecCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkFocCtrReq = 0;
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq = (80 * (-pi_out * 30) / 30) * 10;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
|
|
|
|
+ write_data(&canBrkPedalActCtrReqMsg,true);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ nedc_speed_count = rack_test_send_count;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " RTKLocationCallback error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
|
|
|
|
|
|
|
|
|