Pull the medium-specific pieces out of the common code
Change-Id: I9cd1824a6bf535e285553fbe5f79f6d72919f048
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