Skip to content

Commit

Permalink
last flashed 5/9/24 on car
Browse files Browse the repository at this point in the history
  • Loading branch information
@Luke-kC
Luke-kC committed May 9, 2024
1 parent 8b099f3 commit a7913ab
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 31 deletions.
6 changes: 3 additions & 3 deletions lib/systems/include/CASESystem.tpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,13 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
in.TorqueAverageNm = calculate_torque_request(pedals_data, config_.max_regen_torque, config_.max_rpm);

in.YawRaterads = vn_data.angular_rates.z;
// in.YawRaterads = 2.5; // THIS IS FAKE TODO HACK

// REAL
// in.Vx_B = vn_data.velocity_x;
in.Vx_B = vn_data.velocity_x;

// FAKE
in.Vx_B = 5;
// in.Vx_B = 5;

in.FZFL = load_cell_vals.FL.conversion;
in.FZFR = load_cell_vals.FR.conversion;
Expand Down Expand Up @@ -161,7 +162,6 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid_ya);
enqueue_matlab_msg(msg_queue_, res.controllerBus_controller_pid__p);


last_controller_pt1_send_time_ = tick.millis;
}

Expand Down
39 changes: 19 additions & 20 deletions lib/systems/src/TorqueControllerMux.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ void TorqueControllerMux::tick(
// Check if targeted controller is ready to be selected
bool controllerNotReadyPreventsModeChange = (controllerOutputs_[static_cast<int>(dialModeMap_[dashboardDialMode])].ready == false);

if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange))
{
muxMode_ = dialModeMap_[dashboardDialMode];
cur_dial_mode_ = dashboardDialMode;
}
// if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange))
// {
muxMode_ = dialModeMap_[dashboardDialMode];
cur_dial_mode_ = dashboardDialMode;
// }
}

// Check if the current controller is ready. If it has faulted, revert to safe mode
Expand All @@ -89,9 +89,9 @@ void TorqueControllerMux::tick(
drivetrainCommand_ = controllerOutputs_[static_cast<int>(muxMode_)].command;

// apply torque limit before power limit to not power limit
applyRegenLimit(&drivetrainCommand_, &drivetrainData);
applyTorqueLimit(&drivetrainCommand_);
applyPowerLimit(&drivetrainCommand_, &drivetrainData);
// applyRegenLimit(&drivetrainCommand_, &drivetrainData);
// applyTorqueLimit(&drivetrainCommand_);
// applyPowerLimit(&drivetrainCommand_, &drivetrainData);
applyPosSpeedLimit(&drivetrainCommand_);
}
}
Expand All @@ -110,13 +110,13 @@ void TorqueControllerMux::applyRegenLimit(DrivetrainCommand_s *command, const Dr

for (int i = 0; i < NUM_MOTORS; i++)
{
#ifdef ARDUINO_TEENSY41
#ifdef ARDUINO_TEENSY41
maxWheelSpeed = std::max(maxWheelSpeed, abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR);
allWheelsRegen &= (command->speeds_rpm[i] < abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0);
#else
#else
maxWheelSpeed = std::max(maxWheelSpeed, std::abs(drivetrain->measuredSpeeds[i]) * RPM_TO_KILOMETERS_PER_HOUR);
allWheelsRegen &= (command->speeds_rpm[i] < std::abs(drivetrain->measuredSpeeds[i]) || command->speeds_rpm[i] == 0);
#endif
#endif
}

// begin limiting regen at noRegenLimitKPH and completely limit regen at fullRegenLimitKPH
Expand Down Expand Up @@ -145,11 +145,11 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const Dr
// calculate current mechanical power
for (int i = 0; i < NUM_MOTORS; i++)
{
// get the total magnitude of torque from all 4 wheels
#ifdef ARDUINO_TEENSY41 // screw arduino.h macros
// get the total magnitude of torque from all 4 wheels
#ifdef ARDUINO_TEENSY41 // screw arduino.h macros
net_torque_mag += abs(command->torqueSetpoints[i]);
net_power += abs(command->torqueSetpoints[i] * (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND));
#else
#else
// sum up net torque
net_torque_mag += std::abs(command->torqueSetpoints[i]);
// calculate P = T*w for each wheel and sum together
Expand All @@ -171,15 +171,14 @@ void TorqueControllerMux::applyPowerLimit(DrivetrainCommand_s *command, const Dr
// based on the torque percent and max power limit, get the max power each wheel can use
float power_per_corner = (torque_percent * MAX_POWER_LIMIT);

// power / omega (motor rad/s) to get torque per wheel
#ifdef ARDUINO_TEENSY41
// power / omega (motor rad/s) to get torque per wheel
#ifdef ARDUINO_TEENSY41
command->torqueSetpoints[i] = abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND));
#else
#else
command->torqueSetpoints[i] = std::abs(power_per_corner / (drivetrain->measuredSpeeds[i] * RPM_TO_RAD_PER_SECOND));
#endif
#endif
command->torqueSetpoints[i] = std::max(0.0f, std::min(command->torqueSetpoints[i], getMaxTorque()));

}
}
}
}

