Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/medium_salsa.cc b/motors/medium_salsa.cc
index 1fbe9a6..144fc1e 100644
--- a/motors/medium_salsa.cc
+++ b/motors/medium_salsa.cc
@@ -2,15 +2,35 @@
#include <stdio.h>
+#include <atomic>
+
#include "motors/core/time.h"
+#include "motors/motor.h"
+#include "motors/motor_controls.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
#include "motors/usb/usb_serial.h"
#include "motors/util.h"
namespace frc971 {
namespace salsa {
+namespace {
+
+::std::atomic<Motor *> global_motor{nullptr};
extern "C" {
+
void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
extern void usb_init();
int _write(int file, char *ptr, int len) {
(void)file;
@@ -24,7 +44,12 @@
extern uint32_t __heap_start__[];
extern uint32_t __stack_end__[];
+void ftm0_isr(void) {
+ global_motor.load(::std::memory_order_relaxed)->HandleInterrupt();
+}
+
} // extern "C"
+} // namespace
extern "C" int main(void) {
// for background about this startup delay, please see these conversations
@@ -39,38 +64,63 @@
// 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, 0x3);
// Set the LED's pin to output mode.
GPIO_BITBAND(GPIOC_PDDR, 5) = 1;
PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ GPIO_BITBAND(GPIOA_PDDR, 15) = 1;
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ DMA_CR = DMA_CR_EMLM;
usb_serial_init();
usb_descriptor_set_product_id(0x0490);
usb_init();
+ AdcInit();
+ MathInit();
+ delay(1000);
+ can_init();
+
+ MotorControlsImplementation controls;
+
+ delay(1000);
+ Motor motor(FTM0, FTM1, &controls);
+ motor.Init();
+ global_motor.store(&motor, ::std::memory_order_relaxed);
+ // Output triggers to things like the PDBs on initialization.
+ FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
+ // Don't let any memory accesses sneak past here, because we actually
+ // need everything to be starting up.
+ __asm__("" :: : "memory");
// 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
- GPIOC_PSOR = 1 << 5;
+ printf("Going silent to zero motors...\n");
+ // Give the print a chance to make it out.
+ delay(1000);
+ motor.Zero();
+
+ printf("Zeroed motor!\n");
+ // Give stuff a chance to recover from interrupts-disabled.
+ delay(100);
+ motor.Start();
+
+ GPIOA_PCOR = 1 << 15;
+
+ // TODO(Brian): Use SLEEPONEXIT to reduce interrupt latency?
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 salsa
} // namespace frc971