Pull the medium-specific pieces out of the common code
Change-Id: I9cd1824a6bf535e285553fbe5f79f6d72919f048
diff --git a/motors/BUILD b/motors/BUILD
index e75f74b..fd53dd0 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -20,6 +20,7 @@
cc_library(
name = 'motor',
+ visibility = ['//visibility:public'],
hdrs = [
'motor.h',
],
@@ -34,7 +35,7 @@
'//motors/peripheral:adc',
'//motors/peripheral:can',
- '//motors/usb:legacy',
+ '//motors/usb:cdc',
],
restricted_to = mcu_cpus,
)
@@ -96,6 +97,7 @@
cc_library(
name = 'math',
+ visibility = ['//visibility:public'],
hdrs = [
'math.h',
],
diff --git a/motors/medium_salsa.cc b/motors/medium_salsa.cc
index 144fc1e..2a9804a 100644
--- a/motors/medium_salsa.cc
+++ b/motors/medium_salsa.cc
@@ -45,10 +45,104 @@
extern uint32_t __stack_end__[];
void ftm0_isr(void) {
- global_motor.load(::std::memory_order_relaxed)->HandleInterrupt();
+ MediumAdcReadings adc_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ adc_readings = AdcReadMedium(disable_interrupts);
+ }
+ ReadingsToBalance to_balance{{0, 0, 0}, {0, 0, 0}};
+ {
+ for (int reading = 0; reading < 2; ++reading) {
+ for (int phase = 0; phase < 3; ++phase) {
+ to_balance.Add(phase, adc_readings.motor_currents[phase][reading]);
+ }
+ }
+ }
+ const BalancedReadings balanced = BalanceReadings(to_balance);
+
+ global_motor.load(::std::memory_order_relaxed)->HandleInterrupt(
+ balanced,
+ global_motor.load(::std::memory_order_relaxed)->wrapped_encoder());
}
} // 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->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 */;
+
+ // Set the deadtime.
+ pwm_ftm->DEADTIME =
+ FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
+}
+
+// Zeros the encoder. This involves blocking for an arbitrary length of time
+// with interrupts disabled.
+void ZeroMotor() {
+#if 0
+ while (true) {
+ if (GPIO_BITBAND(GPIOE_PDIR, 24)) {
+ 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"(&GPIO_BITBAND(GPIOE_PDIR, 24)),
+ [cnt] "l"(&FTM1->CNT));
+ __enable_irq();
+#endif
+}
+
} // namespace
extern "C" int main(void) {
@@ -73,19 +167,50 @@
GPIO_BITBAND(GPIOA_PDDR, 15) = 1;
PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // Set up the CAN pins.
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
DMA_CR = DMA_CR_EMLM;
usb_serial_init();
usb_descriptor_set_product_id(0x0490);
usb_init();
- AdcInit();
+ AdcInitMedium();
MathInit();
delay(1000);
can_init();
+ GPIOD_PCOR = 1 << 3;
+ GPIO_BITBAND(GPIOD_PDDR, 3) = 1;
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ delay(1000);
+ GPIOD_PSOR = 1 << 3;
+ delay(1000);
+ GPIOD_PCOR = 1 << 3;
+ delay(1000);
+
MotorControlsImplementation controls;
delay(1000);
- Motor motor(FTM0, FTM1, &controls);
+
+ // Index pin
+ PORTE_PCR24 = 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.
@@ -97,27 +222,49 @@
// Give everything a chance to get going.
delay(100);
-#if 0
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__);
-#endif
printf("Going silent to zero motors...\n");
// Give the print a chance to make it out.
delay(1000);
- motor.Zero();
+ ZeroMotor();
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;
- GPIOA_PCOR = 1 << 15;
+ float current_command = 0;
+ while (true) {
+ unsigned char command_data[8];
+ int command_length;
+ can_receive_command(command_data, &command_length);
+ if (command_length == 4) {
+ uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
+ command_data[2] << 8 | command_data[3];
+ float current = static_cast<float>(result) / 1000.0f;
- // TODO(Brian): Use SLEEPONEXIT to reduce interrupt latency?
- while (true) {}
+ static bool high_gear = false;
+ if (controls.estimated_velocity() < -2015) {
+ high_gear = true;
+ }
+ if (current < 1) {
+ high_gear = false;
+ }
+ if (!high_gear) {
+ current = current_command * -120.0f / 120.0f;
+ } else {
+ current = current_command * 115.0f / 120.0f;
+ }
+ motor.SetGoalCurrent(current);
+ current_command = current;
+ }
+ }
return 0;
}
diff --git a/motors/motor.cc b/motors/motor.cc
index 06f9f08..d966e6a 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -1,66 +1,25 @@
#include "motors/motor.h"
#include <limits.h>
+#include <stdio.h>
+#include <inttypes.h>
#include <array>
#include "motors/peripheral/configuration.h"
-
-#include <stdio.h>
-#include <inttypes.h>
-#include "motors/core/time.h"
-#include "motors/usb/usb_serial.h"
#include "motors/peripheral/can.h"
+extern "C" float analog_ratio(uint16_t reading);
+
namespace frc971 {
namespace salsa {
-namespace {
-constexpr int kDeadtimeCompensation = 9;
-
-uint32_t CalculateOnTime(uint32_t width, bool flip) {
- if (width > 0) {
- width += kDeadtimeCompensation;
- if (flip) {
- width += 1;
- }
- }
- return 1500 - width / 2;
-}
-
-uint32_t CalculateOffTime(uint32_t width, bool flip) {
- if (width > 0) {
- width += kDeadtimeCompensation;
- if (!flip) {
- width += 1;
- }
- }
- return 1500 + width / 2;
-}
-
-} // namespace
-
-Motor::Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls)
- : pwm_ftm_(pwm_ftm), encoder_ftm_(encoder_ftm), controls_(controls) {}
-
-static_assert((BUS_CLOCK_FREQUENCY % SWITCHING_FREQUENCY) == 0,
- "Switching frequency needs to divide the bus clock frequency");
-
-static_assert(BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY < UINT16_MAX,
- "Need to prescale");
-
-void Motor::Init() {
- PORTE_PCR24 = PORT_PCR_MUX(1);
- PORTB_PCR0 = PORT_PCR_MUX(6);
- PORTB_PCR1 = PORT_PCR_MUX(6);
-
- PORTC_PCR1 = PORT_PCR_MUX(4);
- PORTC_PCR2 = PORT_PCR_MUX(4);
- PORTC_PCR3 = PORT_PCR_MUX(4);
- PORTC_PCR4 = PORT_PCR_MUX(4);
- PORTD_PCR4 = PORT_PCR_MUX(4);
- PORTD_PCR5 = PORT_PCR_MUX(4);
-
+Motor::Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls,
+ const ::std::array<volatile uint32_t *, 3> &output_registers)
+ : pwm_ftm_(pwm_ftm),
+ encoder_ftm_(encoder_ftm),
+ controls_(controls),
+ output_registers_(output_registers) {
// PWMSYNC doesn't matter because we set SYNCMODE down below.
pwm_ftm_->MODE = FTM_MODE_WPDIS;
pwm_ftm_->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
@@ -71,28 +30,20 @@
encoder_ftm_->SC =
FTM_SC_CLKS(1) /* Use the system clock (not sure it matters) */ |
FTM_SC_PS(0) /* Don't prescale the clock (not sure it matters) */;
+}
+static_assert((BUS_CLOCK_FREQUENCY % SWITCHING_FREQUENCY) == 0,
+ "Switching frequency needs to divide the bus clock frequency");
+
+static_assert(BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY < UINT16_MAX,
+ "Need to prescale");
+
+void Motor::Init() {
pwm_ftm_->CNTIN = encoder_ftm_->CNTIN = 0;
pwm_ftm_->CNT = encoder_ftm_->CNT = 0;
- pwm_ftm_->MOD = (BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY) - 1;
- encoder_ftm_->MOD = controls_->counts_per_revolution() - 1;
-
- // Put them all into combine active-high mode, and all the low ones staying on
- // all the time by default.
- // TODO: update comment
- 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_->MOD = counts_per_cycle() - 1;
+ encoder_ftm_->MOD = controls_->mechanical_counts_per_revolution() - 1;
// I think you have to set this to something other than 0 for the quadrature
// encoder mode to actually work? This is "input capture on rising edge only",
@@ -101,32 +52,10 @@
encoder_ftm_->C1SC = FTM_CSC_ELSA;
// Initialize all the channels to 0.
- // TODO(Brian): Is this really what we want?
pwm_ftm_->OUTINIT = 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 */;
-
- // Set a deadtime of 500ns.
- // TODO: update comment
- pwm_ftm_->DEADTIME =
- FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
-
- // All of the channels are active high.
+ // All of the channels are active high (low-side ones with separate high/low
+ // drives are defaulted on elsewhere).
pwm_ftm_->POL = 0;
encoder_ftm_->FILTER = FTM_FILTER_CH0FVAL(0) /* No filter */ |
@@ -163,138 +92,57 @@
}
}
-void Motor::Zero() {
-#if 0
- while (true) {
- if (GPIO_BITBAND(GPIOE_PDIR, 24)) {
- 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"(&GPIO_BITBAND(GPIOE_PDIR, 24)),
- [cnt] "l"(&encoder_ftm_->CNT));
- __enable_irq();
-#endif
-}
-
void Motor::Start() {
pwm_ftm_->SC = FTM_SC_TOIE /* Interrupt on overflow */ |
FTM_SC_CLKS(1) /* Use the system clock */ |
FTM_SC_PS(0) /* Don't prescale the clock */;
pwm_ftm_->MODE &= ~FTM_MODE_WPDIS;
encoder_ftm_->MODE &= ~FTM_MODE_WPDIS;
-
- NVIC_ENABLE_IRQ(IRQ_FTM0);
}
-void Motor::HandleInterrupt() {
- const uint32_t starting_sc = pwm_ftm_->SC;
- pwm_ftm_->SC = starting_sc & ~FTM_SC_TOF;
+#define USE_ABSOLUTE_CUTOFF 0
+#define DO_CONTROLS 1
+#define DO_FIXED_PULSE 0
- const uint32_t start_count = pwm_ftm_->CNT;
- __asm__("":::"memory");
- const MediumAdcReadings adc_readings = AdcReadMedium();
+#define USE_CUTOFF 1
+#define PRINT_READINGS 0
+#define PRINT_ALL_READINGS 0
+#define TAKE_SAMPLE 0
+#define SAMPLE_UNTIL_DONE 0
+#define DO_STEP_RESPONSE 0
+#define DO_PULSE_SWEEP 0
+#define PRINT_TIMING 0
- // Turn the light on if we're starting too late (this generally means a
- // previous iteration took too long).
- if (start_count > 100) {
- GPIOC_PSOR = 1 << 5;
- }
+void Motor::HandleInterrupt(const BalancedReadings &balanced,
+ uint32_t captured_wrapped_encoder) {
+ pwm_ftm_->SC &= ~FTM_SC_TOF;
- ReadingsToBalance to_balance{{0, 0, 0}, {0, 0, 0}};
- {
- for (int reading = 0; reading < 2; ++reading) {
- for (int phase = 0; phase < 3; ++phase) {
- to_balance.Add(phase, adc_readings.motor_currents[phase][reading]);
- }
- }
- }
- const BalancedReadings balanced = BalanceReadings(to_balance);
-
- static float current_command = 0;
- static uint32_t last_command_receive_time = 0;
- {
- unsigned char command_data[8];
- int command_length;
- can_receive_command(command_data, &command_length);
- if (command_length == 4) {
- last_command_receive_time = micros();
- uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
- command_data[2] << 8 | command_data[3];
- current_command = static_cast<float>(result) / 1000.0f;
- }
- }
- if (!time_after(time_add(last_command_receive_time, 100000), micros())) {
- current_command = 0;
- }
-
- static bool high_gear = false;
- if (controls_->estimated_velocity() < -2015) {
- high_gear = true;
- }
- if (current_command < 1) {
- high_gear = false;
- }
- float current_now = current_command;
- if (!high_gear) {
- current_now = current_now * -120.0f / 120.0f;
- } else {
- current_now = current_now * 115.0f / 120.0f;
- }
-
-#if 0
- static int status_send_counter = 0;
- if (++status_send_counter == 1000) {
- // can_send(uint32_t can_id, const unsigned char *data, unsigned int length)
- unsigned char send_data[8];
- can_send(9 << 8, send_data, 0);
- status_send_counter = 0;
- printf("sent\n");
- }
+#if PRINT_TIMING
+ const uint32_t start_nanos = nanos();
#endif
-#define DO_CONTROLS 1
+ if (!time_after(time_add(last_current_set_time_, 100000), micros())) {
+ goal_current_ = 0;
+ }
+
#if DO_CONTROLS
- static constexpr int kEncoderOffset = 810;
- const uint32_t adjusted_count = (encoder_ftm_->CNT + kEncoderOffset) % 1024;
const ::std::array<uint32_t, 3> switching_points =
- controls_->DoIteration(balanced.readings, adjusted_count, current_now);
- constexpr uint32_t kMax = 2945;
+ controls_->DoIteration(balanced.readings, captured_wrapped_encoder, goal_current_);
+#if USE_CUTOFF
+ //constexpr uint32_t kMax = 2945;
+ constexpr uint32_t kMax = 1445;
static bool done = false;
bool done_now = false;
if (switching_points[0] > kMax || switching_points[1] > kMax ||
switching_points[2] > kMax) {
done_now = true;
}
-#define USE_ABSOLUTE_CUTOFF 1
#if USE_ABSOLUTE_CUTOFF
static unsigned int current_done_count = 0;
bool current_done = false;
for (int phase = 0; phase < 3; ++phase) {
const float scaled_reading =
- MotorControls::scale_current_reading(balanced.readings[0]);
+ controls_->scale_current_reading(balanced.readings[phase]);
static constexpr float kMaxBalancedCurrent = 190.0f;
if (scaled_reading > kMaxBalancedCurrent ||
scaled_reading < -kMaxBalancedCurrent) {
@@ -314,12 +162,6 @@
printf("done now\n");
printf("switching_points %" PRIu32 " %" PRIu32 " %" PRIu32 "\n",
switching_points[0], switching_points[1], switching_points[2]);
- printf("raw %" PRIu16 " %" PRIu16 " %" PRIu16 " %" PRIu16 " %" PRIu16
- " %" PRIu16 "\n",
- adc_readings.motor_currents[0][0], adc_readings.motor_currents[0][1],
- adc_readings.motor_currents[1][0], adc_readings.motor_currents[1][1],
- adc_readings.motor_currents[2][0],
- adc_readings.motor_currents[2][1]);
printf("balanced %" PRIu16 " %" PRIu16 " %" PRIu16 "\n",
static_cast<uint16_t>(balanced.readings[0]),
static_cast<uint16_t>(balanced.readings[1]),
@@ -327,63 +169,85 @@
done = true;
}
if (!done) {
- pwm_ftm_->C0V = CalculateOnTime(switching_points[0], flip_time_offset_);
- pwm_ftm_->C1V = CalculateOffTime(switching_points[0], flip_time_offset_);
- pwm_ftm_->C2V = CalculateOnTime(switching_points[1], flip_time_offset_);
- pwm_ftm_->C3V = CalculateOffTime(switching_points[1], flip_time_offset_);
- pwm_ftm_->C4V = CalculateOnTime(switching_points[2], flip_time_offset_);
- pwm_ftm_->C5V = CalculateOffTime(switching_points[2], flip_time_offset_);
+#else
+ if (true) {
+#endif
+ output_registers_[0][0] = CalculateOnTime(switching_points[0]);
+ output_registers_[0][2] = CalculateOffTime(switching_points[0]);
+ output_registers_[1][0] = CalculateOnTime(switching_points[1]);
+ output_registers_[1][2] = CalculateOffTime(switching_points[1]);
+ output_registers_[2][0] = CalculateOnTime(switching_points[2]);
+ output_registers_[2][2] = CalculateOffTime(switching_points[2]);
flip_time_offset_ = !flip_time_offset_;
} else {
- pwm_ftm_->C0V = 0;
- pwm_ftm_->C1V = 0;
- pwm_ftm_->C2V = 0;
- pwm_ftm_->C3V = 0;
- pwm_ftm_->C4V = 0;
- pwm_ftm_->C5V = 0;
+ output_registers_[0][0] = 0;
+ output_registers_[0][2] = 0;
+ output_registers_[1][0] = 0;
+ output_registers_[1][2] = 0;
+ output_registers_[2][0] = 0;
+ output_registers_[2][2] = 0;
}
-#define DO_FIXED_PULSE 0
#elif DO_FIXED_PULSE
- // An on-width of 60 with 30V in means about 50A through the motor and about
- // 30W total power dumped by the motor.
-#if 0
- static int i = 0;
- ++i;
- if (i == 2) {
- pwm_ftm_->C3V = 111;
- i = 0;
- } else {
- pwm_ftm_->C3V = 0;
+ // 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 = -500000;
+ 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;
+ }
}
-#endif
- pwm_ftm_->C0V = 0;
- pwm_ftm_->C1V = 0;
- pwm_ftm_->C2V = 0;
- //pwm_ftm_->C3V = 100;
- pwm_ftm_->C4V = 0;
- pwm_ftm_->C5V = 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 = 60;
+ output_registers_[0][0] = 0;
+ output_registers_[0][2] = phase_to_fire == 0 ? kPhaseFireWidth : 0;
+ output_registers_[1][0] = 0;
+ output_registers_[1][2] = phase_to_fire == 1 ? kPhaseFireWidth : 0;
+ output_registers_[2][0] = 0;
+ output_registers_[2][2] = phase_to_fire == 2 ? kPhaseFireWidth : 0;
#endif
-#define PRINT_READINGS 0
#if PRINT_READINGS
static int i = 0;
- if (i == 100) {
+ if (i == 1000) {
i = 0;
- printf("i=%" PRIu16 "\n", adc_readings.input_voltage);
- } else if (i == 20) {
- printf("0=%" PRIu16 " r=%" PRIu16 "\n",
- adc_readings.motor_currents[0][0], adc_readings.motor_current_ref);
- } else if (i == 40) {
- printf("1=%" PRIu16 " r=%" PRIu16 "\n",
- adc_readings.motor_currents[1][0], adc_readings.motor_current_ref);
- } else if (i == 60) {
- printf("2=%" PRIu16 " r=%" PRIu16 "\n",
- adc_readings.motor_currents[2][0], adc_readings.motor_current_ref);
+ SmallInitReadings readings;
+ {
+ DisableInterrupts disable_interrupts;
+ readings = AdcReadSmallInit(disable_interrupts);
+ }
+ printf(
+ "enc %d %d %d\n", captured_wrapped_encoder,
+ static_cast<int>((1.0f - analog_ratio(readings.motor0_abs)) * 7000.0f),
+ static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
+ } else if (i == 200) {
+#ifdef DO_CONTROLS
+ printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", switching_points[0],
+ switching_points[1], switching_points[2]);
+#else
+ 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);
}
++i;
-#define PRINT_ALL_READINGS 0
#elif PRINT_ALL_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",
@@ -391,7 +255,6 @@
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]);
-#define TAKE_SAMPLE 1
#elif TAKE_SAMPLE
#if 0
constexpr int kStartupWait = 50000;
@@ -403,26 +266,27 @@
constexpr int kStartupWait = 2 * 20000;
#endif
constexpr int kSubsampling = 1;
- constexpr int kPoints = 5000;
+ //constexpr int kPoints = 5000;
+ constexpr int kPoints = 1000;
constexpr int kSamplingEnd = kStartupWait + kPoints * kSubsampling;
(void)kSamplingEnd;
static int j = 0;
static int16_t data[kPoints][11];
static int written = 0;
+ static bool done_writing = false;
static_assert((kStartupWait % kSubsampling) == 0, "foo");
static_assert((kPoints % kSubsampling) == 0, "foo");
if (j < kStartupWait) {
// Wait to be started up.
++j;
-#define SAMPLE_UNTIL_DONE 0
-#if !SAMPLE_UNTIL_DONE
- } else if (j < kSamplingEnd && (j % kSubsampling) == 0) {
-#else
+#if SAMPLE_UNTIL_DONE
} else if (!done) {
+#else
+ } else if (j < kSamplingEnd && (j % kSubsampling) == 0) {
#endif
{
const int index = ((j - kStartupWait) / kSubsampling) % kPoints;
- auto point = data[index];
+ auto &point = data[index];
#if 0
point[0] = adc_readings.motor_currents[0][0];
point[1] = adc_readings.motor_currents[1][0];
@@ -431,9 +295,9 @@
point[4] = adc_readings.motor_currents[1][1];
point[5] = adc_readings.motor_currents[2][1];
#else
- point[0] = balanced.readings[0] / 2;
- point[1] = balanced.readings[1] / 2;
- point[2] = balanced.readings[2] / 2;
+ point[0] = balanced.readings[0];
+ point[1] = balanced.readings[1];
+ point[2] = balanced.readings[2];
#if 1
point[3] = controls_->Debug(0);
point[4] = controls_->Debug(1);
@@ -444,113 +308,139 @@
point[9] = controls_->Debug(6);
point[10] = controls_->Debug(7);
#else
+#if 0
point[3] = adc_readings.motor_currents[0][0];
point[4] = adc_readings.motor_currents[1][0];
point[5] = adc_readings.motor_currents[2][0];
point[6] = adc_readings.motor_currents[0][1];
point[7] = adc_readings.motor_currents[1][1];
point[8] = adc_readings.motor_currents[2][1];
- point[9] = temp1;
- point[10] = temp2;
+#else
+ point[3] = 0;
+ point[4] = 0;
+ point[5] = 0;
+ point[6] = 0;
+ point[7] = 0;
+ point[8] = 0;
#endif
- (void)temp1;
- (void)temp2;
+ point[9] = pwm_ftm_->C2V;
+ point[10] = pwm_ftm_->C3V;
+#endif
#if 0
point[3] = pwm_ftm_->C1V - pwm_ftm_->C0V;
point[4] = pwm_ftm_->C3V - pwm_ftm_->C2V;
point[5] = pwm_ftm_->C5V - pwm_ftm_->C4V;
#endif
#endif
-#if 0
- point[6] = adc_readings.motor_current_ref;
- point[7] = adc_readings.input_voltage;
-#else
-#endif
+ point[9] = captured_wrapped_encoder;
+ SmallInitReadings readings;
+ {
+ DisableInterrupts disable_interrupts;
+ readings = AdcReadSmallInit(disable_interrupts);
+ }
+ point[10] = readings.motor0_abs;
}
-#define DO_STEP_RESPONSE 0
#if DO_STEP_RESPONSE
// Step response
- if (j > 25000) {
- pwm_ftm_->C1V = 20;
+ if (j > kStartupWait + 200 / kSubsampling) {
+ pwm_ftm_->C3V = 240;
}
-#define DO_PULSE_SWEEP 0
#elif DO_PULSE_SWEEP
// Sweep the pulse through the ADC sampling points.
static constexpr int kMax = 2500;
static constexpr int kExtraWait = 1500;
if (j > kStartupWait && j < kStartupWait + kExtraWait) {
- pwm_ftm_->C4V = 0;
- pwm_ftm_->C5V = 60;
+ pwm_ftm_->C2V = 0;
+ pwm_ftm_->C3V = 240;
} else if (j < kStartupWait + kMax + kExtraWait) {
uint32_t start = j - kStartupWait - kExtraWait;
- pwm_ftm_->C4V = start;
- pwm_ftm_->C5V = start + 60;
+ pwm_ftm_->C2V = start;
+ pwm_ftm_->C3V = start + 240;
} else {
- pwm_ftm_->C4V = 0;
- pwm_ftm_->C5V = 0;
+ pwm_ftm_->C2V = 0;
+ pwm_ftm_->C3V = 0;
}
#endif
++j;
-#if !SAMPLE_UNTIL_DONE
+#if SAMPLE_UNTIL_DONE
+ } else if (false) {
+#else
} else if (j < kSamplingEnd) {
++j;
} else if (j == kSamplingEnd) {
-#else
- } else if (false) {
#endif
printf("finished\n");
++j;
-#if !SAMPLE_UNTIL_DONE
- } else {
-#else
+#if SAMPLE_UNTIL_DONE
} else if (done) {
+#else
+ } else {
#endif
// Time to write the data out.
- if (written < (int)sizeof(data)) {
+ if (written < (int)sizeof(data) && debug_tty_ != nullptr) {
int to_write = sizeof(data) - written;
-#if 1
- if (to_write > 20) {
- to_write = 20;
+ if (to_write > 64) {
+ to_write = 64;
}
- // TODO(Brian): Fix usb_serial_write so we can write more than 1 byte at a
- // time.
- if (to_write > 1) {
- to_write = 1;
- }
- int result =
- usb_serial_write(1, ((const char *)data) + written, to_write);
-#else
- if (to_write > 8) {
- to_write = 8;
- }
- int result =
- can_send(97, ((const unsigned char *)data) + written, to_write);
-#endif
+ int result = debug_tty_->Write(((const char *)data) + written, to_write);
if (result >= 0) {
- written += to_write;
+ written += result;
} else {
- //printf("error\n");
+ printf("error\n");
}
- if (written == (int)sizeof(data)) {
- printf("done writing %d\n", written);
- }
+ }
+ if (!done_writing && written >= (int)sizeof(data) &&
+ debug_tty_->write_queue_empty()) {
+ printf("done writing %d\n", written);
+ done_writing = true;
}
}
#endif
- (void)adc_readings;
- (void)start_count;
(void)balanced;
// 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 PRINT_TIMING
+ static int print_timing_count = 0;
+ static uint32_t print_timing_total = 0;
+ print_timing_total += time_subtract(nanos(), start_nanos);
+ if (++print_timing_count == 1000) {
+ printf("took %" PRIu32 "/%d\n", print_timing_total, print_timing_count);
+ print_timing_count = 0;
+ print_timing_total = 0;
+ }
+#endif
+
// If another cycle has already started, turn the light on right now.
if (pwm_ftm_->SC & FTM_SC_TOF) {
GPIOC_PSOR = 1 << 5;
}
}
+uint32_t Motor::CalculateOnTime(uint32_t width) const {
+ if (width > 0) {
+ width += deadtime_compensation_;
+ if (flip_time_offset_) {
+ width += 1;
+ }
+ }
+ return (counts_per_cycle() - width) / 2;
+}
+
+uint32_t Motor::CalculateOffTime(uint32_t width) const {
+ if (width > 0) {
+ width += deadtime_compensation_;
+ if (!flip_time_offset_) {
+ width += 1;
+ }
+ }
+ return (counts_per_cycle() + width) / 2;
+}
+
} // namespace salsa
} // namespace frc971
diff --git a/motors/motor.h b/motors/motor.h
index f5b731f..8161f5e 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -8,7 +8,10 @@
#include "motors/algorithms.h"
#include "motors/core/kinetis.h"
#include "motors/peripheral/adc.h"
+#include "motors/peripheral/configuration.h"
#include "motors/util.h"
+#include "motors/usb/cdc.h"
+#include "motors/core/time.h"
namespace frc971 {
namespace salsa {
@@ -21,16 +24,14 @@
MotorControls(const MotorControls &) = delete;
void operator=(const MotorControls &) = delete;
- static constexpr float scale_current_reading(float reading) {
- 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 */);
- }
+ // Scales a current reading from ADC units to amps.
+ //
+ // Note that this doesn't apply any offset. The common offset will be
+ // automatically removed as part of the balancing process.
+ virtual float scale_current_reading(float reading) const = 0;
- static constexpr int counts_per_revolution() { return 2048 / 2; }
+ virtual int mechanical_counts_per_revolution() const = 0;
+ virtual int electrical_counts_per_revolution() const = 0;
// raw_currents are in amps for each phase.
// theta is in electrical counts, which will be less than
@@ -45,32 +46,63 @@
};
// Controls a single motor.
-class Motor {
+class Motor final {
public:
// pwm_ftm is used to drive the PWM outputs.
// encoder_ftm is used for reading the encoder.
- Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls);
+ Motor(BigFTM *pwm_ftm, LittleFTM *encoder_ftm, MotorControls *controls,
+ const ::std::array<volatile uint32_t *, 3> &output_registers);
Motor(const Motor &) = delete;
void operator=(const Motor &) = delete;
+ void set_debug_tty(teensy::AcmTty *debug_tty) { debug_tty_ = debug_tty; }
+ void set_deadtime_compensation(int deadtime_compensation) {
+ deadtime_compensation_ = deadtime_compensation;
+ }
+ void set_switching_divisor(int switching_divisor) {
+ switching_divisor_ = switching_divisor;
+ }
+ void set_encoder_offset(int encoder_offset) {
+ encoder_offset_ = encoder_offset;
+ // Add mechanical_counts_per_revolution to the offset so that when we mod
+ // below, we are guaranteed to be > 0 regardless of the encoder multiplier.
+ // % isn't well-defined with negative numbers.
+ while (encoder_offset_ < controls_->mechanical_counts_per_revolution()) {
+ encoder_offset_ += controls_->mechanical_counts_per_revolution();
+ }
+ }
+ void set_encoder_multiplier(int encoder_multiplier) {
+ encoder_multiplier_ = encoder_multiplier;
+ }
+
+ int encoder() {
+ return encoder_multiplier_ * encoder_ftm_->CNT;
+ }
+ uint32_t wrapped_encoder() {
+ return (encoder() + encoder_offset_) %
+ controls_->mechanical_counts_per_revolution();
+ }
+
// Sets up everything but doesn't actually start the timers.
//
// This assumes the global time base configuration happens outside so the
// timers for both motors (if applicable) are synced up.
- // TODO(Brian): "40.4.28.1 Enabling the global time base (GTB)" says something
- // about needing to do stuff in a specific order, so this API might need
- // revising.
void Init();
- // Zeros the encoder. This involves blocking for an arbitrary length of time
- // with interrupts disabled.
- void Zero();
-
// Starts the timers.
+ //
+ // If the global time base is in use, it must be activated after this.
void Start();
- void HandleInterrupt();
+ void HandleInterrupt(const BalancedReadings &readings,
+ uint32_t captured_wrapped_encoder);
+
+ void SetGoalCurrent(float goal_current) {
+ DisableInterrupts disable_interrupts;
+ goal_current_ = goal_current;
+ last_current_set_time_ = micros();
+ }
private:
// Represents the ADC reading which is closest to an edge.
@@ -98,11 +130,28 @@
int index = 0;
};
+ inline int counts_per_cycle() const {
+ return BUS_CLOCK_FREQUENCY / SWITCHING_FREQUENCY / switching_divisor_;
+ }
+
+ uint32_t CalculateOnTime(uint32_t width) const;
+ uint32_t CalculateOffTime(uint32_t width) const;
+
bool flip_time_offset_ = false;
+ int deadtime_compensation_ = 0;
BigFTM *const pwm_ftm_;
LittleFTM *const encoder_ftm_;
MotorControls *const controls_;
+ const ::std::array<volatile uint32_t *, 3> output_registers_;
+
+ float goal_current_ = 0;
+ uint32_t last_current_set_time_ = 0;
+ int switching_divisor_ = 1;
+ int encoder_offset_ = 0;
+ int encoder_multiplier_ = 1;
+
+ teensy::AcmTty *debug_tty_ = nullptr;
};
} // namespace salsa
diff --git a/motors/motor_controls.cc b/motors/motor_controls.cc
index 187a781..029f98d 100644
--- a/motors/motor_controls.cc
+++ b/motors/motor_controls.cc
@@ -11,7 +11,8 @@
using Complex = ::std::complex<float>;
-constexpr int kCountsPerRevolution = MotorControls::counts_per_revolution();
+constexpr int kCountsPerRevolution =
+ MotorControlsImplementation::constant_counts_per_revolution();
#if 1
constexpr double kMaxDutyCycle = 0.98;
@@ -169,10 +170,10 @@
const ComplexMatrix<3, 1> E1 =
E1Unrotated_ *
- ImaginaryExpInt<::std::ratio<1, counts_per_revolution()>>(theta);
+ ImaginaryExpInt<::std::ratio<1, constant_counts_per_revolution()>>(theta);
const ComplexMatrix<3, 1> E2 =
E2Unrotated_ *
- ImaginaryExpInt<::std::ratio<5, counts_per_revolution()>>(theta);
+ ImaginaryExpInt<::std::ratio<5, constant_counts_per_revolution()>>(theta);
const float overall_measured_current =
((E1 + E2).real().transpose() * measured_current /
@@ -191,10 +192,12 @@
const ComplexMatrix<3, 3> H1 = Hn(omega, 1);
const ComplexMatrix<3, 3> H2 = Hn(omega, 5);
- const ComplexMatrix<3, 1> p_imaginary = H1 * E1 + H2 * E2;
+ const ComplexMatrix<3, 1> H1E1 = H1 * E1;
+ const ComplexMatrix<3, 1> H2E2 = H2 * E2;
+ const ComplexMatrix<3, 1> p_imaginary = H1E1 + H2E2;
const ComplexMatrix<3, 1> p_next_imaginary =
- ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * H1 * E1 +
- ImaginaryExpFloat(omega * 5 / SWITCHING_FREQUENCY) * H2 * E2;
+ ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * H1E1 +
+ ImaginaryExpFloat(omega * 5 / SWITCHING_FREQUENCY) * H2E2;
const ::Eigen::Matrix<float, 3, 1> p = p_imaginary.real();
const ::Eigen::Matrix<float, 3, 1> p_next = p_next_imaginary.real();
diff --git a/motors/motor_controls.h b/motors/motor_controls.h
index e0fa28c..1f19e6d 100644
--- a/motors/motor_controls.h
+++ b/motors/motor_controls.h
@@ -20,6 +20,23 @@
MotorControlsImplementation();
~MotorControlsImplementation() override = default;
+ static constexpr int constant_counts_per_revolution() { return 1024; }
+
+ int mechanical_counts_per_revolution() const override {
+ return constant_counts_per_revolution();
+ }
+ 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 */);
+ }
+
::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
uint32_t theta,
const float command_current) override;
diff --git a/motors/peripheral/adc.cc b/motors/peripheral/adc.cc
index ba2b247..0f5d947 100644
--- a/motors/peripheral/adc.cc
+++ b/motors/peripheral/adc.cc
@@ -4,6 +4,7 @@
namespace frc971 {
namespace salsa {
+namespace {
#define ADC_SC2_BASE (ADC_SC2_REFSEL(0) /* Use the external reference pins. */)
@@ -43,39 +44,79 @@
ADC##n##_SC3 = 0 /* Disable hardware averaging. */; \
} while (0)
-void AdcInit() {
+void AdcInitCommon() {
SIM_SCGC3 |= SIM_SCGC3_ADC1;
SIM_SCGC6 |= SIM_SCGC6_ADC0;
// TODO(Brian): Mess with SIM_SOPT7 to reconfigure ADC trigger input source?
ADC_INIT_SINGLE(0);
ADC_INIT_SINGLE(1);
+}
- // M_CH2V/M1_CH2F ADC0_SE14
+} // namespace
+
+void AdcInitMedium() {
+ AdcInitCommon();
+
+ // M_CH2V ADC0_SE14
PORTC_PCR0 = PORT_PCR_MUX(0);
- // M_CH0V/M1_CH0F ADC0_SE13
+ // M_CH0V ADC0_SE13
PORTB_PCR3 = PORT_PCR_MUX(0);
- // M_CH1V/M1_CH1F ADC0_SE12
+ // M_CH1V ADC0_SE12
PORTB_PCR2 = PORT_PCR_MUX(0);
- // M0_CH0F/M_CH0F ADC1_SE14
+ // M_CH0F ADC1_SE14
PORTB_PCR10 = PORT_PCR_MUX(0);
- // M0_CH1F/M_CH1F ADC1_SE15
+ // M_CH1F ADC1_SE15
PORTB_PCR11 = PORT_PCR_MUX(0);
- // WHEEL_ABS/M_VREF ADC0_SE18
+ // M_VREF ADC0_SE18
PORTE_PCR25 = PORT_PCR_MUX(0);
- // VIN/VIN ADC1_SE5B
+ // VIN ADC1_SE5B
PORTC_PCR9 = PORT_PCR_MUX(0);
- // M0_CH2F/M_CH2F ADC1_SE17
+ // M_CH2F ADC1_SE17
PORTA_PCR17 = PORT_PCR_MUX(0);
}
-MediumAdcReadings AdcReadMedium() {
+void AdcInitSmall() {
+ AdcInitCommon();
+
+ // M0_CH0F ADC1_SE17
+ PORTA_PCR17 = PORT_PCR_MUX(0);
+
+ // M0_CH1F ADC1_SE14
+ PORTB_PCR10 = PORT_PCR_MUX(0);
+
+ // M0_CH2F ADC1_SE15
+ PORTB_PCR11 = PORT_PCR_MUX(0);
+
+ // M0_ABS ADC0_SE5b
+ PORTD_PCR1 = PORT_PCR_MUX(0);
+
+ // M1_CH0F ADC0_SE13
+ PORTB_PCR3 = PORT_PCR_MUX(0);
+
+ // M1_CH1F ADC0_SE12
+ PORTB_PCR2 = PORT_PCR_MUX(0);
+
+ // M1_CH2F ADC0_SE14
+ PORTC_PCR0 = PORT_PCR_MUX(0);
+
+ // M1_ABS ADC0_SE17
+ PORTE_PCR24 = PORT_PCR_MUX(0);
+
+ // WHEEL_ABS ADC0_SE18
+ PORTE_PCR25 = PORT_PCR_MUX(0);
+
+ // VIN ADC1_SE5B
+ PORTC_PCR9 = PORT_PCR_MUX(0);
+}
+
+MediumAdcReadings AdcReadMedium(const DisableInterrupts &) {
MediumAdcReadings r;
ADC1_SC1A = 14;
@@ -114,5 +155,62 @@
return r;
}
+SmallAdcReadings AdcReadSmall0(const DisableInterrupts &) {
+ SmallAdcReadings r;
+
+ ADC1_SC1A = 17;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 14;
+ r.currents[0] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 15;
+ r.currents[1] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ r.currents[2] = ADC1_RA;
+
+ return r;
+}
+
+SmallAdcReadings AdcReadSmall1(const DisableInterrupts &) {
+ SmallAdcReadings r;
+
+ ADC0_SC1A = 13;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 12;
+ r.currents[0] = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 14;
+ r.currents[1] = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.currents[2] = ADC0_RA;
+
+ return r;
+}
+
+SmallInitReadings AdcReadSmallInit(const DisableInterrupts &) {
+ SmallInitReadings r;
+
+ ADC0_SC1A = 5;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 17;
+ r.motor0_abs = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC0_SC1A = 18;
+ r.motor1_abs = ADC0_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.wheel_abs = ADC0_RA;
+
+ return r;
+}
+
} // namespace salsa
} // namespace frc971
diff --git a/motors/peripheral/adc.h b/motors/peripheral/adc.h
index 4aee769..f9ec22a 100644
--- a/motors/peripheral/adc.h
+++ b/motors/peripheral/adc.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+#include "motors/util.h"
+
namespace frc971 {
namespace salsa {
@@ -12,9 +14,23 @@
uint16_t input_voltage;
};
-void AdcInit();
+struct SmallAdcReadings {
+ uint16_t currents[3];
+};
-MediumAdcReadings AdcReadMedium();
+struct SmallInitReadings {
+ uint16_t motor0_abs;
+ uint16_t motor1_abs;
+ uint16_t wheel_abs;
+};
+
+void AdcInitMedium();
+void AdcInitSmall();
+
+MediumAdcReadings AdcReadMedium(const DisableInterrupts &);
+SmallAdcReadings AdcReadSmall0(const DisableInterrupts &);
+SmallAdcReadings AdcReadSmall1(const DisableInterrupts &);
+SmallInitReadings AdcReadSmallInit(const DisableInterrupts &);
} // namespace salsa
} // namespace frc971
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index 7187ac9..0e731e2 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -33,8 +33,6 @@
void can_init(void) {
printf("can_init\n");
- PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(2);
- PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0;
@@ -175,7 +173,7 @@
void can_receive_command(unsigned char *data, int *length) {
if (0) {
static int i = 0;
- if (i++ == 13) {
+ if (i++ == 10000) {
printf("IFLAG1=%" PRIx32 " ESR=%" PRIx32 " ESR1=%" PRIx32 "\n",
CAN0_IFLAG1, CAN0_ECR, CAN0_ESR1);
i = 0;
diff --git a/motors/pistol_grip/drivers_station.cc b/motors/pistol_grip/drivers_station.cc
index 26f171e..ecc5347 100644
--- a/motors/pistol_grip/drivers_station.cc
+++ b/motors/pistol_grip/drivers_station.cc
@@ -132,10 +132,10 @@
return 0;
}
-} // extern "C"
-
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
diff --git a/motors/usb/cdc.h b/motors/usb/cdc.h
index 4f5aa33..00eec4c 100644
--- a/motors/usb/cdc.h
+++ b/motors/usb/cdc.h
@@ -49,6 +49,8 @@
size_t Read(void *buffer, size_t buffer_size);
size_t Write(const void *buffer, size_t buffer_size);
+ bool write_queue_empty() const { return tx_queue_.empty(); }
+
private:
enum class NextEndpoint0Out {
kNone,
diff --git a/motors/usb/usb.cc b/motors/usb/usb.cc
index fb7fe9f..36c6456 100644
--- a/motors/usb/usb.cc
+++ b/motors/usb/usb.cc
@@ -91,6 +91,8 @@
} // namespace
+constexpr int UsbDevice::kEndpoint0MaxSize;
+
UsbDevice::UsbDevice(int index, uint16_t vendor_id, uint16_t product_id)
: index_(index) {
// TODO(Brian): Pass index_ into all the register access macros. Also sort out