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