Add new receiver code for new robot
Change-Id: I17e54e04ac075b2669745f5042a7a65483ec8b06
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/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/simpler_receiver.cc b/motors/simpler_receiver.cc
new file mode 100644
index 0000000..b3e5a36
--- /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