Code for the motor controller
This is basically what we used in Detroit.
Change-Id: If2820d7ec5fcbc5f33b4082025027a6e969ad0e1
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
new file mode 100644
index 0000000..10fef28
--- /dev/null
+++ b/motors/peripheral/BUILD
@@ -0,0 +1,43 @@
+load("//tools:environments.bzl", "mcu_cpus")
+
+cc_library(
+ name = 'adc',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'adc.h',
+ ],
+ srcs = [
+ 'adc.cc',
+ ],
+ deps = [
+ ':configuration',
+ '//motors:util',
+ '//motors/core',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'configuration',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'configuration.h',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+cc_library(
+ name = 'can',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'can.h',
+ ],
+ srcs = [
+ 'can.c',
+ ],
+ deps = [
+ '//motors/core',
+ '//motors:util',
+ ],
+ restricted_to = mcu_cpus,
+)
diff --git a/motors/peripheral/adc.cc b/motors/peripheral/adc.cc
new file mode 100644
index 0000000..ba2b247
--- /dev/null
+++ b/motors/peripheral/adc.cc
@@ -0,0 +1,118 @@
+#include "motors/peripheral/adc.h"
+
+#include "motors/core/kinetis.h"
+
+namespace frc971 {
+namespace salsa {
+
+#define ADC_SC2_BASE (ADC_SC2_REFSEL(0) /* Use the external reference pins. */)
+
+#define ADC_FINISH_CALIBRATION(n, PM) \
+ do { \
+ uint16_t variable = 0; \
+ variable += ADC##n##_CL##PM##0; \
+ variable += ADC##n##_CL##PM##1; \
+ variable += ADC##n##_CL##PM##2; \
+ variable += ADC##n##_CL##PM##3; \
+ variable += ADC##n##_CL##PM##4; \
+ variable += ADC##n##_CL##PM##S; \
+ variable /= 2; \
+ variable |= 0x8000; \
+ ADC##n##_##PM##G = variable; \
+ } while (0);
+
+#define ADC_INIT_SINGLE(n) \
+ do { \
+ ADC##n##_CFG1 = ADC_CFG1_ADIV(2) /* Divide clock by 4 to get 15MHz. */ | \
+ ADC_CFG1_MODE(1) /* 12 bit mode. */ | \
+ ADC_CFG1_ADICLK(0) /* Use the bus clock (60MHz). */; \
+ ADC##n##_CFG2 = ADC_CFG2_MUXSEL /* Use the b channels. */ | \
+ ADC_CFG2_ADHSC /* Support higher ADC clock speeds. */; \
+ ADC##n##_SC1A = 0; /* Clear SC1A's COCO flag. */ \
+ ADC##n##_SC2 = ADC_SC2_BASE; \
+ do { \
+ ADC##n##_SC3 = ADC_SC3_CAL | ADC_SC3_AVGE | \
+ ADC_SC3_AVGS(3) /* Average 32 samples (max). */; \
+ /* Wait for calibration to complete. */ \
+ while (!(ADC##n##_SC1A & ADC_SC1_COCO)) { \
+ } \
+ } while (ADC##n##_SC3 & ADC_SC3_CALF); \
+ ADC_FINISH_CALIBRATION(n, P); \
+ ADC_FINISH_CALIBRATION(n, M); \
+ \
+ ADC##n##_SC3 = 0 /* Disable hardware averaging. */; \
+ } while (0)
+
+void AdcInit() {
+ 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
+ PORTC_PCR0 = PORT_PCR_MUX(0);
+
+ // M_CH0V/M1_CH0F ADC0_SE13
+ PORTB_PCR3 = PORT_PCR_MUX(0);
+
+ // M_CH1V/M1_CH1F ADC0_SE12
+ PORTB_PCR2 = PORT_PCR_MUX(0);
+
+ // M0_CH0F/M_CH0F ADC1_SE14
+ PORTB_PCR10 = PORT_PCR_MUX(0);
+
+ // M0_CH1F/M_CH1F ADC1_SE15
+ PORTB_PCR11 = PORT_PCR_MUX(0);
+
+ // WHEEL_ABS/M_VREF ADC0_SE18
+ PORTE_PCR25 = PORT_PCR_MUX(0);
+
+ // VIN/VIN ADC1_SE5B
+ PORTC_PCR9 = PORT_PCR_MUX(0);
+
+ // M0_CH2F/M_CH2F ADC1_SE17
+ PORTA_PCR17 = PORT_PCR_MUX(0);
+}
+
+MediumAdcReadings AdcReadMedium() {
+ MediumAdcReadings r;
+
+ ADC1_SC1A = 14;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 15;
+ r.motor_currents[0][0] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 17;
+ ADC0_SC1A = 18;
+ r.motor_currents[1][0] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 5;
+ r.motor_currents[2][0] = ADC1_RA;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_current_ref = ADC0_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 14;
+ r.input_voltage = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 15;
+ r.motor_currents[0][1] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ ADC1_SC1A = 17;
+ r.motor_currents[1][1] = ADC1_RA;
+ while (!(ADC1_SC1A & ADC_SC1_COCO)) {
+ }
+ r.motor_currents[2][1] = ADC1_RA;
+
+ return r;
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/peripheral/adc.h b/motors/peripheral/adc.h
new file mode 100644
index 0000000..4aee769
--- /dev/null
+++ b/motors/peripheral/adc.h
@@ -0,0 +1,22 @@
+#ifndef MOTORS_PERIPHERAL_ADC_H_
+#define MOTORS_PERIPHERAL_ADC_H_
+
+#include <stdint.h>
+
+namespace frc971 {
+namespace salsa {
+
+struct MediumAdcReadings {
+ uint16_t motor_currents[3][2];
+ uint16_t motor_current_ref;
+ uint16_t input_voltage;
+};
+
+void AdcInit();
+
+MediumAdcReadings AdcReadMedium();
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_PERIPHERAL_ADC_H_
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
new file mode 100644
index 0000000..7187ac9
--- /dev/null
+++ b/motors/peripheral/can.c
@@ -0,0 +1,189 @@
+#include "motors/peripheral/can.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#include "motors/core/kinetis.h"
+#include "motors/util.h"
+
+#include <stdio.h>
+#include <inttypes.h>
+
+// General note: this peripheral is really weird about accessing its memory. It
+// goes much farther than normal memory-mapped device semantics. In particular,
+// it "locks" various regions of memory under complicated conditions. Because of
+// this, all the code in here touching the device memory is fairly paranoid
+// about how it does that.
+
+// The number of message buffers we're actually going to use. The chip only has
+// 16. Using fewer means less for the CAN module (and CPU) to go through looking
+// for actual data.
+// 0 is for sending and 1 is for receiving commands.
+#define NUMBER_MESSAGE_BUFFERS 2
+
+#if NUMBER_MESSAGE_BUFFERS > 16
+#error Only have 16 message buffers on this part.
+#endif
+
+// TODO(Brian): Do something about CAN errors and warnings (enable interrupts?).
+
+// Flags for the interrupt to process which don't actually come from the
+// hardware. Currently, only used for tx buffers.
+static volatile uint32_t can_manual_flags = 0;
+
+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;
+
+ // Put it into freeze mode and wait for it to actually do that.
+ // Don't OR these bits in because it starts in module-disable mode, which
+ // isn't what we want. It will ignore the attempt to change some of the bits
+ // because it's not in freeze mode, but whatever.
+ CAN0_MCR = CAN_MCR_FRZ | CAN_MCR_HALT;
+ while (!(CAN0_MCR & CAN_MCR_FRZACK)) {}
+
+ // Initializing this before touching the mailboxes because the reference
+ // manual slightly implies you have to, and the registers and RAM on this
+ // thing are weird (get locked sometimes) so it actually might matter.
+ CAN0_MCR =
+ CAN_MCR_FRZ | CAN_MCR_HALT /* Stay in freeze mode. */ |
+ CAN_MCR_SRXDIS /* Don't want to see our own frames at all. */ |
+ CAN_MCR_IRMQ /* Use individual masks for each filter. */ |
+ CAN_MCR_LPRIOEN /* Let us prioritize TX mailboxes. */ |
+ (0 << 8) /* No need to pack IDs tightly, so it's easier not to. */ |
+ (NUMBER_MESSAGE_BUFFERS - 1);
+
+ // Initialize all the buffers and RX filters we're enabling.
+
+ // Just in case this does anything...
+ CAN0_RXIMRS[0] = 0;
+ CAN0_MESSAGES[0].prio_id = 0;
+ CAN0_MESSAGES[0].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE) | CAN_MB_CONTROL_IDE;
+
+ CAN0_RXIMRS[1] = (1 << 31) /* Want to filter out RTRs. */ |
+ (1 << 30) /* Want to only get extended frames. */ |
+ 0xFF /* Filter on the 1-byte VESC id. */;
+ CAN0_MESSAGES[1].prio_id = 0;
+ CAN0_MESSAGES[1].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY) | CAN_MB_CONTROL_IDE;
+
+ // Using the oscillator clock directly because it's a reasonable frequency and
+ // 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.
+ 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. */ |
+ CAN_CTRL1_PSEG1(7) /* 8 time quanta before sampling. */ |
+ CAN_CTRL1_PSEG2(1) /* 2 time quanta after sampling. */ |
+ CAN_CTRL1_SMP /* Use triple sampling. */ |
+ CAN_CTRL1_PROPSEG(4) /* 5 time quanta before sampling. */;
+ CAN0_CTRL2 = CAN_CTRL2_TASD(25) /* We have so few mailboxes and */
+ /* such a fast peripheral clock, this has lots of margin. */ |
+ CAN_CTRL2_EACEN /* Match on IDE and RTR. */;
+
+ // Enable interrupts for the RX mailbox.
+ CAN0_IMASK1 = 1 << 1;
+
+ // Now take it out of freeze mode.
+ CAN0_MCR &= ~CAN_MCR_HALT;
+
+ //NVIC_ENABLE_IRQ(IRQ_CAN_MESSAGE);
+}
+
+static void can_vesc_process_rx(volatile CanMessageBuffer *buffer,
+ unsigned char *data_out, int *length_out) {
+ // Wait until the buffer is marked as not being busy. The reference manual
+ // says to do this, although it's unclear how we could get an interrupt
+ // asserted while it's still busy. Maybe if the interrupt was slow and now
+ // it's being overwritten?
+ uint32_t control_timestamp;
+ do {
+ control_timestamp = buffer->control_timestamp;
+ } while (control_timestamp & CAN_MB_CONTROL_CODE_BUSY_MASK);
+ // The message buffer is now locked, so it won't be modified by the hardware.
+
+ const uint32_t prio_id = buffer->prio_id;
+ // Making sure to access the data 32 bits at a time, copy it out. It's
+ // ambiguous whether you're allowed to access the individual bytes, and this
+ // memory is weird enough to not make sense risking it. Also, it's only 2
+ // cycles, which is pretty hard to beat by doing anything with the length...
+ // Also, surprise!: the hardware stores the data big-endian.
+ uint32_t data[2];
+ data[0] = __builtin_bswap32(buffer->data[0]);
+ data[1] = __builtin_bswap32(buffer->data[1]);
+
+ // Yes, it might actually matter that we clear the interrupt flag before
+ // unlocking it...
+ CAN0_IFLAG1 = 1 << (buffer - CAN0_MESSAGES);
+
+ // Now read the timer to unlock the message buffer. Want to do this ASAP
+ // rather than waiting until we get to processing the next buffer, plus we
+ // might want to write to the next one, which results in weird, bad things.
+ {
+ uint16_t dummy = CAN0_TIMER;
+ (void)dummy;
+ }
+
+ // The message buffer is now unlocked and "serviced", but its control word
+ // code is still CAN_MB_CODE_RX_FULL. However, said code will stay
+ // CAN_MB_CODE_RX_FULL the next time a message is received into it (the code
+ // won't change to CAN_MB_CODE_RX_OVERRUN because it has been "serviced").
+ // Yes, really...
+
+ memcpy(data_out, data, 8);
+ *length_out = CAN_MB_CONTROL_EXTRACT_DLC(control_timestamp);
+ (void)prio_id;
+}
+
+int can_send(uint32_t can_id, const unsigned char *data, unsigned int length) {
+ volatile CanMessageBuffer *const message_buffer = &CAN0_MESSAGES[0];
+
+ if (CAN_MB_CONTROL_EXTRACT_CODE(message_buffer->control_timestamp) ==
+ CAN_MB_CODE_TX_DATA) {
+ return -1;
+ }
+
+ // Yes, it might actually matter that we clear the interrupt flag before
+ // doing stuff...
+ CAN0_IFLAG1 = 1 << (message_buffer - CAN0_MESSAGES);
+ message_buffer->prio_id = can_id;
+ // Copy only the bytes from data that we're supposed to onto the stack, and
+ // then move it into the message buffer 32 bits at a time (because it might
+ // get unhappy about writing individual bytes). Plus, we have to byte-swap
+ // each 32-bit word because this hardware is weird...
+ {
+ uint32_t data_words[2] = {0, 0};
+ for (uint8_t *dest = (uint8_t *)&data_words[0];
+ dest - (uint8_t *)&data_words[0] < (ptrdiff_t)length; ++dest) {
+ *dest = *data;
+ ++data;
+ }
+ message_buffer->data[0] = __builtin_bswap32(data_words[0]);
+ message_buffer->data[1] = __builtin_bswap32(data_words[1]);
+ }
+ message_buffer->control_timestamp =
+ CAN_MB_CONTROL_INSERT_DLC(length) | CAN_MB_CONTROL_SRR |
+ CAN_MB_CONTROL_IDE | CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_DATA);
+ return 0;
+}
+
+void can_receive_command(unsigned char *data, int *length) {
+ if (0) {
+ static int i = 0;
+ if (i++ == 13) {
+ printf("IFLAG1=%" PRIx32 " ESR=%" PRIx32 " ESR1=%" PRIx32 "\n",
+ CAN0_IFLAG1, CAN0_ECR, CAN0_ESR1);
+ i = 0;
+ }
+ }
+ if ((CAN0_IFLAG1 & (1 << 1)) == 0) {
+ *length = -1;
+ return;
+ }
+ can_vesc_process_rx(&CAN0_MESSAGES[1], data, length);
+}
diff --git a/motors/peripheral/can.h b/motors/peripheral/can.h
new file mode 100644
index 0000000..acd6cce
--- /dev/null
+++ b/motors/peripheral/can.h
@@ -0,0 +1,24 @@
+#ifndef PERIPHERAL_CAN_H_
+#define PERIPHERAL_CAN_H_
+
+#include <stdint.h>
+
+// The code defined here calls functions in vesc/vesc_can.h from various
+// interrupts and expects them to call back into this file to do something.
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void can_init(void);
+
+int can_send(uint32_t can_id, const unsigned char *data, unsigned int length);
+
+// Sets *length to -1 if there isn't a new piece of data to receive.
+void can_receive_command(unsigned char *data, int *length);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // PERIPHERAL_CAN_H_
diff --git a/motors/peripheral/configuration.h b/motors/peripheral/configuration.h
new file mode 100644
index 0000000..3fa686a
--- /dev/null
+++ b/motors/peripheral/configuration.h
@@ -0,0 +1,24 @@
+#ifndef MOTORS_PERIPHERAL_CONFIGURATION_H_
+#define MOTORS_PERIPHERAL_CONFIGURATION_H_
+
+// We're just going to leave the default DMA priorities which correspond to the
+// channel numbers and fixed priority mode, so channel 0 is the lowest priority
+// and 15 is the highest.
+// We're also going to leave DMA_CR alone except for setting EMLM.
+
+// The frequency of the peripheral bus(es) in hz.
+#define BUS_CLOCK_FREQUENCY (F_CPU / 2)
+
+// The frequency we switch the motor FETs at in hz.
+#define SWITCHING_FREQUENCY 20000
+
+#if 0
+// Which PDB the ADC triggering uses.
+#define ADC_TRIGGER_PDB 0
+// The DMA channel which copies ADC results.
+#define ADC_RESULT_DMA_CHANNEL 7
+// The DMA channel which reconfigures the ADCs to take the next samples.
+#define ADC_RECONFIGURE_DMA_CHANNEL 8
+#endif
+
+#endif // MOTORS_PERIPHERAL_CONFIGURATION_H_