bus_adapter.cpp 8.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330
  1. #include <adc_adapter/bus_adapter.h>
  2. #include <common/constants.h>
  3. #include <common/car_status.h>
  4. #include <math.h>
  5. #include <iostream>
  6. #include <fstream>
  7. using namespace std;
  8. iv::decition::BusAdapter::BusAdapter(){
  9. adapter_id=4;
  10. adapter_name="bus";
  11. }
  12. iv::decition::BusAdapter::~BusAdapter(){
  13. }
  14. iv::decition::Decition iv::decition::BusAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D> trace , float dSpeed, float obsDistance ,
  15. float obsSpeed,float accAim,float accNow ,bool changingDangWei,Decition *decition){
  16. accAim=min(0.7f,accAim);
  17. float controlSpeed=0;
  18. float controlBrake=0;
  19. float u = 0- accAim;
  20. float realSpeed = now_gps_ins.speed;
  21. float ttc = 0-(obsDistance/obsSpeed);
  22. bool turn_around=false;
  23. if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
  24. turn_around=true;
  25. }
  26. if(ttc<0){
  27. ttc=15;
  28. }
  29. if (accAim < 0)
  30. {
  31. controlSpeed=0;
  32. controlBrake=u; //102
  33. if(obsDistance>0 && obsDistance<10){
  34. controlBrake= u*1.5;
  35. }
  36. if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
  37. && dSpeed>0 && lastBrake==0){
  38. controlBrake=0;
  39. controlSpeed=0;
  40. }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
  41. controlBrake=min(controlBrake,0.5f);
  42. controlSpeed=0;
  43. }
  44. else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
  45. && !turn_around ){
  46. controlBrake=min(controlBrake,0.3f);
  47. controlSpeed=0;
  48. }
  49. else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
  50. && dSpeed>0 && !turn_around ){
  51. controlBrake=min(controlBrake,0.5f);
  52. controlSpeed=0;
  53. }
  54. //0110
  55. if(changingDangWei){
  56. controlBrake=max(0.5f,controlBrake);
  57. }
  58. //斜坡刹车加大 lsn 0217
  59. if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
  60. controlBrake=max(2.0f,controlBrake);
  61. }
  62. //斜坡刹车加大 end
  63. }
  64. else
  65. {
  66. controlBrake = 0;
  67. if(ServiceCarStatus.torque_st==0){
  68. controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
  69. controlSpeed =min( controlSpeed,2.0f);
  70. }else{
  71. controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115 *10
  72. }
  73. // if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
  74. // (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
  75. // if(realSpeed<5 ){
  76. // controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao 30
  77. // }else if(realSpeed<10){
  78. // controlSpeed=min((float)25.0,controlSpeed); //40
  79. // }
  80. // }
  81. // controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
  82. // if(accAim>0 && ServiceCarStatus.mfCalAcc>0 && realSpeed>1 ){
  83. // if(controlSpeed>0){
  84. // controlSpeed = ServiceCarStatus.torque_st;
  85. // }
  86. // }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
  87. // if(controlSpeed>0 ){
  88. // controlSpeed = ServiceCarStatus.torque_st;
  89. // }
  90. // }
  91. if(controlSpeed>ServiceCarStatus.torque_st){
  92. controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
  93. }else if(controlSpeed<ServiceCarStatus.torque_st){
  94. controlSpeed = ServiceCarStatus.torque_st-1.0;
  95. }
  96. //Seizing
  97. if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
  98. seizingCount++;
  99. }else{
  100. seizingCount=0;
  101. }
  102. if(seizingCount>=10){
  103. controlSpeed=ServiceCarStatus.torque_st+2;
  104. }
  105. // if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
  106. // controlSpeed=2.4;
  107. // }
  108. //seizing end
  109. // 斜坡加大油门 0217 lsn
  110. if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
  111. (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
  112. && abs(realSpeed)<1.0){
  113. controlSpeed=max((float)20.0,controlSpeed);
  114. controlSpeed=min((float)40.0,controlSpeed);
  115. }
  116. // 斜坡加大油门 end
  117. else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
  118. (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
  119. && abs(realSpeed)<10.0){
  120. controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao 30
  121. }
  122. }
  123. if(controlSpeed>0){
  124. controlSpeed=max(controlSpeed,2.4f);
  125. }
  126. //0227 10m nei xianzhi shache
  127. if(obsDistance<10 &&obsDistance>0){
  128. controlSpeed=0;
  129. controlBrake=max(controlBrake,0.8f);
  130. }
  131. // if(DecideGps00::minDecelerate<0){
  132. // controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
  133. // controlSpeed=0;
  134. // }
  135. if(minDecelerate<0){
  136. controlBrake = max((0-minDecelerate),controlBrake);
  137. controlSpeed=0;
  138. }
  139. controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
  140. controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
  141. // (*decition)->brake = controlBrake*10;
  142. (*decition)->brake = controlBrake*9;
  143. (*decition)->torque=controlSpeed;
  144. lastBrake= (*decition)->brake;
  145. lastTorque=(*decition)->torque;
  146. (*decition)->grade=1;
  147. (*decition)->mode=1;
  148. (*decition)->speak=0;
  149. (*decition)->handBrake=1; // 0: laqi 1: shifang
  150. (*decition)->bright=false;
  151. (*decition)->engine=0;
  152. (*decition)->dangweiEnable=true;
  153. (*decition)->angleEnable=true;
  154. (*decition)->speedEnable=true;
  155. (*decition)-> brakeEnable=true;
  156. (*decition)->driveMode=1;
  157. (*decition)->brakeType=0;
  158. (*decition)->angSpeed=60;
  159. (*decition)->topLight=0;
  160. if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
  161. (*decition)->dangWei=2;
  162. (*decition)->backLight=1;
  163. }
  164. //1220
  165. else{
  166. (*decition)->dangWei=4;
  167. (*decition)->backLight=0;
  168. }
  169. if((*decition)->brake>0){
  170. (*decition)->brakeLight=1;
  171. }else{
  172. (*decition)->brakeLight=0;
  173. }
  174. if((*decition)->leftlamp){
  175. (*decition)->directLight=1;
  176. }else if((*decition)->rightlamp){
  177. (*decition)->directLight=2;
  178. }else{
  179. (*decition)->directLight=0;
  180. }
  181. (*decition)->handBrake=false;
  182. (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
  183. (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
  184. (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
  185. //dangweiTrasfer
  186. // if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
  187. // decition_gps->dangWei=2;
  188. // decition_gps->torque=0;
  189. // }
  190. lastDangWei= (*decition)->dangWei;
  191. (*decition)->topLight=0; //1116
  192. (*decition)->nearLight=0;
  193. (*decition)->farLight=0;
  194. //(*decition)->door=3;
  195. std::cout<<"==========================================="<<std::endl;
  196. std::cout<<"dSpeed:"<<dSpeed<<" realSpeed:"<<realSpeed<<" acc:"<<accAim<<" torque_st:"<<ServiceCarStatus.torque_st
  197. <<" decideTorque:"<<(*decition)->torque <<" decideBrake:"<<(*decition)->brake
  198. <<std::endl;
  199. std::cout<<"========================================="<<std::endl;
  200. (*decition)->accelerator= (*decition)->torque ;
  201. return *decition;
  202. }
  203. float iv::decition::BusAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
  204. //刹车限制
  205. //刹车限制
  206. float vs =realSpeed*3.6;
  207. int BrakeIntMax = 10;
  208. if (vs < 10.0 / 3.6) BrakeIntMax = 4;
  209. else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
  210. //`
  211. if(ttc>10){
  212. BrakeIntMax = 2;
  213. }else if(ttc>6){
  214. BrakeIntMax = 3;
  215. }else if(ttc>3){
  216. BrakeIntMax = 4;
  217. }else {
  218. BrakeIntMax = 5;
  219. }
  220. if(obsDistance>0 && obsDistance<10){
  221. BrakeIntMax=max(BrakeIntMax,3);
  222. }
  223. if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
  224. controlBrake=min(5.0f,controlBrake);
  225. controlBrake=max(0.0f,controlBrake);
  226. return controlBrake;
  227. }
  228. float iv::decition::BusAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
  229. controlSpeed=min((float)100.0,controlSpeed);
  230. controlSpeed=max((float)0.0,controlSpeed);
  231. return controlSpeed;
  232. }