Made new motor work
-Calibrate resistance, inductance, and Kv (also, we never committed the
Python calibrations for the old motor).
-Fix feedback term--we were comparing a current to the commanded current
corresponding to the cycle after it, which could've been an issue.
-Add some more debugging code that is useful when calibrating the motor.
-Increase baud rate of ITM to 128k
-Increase motor braking current to 100A from 80A
-Use measured current instead of goal current on fuse monitoring--should
cause it to perform better in situations where we track poorly (e.g.,
near full speed).
Change-Id: I1b5ff71b42323cb96f229c70821e9e0860ac4e4c
diff --git a/motors/big/motor_controls.cc b/motors/big/motor_controls.cc
index a26ae83..abc71e8 100644
--- a/motors/big/motor_controls.cc
+++ b/motors/big/motor_controls.cc
@@ -178,6 +178,7 @@
const float overall_measured_current =
((E1 + E2).real().transpose() * measured_current /
static_cast<float>(kOneAmpScalar))(0);
+ overall_measured_current_ = overall_measured_current;
const float current_error = goal_current - overall_measured_current;
estimated_velocity_ += current_error * 0.1f;
debug_[3] = theta;
diff --git a/motors/big/motor_controls.h b/motors/big/motor_controls.h
index aae97d0..5c59ef1 100644
--- a/motors/big/motor_controls.h
+++ b/motors/big/motor_controls.h
@@ -51,11 +51,16 @@
return static_cast<int16_t>(I_last_[ii] * 10.0f);
}
+ float overall_measured_current() const override {
+ return overall_measured_current_;
+ }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_, E2Unrotated_;
float estimated_velocity_ = 0;
float filtered_current_ = 0;
+ float overall_measured_current_ = 0;
::Eigen::Matrix<float, 3, 1> I_last_ = ::Eigen::Matrix<float, 3, 1>::Zero();
diff --git a/motors/core/itm.cc b/motors/core/itm.cc
index 2dcc5e6..ce2aa08 100644
--- a/motors/core/itm.cc
+++ b/motors/core/itm.cc
@@ -7,7 +7,7 @@
namespace itm {
namespace {
-constexpr int kDesiredTraceFrequency = 64000;
+constexpr int kDesiredTraceFrequency = 128000;
// TODO(Brian): Is this actually the frequency our parts use?
constexpr int kTpiuAsynchronousClockFrequency = F_CPU;
diff --git a/motors/fet12/fet12v2.cc b/motors/fet12/fet12v2.cc
index 2461666..ae7c3c0 100644
--- a/motors/fet12/fet12v2.cc
+++ b/motors/fet12/fet12v2.cc
@@ -133,10 +133,11 @@
adc_dma->Reset();
const uint32_t wrapped_encoder =
global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
-#if 1
const BalancedReadings balanced =
BalanceSimpleReadings(decoupled);
+#if 1
+
static float fuse_badness = 0;
static uint32_t cycles_since_start = 0u;
@@ -220,7 +221,7 @@
static_cast<float>(kVcc) * max_bat_cur))) /
(2.0f * 1.5f * static_cast<float>(kR)));
- constexpr float kNegativeCurrent = 80.0f;
+ constexpr float kNegativeCurrent = 100.0f;
float goal_current =
-::std::min(
::std::max(filtered_throttle * (kPeakCurrent + kNegativeCurrent) -
@@ -231,7 +232,6 @@
if (!throttle_zeroed) {
goal_current = 0.0f;
}
-
// Note: current reduction is 12/70 belt, 15 / 54 on chain, and 10 inch
// diameter wheels, so cutoff of 500 electrical rad/sec * 1 mechanical rad / 2
// erad * 12 / 70 * 15 / 54 * 0.127 m = 1.5m/s = 3.4 mph
@@ -240,11 +240,15 @@
goal_current = 0.0f;
}
}
+
//float goal_current =
//-::std::min(filtered_throttle * kPeakCurrent, throttle_limit);
+ const float overall_measured_current =
+ global_motor.load(::std::memory_order_relaxed)
+ ->overall_measured_current();
const float fuse_current =
- goal_current *
- (bemf + goal_current * static_cast<float>(kR) * 1.5f) /
+ overall_measured_current *
+ (bemf + overall_measured_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);
@@ -256,7 +260,7 @@
global_debug_buffer.count.fetch_add(1);
- const bool trigger = false;
+ const bool trigger = false && i > 10000;
// global_debug_buffer.count.load(::std::memory_order_relaxed) >= 0;
size_t buffer_size =
global_debug_buffer.size.load(::std::memory_order_relaxed);
@@ -322,14 +326,55 @@
#else
#endif
#else
+ // Useful code when calculating resistance/inductance of motor
FTM0->SC &= ~FTM_SC_TOF;
FTM0->C0V = 0;
FTM0->C1V = 0;
FTM0->C2V = 0;
FTM0->C3V = 0;
FTM0->C4V = 0;
- FTM0->C5V = 0;
+ FTM0->C5V = 10;
FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
+ (void)wrapped_encoder;
+ (void)real_throttle;
+ size_t buffer_size =
+ global_debug_buffer.size.load(::std::memory_order_relaxed);
+ bool trigger = true || i > 20000;
+ if ((trigger || buffer_size > 0) &&
+ 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].commands[0] = FTM0->C1V;
+ global_debug_buffer.samples[buffer_size].commands[1] = FTM0->C3V;
+ global_debug_buffer.samples[buffer_size].commands[2] = FTM0->C5V;
+ global_debug_buffer.samples[buffer_size].position =
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder();
+ 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 = 0;
+ }
+ ++i;
#endif
}
@@ -580,7 +625,8 @@
motor.set_encoder_multiplier(-1);
motor.set_encoder_calibration_offset(
- 558 + 1034 + 39 /*big data bemf comp*/ - 14 /*just backwardsbackwards comp*/);
+ 364 /*from running constant phases*/ - 26 /*average offset from lstsq*/ -
+ 14 /* compensation for going backwards */);
printf("Zeroed motor!\n");
// Give stuff a chance to recover from interrupts-disabled.
@@ -593,9 +639,10 @@
NVIC_ENABLE_IRQ(IRQ_FTM0);
GPIOC_PSOR = 1 << 5;
- constexpr bool dump_full_sample = false;
+ constexpr bool dump_full_sample = true;
+ constexpr bool dump_resist_calib = false;
while (true) {
- if (dump_full_sample) {
+ if (dump_resist_calib || 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);
@@ -609,7 +656,16 @@
while (global_debug_buffer.size.load(::std::memory_order_relaxed) <
global_debug_buffer.samples.size()) {
}
- if (dump_full_sample) {
+ if (dump_resist_calib) {
+ // Useful prints for when calibrating resistance/inductance of motor
+ 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\n", i,
+ sample.currents[0], sample.currents[1], sample.currents[2],
+ sample.commands[0], sample.commands[1], sample.commands[2],
+ sample.position);
+ }
+ } else 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];
diff --git a/motors/fet12/motor_controls.cc b/motors/fet12/motor_controls.cc
index 7fac76f..d6afab1 100644
--- a/motors/fet12/motor_controls.cc
+++ b/motors/fet12/motor_controls.cc
@@ -35,20 +35,18 @@
// volts
constexpr double kVcc = 31.5;
-constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 3.6;
+// 3.6 and 1.15 are adjustments from calibrations.
+constexpr double Kv = 22000.0 * 2.0 * M_PI / 60.0 / 30.0 * 3.6 * 1.15;
-constexpr double kL = 6e-06;
+constexpr double kL = 3e-06;
constexpr double kM = 0;
-constexpr double kR = 0.0084;
-constexpr float kAdiscrete_diagonal = 0.932394f;
+constexpr double kR = 0.01008;
+constexpr float kAdiscrete_diagonal = 0.845354f;
constexpr float kAdiscrete_offdiagonal = 0.0f;
-constexpr float kBdiscrete_inv_diagonal = 0.124249f;
+constexpr float kBdiscrete_inv_diagonal = 0.0651811f;
constexpr float kBdiscrete_inv_offdiagonal = 0.0f;
-
-// 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;
+constexpr double kOneAmpScalar = 1.46785;
+constexpr double kMaxOneAmpDrivingVoltage = 0.0265038;
::Eigen::Matrix<float, 3, 3> A_discrete() {
@@ -178,6 +176,7 @@
const float overall_measured_current =
((E1 + E2).real().transpose() * measured_current /
static_cast<float>(kOneAmpScalar))(0);
+ overall_measured_current_ = overall_measured_current;
const float current_error = goal_current - overall_measured_current;
estimated_velocity_ += current_error * 0.1f;
debug_[3] = theta;
@@ -211,7 +210,7 @@
const ::Eigen::Matrix<float, 3, 1> Vn_ff =
B_discrete_inverse() * (I_next - A_discrete() * (I_now - p) - p_next);
const ::Eigen::Matrix<float, 3, 1> Vn =
- Vn_ff + MakeK() * (I_now - measured_current);
+ Vn_ff + MakeK() * (I_prev_ - measured_current);
debug_[0] = (I_next)(0) * 100;
debug_[1] = (I_next)(1) * 100;
@@ -234,6 +233,7 @@
times *= scalar;
}
+ I_prev_ = I_now;
I_last_ = I_next;
// TODO(Austin): Figure out why we need the min here.
diff --git a/motors/fet12/motor_controls.h b/motors/fet12/motor_controls.h
index 69ed847..eea1c30 100644
--- a/motors/fet12/motor_controls.h
+++ b/motors/fet12/motor_controls.h
@@ -47,13 +47,18 @@
return static_cast<int16_t>(I_last_[ii] * 10.0f);
}
+ float overall_measured_current() const { return overall_measured_current_; }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_, E2Unrotated_;
float estimated_velocity_ = 0;
float filtered_current_ = 0;
+ float overall_measured_current_ = 0;
::Eigen::Matrix<float, 3, 1> I_last_ = ::Eigen::Matrix<float, 3, 1>::Zero();
+ ::Eigen::Matrix<float, 3, 1> I_prev_ =
+ ::Eigen::Matrix<float, 3, 1>::Zero();
int16_t debug_[9];
};
diff --git a/motors/motor.h b/motors/motor.h
index 4a65f63..808783e 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -46,6 +46,7 @@
virtual float estimated_velocity() const = 0;
virtual int16_t i_goal(size_t ii) const = 0;
+ virtual float overall_measured_current() const = 0;
};
// Controls a single motor.
@@ -159,6 +160,10 @@
return goal_current_;
}
+ inline float overall_measured_current() const {
+ return controls_->overall_measured_current();
+ }
+
private:
uint32_t CalculateOnTime(uint32_t width) const;
uint32_t CalculateOffTime(uint32_t width) const;
diff --git a/motors/pistol_grip/motor_controls.cc b/motors/pistol_grip/motor_controls.cc
index c9eae40..70e5464 100644
--- a/motors/pistol_grip/motor_controls.cc
+++ b/motors/pistol_grip/motor_controls.cc
@@ -153,6 +153,7 @@
const float overall_measured_current =
(E1.real().transpose() * measured_current /
static_cast<float>(kOneAmpScalar))(0);
+ overall_measured_current_ = overall_measured_current;
const float current_error = goal_current - overall_measured_current;
estimated_velocity_ += current_error * 0.1f;
diff --git a/motors/pistol_grip/motor_controls.h b/motors/pistol_grip/motor_controls.h
index 7992472..5e333dc 100644
--- a/motors/pistol_grip/motor_controls.h
+++ b/motors/pistol_grip/motor_controls.h
@@ -52,11 +52,16 @@
return static_cast<int16_t>(I_last_[ii] * 10.0f);
}
+ float overall_measured_current() const override {
+ return overall_measured_current_;
+ }
+
private:
const ComplexMatrix<3, 1> E1Unrotated_;
float estimated_velocity_ = 0;
float filtered_current_ = 0;
+ float overall_measured_current_ = 0;
::Eigen::Matrix<float, 3, 1> I_last_ = ::Eigen::Matrix<float, 3, 1>::Zero();
diff --git a/motors/python/big_phase_current.py b/motors/python/big_phase_current.py
index 718d4d9..5ddca2a 100755
--- a/motors/python/big_phase_current.py
+++ b/motors/python/big_phase_current.py
@@ -16,14 +16,15 @@
K2 /= K1
K1 = 1
-vcc = 30.0 # volts
-R_motor = 0.0055 # ohms for the motor
-R = 0.008 # ohms for system
+vcc = 31.5 # volts
+R = 0.01008 # ohms for system
-L = 10.0 * 1e-6 # Henries
-M = L / 10.0
+L = 3.0 * 1e-6 # Henries
+M = 0.0
-Kv = 22000.0 * 2.0 * numpy.pi / 60.0 / vcc * 2.0
+Kv_vcc = 30.0
+Kv = 22000.0 * 2.0 * numpy.pi / 60.0 / Kv_vcc * 2.0
+Kv = 1.0 / 0.00315
J = 0.0000007
R_shunt = 0.0003