Commit currently running fet12v2 code
Works on the real power-wheels cart. Can't say too much more about it;
leaving more substantial clean-up for later.
Change-Id: I1b06b04f5cc52240c028b48828d76873e86c26c1
diff --git a/motors/algorithms.h b/motors/algorithms.h
index 63d6d19..c67383b 100644
--- a/motors/algorithms.h
+++ b/motors/algorithms.h
@@ -2,6 +2,7 @@
#define MOTORS_ALGORITHMS_H_
#include <stdint.h>
+#include <array>
namespace frc971 {
namespace motors {
@@ -26,6 +27,22 @@
// and the corresponding outputs will be inversely proportional to the weights.
BalancedReadings BalanceReadings(ReadingsToBalance to_balance);
+inline BalancedReadings BalanceSimpleReadings(
+ const ::std::array<float, 3> readings) {
+ float offset = 0;
+ for (int i = 0; i < 3; ++i) {
+ offset += readings[i];
+ }
+
+ offset = offset / 3.0f;
+
+ BalancedReadings r;
+ for (int i = 0; i < 3; ++i) {
+ r.readings[i] = static_cast<float>(readings[i]) - offset;
+ }
+ return r;
+}
+
inline BalancedReadings BalanceSimpleReadings(const uint16_t readings[3]) {
float offset = 0;
for (int i = 0; i < 3; ++i) {
diff --git a/motors/big/motor_controls.cc b/motors/big/motor_controls.cc
index 82dcc0b..a26ae83 100644
--- a/motors/big/motor_controls.cc
+++ b/motors/big/motor_controls.cc
@@ -125,7 +125,7 @@
MakeFluxLinkageTable();
}
-::std::array<uint32_t, 3> MotorControlsImplementation::DoIteration(
+::std::array<float, 3> MotorControlsImplementation::DoIteration(
const float raw_currents[3], const uint32_t theta_in,
const float command_current) {
static constexpr float kCurrentSlewRate = 0.05f;
@@ -230,9 +230,8 @@
I_last_ = I_next;
// TODO(Austin): Figure out why we need the min here.
- return {static_cast<uint32_t>(::std::max(0.0f, times(0)) * 3000.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(1)) * 3000.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(2)) * 3000.0f)};
+ return {::std::max(0.0f, times(0)), ::std::max(0.0f, times(1)),
+ ::std::max(0.0f, times(2))};
}
int16_t MotorControlsImplementation::Debug(uint32_t theta) {
diff --git a/motors/big/motor_controls.h b/motors/big/motor_controls.h
index b9514fd..aae97d0 100644
--- a/motors/big/motor_controls.h
+++ b/motors/big/motor_controls.h
@@ -20,6 +20,8 @@
MotorControlsImplementation();
~MotorControlsImplementation() override = default;
+ void Reset() override {}
+
static constexpr int constant_counts_per_revolution() { return 1024; }
int mechanical_counts_per_revolution() const override {
@@ -37,7 +39,7 @@
0.0003 /* Sense resistor */);
}
- ::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
+ ::std::array<float, 3> DoIteration(const float raw_currents[3],
uint32_t theta,
const float command_current) override;
@@ -45,6 +47,10 @@
float estimated_velocity() const override { return estimated_velocity_; }
+ int16_t i_goal(size_t ii) const override {
+ return static_cast<int16_t>(I_last_[ii] * 10.0f);
+ }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_, E2Unrotated_;
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index 3787e24..50496d5 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -4,6 +4,7 @@
cc_binary(
name = "fet12.elf",
srcs = [
+ "current_equalization.h",
"fet12.cc",
],
restricted_to = mcu_cpus,
@@ -19,6 +20,24 @@
],
)
+cc_binary(
+ name = "fet12v2.elf",
+ srcs = [
+ "current_equalization.h",
+ "fet12v2.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":motor_controls",
+ "//motors:motor",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/peripheral:uart",
+ ],
+)
+
hex_from_elf(
name = "fet12",
restricted_to = mcu_cpus,
@@ -60,3 +79,11 @@
"//third_party/eigen",
],
)
+
+genrule(
+ name = "gen_current_equalization",
+ outs = ["current_equalization.h"],
+ cmd = "./$(location current_equalize.py) > \"$@\"",
+ restricted_to = mcu_cpus,
+ tools = ["current_equalize.py"],
+)
diff --git a/motors/fet12/current_equalize.py b/motors/fet12/current_equalize.py
new file mode 100755
index 0000000..698983f
--- /dev/null
+++ b/motors/fet12/current_equalize.py
@@ -0,0 +1,50 @@
+#!/usr/bin/python3
+
+import numpy
+import sys
+
+def main():
+ # 38 27 -84
+ # 36 -64 39
+ # -74 21 35
+ Is0 = numpy.matrix([[38.0, 27.0, -84.0]]).T
+ Is1 = numpy.matrix([[36.0, -64.0, 39.0]]).T
+ Is2 = numpy.matrix([[-74.0, 21.0, 35.0]]).T
+ Is = numpy.matrix(numpy.hstack((Is0, Is1, Is2)))
+
+ current = 46.0
+ I = numpy.matrix([[current, -current / 2.0, -current / 2.0],
+ [-current / 2.0, current, -current / 2.0],
+ [-current / 2.0, -current / 2.0, current]])
+ transform = I * numpy.linalg.inv(Is)
+
+ print("#ifndef MOTORS_FET12_CURRENT_MATRIX_")
+ print("#define MOTORS_FET12_CURRENT_MATRIX_")
+ print("")
+ print("#include <array>")
+ print("")
+ print("namespace frc971 {")
+ print("namespace motors {")
+ print("")
+ print(
+ "inline ::std::array<float, 3> DecoupleCurrents(int16_t currents[3]) {")
+ print(" ::std::array<float, 3> ans;")
+
+ for i in range(3):
+ print(" ans[%d] = %ff * static_cast<float>(currents[0]) +" %
+ (i, transform[i, 0]))
+ print(" %ff * static_cast<float>(currents[1]) +" %
+ transform[i, 1])
+ print(" %ff * static_cast<float>(currents[2]);" % transform[i, 2])
+
+ print(" return ans;")
+ print("}")
+ print("")
+ print("} // namespace motors")
+ print("} // namespace frc971")
+ print("#endif // MOTORS_FET12_CURRENT_MATRIX_")
+
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main())
diff --git a/motors/fet12/fet12v2.cc b/motors/fet12/fet12v2.cc
new file mode 100644
index 0000000..01a6336
--- /dev/null
+++ b/motors/fet12/fet12v2.cc
@@ -0,0 +1,590 @@
+#include "motors/core/kinetis.h"
+
+#include <inttypes.h>
+#include <stdio.h>
+
+#include <atomic>
+
+#include "motors/core/time.h"
+#include "motors/fet12/current_equalization.h"
+#include "motors/fet12/motor_controls.h"
+#include "motors/motor.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/peripheral/uart.h"
+#include "motors/util.h"
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 3.6;
+constexpr double kVcc = 31.5;
+constexpr double kIcc = 125.0;
+constexpr double kR = 0.0084;
+
+struct Fet12AdcReadings {
+ int16_t motor_currents[3];
+ int16_t throttle, fuse_voltage;
+};
+
+void AdcInitFet12() {
+ AdcInitCommon(AdcChannels::kB, AdcChannels::kA);
+
+ // M_CH0V ADC0_SE5b
+ PORTD_PCR1 = PORT_PCR_MUX(0);
+
+ // M_CH1V ADC0_SE7b
+ PORTD_PCR6 = PORT_PCR_MUX(0);
+
+ // M_CH2V ADC0_SE14
+ PORTC_PCR0 = PORT_PCR_MUX(0);
+
+ // M_CH0F ADC1_SE5a
+ PORTE_PCR1 = PORT_PCR_MUX(0);
+
+ // M_CH1F ADC1_SE6a
+ PORTE_PCR2 = PORT_PCR_MUX(0);
+
+ // M_CH2F ADC1_SE7a
+ PORTE_PCR3 = PORT_PCR_MUX(0);
+
+ // SENSE0 ADC0_SE23
+ // dedicated
+
+ // SENSE1 ADC0_SE13
+ PORTB_PCR3 = PORT_PCR_MUX(0);
+}
+
+Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
+ Fet12AdcReadings r;
+
+ ADC1_SC1A = 5;
+ ADC0_SC1A = 23;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 6;
+ r.motor_currents[0] = static_cast<int16_t>(ADC1_RA) - 2032;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 13;
+ r.throttle = static_cast<int16_t>(ADC0_RA);
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 7;
+ r.motor_currents[1] = static_cast<int16_t>(ADC1_RA) - 2032;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.fuse_voltage = static_cast<int16_t>(ADC0_RA);
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_currents[2] = static_cast<int16_t>(ADC1_RA) - 2032;
+
+ return r;
+}
+
+::std::atomic<Motor *> global_motor{nullptr};
+::std::atomic<teensy::InterruptBufferedUart *> global_stdout{nullptr};
+
+extern "C" {
+
+void uart0_status_isr(void) {
+ teensy::InterruptBufferedUart *const tty =
+ global_stdout.load(::std::memory_order_relaxed);
+ DisableInterrupts disable_interrupts;
+ tty->HandleInterrupt(disable_interrupts);
+}
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+int _write(int /*file*/, char *ptr, int len) {
+ teensy::InterruptBufferedUart *const tty =
+ global_stdout.load(::std::memory_order_acquire);
+ if (tty != nullptr) {
+ DisableInterrupts disable_interrupts;
+ tty->Write(gsl::make_span(ptr, len), disable_interrupts);
+ return len;
+ }
+ return 0;
+}
+
+void __stack_chk_fail(void);
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+struct DebugBuffer {
+ struct Sample {
+ ::std::array<int16_t, 3> currents;
+ ::std::array<int16_t, 3> commanded_currents;
+ ::std::array<uint16_t, 3> commands;
+ uint16_t position;
+ // Driver requested current.
+ float driver_request;
+ // Requested current.
+ int16_t total_command;
+
+ float est_omega;
+ float fuse_voltage;
+ int16_t fuse_current;
+
+ float fuse_badness;
+ uint32_t cycles_since_start;
+ };
+
+ // The amount of data in the buffer. This will never decrement. This will be
+ // transferred out the serial port after it fills up.
+ ::std::atomic<size_t> size{0};
+ ::std::atomic<uint32_t> count{0};
+ // The data.
+ ::std::array<Sample, 512> samples;
+};
+
+DebugBuffer global_debug_buffer;
+
+void ftm0_isr(void) {
+ const auto wrapped_encoder =
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+ Fet12AdcReadings adc_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ adc_readings = AdcReadFet12(disable_interrupts);
+ }
+ const ::std::array<float, 3> decoupled =
+ DecoupleCurrents(adc_readings.motor_currents);
+
+ const BalancedReadings balanced =
+ BalanceSimpleReadings(decoupled);
+
+ static int i = 0;
+ static float fuse_badness = 0;
+
+ static uint32_t cycles_since_start = 0u;
+ ++cycles_since_start;
+#if 0
+ static int count = 0;
+ ++count;
+ static float currents[3] = {0.0f, 0.0f, 0.0f};
+ for (int ii = 0; ii < 3; ++ii) {
+ currents[ii] += static_cast<float>(adc_readings.motor_currents[ii]);
+ }
+
+ if (i == 0) {
+ printf(
+ "foo %d.0, %d.0, %d.0, %.3d %.3d %.3d, switching %d %d %d enc %d\n",
+ static_cast<int>(currents[0] / static_cast<float>(count)),
+ static_cast<int>(currents[1] / static_cast<float>(count)),
+ static_cast<int>(currents[2] / static_cast<float>(count)),
+ static_cast<int>(decoupled[0] * 1.0f),
+ static_cast<int>(decoupled[1] * 1.0f),
+ static_cast<int>(decoupled[2] * 1.0f),
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(0),
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(1),
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(2),
+ static_cast<int>(
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder()));
+ count = 0;
+ currents[0] = 0.0f;
+ currents[1] = 0.0f;
+ currents[2] = 0.0f;
+ }
+#endif
+#if 1
+ constexpr float kAlpha = 0.995f;
+ constexpr float kFuseAlpha = 0.95f;
+
+ // 3400 - 760
+ static float filtered_throttle = 0.0f;
+ constexpr int kMaxThrottle = 3400;
+ constexpr int kMinThrottle = 760;
+ const float throttle = ::std::max(
+ 0.0f,
+ ::std::min(1.0f,
+ static_cast<float>(static_cast<int>(adc_readings.throttle) -
+ kMinThrottle) /
+ static_cast<float>(kMaxThrottle - kMinThrottle)));
+
+ // y(n) = x(n) + a * (y(n-1) - x(n))
+ filtered_throttle = throttle + kAlpha * (filtered_throttle - throttle);
+
+ const float fuse_voltage = static_cast<float>(adc_readings.fuse_voltage);
+ static float filtered_fuse_voltage = 0.0f;
+
+ filtered_fuse_voltage =
+ fuse_voltage + kFuseAlpha * (filtered_fuse_voltage - fuse_voltage);
+
+ const float velocity =
+ global_motor.load(::std::memory_order_relaxed)->estimated_velocity();
+ const float bemf = velocity / (static_cast<float>(Kv) / 1.5f);
+ const float abs_bemf = ::std::abs(bemf);
+ constexpr float kPeakCurrent = 300.0f;
+ constexpr float kLimitedCurrent = 75.0f;
+ const float max_bat_cur =
+ fuse_badness > (kLimitedCurrent * kLimitedCurrent * 0.95f)
+ ? kLimitedCurrent
+ : static_cast<float>(kIcc);
+ const float throttle_limit = ::std::min(
+ kPeakCurrent,
+ (-abs_bemf + ::std::sqrt(static_cast<float>(
+ bemf * bemf +
+ 4.0f * static_cast<float>(kR) * 1.5f *
+ static_cast<float>(kVcc) * max_bat_cur))) /
+ (2.0f * 1.5f * static_cast<float>(kR)));
+
+ constexpr float kNegativeCurrent = 80.0f;
+ float goal_current = -::std::min(
+ filtered_throttle * (kPeakCurrent + kNegativeCurrent) - kNegativeCurrent,
+ throttle_limit);
+
+ if (velocity > -500) {
+ if (goal_current > 0.0f) {
+ goal_current = 0.0f;
+ }
+ }
+ //float goal_current =
+ //-::std::min(filtered_throttle * kPeakCurrent, throttle_limit);
+ const float fuse_current =
+ goal_current *
+ (bemf + goal_current * static_cast<float>(kR) * 1.5f) /
+ static_cast<float>(kVcc);
+ const int16_t fuse_current_10 = static_cast<int16_t>(10.0f * fuse_current);
+ fuse_badness += 0.00002f * (fuse_current * fuse_current - fuse_badness);
+
+ global_motor.load(::std::memory_order_relaxed)
+ ->SetGoalCurrent(goal_current);
+ global_motor.load(::std::memory_order_relaxed)
+ ->HandleInterrupt(balanced, wrapped_encoder);
+#else
+ (void)balanced;
+ FTM0->SC &= ~FTM_SC_TOF;
+ FTM0->C0V = 0;
+ FTM0->C1V = 0;
+ FTM0->C2V = 0;
+ FTM0->C3V = 0;
+ FTM0->C4V = 0;
+ FTM0->C5V = 60;
+ FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
+#endif
+
+ global_debug_buffer.count.fetch_add(1);
+
+ const bool trigger = false;
+ // global_debug_buffer.count.load(::std::memory_order_relaxed) >= 0;
+ size_t buffer_size =
+ global_debug_buffer.size.load(::std::memory_order_relaxed);
+ if ((buffer_size > 0 || trigger) &&
+ buffer_size != global_debug_buffer.samples.size()) {
+ global_debug_buffer.samples[buffer_size].currents[0] =
+ static_cast<int16_t>(balanced.readings[0] * 10.0f);
+ global_debug_buffer.samples[buffer_size].currents[1] =
+ static_cast<int16_t>(balanced.readings[1] * 10.0f);
+ global_debug_buffer.samples[buffer_size].currents[2] =
+ static_cast<int16_t>(balanced.readings[2] * 10.0f);
+ global_debug_buffer.samples[buffer_size].position =
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+ global_debug_buffer.samples[buffer_size].est_omega =
+ global_motor.load(::std::memory_order_relaxed)->estimated_velocity();
+ global_debug_buffer.samples[buffer_size].commands[0] =
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(0);
+ global_debug_buffer.samples[buffer_size].commands[1] =
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(1);
+ global_debug_buffer.samples[buffer_size].commands[2] =
+ global_motor.load(::std::memory_order_relaxed)->get_switching_points_cycles(2);
+ global_debug_buffer.samples[buffer_size].commanded_currents[0] =
+ global_motor.load(::std::memory_order_relaxed)->i_goal(0);
+ global_debug_buffer.samples[buffer_size].commanded_currents[1] =
+ global_motor.load(::std::memory_order_relaxed)->i_goal(1);
+ global_debug_buffer.samples[buffer_size].commanded_currents[2] =
+ global_motor.load(::std::memory_order_relaxed)->i_goal(2);
+ global_debug_buffer.samples[buffer_size].total_command =
+ global_motor.load(::std::memory_order_relaxed)->goal_current();
+ global_debug_buffer.samples[buffer_size].fuse_voltage =
+ filtered_fuse_voltage;
+ global_debug_buffer.samples[buffer_size].fuse_current = fuse_current_10;
+ global_debug_buffer.samples[buffer_size].driver_request =
+ ::std::max(filtered_throttle * (kPeakCurrent + kNegativeCurrent) -
+ kNegativeCurrent,
+ 0.0f);
+ global_debug_buffer.samples[buffer_size].fuse_badness = fuse_badness;
+ global_debug_buffer.samples[buffer_size].cycles_since_start = cycles_since_start;
+
+ global_debug_buffer.size.fetch_add(1);
+ }
+
+ if (buffer_size == global_debug_buffer.samples.size()) {
+ GPIOC_PCOR = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4);
+ GPIOD_PCOR = (1 << 4) | (1 << 5);
+
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 1) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 2) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 3) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 4) = 1;
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 4) = 1;
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 5) = 1;
+
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ }
+
+ ++i;
+ if (i > 1000) {
+ i = 0;
+ }
+}
+
+} // extern "C"
+
+void ConfigurePwmFtm(BigFTM *pwm_ftm) {
+ // Put them all into combine active-high mode, and all the low ones staying on
+ // all the time by default.
+ pwm_ftm->C0SC = FTM_CSC_ELSA;
+ pwm_ftm->C0V = 0;
+ pwm_ftm->C1SC = FTM_CSC_ELSA;
+ pwm_ftm->C1V = 0;
+ pwm_ftm->C2SC = FTM_CSC_ELSA;
+ pwm_ftm->C2V = 0;
+ pwm_ftm->C3SC = FTM_CSC_ELSA;
+ pwm_ftm->C3V = 0;
+ pwm_ftm->C4SC = FTM_CSC_ELSA;
+ pwm_ftm->C4V = 0;
+ pwm_ftm->C5SC = FTM_CSC_ELSA;
+ pwm_ftm->C5V = 0;
+ pwm_ftm->C6SC = FTM_CSC_ELSA;
+ pwm_ftm->C6V = 0;
+ pwm_ftm->C7SC = FTM_CSC_ELSA;
+ pwm_ftm->C7V = 0;
+
+ pwm_ftm->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN3 /* Enable deadtime */ |
+ FTM_COMBINE_COMP3 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE3 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN2 /* Enable deadtime */ |
+ FTM_COMBINE_COMP2 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE2 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN1 /* Enable deadtime */ |
+ FTM_COMBINE_COMP1 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE1 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */ |
+ FTM_COMBINE_DTEN0 /* Enable deadtime */ |
+ FTM_COMBINE_COMP0 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE0 /* Combine the channels */;
+ // Safe state for all channels is low.
+ pwm_ftm->POL = 0;
+
+ // Set the deadtime.
+ pwm_ftm->DEADTIME =
+ FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
+
+ pwm_ftm->CONF =
+ FTM_CONF_BDMMOD(1) /* Set everything to POLn during debug halt */;
+}
+
+// Zeros the encoder. This involves blocking for an arbitrary length of time
+// with interrupts disabled.
+void ZeroMotor() {
+#if 0
+ while (true) {
+ if (PERIPHERAL_BITBAND(GPIOB_PDIR, 11)) {
+ encoder_ftm_->CNT = 0;
+ break;
+ }
+ }
+#else
+ uint32_t scratch;
+ __disable_irq();
+ // Stuff all of this in an inline assembly statement so we can make sure the
+ // compiler doesn't decide sticking constant loads etc in the middle of
+ // the loop is a good idea, because that increases the latency of recognizing
+ // the index pulse edge which makes velocity affect the zeroing accuracy.
+ __asm__ __volatile__(
+ // A label to restart the loop.
+ "0:\n"
+ // Load the current PDIR value for the pin we care about.
+ "ldr %[scratch], [%[pdir_word]]\n"
+ // Terminate the loop if it's non-0.
+ "cbnz %[scratch], 1f\n"
+ // Go back around again.
+ "b 0b\n"
+ // A label to finish the loop.
+ "1:\n"
+ // Reset the count once we're down here. It doesn't actually matter what
+ // value we store because writing anything resets it to CNTIN (ie 0).
+ "str %[scratch], [%[cnt]]\n"
+ : [scratch] "=&l"(scratch)
+ : [pdir_word] "l"(&PERIPHERAL_BITBAND(GPIOB_PDIR, 11)),
+ [cnt] "l"(&FTM1->CNT));
+ __enable_irq();
+#endif
+}
+
+} // namespace
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_UART0_STATUS, 0xE);
+
+ // Set the LED's pin to output mode.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+#if 0
+ PERIPHERAL_BITBAND(GPIOA_PDDR, 15) = 1;
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ DMA.CR = M_DMA_EMLM;
+
+ PORTB_PCR16 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ PORTB_PCR17 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ SIM_SCGC4 |= SIM_SCGC4_UART0;
+ teensy::InterruptBufferedUart debug_uart(&UART0, F_CPU);
+ debug_uart.Initialize(115200);
+ global_stdout.store(&debug_uart, ::std::memory_order_release);
+ NVIC_ENABLE_IRQ(IRQ_UART0_STATUS);
+
+ AdcInitFet12();
+ MathInit();
+ delay(100);
+ can_init(0, 1);
+
+ MotorControlsImplementation controls;
+
+ delay(100);
+
+ // Index pin
+ PORTB_PCR11 = PORT_PCR_MUX(1);
+ // FTM1_QD_PH{A,B}
+ PORTB_PCR0 = PORT_PCR_MUX(6);
+ PORTB_PCR1 = PORT_PCR_MUX(6);
+
+ // FTM0_CH[0-5]
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ Motor motor(FTM0, FTM1, &controls, {&FTM0->C0V, &FTM0->C2V, &FTM0->C4V});
+ motor.set_encoder_offset(810);
+ motor.set_deadtime_compensation(9);
+ ConfigurePwmFtm(FTM0);
+ motor.Init();
+ global_motor.store(&motor, ::std::memory_order_relaxed);
+ // Output triggers to things like the PDBs on initialization.
+ FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
+ // Don't let any memory accesses sneak past here, because we actually
+ // need everything to be starting up.
+ __asm__("" :: : "memory");
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("Ram start: %p\n", __bss_ram_start__);
+ printf("Heap start: %p\n", __heap_start__);
+ printf("Heap end: %p\n", __brkval);
+ printf("Stack start: %p\n", __stack_end__);
+
+ printf("Going silent to zero motors...\n");
+ // Give the print a chance to make it out.
+ delay(100);
+ ZeroMotor();
+
+ motor.set_encoder_multiplier(-1);
+ motor.set_encoder_calibration_offset(
+ 558 + 1034 + 39 /*big data bemf comp*/ - 14 /*just backwardsbackwards comp*/);
+
+ printf("Zeroed motor!\n");
+ // Give stuff a chance to recover from interrupts-disabled.
+ delay(100);
+ motor.Start();
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+ GPIOC_PSOR = 1 << 5;
+
+ constexpr bool dump_full_sample = false;
+ while (true) {
+ if (dump_full_sample) {
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ motor.Reset();
+ }
+ global_debug_buffer.size.store(0);
+ global_debug_buffer.count.store(0);
+ while (global_debug_buffer.size.load(::std::memory_order_relaxed) <
+ global_debug_buffer.samples.size()) {
+ }
+ if (dump_full_sample) {
+ printf("Dumping data\n");
+ for (size_t i = 0; i < global_debug_buffer.samples.size(); ++i) {
+ const auto &sample = global_debug_buffer.samples[i];
+
+ printf("%u, %d, %d, %d, %u, %u, %u, %u, %d, %d, %d, %d\n", i,
+ sample.currents[0], sample.currents[1], sample.currents[2],
+ sample.commands[0], sample.commands[1], sample.commands[2],
+ sample.position, static_cast<int>(sample.est_omega),
+ sample.commanded_currents[0], sample.commanded_currents[1],
+ sample.commanded_currents[2]);
+ }
+ printf("Done dumping data\n");
+ } else {
+ //const auto &sample = global_debug_buffer.samples.back();
+ const DebugBuffer::Sample sample = global_debug_buffer.samples[0];
+#if 1
+ printf("%" PRIu32
+ ", %d, %d, %d, %u, %u, %u, %u, %d, %d, %d, %d, %d, %d, %d\n",
+ sample.cycles_since_start, sample.currents[0], sample.currents[1],
+ sample.currents[2], sample.commands[0], sample.commands[1],
+ sample.commands[2], sample.position,
+ static_cast<int>(sample.est_omega), sample.commanded_currents[0],
+ sample.commanded_currents[1], sample.commanded_currents[2],
+ sample.total_command, static_cast<int>(sample.driver_request),
+ static_cast<int>(sample.fuse_badness));
+#else
+ printf("%d, %d\n", static_cast<int>(sample.fuse_voltage),
+ sample.fuse_current);
+#endif
+ }
+ }
+
+ return 0;
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/motors/fet12/motor_controls.cc b/motors/fet12/motor_controls.cc
index cd048f4..7fac76f 100644
--- a/motors/fet12/motor_controls.cc
+++ b/motors/fet12/motor_controls.cc
@@ -25,45 +25,45 @@
constexpr int kPhaseBOffset = kCountsPerRevolution / 3;
constexpr int kPhaseCOffset = 2 * kCountsPerRevolution / 3;
-constexpr double K1_unscaled = 1.81e04;
-constexpr double K2_unscaled = -2.65e03;
+constexpr double K1_unscaled = 1.0;
+constexpr double K2_unscaled = 1.0 / -6.4786;
// Make the amplitude of the fundamental 1 for ease of math.
constexpr double K2 = K2_unscaled / K1_unscaled;
-constexpr double K1 = 1;
+constexpr double K1 = 1.0;
// volts
-constexpr double vcc = 30.0;
+constexpr double kVcc = 31.5;
-constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 2.0;
+constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 3.6;
-// Henries
-constexpr double L = 1e-05;
+constexpr double kL = 6e-06;
+constexpr double kM = 0;
+constexpr double kR = 0.0084;
+constexpr float kAdiscrete_diagonal = 0.932394f;
+constexpr float kAdiscrete_offdiagonal = 0.0f;
+constexpr float kBdiscrete_inv_diagonal = 0.124249f;
+constexpr float kBdiscrete_inv_offdiagonal = 0.0f;
-constexpr double M = 1e-06;
+// The number to divide the product of the unit BEMF and the per phase current
+// by to get motor current.
+constexpr double kOneAmpScalar = 1.46426;
+constexpr double kMaxOneAmpDrivingVoltage = 0.024884;
-// ohms for system
-constexpr double R = 0.008;
::Eigen::Matrix<float, 3, 3> A_discrete() {
::Eigen::Matrix<float, 3, 3> r;
- static constexpr float kDiagonal = 0.960091f;
- static constexpr float kOffDiagonal = 0.00356245f;
- r << kDiagonal, kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal,
- kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal;
+ r << kAdiscrete_diagonal, kAdiscrete_offdiagonal, kAdiscrete_offdiagonal,
+ kAdiscrete_offdiagonal, kAdiscrete_diagonal, kAdiscrete_offdiagonal,
+ kAdiscrete_offdiagonal, kAdiscrete_offdiagonal, kAdiscrete_diagonal;
return r;
}
::Eigen::Matrix<float, 3, 3> B_discrete_inverse() {
- return ::Eigen::Matrix<float, 1, 3>::Constant(0.18403f).asDiagonal();
+ return ::Eigen::Matrix<float, 1, 3>::Constant(kBdiscrete_inv_diagonal)
+ .asDiagonal();
}
-// The number to divide the product of the unit BEMF and the per phase current
-// by to get motor current.
-constexpr double kOneAmpScalar = 1.46785;
-
-constexpr double kMaxOneAmpDrivingVoltage = 0.0361525;
-
// Use FluxLinkageTable() to access this with a const so you don't accidentally
// modify it.
float flux_linkage_table[kCountsPerRevolution];
@@ -88,7 +88,7 @@
::Eigen::Matrix<float, 3, 3> MakeK() {
::Eigen::Matrix<float, 3, 3> Vconv;
Vconv << 2.0f, -1.0f, -1.0f, -1.0f, 2.0f, -1.0f, -1.0f, -1.0f, 2.0f;
- static constexpr float kControllerGain = 0.07f;
+ static constexpr float kControllerGain = 0.05f;
return kControllerGain * (Vconv / 3.0f);
}
@@ -107,9 +107,9 @@
}
ComplexMatrix<3, 3> Hn(float omega, int scalar) {
- const Complex a(static_cast<float>(R),
- omega * static_cast<float>(scalar * L));
- const Complex b(0, omega * static_cast<float>(scalar * M));
+ const Complex a(static_cast<float>(kR),
+ omega * static_cast<float>(scalar * kL));
+ const Complex b(0, omega * static_cast<float>(scalar * kM));
const Complex temp1 = a + b;
const Complex temp2 = -b;
ComplexMatrix<3, 3> matrix;
@@ -125,10 +125,10 @@
MakeFluxLinkageTable();
}
-::std::array<uint32_t, 3> MotorControlsImplementation::DoIteration(
+::std::array<float, 3> MotorControlsImplementation::DoIteration(
const float raw_currents[3], const uint32_t theta_in,
const float command_current) {
- static constexpr float kCurrentSlewRate = 0.05f;
+ static constexpr float kCurrentSlewRate = 0.10f;
if (command_current > filtered_current_ + kCurrentSlewRate) {
filtered_current_ += kCurrentSlewRate;
} else if (command_current < filtered_current_ - kCurrentSlewRate) {
@@ -138,11 +138,11 @@
}
const float goal_current_in = filtered_current_;
const float max_current =
- (static_cast<float>(vcc * kMaxDutyCycle) -
+ (static_cast<float>(kVcc * kMaxDutyCycle) -
estimated_velocity_ / static_cast<float>(Kv / 2.0)) /
static_cast<float>(kMaxOneAmpDrivingVoltage);
const float min_current =
- (-static_cast<float>(vcc * kMaxDutyCycle) -
+ (-static_cast<float>(kVcc * kMaxDutyCycle) -
estimated_velocity_ / static_cast<float>(Kv / 2.0)) /
static_cast<float>(kMaxOneAmpDrivingVoltage);
const float goal_current =
@@ -154,12 +154,12 @@
#elif 0
const uint32_t theta =
(theta_in + kCountsPerRevolution - 160u) % kCountsPerRevolution;
-#elif 1
+#elif 0
const uint32_t theta =
(theta_in + kCountsPerRevolution +
((estimated_velocity_ > 0) ? (kCountsPerRevolution - 10u) : 60u)) %
kCountsPerRevolution;
-#elif 0
+#elif 1
const uint32_t theta = theta_in;
#endif
@@ -187,17 +187,24 @@
const ::Eigen::Matrix<float, 3, 1> I_now = I_last_;
const ::Eigen::Matrix<float, 3, 1> I_next =
- FluxLinkageAt(theta) * goal_current;
+ FluxLinkageAt(theta +
+ static_cast<int32_t>(
+ 2.0f * omega * kCountsPerRevolution /
+ static_cast<float>(2.0 * M_PI * SWITCHING_FREQUENCY))) *
+ goal_current;
const ComplexMatrix<3, 3> H1 = Hn(omega, 1);
const ComplexMatrix<3, 3> H2 = Hn(omega, 5);
- const ComplexMatrix<3, 1> H1E1 = H1 * E1;
- const ComplexMatrix<3, 1> H2E2 = H2 * E2;
+ const ::std::complex<float> first_speed_delay =
+ ImaginaryExpFloat(omega / SWITCHING_FREQUENCY);
+ const ::std::complex<float> fifth_speed_delay =
+ ImaginaryExpFloat(omega * 5.0f / SWITCHING_FREQUENCY);
+ const ComplexMatrix<3, 1> H1E1 = first_speed_delay * H1 * E1;
+ const ComplexMatrix<3, 1> H2E2 = fifth_speed_delay * H2 * E2;
const ComplexMatrix<3, 1> p_imaginary = H1E1 + H2E2;
const ComplexMatrix<3, 1> p_next_imaginary =
- ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * H1E1 +
- ImaginaryExpFloat(omega * 5 / SWITCHING_FREQUENCY) * H2E2;
+ first_speed_delay * H1E1 + fifth_speed_delay * H2E2;
const ::Eigen::Matrix<float, 3, 1> p = p_imaginary.real();
const ::Eigen::Matrix<float, 3, 1> p_next = p_next_imaginary.real();
@@ -214,7 +221,7 @@
debug_[6] = Vn(1) * 100;
debug_[7] = Vn(2) * 100;
- ::Eigen::Matrix<float, 3, 1> times = Vn / vcc;
+ ::Eigen::Matrix<float, 3, 1> times = Vn / kVcc;
{
const float min_time = times.minCoeff();
times -= ::Eigen::Matrix<float, 3, 1>::Constant(min_time);
@@ -230,9 +237,8 @@
I_last_ = I_next;
// TODO(Austin): Figure out why we need the min here.
- return {static_cast<uint32_t>(::std::max(0.0f, times(0)) * 3000.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(1)) * 3000.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(2)) * 3000.0f)};
+ return {::std::max(0.0f, times(0)), ::std::max(0.0f, times(1)),
+ ::std::max(0.0f, times(2))};
}
int16_t MotorControlsImplementation::Debug(uint32_t theta) {
diff --git a/motors/fet12/motor_controls.h b/motors/fet12/motor_controls.h
index b9514fd..69ed847 100644
--- a/motors/fet12/motor_controls.h
+++ b/motors/fet12/motor_controls.h
@@ -20,7 +20,12 @@
MotorControlsImplementation();
~MotorControlsImplementation() override = default;
- static constexpr int constant_counts_per_revolution() { return 1024; }
+ void Reset() override {
+ estimated_velocity_ = 0;
+ filtered_current_ = 0;
+ }
+
+ static constexpr int constant_counts_per_revolution() { return 2048; }
int mechanical_counts_per_revolution() const override {
return constant_counts_per_revolution();
@@ -28,23 +33,20 @@
int electrical_counts_per_revolution() const override {
return constant_counts_per_revolution();
}
- float scale_current_reading(float reading) const override {
- 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 */);
- }
+ float scale_current_reading(float reading) const override { return reading; }
- ::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
- uint32_t theta,
- const float command_current) override;
+ ::std::array<float, 3> DoIteration(const float raw_currents[3],
+ uint32_t theta,
+ const float command_current) override;
int16_t Debug(uint32_t theta) override;
float estimated_velocity() const override { return estimated_velocity_; }
+ int16_t i_goal(size_t ii) const override {
+ return static_cast<int16_t>(I_last_[ii] * 10.0f);
+ }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_, E2Unrotated_;
diff --git a/motors/fet12/plot.gnuplot b/motors/fet12/plot.gnuplot
new file mode 100755
index 0000000..d73b7e0
--- /dev/null
+++ b/motors/fet12/plot.gnuplot
@@ -0,0 +1,19 @@
+#!/usr/bin/gnuplot
+
+plot '/tmp/50' using 1:($2 / 10.0) title 'current0' lc 1 lt 'dashed' with lines, \
+ '/tmp/50' using 1:($3 / 10.0) title 'current1' lc 2 lt 'dashed' with lines, \
+ '/tmp/50' using 1:($4 / 10.0) title 'current2' lc 3 lt 'dashed' with lines, \
+ '/tmp/50' using 1:(($5 * 0.666 - $6 * 0.333 - $7 * 0.333) / 10.0) title 'duty0' lc 1 lt 2, \
+ '/tmp/50' using 1:((-$5 * 0.333 + $6 * 0.666 - $7 * 0.333) / 10.0) title 'duty1' lc 2 lt 2, \
+ '/tmp/50' using 1:((-$5 * 0.333 - $6 * 0.333 + $7 * 0.666) / 10.0) title 'duty2' lc 3 lt 2, \
+ '/tmp/50' using 1:($8 / 2048.0 * 10.0 * 6.28) title 'angle', \
+ '/tmp/50' using 1:($9 / 1000.0 * 10.0) title 'velocity', \
+ '/tmp/50' using 1:($10 / 10.0) title 'goal_current0' lc 1 with lines, \
+ '/tmp/50' using 1:($11 / 10.0) title 'goal_current1' lc 2 with lines, \
+ '/tmp/50' using 1:($12 / 10.0) title 'goal_current2' lc 3 with lines
+
+ #'/tmp/50' using 1:($5 / 3000.0 * 100.0) title 'duty0' lc 1 lt 2, \
+ #'/tmp/50' using 1:($6 / 3000.0 * 100.0) title 'duty1' lc 2 lt 2, \
+ #'/tmp/50' using 1:($7 / 3000.0 * 100.0) title 'duty2' lc 3 lt 2, \
+
+pause -1
diff --git a/motors/motor.cc b/motors/motor.cc
index 93f2a4d..5bd2271 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -120,18 +120,23 @@
#if PRINT_TIMING
const uint32_t start_nanos = nanos();
-#endif
+#endif // PRINT_TIMING
if (!time_after(time_add(last_current_set_time_, 100000), micros())) {
goal_current_ = 0;
}
#if DO_CONTROLS
- const ::std::array<uint32_t, 3> switching_points =
- controls_->DoIteration(balanced.readings, captured_wrapped_encoder, goal_current_);
+ switching_points_ratio_ = controls_->DoIteration(
+ balanced.readings, captured_wrapped_encoder, goal_current_);
+ const float switching_points_max = static_cast<float>(counts_per_cycle());
+ const ::std::array<uint32_t, 3> switching_points = {
+ static_cast<uint32_t>(switching_points_ratio_[0] * switching_points_max),
+ static_cast<uint32_t>(switching_points_ratio_[1] * switching_points_max),
+ static_cast<uint32_t>(switching_points_ratio_[2] * switching_points_max)};
#if USE_CUTOFF
- //constexpr uint32_t kMax = 2945;
- constexpr uint32_t kMax = 1445;
+ constexpr uint32_t kMax = 2995;
+ //constexpr uint32_t kMax = 1445;
static bool done = false;
bool done_now = false;
if (switching_points[0] > kMax || switching_points[1] > kMax ||
@@ -144,7 +149,7 @@
for (int phase = 0; phase < 3; ++phase) {
const float scaled_reading =
controls_->scale_current_reading(balanced.readings[phase]);
- static constexpr float kMaxBalancedCurrent = 190.0f;
+ static constexpr float kMaxBalancedCurrent = 50.0f;
if (scaled_reading > kMaxBalancedCurrent ||
scaled_reading < -kMaxBalancedCurrent) {
current_done = true;
@@ -158,7 +163,7 @@
} else {
current_done_count = 0;
}
-#endif
+#endif // USE_ABSOLUTE_CUTOFF
if (done_now && !done) {
printf("done now\n");
printf("switching_points %" PRIu32 " %" PRIu32 " %" PRIu32 "\n",
@@ -170,9 +175,9 @@
done = true;
}
if (!done) {
-#else
+#else // USE_CUTOFF
if (true) {
-#endif
+#endif // USE_CUTOFF
output_registers_[0][0] = CalculateOnTime(switching_points[0]);
output_registers_[0][2] = CalculateOffTime(switching_points[0]);
output_registers_[1][0] = CalculateOnTime(switching_points[1]);
@@ -188,7 +193,7 @@
output_registers_[2][0] = 0;
output_registers_[2][2] = 0;
}
-#elif DO_FIXED_PULSE
+#elif DO_FIXED_PULSE // DO_CONTROLS
// Step through all the phases one by one in a loop. This should slowly move
// the trigger.
// If we fire phase 1, we should go to 0 radians.
@@ -210,61 +215,68 @@
// 30W total power dumped by the motor for the big one.
// For the small one, an on-width of 120/3000 with 14V in means about 2A
// through the motor.
- constexpr int kPhaseFireWidth = 60;
+ constexpr int kPhaseFireWidth = 90;
output_registers_[0][0] = 0;
- output_registers_[0][2] = phase_to_fire == 0 ? kPhaseFireWidth : 0;
+ output_registers_[0][2] =
+ phase_to_fire == 0 ? kPhaseFireWidth : 0;
+
+ const float switching_points_max = static_cast<float>(counts_per_cycle());
+ switching_points_ratio_[0] =
+ static_cast<float>(output_registers_[0][2]) / switching_points_max;
output_registers_[1][0] = 0;
output_registers_[1][2] = phase_to_fire == 1 ? kPhaseFireWidth : 0;
+ switching_points_ratio_[1] =
+ static_cast<float>(output_registers_[1][2]) / switching_points_max;
output_registers_[2][0] = 0;
output_registers_[2][2] = phase_to_fire == 2 ? kPhaseFireWidth : 0;
-#endif
+ switching_points_ratio_[2] =
+ static_cast<float>(output_registers_[2][2]) / switching_points_max;
+#endif // DO_CONTROLS/DO_FIXED_PULSE
(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);
- }
+ //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));
+ //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 * 7.0f / 4096.0f * 1000.0f),
- static_cast<int>(wheel_position * 1000.0f));
+ "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
+#else // DO_CONTROLS
printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", output_registers_[0][2],
output_registers_[1][2], output_registers_[2][2]);
-#endif
- //printf("0=%f\n", (double)balanced.readings[0]);
- } else if (i == 400) {
- //printf("1=%f\n", (double)balanced.readings[1]);
- } else if (i == 600) {
- //printf("2=%f\n", (double)balanced.readings[2]);
- } else {
- //printf("%" PRIu32 " to %" PRIu32 "\n", start_count, end_count);
+#endif // DO_CONTROLS
}
++i;
-#elif PRINT_ALL_READINGS
+#elif PRINT_ALL_READINGS // PRINT_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
+#elif TAKE_SAMPLE // PRINT_READINGS/PRINT_ALL_READINGS
#if 0
constexpr int kStartupWait = 50000;
#elif 0
@@ -290,12 +302,13 @@
++j;
#if SAMPLE_UNTIL_DONE
} else if (!done) {
-#else
+#else // SAMPLE_UNTIL_DONE
} else if (j < kSamplingEnd && (j % kSubsampling) == 0) {
-#endif
+#endif // SAMPLE_UNTIL_DONE
{
const int index = ((j - kStartupWait) / kSubsampling) % kPoints;
auto &point = data[index];
+// Start obnoxious #if 0/#if 1
#if 0
point[0] = adc_readings.motor_currents[0][0];
point[1] = adc_readings.motor_currents[1][0];
@@ -341,13 +354,14 @@
point[5] = pwm_ftm_->C5V - pwm_ftm_->C4V;
#endif
#endif
+// End obnoxious #if 0/#if 1
point[9] = captured_wrapped_encoder;
- SmallInitReadings readings;
- {
- DisableInterrupts disable_interrupts;
- readings = AdcReadSmallInit(disable_interrupts);
- }
- point[10] = readings.motor0_abs;
+ //SmallInitReadings readings;
+ //{
+ //DisableInterrupts disable_interrupts;
+ //readings = AdcReadSmallInit(disable_interrupts);
+ //}
+ //point[10] = readings.motor0_abs;
}
#if DO_STEP_RESPONSE
@@ -355,7 +369,7 @@
if (j > kStartupWait + 200 / kSubsampling) {
pwm_ftm_->C3V = 240;
}
-#elif DO_PULSE_SWEEP
+#elif DO_PULSE_SWEEP // DO_STEP_RESPONSE
// Sweep the pulse through the ADC sampling points.
static constexpr int kMax = 2500;
static constexpr int kExtraWait = 1500;
@@ -370,23 +384,23 @@
pwm_ftm_->C2V = 0;
pwm_ftm_->C3V = 0;
}
-#endif
+#endif // DO_STEP_RESPONSE/DO_PULSE_SWEEP
++j;
#if SAMPLE_UNTIL_DONE
} else if (false) {
-#else
+#else // SAMPLE_UNTIL_DONE
} else if (j < kSamplingEnd) {
++j;
} else if (j == kSamplingEnd) {
-#endif
+#endif // SAMPLE_UNTIL_DONE
printf("finished\n");
++j;
#if SAMPLE_UNTIL_DONE
} else if (done) {
-#else
+#else // SAMPLE_UNTIL_DONE
} else {
-#endif
+#endif // SAMPLE_UNTIL_DONE
// Time to write the data out.
if (written < (int)sizeof(data) && debug_tty_ != nullptr) {
int to_write = sizeof(data) - written;
@@ -406,7 +420,7 @@
done_writing = true;
}
}
-#endif
+#endif // PRINT_READINGS/PRINT_ALL_READINGS/TAKE_SAMPLE
(void)balanced;
// Tell the hardware to use the new switching points.
@@ -423,7 +437,7 @@
print_timing_count = 0;
print_timing_total = 0;
}
-#endif
+#endif // PRINT_TIMING
// If another cycle has already started, turn the light on right now.
if (pwm_ftm_->SC & FTM_SC_TOF) {
diff --git a/motors/motor.h b/motors/motor.h
index ec96efb..4c5e604 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -24,6 +24,8 @@
MotorControls(const MotorControls &) = delete;
void operator=(const MotorControls &) = delete;
+ virtual void Reset() = 0;
+
// Scales a current reading from ADC units to amps.
//
// Note that this doesn't apply any offset. The common offset will be
@@ -36,13 +38,14 @@
// 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 ::std::array<float, 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;
+ virtual int16_t i_goal(size_t ii) const = 0;
};
// Controls a single motor.
@@ -56,6 +59,8 @@
Motor(const Motor &) = delete;
void operator=(const Motor &) = delete;
+ void Reset() { controls_->Reset(); }
+
void set_debug_tty(teensy::AcmTty *debug_tty) { debug_tty_ = debug_tty; }
void set_deadtime_compensation(int deadtime_compensation) {
deadtime_compensation_ = deadtime_compensation;
@@ -130,11 +135,28 @@
last_current_set_time_ = micros();
}
- private:
inline int counts_per_cycle() const {
return BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY / switching_divisor_;
}
+ inline uint16_t get_switching_points_cycles(size_t ii) const {
+ return static_cast<uint16_t>(switching_points_ratio_[ii] *
+ counts_per_cycle());
+ }
+
+ inline float estimated_velocity() const {
+ return controls_->estimated_velocity();
+ }
+
+ inline int16_t i_goal(size_t ii) const {
+ return controls_->i_goal(ii);
+ }
+
+ inline int16_t goal_current() const {
+ return goal_current_;
+ }
+
+ private:
uint32_t CalculateOnTime(uint32_t width) const;
uint32_t CalculateOffTime(uint32_t width) const;
@@ -145,6 +167,7 @@
LittleFTM *const encoder_ftm_;
MotorControls *const controls_;
const ::std::array<volatile uint32_t *, 3> output_registers_;
+ ::std::array<float, 3> switching_points_ratio_;
float goal_current_ = 0;
uint32_t last_current_set_time_ = 0;
diff --git a/motors/pistol_grip/motor_controls.cc b/motors/pistol_grip/motor_controls.cc
index 2f4f00f..c9eae40 100644
--- a/motors/pistol_grip/motor_controls.cc
+++ b/motors/pistol_grip/motor_controls.cc
@@ -116,7 +116,7 @@
MakeFluxLinkageTable();
}
-::std::array<uint32_t, 3> LittleMotorControlsImplementation::DoIteration(
+::std::array<float, 3> LittleMotorControlsImplementation::DoIteration(
const float raw_currents[3], const uint32_t theta_in,
const float command_current) {
static constexpr float kCurrentSlewRate = 0.05f;
@@ -199,9 +199,8 @@
I_last_ = I_next;
// TODO(Austin): Figure out why we need the min here.
- return {static_cast<uint32_t>(::std::max(0.0f, times(0)) * 1500.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(1)) * 1500.0f),
- static_cast<uint32_t>(::std::max(0.0f, times(2)) * 1500.0f)};
+ return {::std::max(0.0f, times(0)), ::std::max(0.0f, times(1)),
+ ::std::max(0.0f, times(2))};
}
int16_t LittleMotorControlsImplementation::Debug(uint32_t theta) {
diff --git a/motors/pistol_grip/motor_controls.h b/motors/pistol_grip/motor_controls.h
index 88ef8a2..7992472 100644
--- a/motors/pistol_grip/motor_controls.h
+++ b/motors/pistol_grip/motor_controls.h
@@ -20,6 +20,8 @@
LittleMotorControlsImplementation();
~LittleMotorControlsImplementation() override = default;
+ void Reset() override {}
+
static constexpr int constant_counts_per_revolution() { return 4096; }
static constexpr int poles_per_revolution() { return 7; }
@@ -38,7 +40,7 @@
0.195 /* V/A */);
}
- ::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
+ ::std::array<float, 3> DoIteration(const float raw_currents[3],
uint32_t theta,
const float command_current) override;
@@ -46,6 +48,10 @@
float estimated_velocity() const override { return estimated_velocity_; }
+ int16_t i_goal(size_t ii) const override {
+ return static_cast<int16_t>(I_last_[ii] * 10.0f);
+ }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_;