Use cogging constants per pistol grip
Change-Id: Ia48a981e43e7a80cdac913048d8eb090231597d1
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 52bba6d..b9e6683 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -21,13 +21,26 @@
#define MOTOR1_PWM_FTM FTM0
#define MOTOR1_ENCODER_FTM FTM1
-extern const float kWheelCoggingTorque[4096];
-extern const float kTriggerCoggingTorque[4096];
+extern const float kWheelCoggingTorque0[4096];
+extern const float kWheelCoggingTorque1[4096];
+extern const float kTriggerCoggingTorque0[4096];
+extern const float kTriggerCoggingTorque1[4096];
namespace frc971 {
namespace motors {
namespace {
+::std::atomic<const float *> trigger_cogging_torque{nullptr};
+::std::atomic<const float *> wheel_cogging_torque{nullptr};
+
+float TriggerCoggingTorque(uint32_t index) {
+ return trigger_cogging_torque.load(::std::memory_order_relaxed)[index];
+}
+
+float WheelCoggingTorque(uint32_t index) {
+ return wheel_cogging_torque.load(::std::memory_order_relaxed)[index];
+}
+
using ::frc971::control_loops::drivetrain::MakeIntegralHapticTriggerPlant;
using ::frc971::control_loops::drivetrain::MakeIntegralHapticTriggerObserver;
using ::frc971::control_loops::drivetrain::MakeIntegralHapticWheelPlant;
@@ -314,7 +327,7 @@
(void)CoggingCurrent1;
float goal_current = global_wheel_current.load(::std::memory_order_relaxed) +
- kWheelCoggingTorque[encoder];
+ WheelCoggingTorque(encoder);
//float goal_current = CoggingCurrent1(encoder, absolute_encoder);
//float goal_current = kWheelCoggingTorque[encoder];
//float goal_current = 0.0f;
@@ -451,7 +464,7 @@
(void)CoggingCurrent0;
const float goal_current =
global_trigger_torque.load(::std::memory_order_relaxed) +
- kTriggerCoggingTorque[encoder];
+ TriggerCoggingTorque(encoder);
//const float goal_current = kTriggerCoggingTorque[encoder];
//const float goal_current = 0.0f;
//const float goal_current = CoggingCurrent0(encoder, absolute_encoder);
@@ -1000,6 +1013,11 @@
printf("heap start: %p\n", __heap_start__);
printf("stack start: %p\n", __stack_end__);
+ trigger_cogging_torque.store(ProcessorIndex() == 0 ? kTriggerCoggingTorque0
+ : kTriggerCoggingTorque1);
+ wheel_cogging_torque.store(ProcessorIndex() == 0 ? kWheelCoggingTorque0
+ : kWheelCoggingTorque1);
+
printf("Zeroing motors for %d:%x\n", static_cast<int>(ProcessorIndex()),
(unsigned int)ProcessorIdentifier());
uint16_t motor0_offset, motor1_offset, wheel_offset;