Move motor print debugging code to the application
This gets rid of one of the macros in motor.cc
Change-Id: I77f4ba6574f28ab6c3b744d7f93ab3f9e236bdb9
diff --git a/motors/motor.cc b/motors/motor.cc
index 1b9f2e5..c8bc81e 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -105,7 +105,6 @@
#define DO_CONTROLS 1
#define USE_CUTOFF 1
-#define PRINT_READINGS 0
#define PRINT_ALL_READINGS 0
#define TAKE_SAMPLE 0
#define SAMPLE_UNTIL_DONE 0
@@ -247,49 +246,14 @@
#endif // DO_CONTROLS
(void)balanced;
(void)captured_wrapped_encoder;
-#if PRINT_READINGS
- static int i = 0;
- if (i == 1000) {
- i = 0;
- //SmallInitReadings readings;
- //{
- //DisableInterrupts disable_interrupts;
- //readings = AdcReadSmallInit(disable_interrupts);
- //}
- //printf(
- //"enc %" PRIu32 " %d %d\n", captured_wrapped_encoder,
- //static_cast<int>((1.0f - analog_ratio(readings.motor1_abs)) * 7000.0f),
- //static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
- //float wheel_position = absolute_wheel(analog_ratio(readings.wheel_abs));
-
- printf(
- "ecnt %" PRIu32
- //" arev:%d"
- " erev:%d"
- //" %d"
- "\n", captured_wrapped_encoder,
- //static_cast<int>((analog_ratio(readings.motor1_abs)) * 7000.0f),
- static_cast<int>(captured_wrapped_encoder / 4096.0f * 1000.0f)
- //static_cast<int>(wheel_position * 1000.0f)
- );
- } else if (i == 200) {
-#if DO_CONTROLS
- printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", switching_points[0],
- switching_points[1], switching_points[2]);
-#else // DO_CONTROLS
- printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", output_registers_[0][2],
- output_registers_[1][2], output_registers_[2][2]);
-#endif // DO_CONTROLS
- }
- ++i;
-#elif PRINT_ALL_READINGS // PRINT_READINGS
+#if PRINT_ALL_READINGS
printf("ref=%" PRIu16 " 0.0=%" PRIu16 " 1.0=%" PRIu16 " 2.0=%" PRIu16
" in=%" PRIu16 " 0.1=%" PRIu16 " 1.1=%" PRIu16 " 2.1=%" PRIu16 "\n",
adc_readings.motor_current_ref, adc_readings.motor_currents[0][0],
adc_readings.motor_currents[1][0], adc_readings.motor_currents[2][0],
adc_readings.input_voltage, adc_readings.motor_currents[0][1],
adc_readings.motor_currents[1][1], adc_readings.motor_currents[2][1]);
-#elif TAKE_SAMPLE // PRINT_READINGS/PRINT_ALL_READINGS
+#elif TAKE_SAMPLE // PRINT_ALL_READINGS
#if 0
constexpr int kStartupWait = 50000;
#elif 0
@@ -435,7 +399,7 @@
}
#endif
}
-#endif // PRINT_READINGS/PRINT_ALL_READINGS/TAKE_SAMPLE
+#endif // PRINT_ALL_READINGS/TAKE_SAMPLE
(void)balanced;
// Tell the hardware to use the new switching points.
diff --git a/motors/motor.h b/motors/motor.h
index 7721f1a..03160ce 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -167,6 +167,10 @@
return controls_->overall_measured_current();
}
+ ::std::array<volatile uint32_t *, 3> output_registers() const {
+ return output_registers_;
+ }
+
private:
uint32_t CalculateOnTime(uint32_t width) const;
uint32_t CalculateOffTime(uint32_t width) const;
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 5a52afb..a951c6a 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -272,6 +272,39 @@
->CurrentInterrupt(BalanceSimpleReadings(readings.currents), encoder);
global_wheel_angle.store(angle);
+
+ /*
+ SmallInitReadings position_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ position_readings = AdcReadSmallInit(disable_interrupts);
+ }
+
+ static int i = 0;
+ if (i == 1000) {
+ i = 0;
+ float wheel_position =
+ absolute_wheel(analog_ratio(position_readings.wheel_abs));
+ printf(
+ "ecnt %" PRIu32 " arev:%d erev:%d abs:%d awp:%d uncalwheel:%d\n",
+ encoder,
+ static_cast<int>((1.0f - analog_ratio(position_readings.motor1_abs)) *
+ 7000.0f),
+ static_cast<int>(encoder * 7.0f / 4096.0f * 1000.0f),
+ static_cast<int>(absolute_encoder),
+ static_cast<int>(wheel_position * 1000.0f),
+ static_cast<int>(analog_ratio(position_readings.wheel_abs) * 1000.0f));
+ } else if (i == 200) {
+ printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n",
+ global_motor1.load(::std::memory_order_relaxed)
+ ->output_registers()[0][2],
+ global_motor1.load(::std::memory_order_relaxed)
+ ->output_registers()[1][2],
+ global_motor1.load(::std::memory_order_relaxed)
+ ->output_registers()[2][2]);
+ }
+ ++i;
+ */
}
constexpr float kTriggerMaxExtension = -0.70f;
@@ -328,6 +361,33 @@
->CurrentInterrupt(BalanceSimpleReadings(readings.currents), encoder);
global_trigger_angle.store(trigger_angle);
+
+ /*
+ SmallInitReadings position_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ position_readings = AdcReadSmallInit(disable_interrupts);
+ }
+
+ static int i = 0;
+ if (i == 1000) {
+ i = 0;
+ printf("ecnt %" PRIu32 " arev:%d erev:%d abs:%d\n", encoder,
+ static_cast<int>((analog_ratio(position_readings.motor0_abs)) *
+ 7000.0f),
+ static_cast<int>(encoder * 7.0f / 4096.0f * 1000.0f),
+ static_cast<int>(absolute_encoder));
+ } else if (i == 200) {
+ printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n",
+ global_motor0.load(::std::memory_order_relaxed)
+ ->output_registers()[0][2],
+ global_motor0.load(::std::memory_order_relaxed)
+ ->output_registers()[1][2],
+ global_motor0.load(::std::memory_order_relaxed)
+ ->output_registers()[2][2]);
+ }
+ ++i;
+ */
}
int ConvertFloat16(float val) {