Expand Down
16 changes: 8 additions & 8 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,13 +155,15 @@ TorqueControllerMux torque_controller_mux(1.0, 0.4);
CASEConfiguration case_config = {
// Following used for generated code
.AbsoluteTorqueLimit = 21.42, // N-m, Torque limit used for yaw pid torque split overflow
.yaw_pid_p = 1.369,
.yaw_pid_p = 1.5,
.yaw_pid_i = 0.25,
// .yaw_pid_p = 0.5, // SKIDPAD Testing
// .yaw_pid_i = 0.0, // SKIDPAD Testing
.yaw_pid_d = 0.0,
.tcs_pid_p_lowerBound_front = 35.0, // if tcs_pid_p_lowerBound_front > tcs_pid_p_upperBound_front, inverse relationship, no error
.tcs_pid_p_upperBound_front = 45.0,
.tcs_pid_p_lowerBound_rear = 28.0,
.tcs_pid_p_upperBound_rear = 35.0,
.tcs_pid_p_lowerBound_rear = 32.0,
.tcs_pid_p_upperBound_rear = 45.0,
.tcs_pid_i = 0.0,
.tcs_pid_d = 0.0,
.useLaunch = false,
Expand All @@ -177,10 +179,10 @@ CASEConfiguration case_config = {
.launchSL = 0.3,
.launchDeadZone = 20.0, // N-m
.launchVelThreshold = 0.15, // m/s
.tcsVelThreshold = 2.5, // m/s
.tcsVelThreshold = 0.15, // m/s
.yawPIDMaxDifferential = 10.0, // N-m
.yawPIDErrorThreshold = 0.1, // rad/s
.yawPIDVelThreshold = 1.0, // m/s
.yawPIDVelThreshold = 0.35, // m/s
.yawPIDCoastThreshold = 2.5, // m/s
.yaw_pid_brakes_p = 0.25,
.yaw_pid_brakes_i = 0.0,
Expand All @@ -199,7 +201,7 @@ CASEConfiguration case_config = {
.tcs_pid_upper_rpm_front = 5000.0, // RPM
.tcs_pid_lower_rpm_rear = 0.0, // RPM
.tcs_pid_upper_rpm_rear = 5000.0, // RPM
.maxNormalLoadBrakeScalingFront = 1.95,
.maxNormalLoadBrakeScalingFront = 1.25,

// Following used for calculate_torque_request in CASESystem.tpp
.max_rpm = 20000,
Expand Down Expand Up @@ -502,8 +504,6 @@ void tick_all_systems(const SysTick_s &current_system_tick)
dashboard.torqueModeButtonPressed(),
vn_interface.get_vn_struct(),
controller_output);


}

void handle_ethernet_interface_comms()
Expand Down

0 comments on commit a7913ab

Please sign in to comment.