Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/motor.h b/motors/motor.h
new file mode 100644
index 0000000..f5b731f
--- /dev/null
+++ b/motors/motor.h
@@ -0,0 +1,111 @@
+#ifndef MOTORS_MOTOR_H_
+#define MOTORS_MOTOR_H_
+
+#include <limits.h>
+
+#include <array>
+
+#include "motors/algorithms.h"
+#include "motors/core/kinetis.h"
+#include "motors/peripheral/adc.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace salsa {
+
+class MotorControls {
+ public:
+ MotorControls() = default;
+ virtual ~MotorControls() = default;
+
+ MotorControls(const MotorControls &) = delete;
+ void operator=(const MotorControls &) = delete;
+
+ static constexpr float scale_current_reading(float reading) {
+ return reading *
+ static_cast<float>(1.0 / 4096.0 /* Full-scale ADC reading */ *
+ 3.3 /* ADC reference voltage */ /
+ (1.47 / (0.768 + 1.47)) /* 5V -> 3.3V divider */ /
+ 50.0 /* Current sense amplification */ /
+ 0.0003 /* Sense resistor */);
+ }
+
+ static constexpr int counts_per_revolution() { return 2048 / 2; }
+
+ // raw_currents are in amps for each phase.
+ // theta is in electrical counts, which will be less than
+ // counts_per_revolution().
+ virtual ::std::array<uint32_t, 3> DoIteration(
+ const float raw_currents[3], uint32_t theta,
+ const float command_current) = 0;
+
+ virtual int16_t Debug(uint32_t theta) = 0;
+
+ virtual float estimated_velocity() const = 0;
+};
+
+// Controls a single motor.
+class Motor {
+ public:
+ // pwm_ftm is used to drive the PWM outputs.
+ // encoder_ftm is used for reading the encoder.
+ Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls);
+
+ Motor(const Motor &) = delete;
+ void operator=(const Motor &) = delete;
+
+ // Sets up everything but doesn't actually start the timers.
+ //
+ // This assumes the global time base configuration happens outside so the
+ // timers for both motors (if applicable) are synced up.
+ // TODO(Brian): "40.4.28.1 Enabling the global time base (GTB)" says something
+ // about needing to do stuff in a specific order, so this API might need
+ // revising.
+ void Init();
+
+ // Zeros the encoder. This involves blocking for an arbitrary length of time
+ // with interrupts disabled.
+ void Zero();
+
+ // Starts the timers.
+ void Start();
+
+ void HandleInterrupt();
+
+ private:
+ // Represents the ADC reading which is closest to an edge.
+ struct CloseAdcReading {
+ // Adds a new reading to the readings to balance or pushes the previous
+ // closest one there and saves off this one.
+ //
+ // Returns true if it saves off the new reading.
+ bool MaybeUse(int new_distance, const MediumAdcReadings &adc_readings,
+ int phase, int sample, ReadingsToBalance *to_balance) {
+ if (new_distance < distance) {
+ if (distance != INT_MAX) {
+ to_balance->Add(index, value);
+ }
+ distance = new_distance;
+ value = adc_readings.motor_currents[phase][sample];
+ index = phase;
+ return true;
+ }
+ return false;
+ }
+
+ int distance = INT_MAX;
+ int32_t value = 0;
+ int index = 0;
+ };
+
+ bool flip_time_offset_ = false;
+
+ BigFTM *const pwm_ftm_;
+ LittleFTM *const encoder_ftm_;
+ MotorControls *const controls_;
+};
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_MOTOR_H_