Finished motor control implementations

README.md

@@ -174,61 +174,7 @@ To undo the going back:
 - `git switch -` <-- goes back to latest commit
 
 ## CppCheck Warnings
-
-```
-src/frc846/cpp/frc846/other/trajectory_generator.cc:68:18: warning: Consider using std::transform algorithm instead of a raw loop. [useStlAlgorithm]
-src/y2024/cpp/commands/teleop/drive_command.cc:67:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
-src/frc846/cpp/frc846/util/math.cc:19:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-src/frc846/cpp/frc846/util/math.cc:46:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```
-
-## CppCheck Warnings
-
-```
-srcfrc846cppfrc846ntinffstore.cc:287:27: warning: Consider using std::any_of algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppcommandsteleopdrive_command.cc:56:8: warning: Condition 'is_robot_centric' is always false [knownConditionTrueFalse]
-srcy2025cppfield.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
-srcy2025cppfield.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
-srcy2025cppfield.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcfrc846cppfrc846mathcollection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
-```
-
-## CppCheck Warnings
-
-```
-srcfrc846cppfrc846controlsimulationMCSimulator.cc:16:12: warning: The scope of the variable 'duty_cycle' can be reduced. [variableScope]
-srcfrc846cppfrc846controlsimulationMCSimulator.cc:21:14: warning: Local variable 'duty_cycle' shadows outer variable [shadowVariable]
-srcfrc846cppfrc846controlsimulationMCSimulator.cc:16:23: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
-srcfrc846cppfrc846controlsimulationMCSimulator.cc:18:18: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
-srcfrc846cppfrc846controlsimulationMCSimulator.cc:21:25: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
-srcy2025cppfield.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
-srcy2025cppfield.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
-srcy2025cppfield.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:22:29: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:67:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:77:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcy2025cppfield.cc:60:10: warning: Consider using std::replace_if algorithm instead of a raw loop. [useStlAlgorithm]
-srcfrc846cppfrc846mathcollection.cc:7:0: warning: The function 'DEquals' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
-srcfrc846cppfrc846mathcollection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
-```
-
-## CppCheck Warnings
-
-```
-src/frc846/cpp/frc846/control/simulation/MCSimulator.cc:16:12: warning: The scope of the variable 'duty_cycle' can be reduced. [variableScope]
-src/frc846/cpp/frc846/control/simulation/MCSimulator.cc:21:14: warning: Local variable 'duty_cycle' shadows outer variable [shadowVariable]
-src/frc846/cpp/frc846/control/simulation/MCSimulator.cc:16:23: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
-src/frc846/cpp/frc846/control/simulation/MCSimulator.cc:18:18: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
-src/frc846/cpp/frc846/control/simulation/MCSimulator.cc:21:25: warning: Variable 'duty_cycle' is assigned a value that is never used. [unreadVariable]
 src/y2025/cpp/field.cc:9:16: warning: Variable 'point' can be declared as reference to const [constVariableReference]
 src/y2025/cpp/field.cc:21:14: warning: Variable 'path' can be declared as reference to const [constVariableReference]
 src/y2025/cpp/field.cc:10:32: warning: Consider using std::find_if algorithm instead of a raw loop. [useStlAlgorithm]
@@ -241,4 +187,4 @@ src/frc846/cpp/frc846/math/collection.cc:11:0: warning: The function 'Horizontal
 src/frc846/cpp/frc846/math/collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
 src/frc846/cpp/frc846/math/collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
 src/frc846/cpp/frc846/math/collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
-```
+```

src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc

@@ -36,22 +36,37 @@ void TalonFX_interm::SetNeutralMode(bool brake_mode) {
 }
 void TalonFX_interm::SetCurrentLimit(units::ampere_t current_limit,
                                      units::second_t threshold_time) {
-  // TODO: implement
+  ctre::phoenix6::configs::CurrentLimitsConfigs configs{};
+  configs.WithSupplyCurrentLimitEnable(true);
+  configs.WithSupplyCurrentLimit(current_limit.to<double>());
+  configs.WithSupplyTimeThreshold(threshold_time.to<double>());
+  last_error_ = getErrorCode(talon_.GetConfigurator().Apply(configs));
 }
 
 void TalonFX_interm::SetSoftLimits(units::radian_t forward_limit,
                                    units::radian_t reverse_limit) {
-  // TODO: implement
+  ctre::phoenix6::configs::SoftwareLimitSwitchConfigs configs{};
+  configs.WithForwardSoftLimitEnable(true);
+  configs.WithForwardSoftLimitThreshold(forward_limit.to<double>());
+  configs.WithReverseSoftLimitEnable(true);
+  configs.WithReverseSoftLimitThreshold(reverse_limit.to<double>());
+  last_error_ = getErrorCode(talon_.GetConfigurator().Apply(configs));
 }
 
 void TalonFX_interm::SetVoltageCompensation(
     units::volt_t voltage_compensation) {
-  // TODO: implement
+  ctre::phoenix6::configs::VoltageConfigs configs{};
+  configs.WithPeakForwardVoltage(voltage_compensation.to<double>());
+  configs.WithPeakReverseVoltage(-voltage_compensation.to<double>());
+  last_error_ = getErrorCode(talon_.GetConfigurator().Apply(configs));
 }
 
 void TalonFX_interm::SetGains(frc846::control::base::MotorGains gains) {
   gains_ = gains;
-  // TODO: implement
+  ctre::phoenix6::configs::Slot0Configs configs{};
+  configs.WithKP(gains_.kP).WithKI(gains_.kI).WithKD(gains_.kD).WithKS(
+      gains_.kFF);
+  last_error_ = getErrorCode(talon_.GetConfigurator().Apply(configs));
 }
 
 void TalonFX_interm::WriteDC(double duty_cycle) { last_command_ = duty_cycle; }