|
@@ -979,28 +979,57 @@ void Canbus::AccLoopOnTimer(const ros::TimerEvent &event)
|
|
tAccCtrReq.uiAccPsngCtrReq = accPsng;
|
|
tAccCtrReq.uiAccPsngCtrReq = accPsng;
|
|
MessageCoder::CanMsgPackage((void*)&tAccCtrReq, robot::common::AccPedalActCtrReq, &canAccCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&tAccCtrReq, robot::common::AccPedalActCtrReq, &canAccCtrReqMsg);
|
|
write_data(&canAccCtrReqMsg);
|
|
write_data(&canAccCtrReqMsg);
|
|
- if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
|
- accLoopNum++;
|
|
|
|
- if(accLoopNum > accLpReq.uiAccFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
|
|
|
+ if(accLpReq.uiAccPsngStart < accLpReq.uiAccPsngEnd)
|
|
{
|
|
{
|
|
- acc_timer_.stop();
|
|
|
|
- accTimerState = false;
|
|
|
|
- accLoopNum = 1;
|
|
|
|
- tAccCtrReq = {0};
|
|
|
|
- return;
|
|
|
|
- }
|
|
|
|
- if (accIncrease)
|
|
|
|
- {
|
|
|
|
- accPsng += accLpReq.uiAccSpan;
|
|
|
|
- if (accPsng >= accLpReq.uiAccPsngEnd)
|
|
|
|
- accIncrease = false;
|
|
|
|
|
|
+ if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
|
+ accLoopNum++;
|
|
|
|
+ if(accLoopNum > accLpReq.uiAccFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
|
+ {
|
|
|
|
+ acc_timer_.stop();
|
|
|
|
+ accTimerState = false;
|
|
|
|
+ accLoopNum = 1;
|
|
|
|
+ tAccCtrReq = {0};
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ if (accIncrease)
|
|
|
|
+ {
|
|
|
|
+ accPsng += accLpReq.uiAccSpan;
|
|
|
|
+ if (accPsng >= accLpReq.uiAccPsngEnd)
|
|
|
|
+ accIncrease = false;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ accPsng -= accLpReq.uiAccSpan;
|
|
|
|
+ if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
|
+ accIncrease = true;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- accPsng -= accLpReq.uiAccSpan;
|
|
|
|
- if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
|
- accIncrease = true;
|
|
|
|
|
|
+ if (accPsng >= accLpReq.uiAccPsngStart)
|
|
|
|
+ accLoopNum++;
|
|
|
|
+ if(accLoopNum > accLpReq.uiAccFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
|
+ {
|
|
|
|
+ acc_timer_.stop();
|
|
|
|
+ accTimerState = false;
|
|
|
|
+ accLoopNum = 1;
|
|
|
|
+ tAccCtrReq = {0};
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ if (accIncrease)
|
|
|
|
+ {
|
|
|
|
+ accPsng -= accLpReq.uiAccSpan;
|
|
|
|
+ if (accPsng <= accLpReq.uiAccPsngEnd)
|
|
|
|
+ accIncrease = false;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ accPsng += accLpReq.uiAccSpan;
|
|
|
|
+ if (accPsng >= accLpReq.uiAccPsngStart)
|
|
|
|
+ accIncrease = true;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
+
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
{
|
|
{
|