else if(mGear != GEAR_R) { //std::ofstream Dspeed; // Dspeed.open("/home/jjcc2/mogu-master/control3.csv",std::ios::app); // Dspeed<<std::fixed<<std::setprecision(3)<<mSpeed<<"\n"; // park_stations_x,park_stations_y if(CrossBorder(cur_vehicle_x,cur_vehicle_y,park_stations_x,park_stations_y,cur_vehicle_heading)) { mControlData.brakePercent = 100;; mControlData.throttlePercent = 0.0; std::cout << "CrossBorder: " << std::endl; std::ofstream C; C.open("/home/jjcc2/mogu-master/controllB.csv",std::ios::app); C<<std::fixed<<std::setprecision(3)<< mControlData.brakePercent <<"\n"; } else { fpid.setParam(5,20,30,4); double PidOut = fpid.getFpidOutput(mControlData.vehicleSpeed,mSpeed); if(PidOut >= 0) { // if(PidOut > throttle_feedback+20 ) // { // mControlData.brakePercent = 0.0; // mControlData.throttlePercent = throttle_feedback+20; // } // else // { mControlData.brakePercent = 0.0; mControlData.throttlePercent = PidOut; // } } else { mControlData.brakePercent = fabs(PidOut);//fabs(PidOut)/50 mControlData.throttlePercent = 0.0; } }
时间: 2024-02-10 11:27:55 浏览: 65
这段代码是一个`else if`条件语句块,表示当满足条件`mGear != GEAR_R`时执行其中的代码块。
在代码块中,首先有一段被注释掉的代码,被注释掉的部分通过打开文件并向文件中写入数据。
接下来,有一个条件判断 `if(CrossBorder(cur_vehicle_x,cur_vehicle_y,park_stations_x,park_stations_y,cur_vehicle_heading))`,调用`CrossBorder`函数判断当前车辆是否越过了边界。如果越过了边界,则执行条件块中的代码。
在条件块中,将变量`mControlData.brakePercent`设置为100,表示刹车百分比为100%。
将变量`mControlData.throttlePercent`设置为0.0,表示油门百分比为0%。
然后,有一个输出调试信息的语句,打印"CrossBorder"。
接下来是一个`else`语句块,表示当未满足上述条件时执行其中的代码块。
在`else`语句块中,首先调用了`fpid.setParam(5,20,30,4)`,设置了PID控制器的参数。
然后,调用`fpid.getFpidOutput(mControlData.vehicleSpeed,mSpeed)`计算PID控制器的输出值,并将结果赋给变量`PidOut`。
接下来有一个条件判断 `if(PidOut >= 0)`,判断变量`PidOut`是否大于等于0。如果满足条件,则执行条件块中的代码。
在条件块中,将刹车百分比`mControlData.brakePercent`设置为0.0,表示刹车百分比为0%。
将油门百分比`mControlData.throttlePercent`设置为`PidOut`,即PID控制器的输出值。
如果不满足条件,则执行`else`语句块中的代码。
在`else`语句块中,将刹车百分比`mControlData.brakePercent`设置为`fabs(PidOut)`的绝对值。
将油门百分比`mControlData.throttlePercent`设置为0.0,表示油门百分比为0%。
总结来说,这段代码是一个条件语句块,根据不同的条件执行不同的操作。当`mGear`不等于`GEAR_R`时,根据车辆是否越过边界和PID控制器的输出值来设置刹车百分比和油门百分比。
阅读全文