Merge "Gain schedule elevator and wrist."
diff --git a/motors/BUILD b/motors/BUILD
index 9fe638c..ac65f37 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -144,6 +144,27 @@
)
hex_from_elf(
+ name = "simpler_receiver",
+ restricted_to = mcu_cpus,
+)
+
+cc_binary(
+ name = "simpler_receiver.elf",
+ srcs = [
+ "simpler_receiver.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:can",
+ "//motors/peripheral:configuration",
+ "//motors/print:usb",
+ "//motors/seems_reasonable:drivetrain_lib",
+ ],
+)
+
+hex_from_elf(
name = "simple_receiver",
restricted_to = mcu_cpus,
)
diff --git a/motors/big/medium_salsa.cc b/motors/big/medium_salsa.cc
index 7e01869..6c3f339 100644
--- a/motors/big/medium_salsa.cc
+++ b/motors/big/medium_salsa.cc
@@ -133,7 +133,7 @@
}
const BalancedReadings balanced = BalanceReadings(to_balance);
- global_motor.load(::std::memory_order_relaxed)->HandleInterrupt(
+ global_motor.load(::std::memory_order_relaxed)->CurrentInterrupt(
balanced,
global_motor.load(::std::memory_order_relaxed)->wrapped_encoder());
}
diff --git a/motors/fet12/fet12v2.cc b/motors/fet12/fet12v2.cc
index ae7c3c0..6722640 100644
--- a/motors/fet12/fet12v2.cc
+++ b/motors/fet12/fet12v2.cc
@@ -256,7 +256,7 @@
global_motor.load(::std::memory_order_relaxed)
->SetGoalCurrent(goal_current);
global_motor.load(::std::memory_order_relaxed)
- ->HandleInterrupt(balanced, wrapped_encoder);
+ ->CurrentInterrupt(balanced, wrapped_encoder);
global_debug_buffer.count.fetch_add(1);
diff --git a/motors/motor.cc b/motors/motor.cc
index ebeec22..1b9f2e5 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -103,7 +103,6 @@
#define USE_ABSOLUTE_CUTOFF 0
#define DO_CONTROLS 1
-#define DO_FIXED_PULSE 0
#define USE_CUTOFF 1
#define PRINT_READINGS 0
@@ -114,8 +113,60 @@
#define DO_PULSE_SWEEP 0
#define PRINT_TIMING 0
-void Motor::HandleInterrupt(const BalancedReadings &balanced,
- uint32_t captured_wrapped_encoder) {
+void Motor::CycleFixedPhaseInterupt() {
+ pwm_ftm_->SC &= ~FTM_SC_TOF;
+ // 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 PI radians.
+ // If we fire phase 2, we should go to 1.0 * PI / 3.0 radians.
+ // If we fire phase 3, we should go to -1.0 * PI / 3.0 radians.
+ // These numbers were confirmed by the python motor simulation.
+ static int phase_to_fire_count = -300000;
+ static int phase_to_fire = 0;
+ ++phase_to_fire_count;
+ if (phase_to_fire_count > 500000) {
+ phase_to_fire_count = 0;
+ ++phase_to_fire;
+ if (phase_to_fire > 2) {
+ phase_to_fire = 0;
+ }
+ }
+ phase_to_fire = 0;
+
+ // An on-width of 60 with 30V in means about 50A through the motor and about
+ // 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 = 80;
+ constexpr int kPhaseFireWidth = 80;
+ output_registers_[0][0] = 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;
+ switching_points_ratio_[2] =
+ static_cast<float>(output_registers_[2][2]) / switching_points_max;
+
+ // Tell the hardware to use the new switching points.
+ // TODO(Brian): Somehow verify that we consistently hit the first or second
+ // timer-cycle with the new values (if there's two).
+ pwm_ftm_->PWMLOAD = FTM_PWMLOAD_LDOK;
+
+ // If another cycle has already started, turn the light on right now.
+ if (pwm_ftm_->SC & FTM_SC_TOF) {
+ GPIOC_PSOR = 1 << 5;
+ }
+}
+
+void Motor::CurrentInterrupt(const BalancedReadings &balanced,
+ uint32_t captured_wrapped_encoder) {
pwm_ftm_->SC &= ~FTM_SC_TOF;
#if PRINT_TIMING
@@ -193,45 +244,7 @@
output_registers_[2][0] = 0;
output_registers_[2][2] = 0;
}
-#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.
- // If we fire phase 2, we should go to -2.0 * PI / 3.0 radians.
- // If we fire phase 3, we should go to 2.0 * PI / 3.0 radians.
- // These numbers were confirmed by the python motor simulation.
- static int phase_to_fire_count = -300000;
- static int phase_to_fire = 0;
- ++phase_to_fire_count;
- if (phase_to_fire_count > 200000) {
- phase_to_fire_count = 0;
- ++phase_to_fire;
- if (phase_to_fire > 2) {
- phase_to_fire = 0;
- }
- }
-
- // An on-width of 60 with 30V in means about 50A through the motor and about
- // 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 = 90;
- output_registers_[0][0] = 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;
- switching_points_ratio_[2] =
- static_cast<float>(output_registers_[2][2]) / switching_points_max;
-#endif // DO_CONTROLS/DO_FIXED_PULSE
+#endif // DO_CONTROLS
(void)balanced;
(void)captured_wrapped_encoder;
#if PRINT_READINGS
diff --git a/motors/motor.h b/motors/motor.h
index 808783e..7721f1a 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -130,8 +130,11 @@
// 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 CurrentInterrupt(const BalancedReadings &readings,
+ uint32_t captured_wrapped_encoder);
+
+ // Runs each phase at a fixed duty cycle.
+ void CycleFixedPhaseInterupt();
void SetGoalCurrent(float goal_current) {
DisableInterrupts disable_interrupts;
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index ccd8d7d..de593c7 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -86,6 +86,7 @@
// more stable than the PLL-based peripheral clock, which matters.
// We're going with a sample point fraction of 0.875 because that's what
// SocketCAN defaults to.
+ // This results in a baud rate of 500 kHz.
CAN0_CTRL1 = CAN_CTRL1_PRESDIV(
1) /* Divide the crystal frequency by 2 to get 8 MHz. */ |
CAN_CTRL1_RJW(0) /* RJW/SJW of 1, which is most common. */ |
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 9f292d1..5a52afb 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -33,6 +33,30 @@
using ::frc971::control_loops::drivetrain::MakeIntegralHapticWheelPlant;
using ::frc971::control_loops::drivetrain::MakeIntegralHapticWheelObserver;
+// Returns an identifier for the processor we're running on.
+// This isn't guaranteed to be unique, but it should be close enough.
+uint8_t ProcessorIdentifier() {
+ // This XORs together all the bytes of the unique identifier provided by the
+ // hardware.
+ uint8_t r = 0;
+ for (uint8_t uid : {SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL}) {
+ r = r ^ ((uid >> 0) & 0xFF);
+ r = r ^ ((uid >> 8) & 0xFF);
+ r = r ^ ((uid >> 16) & 0xFF);
+ r = r ^ ((uid >> 24) & 0xFF);
+ }
+ return r;
+}
+
+uint8_t ProcessorIndex() {
+ switch (ProcessorIdentifier()) {
+ case static_cast<uint8_t>(0xaa):
+ return 1;
+ default:
+ return 0;
+ }
+}
+
struct SmallAdcReadings {
uint16_t currents[3];
};
@@ -237,14 +261,17 @@
->absolute_encoder(encoder);
const float angle = absolute_encoder / static_cast<float>((15320 - 1488) / 2);
- global_wheel_angle.store(angle);
- float goal_current = -global_wheel_current.load(::std::memory_order_relaxed) +
+ float goal_current = global_wheel_current.load(::std::memory_order_relaxed) +
kWheelCoggingTorque[encoder];
+ //float goal_current = kWheelCoggingTorque[encoder];
+ //float goal_current = 0.0f;
global_motor1.load(::std::memory_order_relaxed)->SetGoalCurrent(goal_current);
global_motor1.load(::std::memory_order_relaxed)
- ->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
+ ->CurrentInterrupt(BalanceSimpleReadings(readings.currents), encoder);
+
+ global_wheel_angle.store(angle);
}
constexpr float kTriggerMaxExtension = -0.70f;
@@ -281,20 +308,24 @@
DisableInterrupts disable_interrupts;
readings = AdcReadSmall0(disable_interrupts);
}
- uint32_t encoder =
- global_motor0.load(::std::memory_order_relaxed)->wrapped_encoder();
- int32_t absolute_encoder = global_motor0.load(::std::memory_order_relaxed)
- ->absolute_encoder(encoder);
- float trigger_angle = absolute_encoder / 1370.f;
+ const uint32_t encoder =
+ global_motor0.load(::std::memory_order_relaxed)->wrapped_encoder();
+ const int32_t absolute_encoder =
+ global_motor0.load(::std::memory_order_relaxed)
+ ->absolute_encoder(encoder);
+
+ const float trigger_angle = absolute_encoder / 1370.f;
const float goal_current =
- -global_trigger_torque.load(::std::memory_order_relaxed) +
+ global_trigger_torque.load(::std::memory_order_relaxed) +
kTriggerCoggingTorque[encoder];
+ //const float goal_current = kTriggerCoggingTorque[encoder];
+ //const float goal_current = 0.0f;
global_motor0.load(::std::memory_order_relaxed)->SetGoalCurrent(goal_current);
global_motor0.load(::std::memory_order_relaxed)
- ->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
+ ->CurrentInterrupt(BalanceSimpleReadings(readings.currents), encoder);
global_trigger_angle.store(trigger_angle);
}
@@ -658,21 +689,6 @@
return true;
}
-// Returns an identifier for the processor we're running on.
-// This isn't guaranteed to be unique, but it should be close enough.
-uint8_t ProcessorIdentifier() {
- // This XORs together all the bytes of the unique identifier provided by the
- // hardware.
- uint8_t r = 0;
- for (uint8_t uid : {SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL}) {
- r = r ^ ((uid >> 0) & 0xFF);
- r = r ^ ((uid >> 8) & 0xFF);
- r = r ^ ((uid >> 16) & 0xFF);
- r = r ^ ((uid >> 24) & 0xFF);
- }
- return r;
-}
-
} // namespace
extern "C" int main() {
@@ -823,7 +839,8 @@
printf("heap start: %p\n", __heap_start__);
printf("stack start: %p\n", __stack_end__);
- printf("Zeroing motors for %x\n", (unsigned int)ProcessorIdentifier());
+ printf("Zeroing motors for %d:%x\n", static_cast<int>(ProcessorIndex()),
+ (unsigned int)ProcessorIdentifier());
uint16_t motor0_offset, motor1_offset, wheel_offset;
while (!ZeroMotors(&motor0_offset, &motor1_offset, &wheel_offset)) {
}
diff --git a/motors/simpler_receiver.cc b/motors/simpler_receiver.cc
new file mode 100644
index 0000000..86539bd
--- /dev/null
+++ b/motors/simpler_receiver.cc
@@ -0,0 +1,512 @@
+// This file has the main for the Teensy on the simple receiver board v2 in the
+// new robot.
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/can.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/print/print.h"
+#include "motors/seems_reasonable/drivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
+using ::frc971::constants::ShifterHallEffect;
+using ::frc971::control_loops::DrivetrainQueue_Goal;
+using ::frc971::control_loops::DrivetrainQueue_Output;
+
+namespace chrono = ::std::chrono;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<float> &GetDrivetrainConfig() {
+ static DrivetrainConfig<float> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+
+ ::motors::seems_reasonable::MakeDrivetrainLoop,
+ ::motors::seems_reasonable::MakeVelocityDrivetrainLoop,
+ ::std::function<StateFeedbackLoop<7, 2, 4, float>()>(),
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<float>(::motors::seems_reasonable::kDt)),
+ ::motors::seems_reasonable::kRobotRadius,
+ ::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV,
+
+ ::motors::seems_reasonable::kHighGearRatio,
+ ::motors::seems_reasonable::kLowGearRatio, ::motors::seems_reasonable::kJ,
+ ::motors::seems_reasonable::kMass, kThreeStateDriveShifter,
+ kThreeStateDriveShifter, true /* default_high_gear */,
+ 0 /* down_offset */, 0.8 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */, 1.5 /* wheel_multiplier */,
+ };
+
+ return kDrivetrainConfig;
+};
+
+
+::std::atomic<PolyDrivetrain<float> *> global_polydrivetrain{nullptr};
+
+// Last width we received on each channel.
+uint16_t pwm_input_widths[6];
+// When we received a pulse on each channel in milliseconds.
+uint32_t pwm_input_times[6];
+
+constexpr int kChannelTimeout = 100;
+
+bool lost_channel(int channel) {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return true;
+ }
+ return false;
+}
+
+// Returns the most recently captured value for the specified input channel
+// scaled from -1 to 1, or 0 if it was captured over 100ms ago.
+float convert_input_width(int channel) {
+ uint16_t width;
+ {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return 0;
+ }
+
+ width = pwm_input_widths[channel];
+ }
+
+ // Values measured with a channel mapped to a button.
+ static constexpr uint16_t kMinWidth = 4133;
+ static constexpr uint16_t kMaxWidth = 7177;
+ if (width < kMinWidth) {
+ width = kMinWidth;
+ } else if (width > kMaxWidth) {
+ width = kMaxWidth;
+ }
+ return (static_cast<float>(2 * (width - kMinWidth)) /
+ static_cast<float>(kMaxWidth - kMinWidth)) -
+ 1.0f;
+}
+
+// Sends a SET_RPM command to the specified VESC.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_rpm(int vesc_id, float rpm) {
+ const int32_t rpm_int = rpm;
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x03 /* SET_RPM */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((rpm_int >> 24) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 16) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 8) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// Sends a SET_CURRENT command to the specified VESC.
+// current is in amps.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_current(int vesc_id, float current) {
+ constexpr float kMaxCurrent = 80.0f;
+ const int32_t current_int =
+ ::std::max(-kMaxCurrent, ::std::min(kMaxCurrent, current)) * 1000.0f;
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x01 /* SET_CURRENT */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((current_int >> 24) & 0xFF),
+ static_cast<uint8_t>((current_int >> 16) & 0xFF),
+ static_cast<uint8_t>((current_int >> 8) & 0xFF),
+ static_cast<uint8_t>((current_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// Sends a SET_DUTY command to the specified VESC.
+// duty is from -1 to 1.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_duty(int vesc_id, float duty) {
+ constexpr int32_t kMaxDuty = 99999;
+ const int32_t duty_int = ::std::max(
+ -kMaxDuty, ::std::min(kMaxDuty, static_cast<int32_t>(duty * 100000.0f)));
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x00 /* SET_DUTY */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((duty_int >> 24) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 16) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 8) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// TODO(Brian): Move these two test functions somewhere else.
+__attribute__((unused)) void DoVescTest() {
+ uint32_t time = micros();
+ while (true) {
+ for (int i = 0; i < 6; ++i) {
+ const uint32_t end = time_add(time, 500000);
+ while (true) {
+ const bool done = time_after(micros(), end);
+ float current;
+ if (done) {
+ current = -6;
+ } else {
+ current = 6;
+ }
+ vesc_set_current(i, current);
+ if (done) {
+ break;
+ }
+ delay(5);
+ }
+ time = end;
+ }
+ }
+}
+
+__attribute__((unused)) void DoReceiverTest2() {
+ static constexpr float kMaxRpm = 10000.0f;
+ while (true) {
+ const bool flip = convert_input_width(2) > 0;
+
+ {
+ const float value = convert_input_width(0);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(0, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(1, rpm);
+ }
+ }
+
+ {
+ const float value = convert_input_width(1);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(2, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(3, rpm);
+ }
+ }
+
+ {
+ const float value = convert_input_width(4);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(4, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(5, rpm);
+ }
+ }
+ // Give the CAN frames a chance to go out.
+ delay(5);
+ }
+}
+
+void SetupPwmFtm(BigFTM *ftm) {
+ ftm->MODE = FTM_MODE_WPDIS;
+ ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ ftm->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
+
+ // Can't change MOD according to the reference manual ("The Dual Edge Capture
+ // mode must be used with ... the FTM counter in Free running counter.").
+ ftm->MOD = 0xFFFF;
+
+ // Capturing rising edge.
+ ftm->C0SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C1SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C2SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C3SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C4SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C5SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C6SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C7SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ (void)ftm->STATUS;
+ ftm->STATUS = 0x00;
+
+ ftm->COMBINE = FTM_COMBINE_DECAP3 | FTM_COMBINE_DECAPEN3 |
+ FTM_COMBINE_DECAP2 | FTM_COMBINE_DECAPEN2 |
+ FTM_COMBINE_DECAP1 | FTM_COMBINE_DECAPEN1 |
+ FTM_COMBINE_DECAP0 | FTM_COMBINE_DECAPEN0;
+
+ // 34.95ms max period before it starts wrapping and being weird.
+ ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(4) /* Prescaler=32 */;
+
+ ftm->MODE &= ~FTM_MODE_WPDIS;
+}
+
+extern "C" void ftm0_isr() {
+ while (true) {
+ const uint32_t status = FTM0->STATUS;
+ if (status == 0) {
+ return;
+ }
+
+ if (status & (1 << 1)) {
+ const uint32_t start = FTM0->C0V;
+ const uint32_t end = FTM0->C1V;
+ pwm_input_widths[1] = (end - start) & 0xFFFF;
+ pwm_input_times[1] = millis();
+ }
+ if (status & (1 << 5)) {
+ const uint32_t start = FTM0->C4V;
+ const uint32_t end = FTM0->C5V;
+ pwm_input_widths[3] = (end - start) & 0xFFFF;
+ pwm_input_times[3] = millis();
+ }
+ if (status & (1 << 3)) {
+ const uint32_t start = FTM0->C2V;
+ const uint32_t end = FTM0->C3V;
+ pwm_input_widths[4] = (end - start) & 0xFFFF;
+ pwm_input_times[4] = millis();
+ }
+
+ FTM0->STATUS = 0;
+ }
+}
+
+extern "C" void ftm3_isr() {
+ while (true) {
+ const uint32_t status = FTM3->STATUS;
+ if (status == 0) {
+ return;
+ }
+
+ FTM3->STATUS = 0;
+ }
+}
+
+extern "C" void pit3_isr() {
+ PIT_TFLG3 = 1;
+ PolyDrivetrain<float> *polydrivetrain =
+ global_polydrivetrain.load(::std::memory_order_acquire);
+
+ const bool lost_drive_channel = lost_channel(3) || lost_channel(1);
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("0: %d 1: %d\n", (int)pwm_input_widths[3],
+ (int)pwm_input_widths[1]);
+ }
+ }
+
+ if (polydrivetrain != nullptr) {
+ DrivetrainQueue_Goal goal;
+ goal.control_loop_driving = false;
+ if (lost_drive_channel) {
+ goal.throttle = 0.0f;
+ goal.wheel = 0.0f;
+ } else {
+ goal.throttle = convert_input_width(1);
+ goal.wheel = -convert_input_width(3);
+ }
+ goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("throttle: %d wheel: %d\n", (int)(goal.throttle * 100),
+ (int)(goal.wheel * 100));
+ }
+ }
+
+ DrivetrainQueue_Output output;
+
+ polydrivetrain->SetGoal(goal);
+ polydrivetrain->Update(12.0f);
+ polydrivetrain->SetOutput(&output);
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("l: %d r: %d\n", (int)(output.left_voltage * 100),
+ (int)(output.right_voltage * 100));
+ }
+ }
+ vesc_set_duty(0, -output.left_voltage / 12.0f);
+ vesc_set_duty(1, -output.left_voltage / 12.0f);
+
+ vesc_set_duty(2, output.right_voltage / 12.0f);
+ vesc_set_duty(3, output.right_voltage / 12.0f);
+ }
+}
+
+} // namespace
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void);
+
+} // extern "C"
+
+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_USBOTG, 0x7);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0xa);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM3, 0xa);
+ NVIC_SET_SANE_PRIORITY(IRQ_PIT_CH3, 0x5);
+
+ // Builtin LED.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // PWM_IN0
+ // FTM0_CH1 (doesn't work)
+ // PORTC_PCR2 = PORT_PCR_MUX(4);
+
+ // PWM_IN1
+ // FTM0_CH0
+ PORTC_PCR1 = PORT_PCR_MUX(4);
+
+ // PWM_IN2
+ // FTM0_CH5 (doesn't work)
+ // PORTD_PCR5 = PORT_PCR_MUX(4);
+
+ // PWM_IN3
+ // FTM0_CH4
+ PORTD_PCR4 = PORT_PCR_MUX(4);
+
+ // PWM_IN4
+ // FTM0_CH2
+ PORTC_PCR3 = PORT_PCR_MUX(4);
+
+ // PWM_IN5
+ // FTM0_CH3 (doesn't work)
+ // PORTC_PCR4 = PORT_PCR_MUX(4);
+
+ delay(100);
+
+ PrintingParameters printing_parameters;
+ printing_parameters.dedicated_usb = true;
+ const ::std::unique_ptr<PrintingImplementation> printing =
+ CreatePrinting(printing_parameters);
+ printing->Initialize();
+
+ SIM_SCGC6 |= SIM_SCGC6_PIT;
+ // Workaround for errata e7914.
+ (void)PIT_MCR;
+ PIT_MCR = 0;
+ PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 200) - 1;
+ PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
+
+ can_init(0, 1);
+ SetupPwmFtm(FTM0);
+ SetupPwmFtm(FTM3);
+
+ PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr);
+ global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release);
+
+ // Leave the LED on for a bit longer.
+ delay(300);
+ printf("Done starting up\n");
+
+ // Done starting up, now turn the LED off.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
+
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+ NVIC_ENABLE_IRQ(IRQ_FTM3);
+ NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
+ printf("Done starting up2\n");
+
+ while (true) {
+ }
+
+ return 0;
+}
+
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/y2019/vision/undistort.py b/y2019/vision/undistort.py
new file mode 100755
index 0000000..9a35d9c
--- /dev/null
+++ b/y2019/vision/undistort.py
@@ -0,0 +1,135 @@
+#!/usr/bin/python
+
+import cv2
+import glob
+import math
+import numpy as np
+import sys
+"""
+Usage:
+ undistort.py [display]
+
+Finds files in /tmp/*.yuyv to compute distortion constants for.
+"""
+
+
+def undist(orig, mtx, dist, newcameramtx, its=1):
+ """
+ This function runs a manual undistort over the entire image to compare to the
+ golden as proof that the algorithm works and the generated constants are correct.
+ """
+ output = np.full(orig.shape, 255, dtype=np.uint8)
+ for i in range(480):
+ for j in range(640):
+ x0 = (i - mtx[1, 2]) / mtx[1, 1]
+ y0 = (j - mtx[0, 2]) / mtx[0, 0]
+ x = x0
+ y = y0
+ for k in range(its):
+ r2 = x * x + y * y
+ coeff = (1 + dist[0, 0] * r2 + dist[0, 1] * math.pow(r2, 2) +
+ dist[0, 4] * math.pow(r2, 3))
+ x = x0 / coeff
+ y = y0 / coeff
+ ip = x * newcameramtx[1, 1] + newcameramtx[1, 2]
+ jp = y * newcameramtx[0, 0] + newcameramtx[0, 2]
+ if ip < 0 or jp < 0 or ip >= 480 or jp >= 640:
+ continue
+ output[int(ip), int(jp)] = orig[i, j]
+ return output
+
+
+def main(argv):
+ # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+ objp = np.zeros((6 * 9, 3), np.float32)
+ objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
+
+ # Arrays to store object points and image points from all the images.
+ objpoints = [] # 3d point in real world space
+ imgpoints = [] # 2d points in image plane.
+
+ images = glob.glob('/tmp/*.yuyv')
+
+ cols = 640
+ rows = 480
+
+ # Iterate through all the available images
+ for fname in images:
+ fd = open(fname, 'rb')
+ f = np.fromfile(fd, np.uint8, cols * rows * 2)
+ # Convert yuyv color space to single channel grey.
+ grey = f[::2]
+ grey = np.reshape(grey, (rows, cols))
+
+ ret, corners = cv2.findChessboardCorners(grey, (9, 6), None)
+ if ret:
+ objpoints.append(objp)
+ imgpoints.append(corners)
+ # Draw the chessboard with corners marked.
+ if len(argv) > 1 and argv[1] == 'display':
+ rgb = cv2.cvtColor(grey, cv2.COLOR_GRAY2RGB)
+ cv2.drawChessboardCorners(rgb, (9, 6), corners, ret)
+ cv2.imshow('', rgb)
+ cv2.waitKey(0)
+ cv2.destroyAllWindows()
+ fd.close()
+
+ ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
+ objpoints, imgpoints, grey.shape[::-1], None, None)
+ newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (rows, cols),
+ 1, (rows, cols))
+
+ dist[0, 2] = 0
+ dist[0, 3] = 0
+ print("Formatted for Game Config:")
+ print("""distortion {
+ f_x: %f
+ c_x: %f
+ f_y: %f
+ c_y :%f
+ f_x_prime: %f
+ c_x_prime: %f
+ f_y_prime: %f
+ c_y_prime: %f
+ k_1: %f
+ k_2: %f
+ k_3: %f
+ distortion_iterations: 7
+}""" % (
+ # f_x c_x
+ mtx[0][0],
+ mtx[0][2],
+ # f_y c_y
+ mtx[1][1],
+ mtx[1][2],
+ # f_x c_x prime
+ newcameramtx[0][0],
+ newcameramtx[0][2],
+ # f_y c_y prime
+ newcameramtx[1][1],
+ newcameramtx[1][2],
+ # k_1, k_2, k_3
+ dist[0, 0],
+ dist[0, 1],
+ dist[0, 4]))
+
+ # Draw the original image, open-cv undistort, and our undistort in separate
+ # windows for each available image.
+ if len(argv) > 1 and argv[1] == 'display':
+ for fname in images:
+ fd = open(fname, 'rb')
+ f = np.fromfile(fd, np.uint8, cols * rows * 2)
+ grey_t = f[::2]
+ grey_t = np.reshape(grey_t, (rows, cols))
+ dst_expected = cv2.undistort(grey_t, mtx, dist, None, newcameramtx)
+ dst_actual = undist(grey_t, mtx, dist, newcameramtx, 5)
+ cv2.imshow('orig', grey_t)
+ cv2.imshow('opencv undistort', dst_expected)
+ cv2.imshow('our undistort', dst_actual)
+ cv2.waitKey(0)
+ cv2.destroyAllWindows()
+ fd.close()
+
+
+if __name__ == '__main__':
+ main(sys.argv)