motors/motor.h

#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/peripheral/configuration.h"
#include "motors/util.h"
#include "motors/usb/cdc.h"
#include "motors/core/time.h"

namespace frc971 {
namespace motors {

class MotorControls {
 public:
  MotorControls() = default;
  virtual ~MotorControls() = default;

  MotorControls(const MotorControls &) = delete;
  void operator=(const MotorControls &) = delete;

  // Scales a current reading from ADC units to amps.
  //
  // Note that this doesn't apply any offset. The common offset will be
  // automatically removed as part of the balancing process.
  virtual float scale_current_reading(float reading) const = 0;

  virtual int mechanical_counts_per_revolution() const = 0;
  virtual int electrical_counts_per_revolution() const = 0;

  // 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 final {
 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,
        const ::std::array<volatile uint32_t *, 3> &output_registers);

  Motor(const Motor &) = delete;
  void operator=(const Motor &) = delete;

  void set_debug_tty(teensy::AcmTty *debug_tty) { debug_tty_ = debug_tty; }
  void set_deadtime_compensation(int deadtime_compensation) {
    deadtime_compensation_ = deadtime_compensation;
  }
  void set_switching_divisor(int switching_divisor) {
    switching_divisor_ = switching_divisor;
  }
  void set_encoder_offset(int32_t encoder_offset) {
    encoder_offset_ = encoder_offset;
    last_wrapped_encoder_reading_ = wrapped_encoder();
  }
  int32_t encoder_offset() const { return encoder_offset_; }

  void set_encoder_calibration_offset(int encoder_offset) {
    encoder_calibration_offset_ = encoder_offset;
    // Add mechanical_counts_per_revolution to the offset so that when we mod
    // below, we are guaranteed to be > 0 regardless of the encoder multiplier.
    // % isn't well-defined with negative numbers.
    while (encoder_calibration_offset_ <
           controls_->mechanical_counts_per_revolution()) {
      encoder_calibration_offset_ +=
          controls_->mechanical_counts_per_revolution();
    }
  }
  void set_encoder_multiplier(int encoder_multiplier) {
    encoder_multiplier_ = encoder_multiplier;
  }

  int32_t absolute_encoder(uint32_t wrapped_encoder_reading) {
    const uint32_t counts_per_revolution =
        controls_->mechanical_counts_per_revolution();
    const uint32_t wrap_down = counts_per_revolution / 4;
    const uint32_t wrap_up = wrap_down * 3;
    if (last_wrapped_encoder_reading_ > wrap_up &&
        wrapped_encoder_reading < wrap_down) {
      encoder_offset_ += counts_per_revolution;
    } else if (last_wrapped_encoder_reading_ < wrap_down &&
               wrapped_encoder_reading > wrap_up) {
      encoder_offset_ -= counts_per_revolution;
    }

    last_wrapped_encoder_reading_ = wrapped_encoder_reading;

    return static_cast<int32_t>(wrapped_encoder_reading) + encoder_offset_;
  }

  int encoder() {
    return encoder_multiplier_ * encoder_ftm_->CNT;
  }
  uint32_t wrapped_encoder() {
    return (encoder() + encoder_calibration_offset_) %
           controls_->mechanical_counts_per_revolution();
  }

  // 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.
  void Init();

  // Starts the timers.
  //
  // If the global time base is in use, it must be activated after this.
  void Start();

  void HandleInterrupt(const BalancedReadings &readings,
                       uint32_t captured_wrapped_encoder);

  void SetGoalCurrent(float goal_current) {
    DisableInterrupts disable_interrupts;
    goal_current_ = goal_current;
    last_current_set_time_ = micros();
  }

 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;
  };

  inline int counts_per_cycle() const {
    return BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY / switching_divisor_;
  }

  uint32_t CalculateOnTime(uint32_t width) const;
  uint32_t CalculateOffTime(uint32_t width) const;

  bool flip_time_offset_ = false;
  int deadtime_compensation_ = 0;

  BigFTM *const pwm_ftm_;
  LittleFTM *const encoder_ftm_;
  MotorControls *const controls_;
  const ::std::array<volatile uint32_t *, 3> output_registers_;

  float goal_current_ = 0;
  uint32_t last_current_set_time_ = 0;
  int switching_divisor_ = 1;
  int encoder_calibration_offset_ = 0;
  int32_t encoder_offset_ = 0;
  int encoder_multiplier_ = 1;
  uint32_t last_wrapped_encoder_reading_ = 0;

  teensy::AcmTty *debug_tty_ = nullptr;
};

}  // namespace motors
}  // namespace frc971

#endif  // MOTORS_MOTOR_H_