#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 앵글 스피드 조절, 튜닝 변수
int RWA_Angle_Speed_limit = 2.7;

// RWS 앵글 스피드 조절, 튜닝 변수
int RWS_Angle_Speed_limit = 2.7;
// 조향 앵글 각 120/s 로 제한
// 정인제 담당자 피드백 -> 50% 스피드 상향 요청
void Angle_Speed(void){


	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+RWA_Angle_Speed_limit;

				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;

				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;

				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;

				GV_RWA_040_RackAngleCmd = Cur_RWA_RACK_ANGLE;
				GV_RWA2_040_RackAngleCmd= Cur_RWA_RACK_ANGLE;
			}

		}

	}

}

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;
			}

		}

	}

}