mirror of
https://github.com/Dev-KATECH/ADM.git
synced 2026-05-17 01:43:59 +09:00
131 lines
3.3 KiB
C
131 lines
3.3 KiB
C
#include "Ignition.h"
|
|
#include "Global_Variable.h"
|
|
#include "BMS_SOC.h"
|
|
#include "Low_SOC_mode.h"
|
|
#include "CE_ON_CMD.h"
|
|
#include "Angle_Speed.h"
|
|
#include "can.h"
|
|
#include "board.h"
|
|
|
|
// RWA 앵글 스피드 조절, 튜닝 변수
|
|
float RWA_Angle_Speed_limit = 4; //updated 0829, Author : JaeminSong
|
|
|
|
float Angle_Speed_Gain = 0.065;
|
|
// RWS 앵글 스피드 조절, 튜닝 변수
|
|
float RWS_Angle_Speed_limit = 2.7;
|
|
// 조향 앵글 각 270/s 로 제한
|
|
|
|
void Angle_Speed(void){
|
|
GV_RWA_040_RackAngleCmd = GV_MASTER_RWA_RackAngleCommand;
|
|
GV_RWA2_040_RackAngleCmd = GV_MASTER_RWA_RackAngleCommand;
|
|
#if 0
|
|
if(GV_VCU_Drive_Mode == 1 ){
|
|
|
|
|
|
|
|
if(GV_MASTER_RWA_RackAngleCommand != Cur_RWA_RACK_ANGLE){
|
|
|
|
if(GV_MASTER_RWA_RackAngleCommand < 0){ // 우회전 일 때
|
|
|
|
|
|
if(Cur_RWA_RACK_ANGLE < GV_MASTER_RWA_RackAngleCommand){ // 우회전 가다가 좌회전 꺾을때
|
|
Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE+(4 - 0.065 *GV_VCU_Vx_Fbk) ;
|
|
|
|
GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
|
|
GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
|
|
}
|
|
|
|
if(Cur_RWA_RACK_ANGLE > GV_MASTER_RWA_RackAngleCommand){ // 우회전 계속 할때
|
|
Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE-(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk);
|
|
|
|
GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
|
|
GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
if(GV_MASTER_RWA_RackAngleCommand > 0){ // 좌회전 일 때
|
|
|
|
|
|
if(Cur_RWA_RACK_ANGLE > GV_MASTER_RWA_RackAngleCommand){// 좌회전 가다가 우회전 꺾을때
|
|
Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE-(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk) ;
|
|
|
|
GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
|
|
GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
|
|
}
|
|
|
|
if(Cur_RWA_RACK_ANGLE < GV_MASTER_RWA_RackAngleCommand){// 좌회전 계속 할때
|
|
Cur_RWA_RACK_ANGLE = Cur_RWA_RACK_ANGLE+(RWA_Angle_Speed_limit - Angle_Speed_Gain *GV_VCU_Vx_Fbk) ;
|
|
|
|
GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
|
|
GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}else {
|
|
|
|
GV_RWA_040_RackAngleCmd = GV_MASTER_RWA_RackAngleCommand;
|
|
GV_RWA2_040_RackAngleCmd= GV_MASTER_RWA_RackAngleCommand;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
#endif
|
|
|
|
|
|
}
|
|
|
|
void RWS_Angle_Speed(void){
|
|
|
|
|
|
if(GV_MASTER_RWS_RackAngleCommand != Cur_RWS_RACK_ANGLE){
|
|
|
|
if(GV_MASTER_RWS_RackAngleCommand < 0){ // 우회전 일 때
|
|
|
|
|
|
if(Cur_RWS_RACK_ANGLE < GV_MASTER_RWS_RackAngleCommand){ // 우회전 가다가 좌회전 꺾을때
|
|
Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE+RWS_Angle_Speed_limit;
|
|
|
|
GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
|
|
}
|
|
|
|
if(Cur_RWS_RACK_ANGLE > GV_MASTER_RWS_RackAngleCommand){ // 우회전 계속 할때
|
|
Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE-RWS_Angle_Speed_limit;
|
|
|
|
GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
if(GV_MASTER_RWS_RackAngleCommand > 0){ // 좌회전 일 때
|
|
|
|
|
|
if(Cur_RWS_RACK_ANGLE > GV_MASTER_RWS_RackAngleCommand){// 좌회전 가다가 우회전 꺾을때
|
|
Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE-RWS_Angle_Speed_limit;
|
|
|
|
GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
}
|
|
|
|
if(Cur_RWS_RACK_ANGLE < GV_MASTER_RWS_RackAngleCommand){// 좌회전 계속 할때
|
|
Cur_RWS_RACK_ANGLE = Cur_RWS_RACK_ANGLE+RWS_Angle_Speed_limit;
|
|
|
|
GV_RWS1_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
GV_RWS2_040_RackAngleCmd = Cur_RWS_RACK_ANGLE;
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|