Skip to content

Commit

Permalink
Added torque limit.
Browse files Browse the repository at this point in the history
  • Loading branch information
@IanMurphy-Rockfort
IanMurphy-Rockfort committed Jul 4, 2021
1 parent ce5c7a1 commit b2f9082
Show file tree
Hide file tree
Showing 3 changed files with 39 additions and 16 deletions.
39 changes: 32 additions & 7 deletions FS-AI_API/fs-ai_api.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -712,9 +712,16 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) {
float t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 0;
float t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = 0;

float t_FRONT_AXLE_TORQUE_MAX = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw);
float t_REAR_AXLE_TORQUE_MAX = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw);
float t_STEER_ANGLE_MAX = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw);
float t_FRONT_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_FRONT_AXLE_TORQUE_MAX_raw);
float t_REAR_AXLE_TORQUE_MAX_Nm = (0.1f*VCU2AI_REAR_AXLE_TORQUE_MAX_raw);
float t_STEER_ANGLE_MAX_deg = (0.1f*VCU2AI_STEER_ANGLE_MAX_raw);

uint16_t t_fastest_wheel_rpm = 0;

if(VCU2AI_FL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FL_WHEEL_SPEED_rpm; }
if(VCU2AI_FR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_FR_WHEEL_SPEED_rpm; }
if(VCU2AI_RL_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RL_WHEEL_SPEED_rpm; }
if(VCU2AI_RR_WHEEL_SPEED_rpm > t_fastest_wheel_rpm) { t_fastest_wheel_rpm = VCU2AI_RR_WHEEL_SPEED_rpm; }

t_AI2VCU_MISSION_STATUS = data->AI2VCU_MISSION_STATUS;
t_AI2VCU_DIRECTION_REQUEST = data->AI2VCU_DIRECTION_REQUEST;
Expand All @@ -728,20 +735,38 @@ void fs_ai_api_ai2vcu_set_data(fs_ai_api_ai2vcu *data) {
t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct;
t_AI2VCU_REAR_BRAKE_PRESS_REQUEST_pct = data->AI2VCU_BRAKE_PRESS_REQUEST_pct;

// additional torque limit to maintain constant electrical power and minimise risk of over-current trip
if(t_fastest_wheel_rpm > 700) {
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 50.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 50.0f; }
if(t_REAR_AXLE_TORQUE_MAX_Nm > 50.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 50.0f; }
} else if(t_fastest_wheel_rpm > 600) {
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 85.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 85.0f; }
if(t_REAR_AXLE_TORQUE_MAX_Nm > 85.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 85.0f; }
} else if(t_fastest_wheel_rpm > 500) {
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 100.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 100.0f; }
if(t_REAR_AXLE_TORQUE_MAX_Nm > 100.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 100.0f; }
} else if(t_fastest_wheel_rpm > 400) {
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 120.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 120.0f; }
if(t_REAR_AXLE_TORQUE_MAX_Nm > 120.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 120.0f; }
} else if(t_fastest_wheel_rpm > 300) {
if(t_FRONT_AXLE_TORQUE_MAX_Nm > 150.0f) { t_FRONT_AXLE_TORQUE_MAX_Nm = 150.0f; }
if(t_REAR_AXLE_TORQUE_MAX_Nm > 150.0f) { t_REAR_AXLE_TORQUE_MAX_Nm = 150.0f; }
}

// validate the 'float' requests
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX; }
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg < (-1.0f*t_STEER_ANGLE_MAX)) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = (-1.0f*t_STEER_ANGLE_MAX); }
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg > t_STEER_ANGLE_MAX_deg) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = t_STEER_ANGLE_MAX_deg; }
if(t_AI2VCU_STEER_ANGLE_REQUEST_deg < (-1.0f*t_STEER_ANGLE_MAX_deg)) { t_AI2VCU_STEER_ANGLE_REQUEST_deg = (-1.0f*t_STEER_ANGLE_MAX_deg); }

if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; }
if(t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_FRONT_MOTOR_SPEED_REQUEST_rpm = 0.0f; }

