123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330 |
- #include <adc_adapter/bus_adapter.h>
- #include <common/constants.h>
- #include <common/car_status.h>
- #include <math.h>
- #include <iostream>
- #include <fstream>
- using namespace std;
- iv::decition::BusAdapter::BusAdapter(){
- adapter_id=4;
- adapter_name="bus";
- }
- iv::decition::BusAdapter::~BusAdapter(){
- }
- iv::decition::Decition iv::decition::BusAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D> trace , float dSpeed, float obsDistance ,
- float obsSpeed,float accAim,float accNow ,bool changingDangWei,Decition *decition){
- accAim=min(0.7f,accAim);
- float controlSpeed=0;
- float controlBrake=0;
- float u = 0- accAim;
- float realSpeed = now_gps_ins.speed;
- float ttc = 0-(obsDistance/obsSpeed);
- bool turn_around=false;
- if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
- turn_around=true;
- }
- if(ttc<0){
- ttc=15;
- }
- if (accAim < 0)
- {
- controlSpeed=0;
- controlBrake=u; //102
- if(obsDistance>0 && obsDistance<10){
- controlBrake= u*1.5;
- }
- if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
- && dSpeed>0 && lastBrake==0){
- controlBrake=0;
- controlSpeed=0;
- }else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around){
- controlBrake=min(controlBrake,0.5f);
- controlSpeed=0;
- }
- else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
- && !turn_around ){
- controlBrake=min(controlBrake,0.3f);
- controlSpeed=0;
- }
- else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
- && dSpeed>0 && !turn_around ){
- controlBrake=min(controlBrake,0.5f);
- controlSpeed=0;
- }
- //0110
- if(changingDangWei){
- controlBrake=max(0.5f,controlBrake);
- }
- //斜坡刹车加大 lsn 0217
- if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
- controlBrake=max(2.0f,controlBrake);
- }
- //斜坡刹车加大 end
- }
- else
- {
- controlBrake = 0;
- if(ServiceCarStatus.torque_st==0){
- controlSpeed = (u*(-16)+((0-u)-ServiceCarStatus.mfCalAcc)*10)*0.7; //*1.0
- controlSpeed =min( controlSpeed,2.0f);
- }else{
- controlSpeed= ServiceCarStatus.torque_st+((0-u)-ServiceCarStatus.mfCalAcc)*20;//1115 *10
- }
- // if(((gpsIns.ins_pitch_angle>-2.5 && ServiceCarStatus.daocheMode)||
- // (gpsIns.ins_pitch_angle<2.5 && !ServiceCarStatus.daocheMode))){
- // if(realSpeed<5 ){
- // controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao 30
- // }else if(realSpeed<10){
- // controlSpeed=min((float)25.0,controlSpeed); //40
- // }
- // }
- // controlSpeed = min((float)(ServiceCarStatus.torque_st+1.0),controlSpeed); //1115 5.0
- // if(accAim>0 && ServiceCarStatus.mfCalAcc>0 && realSpeed>1 ){
- // if(controlSpeed>0){
- // controlSpeed = ServiceCarStatus.torque_st;
- // }
- // }else if(accAim<0 && ServiceCarStatus.mfCalAcc<0 && dSpeed>0){
- // if(controlSpeed>0 ){
- // controlSpeed = ServiceCarStatus.torque_st;
- // }
- // }
- if(controlSpeed>ServiceCarStatus.torque_st){
- controlSpeed = min(ServiceCarStatus.torque_st+1.0f,controlSpeed);
- }else if(controlSpeed<ServiceCarStatus.torque_st){
- controlSpeed = ServiceCarStatus.torque_st-1.0;
- }
- //Seizing
- if((ServiceCarStatus.torque_st==controlSpeed-1)&&dSpeed>1){
- seizingCount++;
- }else{
- seizingCount=0;
- }
- if(seizingCount>=10){
- controlSpeed=ServiceCarStatus.torque_st+2;
- }
- // if(controlSpeed==2.0 && ServiceCarStatus.torque_st<2){
- // controlSpeed=2.4;
- // }
- //seizing end
- // 斜坡加大油门 0217 lsn
- if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
- (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
- && abs(realSpeed)<1.0){
- controlSpeed=max((float)20.0,controlSpeed);
- controlSpeed=min((float)40.0,controlSpeed);
- }
- // 斜坡加大油门 end
- else if(((now_gps_ins.ins_pitch_angle<-2.5 && ServiceCarStatus.daocheMode)||
- (now_gps_ins.ins_pitch_angle>2.5 && !ServiceCarStatus.daocheMode))
- && abs(realSpeed)<10.0){
- controlSpeed=min((float)25.0,controlSpeed); //youmenxianxiao 30
- }
- }
- if(controlSpeed>0){
- controlSpeed=max(controlSpeed,2.4f);
- }
- //0227 10m nei xianzhi shache
- if(obsDistance<10 &&obsDistance>0){
- controlSpeed=0;
- controlBrake=max(controlBrake,0.8f);
- }
- // if(DecideGps00::minDecelerate<0){
- // controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
- // controlSpeed=0;
- // }
- if(minDecelerate<0){
- controlBrake = max((0-minDecelerate),controlBrake);
- controlSpeed=0;
- }
- controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
- controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
- // (*decition)->brake = controlBrake*10;
- (*decition)->brake = controlBrake*9;
- (*decition)->torque=controlSpeed;
- lastBrake= (*decition)->brake;
- lastTorque=(*decition)->torque;
- (*decition)->grade=1;
- (*decition)->mode=1;
- (*decition)->speak=0;
- (*decition)->handBrake=1; // 0: laqi 1: shifang
- (*decition)->bright=false;
- (*decition)->engine=0;
- (*decition)->dangweiEnable=true;
- (*decition)->angleEnable=true;
- (*decition)->speedEnable=true;
- (*decition)-> brakeEnable=true;
- (*decition)->driveMode=1;
- (*decition)->brakeType=0;
- (*decition)->angSpeed=60;
- (*decition)->topLight=0;
- if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
- (*decition)->dangWei=2;
- (*decition)->backLight=1;
- }
- //1220
- else{
- (*decition)->dangWei=4;
- (*decition)->backLight=0;
- }
- if((*decition)->brake>0){
- (*decition)->brakeLight=1;
- }else{
- (*decition)->brakeLight=0;
- }
- if((*decition)->leftlamp){
- (*decition)->directLight=1;
- }else if((*decition)->rightlamp){
- (*decition)->directLight=2;
- }else{
- (*decition)->directLight=0;
- }
- (*decition)->handBrake=false;
- (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
- (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
- (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
- //dangweiTrasfer
- // if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
- // decition_gps->dangWei=2;
- // decition_gps->torque=0;
- // }
- lastDangWei= (*decition)->dangWei;
- (*decition)->topLight=0; //1116
- (*decition)->nearLight=0;
- (*decition)->farLight=0;
- //(*decition)->door=3;
- std::cout<<"==========================================="<<std::endl;
- std::cout<<"dSpeed:"<<dSpeed<<" realSpeed:"<<realSpeed<<" acc:"<<accAim<<" torque_st:"<<ServiceCarStatus.torque_st
- <<" decideTorque:"<<(*decition)->torque <<" decideBrake:"<<(*decition)->brake
- <<std::endl;
- std::cout<<"========================================="<<std::endl;
- (*decition)->accelerator= (*decition)->torque ;
- return *decition;
- }
- float iv::decition::BusAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
- //刹车限制
- //刹车限制
- float vs =realSpeed*3.6;
- int BrakeIntMax = 10;
- if (vs < 10.0 / 3.6) BrakeIntMax = 4;
- else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
- //`
- if(ttc>10){
- BrakeIntMax = 2;
- }else if(ttc>6){
- BrakeIntMax = 3;
- }else if(ttc>3){
- BrakeIntMax = 4;
- }else {
- BrakeIntMax = 5;
- }
- if(obsDistance>0 && obsDistance<10){
- BrakeIntMax=max(BrakeIntMax,3);
- }
- if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
- controlBrake=min(5.0f,controlBrake);
- controlBrake=max(0.0f,controlBrake);
- return controlBrake;
- }
- float iv::decition::BusAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
- controlSpeed=min((float)100.0,controlSpeed);
- controlSpeed=max((float)0.0,controlSpeed);
- return controlSpeed;
- }
|