if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm > MOTOR_MAX_RPM) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = MOTOR_MAX_RPM; }
if(t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm < 0.0f) { t_AI2VCU_REAR_MOTOR_SPEED_REQUEST_rpm = 0.0f; }

if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm > t_FRONT_AXLE_TORQUE_MAX) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = t_FRONT_AXLE_TORQUE_MAX; }
if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm > t_FRONT_AXLE_TORQUE_MAX_Nm) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = t_FRONT_AXLE_TORQUE_MAX_Nm; }
if(t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_FRONT_AXLE_TORQUE_REQUEST_Nm = 0.0f; }

if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm > t_REAR_AXLE_TORQUE_MAX) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = t_REAR_AXLE_TORQUE_MAX; }
if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm > t_REAR_AXLE_TORQUE_MAX_Nm) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = t_REAR_AXLE_TORQUE_MAX_Nm; }
if(t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm < 0.0f) { t_AI2VCU_REAR_AXLE_TORQUE_REQUEST_Nm = 0.0f; }

if(t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct > 100.0f) { t_AI2VCU_FRONT_BRAKE_PRESS_REQUEST_pct = 100.0f; }
Expand Down
6 changes: 2 additions & 4 deletions FS-AI_API_Console/fs-ai_api_console.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ static void *loop_thread();
static fs_ai_api_ai2vcu ai2vcu_data; // assume initialised to zeros
static char inputs[10] = "", outputs[80] = " ";
static int data = 0;
static int timing_us = 5000;
static int timing_us = 10000;


int main(int argc, char** argv) {
Expand Down Expand Up @@ -154,8 +154,6 @@ static void *loop_thread() {
printf(" \r\n");
printf("%s \r\n",outputs);

// printf("\033[27A"); // move cursor back up

// send some data
if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) {
ai2vcu_data.AI2VCU_HANDSHAKE_SEND_BIT = HANDSHAKE_SEND_BIT_OFF;
Expand All @@ -167,7 +165,7 @@ static void *loop_thread() {

fs_ai_api_ai2vcu_set_data(&ai2vcu_data);

// repeat roughly every 5ms
// repeat according to loop timing (default roughly every 10ms)
usleep(timing_us);
}

Expand Down
10 changes: 5 additions & 5 deletions FS-AI_API_Tester/fs-ai_api_tester.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,8 @@ int main(int argc, char** argv) {
printf("VCU2AI_FR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_FR_PULSE_COUNT);
printf("VCU2AI_RL_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RL_PULSE_COUNT);
printf("VCU2AI_RR_PULSE_COUNT %5u \r\n",vcu2ai_data.VCU2AI_RR_PULSE_COUNT);
// printf("\033[21A"); // move cursor back up

// TODO: add GPS & IMU data

// send some data
if(HANDSHAKE_RECEIVE_BIT_OFF == vcu2ai_data.VCU2AI_HANDSHAKE_RECEIVE_BIT) {
Expand All @@ -90,18 +90,18 @@ int main(int argc, char** argv) {
printf("HANDSHAKE_BIT error\r\n");
}

ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED;
ai2vcu_data.AI2VCU_DIRECTION_REQUEST = DIRECTION_FORWARD;
ai2vcu_data.AI2VCU_ESTOP_REQUEST = ESTOP_NO;
ai2vcu_data.AI2VCU_MISSION_STATUS = MISSION_SELECTED;
ai2vcu_data.AI2VCU_STEER_ANGLE_REQUEST_deg = 9.9f;
ai2vcu_data.AI2VCU_AXLE_SPEED_REQUEST_rpm = 999.0f;
ai2vcu_data.AI2VCU_AXLE_TORQUE_REQUEST_Nm = 99.0f;
ai2vcu_data.AI2VCU_BRAKE_PRESS_REQUEST_pct = 99.0f;

fs_ai_api_ai2vcu_set_data(&ai2vcu_data);

// repeat roughly every 5ms
usleep(5000);
// repeat roughly every 10ms
usleep(10000);
}

return(0);
Expand Down

0 comments on commit b2f9082

Please sign in to comment.