| // This file has the main for the Teensy on the button board. |
| |
| // ButtonBoard is the main outline of how we communicate with the teensy, along |
| // with what we tell it to do |
| |
| // ButtonBoardBB1 and ButtonBoardBB2 only differ in what teensy pins manage what |
| // on the pcb |
| // teensy (3.5) schematic: https://www.pjrc.com/teensy/schematic.html |
| // datasheets for the chip (MK64FX512) used in the teensy: |
| // https://www.pjrc.com/teensy/datasheets.html |
| // comments (especially on BB2) inform which pins are used |
| |
| #include "motors/driver_station.h" |
| |
| #include <inttypes.h> |
| #include <stdio.h> |
| |
| #include <atomic> |
| #include <cmath> |
| |
| #include "motors/core/kinetis.h" |
| #include "motors/core/time.h" |
| #include "motors/peripheral/adc.h" |
| #include "motors/peripheral/can.h" |
| #include "motors/print/print.h" |
| #include "motors/usb/cdc.h" |
| #include "motors/usb/hid.h" |
| #include "motors/usb/usb.h" |
| #include "motors/util.h" |
| |
| namespace frc971 { |
| namespace motors { |
| namespace { |
| |
| // The HID report descriptor we use. |
| constexpr char kReportDescriptor1[] = { |
| 0x05, 0x01, // Usage Page (Generic Desktop), |
| 0x09, 0x04, // Usage (Joystick), |
| 0xA1, 0x01, // Collection (Application), |
| 0x75, 0x08, // Report Size (8), |
| 0x95, 0x05, // Report Count (5), |
| 0x15, 0x00, // Logical Minimum (0), |
| 0x26, 0xFF, 0x00, // Logical Maximum (255), |
| 0x35, 0x00, // Physical Minimum (0), |
| 0x46, 0xFF, 0x00, // Physical Maximum (255), |
| 0x09, 0x30, // Usage (X), |
| 0x09, 0x31, // Usage (Y), |
| 0x09, 0x32, // Usage (Z), |
| 0x09, 0x33, // Usage (Rx), |
| 0x09, 0x34, // Usage (Ry), |
| 0x81, 0x02, // Input (Variable), |
| 0x75, 0x01, // Report Size (1), |
| 0x95, 0x10, // Report Count (16), |
| 0x25, 0x01, // Logical Maximum (1), |
| 0x45, 0x01, // Physical Maximum (1), |
| 0x05, 0x09, // Usage Page (Button), |
| 0x19, 0x01, // Usage Minimum (01), |
| 0x29, 0x10, // Usage Maximum (16), |
| 0x81, 0x02, // Input (Variable), |
| 0xC0 // End Collection |
| }; |
| } // namespace |
| |
| bool IsMyProcessorId(uint32_t id[4]) { |
| uint32_t my_uuid[4] = {SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL}; |
| |
| for (int byte_index = 0; byte_index < 4; byte_index++) { |
| if (id[byte_index] != my_uuid[byte_index]) { |
| return false; |
| } |
| } |
| |
| return true; |
| } |
| |
| uint8_t ProcessorIndex() { |
| static uint32_t kPistolgripProcessorIds[PROCESSOR_ID_COUNT][4] = { |
| {0x00000000, 0x00000000, 0x00000000, 0x00000000}, |
| {0x0036FFFF, 0xFFFFFFFF, 0x4E457285, |
| 0x60110022}, // One with cable tie labels |
| {0x0035FFFF, 0xFFFFFFFF, 0x4E457285, 0x60130022}, |
| }; |
| |
| for (int i = 0; i < PROCESSOR_ID_COUNT; i++) { |
| if (IsMyProcessorId(kPistolgripProcessorIds[i])) { |
| return i; |
| } |
| } |
| |
| return 0; |
| } |
| |
| uint16_t TareEncoder(ENCODER_DATA_S *encoder_data) { |
| return static_cast<uint16_t>((static_cast<uint32_t>(encoder_data->angle) - |
| static_cast<uint32_t>(encoder_data->enc_trim) + |
| 0x800) & |
| 0xfff); |
| } |
| |
| // Scale and center controller readings. |
| // |
| // The encoder has a larger range of motion in the + direction than the - |
| // direction. We scale the readings so that min (-) maps to 0x0000, max (+) maps |
| // to 0xFFFF and the center is at 0x8000. |
| uint16_t ScaleEncoder(ENCODER_DATA_S *encoder_data, uint16_t adjusted_angle) { |
| uint16_t result = 0x8000; |
| if (adjusted_angle > 0x800) { |
| const float scaled_angle = |
| static_cast<float>(static_cast<int>(adjusted_angle) - 0x800) / |
| static_cast<float>(encoder_data->enc_max - 0x800); |
| result += std::min<int>(0xffff - 0x8000, |
| static_cast<int>(scaled_angle * (0xffff - 0x8000))); |
| } else { |
| const float scaled_angle = |
| static_cast<float>(static_cast<int>(adjusted_angle) - 0x800) / |
| static_cast<float>(0x800 - encoder_data->enc_min); |
| result -= std::min<int>(0x8000, static_cast<int>(-scaled_angle * 0x8000)); |
| } |
| |
| return result; |
| } |
| |
| uint16_t filterIIR(uint32_t *filterValue, uint16_t currentValue, |
| uint16_t filterDepth, bool initFilter) { |
| uint32_t localValue = 0; |
| |
| if (initFilter) { |
| *filterValue = currentValue << filterDepth; |
| return currentValue; |
| } |
| |
| localValue = *filterValue - (*filterValue >> filterDepth); |
| |
| localValue += currentValue; |
| |
| *filterValue = localValue; |
| |
| return localValue >> filterDepth; |
| } |
| |
| int DriverStation::DetermineEncoderValues(ENCODER_DATA_S *enc, |
| ABS_POSITION_S *absAngle, |
| int encoderNum, |
| uint16_t resetTime_ms) { |
| uint16_t currentAngle = 0; |
| |
| if (enc->angle > ENCODER_COUNTS_PER_REV) { |
| enc->angle = 0; |
| } |
| |
| if (ReadQuadrature(encoderNum, ¤tAngle)) { |
| return -1; |
| } |
| |
| if (MeasureAbsPosition(encoderNum, absAngle)) { |
| return -1; |
| } |
| |
| if (!absAngle->intialized) { |
| return 0; |
| } |
| |
| enc->resetTimer_ms++; |
| if (abs((int)(((currentAngle + enc->offset) & ENCODER_MOD) - enc->angle)) > |
| 0) { |
| enc->resetTimer_ms = 0; |
| (void)filterIIR(&(enc->angle_filter), absAngle->dutycycle, |
| ENCODER_FILTER_EXPONENT, true); |
| } |
| |
| if (abs((int)(enc->angle - absAngle->dutycycle)) > MAX_FILTER_DELTA && |
| enc->resetTimer_ms > 0) { |
| (void)filterIIR(&(enc->angle_filter), absAngle->dutycycle, |
| ENCODER_FILTER_EXPONENT, true); |
| } |
| |
| if (enc->resetTimer_ms > resetTime_ms) { |
| enc->filtered_duty_cycle = |
| filterIIR(&(enc->angle_filter), absAngle->dutycycle, |
| ENCODER_FILTER_EXPONENT, false); |
| enc->offset = |
| enc->filtered_duty_cycle - |
| currentAngle; /* offset is calculated using filtered abs_position*/ |
| enc->offset &= ENCODER_MOD; |
| enc->resetTimer_ms = resetTime_ms; |
| } |
| |
| enc->angle = currentAngle + enc->offset; |
| |
| enc->angle &= ENCODER_MOD; |
| |
| return 0; |
| } |
| |
| int DriverStation::MeasureAbsPosition(uint32_t encoder_id, |
| ABS_POSITION_S *abs_position) { |
| BigFTM *ftm = NULL; |
| volatile uint32_t *volatile stat_ctrl0 = NULL; // status and control |
| volatile uint32_t *volatile stat_ctrl1 = NULL; |
| volatile uint32_t *volatile channel_val0 = NULL; // channel value |
| volatile uint32_t *volatile channel_val1 = NULL; |
| uint32_t decap_mask = 0; |
| uint32_t decap_enable = 0; |
| uint32_t channel_filter = 0; |
| |
| uint32_t initial = 0; |
| uint32_t final = 0; |
| |
| switch (encoder_id) { |
| case 0: |
| ftm = FTM3; |
| stat_ctrl0 = &(FTM3_C0SC); |
| stat_ctrl1 = &(FTM3_C1SC); |
| channel_val0 = &(FTM3_C0V); |
| channel_val1 = &(FTM3_C1V); |
| decap_mask = FTM_COMBINE_DECAP0; |
| decap_enable = FTM_COMBINE_DECAPEN0; |
| channel_filter = FTM_FILTER_CH0FVAL(0); |
| |
| break; |
| case 1: |
| ftm = FTM0; |
| stat_ctrl0 = &(FTM0_C6SC); |
| stat_ctrl1 = &(FTM0_C7SC); |
| channel_val0 = &(FTM0_C6V); |
| channel_val1 = &(FTM0_C7V); |
| decap_mask = FTM_COMBINE_DECAP3; |
| decap_enable = FTM_COMBINE_DECAPEN3; |
| channel_filter = FTM_FILTER_CH3FVAL(0); |
| |
| break; |
| |
| default: |
| return -1; |
| break; |
| } |
| |
| switch (abs_position->state) { |
| case INIT_ABS: |
| ftm->MODE = FTM_MODE_WPDIS; |
| ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; |
| |
| ftm->CNTIN = 0; |
| ftm->MOD = 0xFFFF; |
| |
| // Disable filters |
| ftm->FILTER = channel_filter; |
| |
| ftm->COMBINE = decap_enable; |
| ftm->COMBINE = decap_enable | decap_mask; |
| |
| // Use PWM decoding rising and falling edges in CH0 and rising edges only |
| // in CH1. |
| *stat_ctrl0 = FTM_CSC_ELSA; |
| *stat_ctrl1 = FTM_CSC_ELSA; |
| *channel_val0 = 0; |
| *channel_val1 = 0; |
| |
| ftm->SYNCONF = |
| FTM_SYNCONF_SWWRBUF /* Software trigger flushes MOD */ | |
| FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ | |
| FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */; |
| |
| ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */; |
| |
| // Wait for the software synchronization to finish. |
| while (ftm->SYNC & FTM_SYNC_SWSYNC) { |
| } |
| |
| ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ | |
| FTM_SC_PS(4) /* Prescaler=16 */; |
| |
| ftm->MODE &= ~FTM_MODE_WPDIS; |
| abs_position->state = START_PERIOD; |
| break; |
| |
| case START_PERIOD: |
| if ((ftm->COMBINE & decap_mask) != decap_mask) { |
| ftm->COMBINE = decap_enable | decap_mask; |
| *stat_ctrl0 = FTM_CSC_ELSA; |
| *stat_ctrl1 = FTM_CSC_ELSA; |
| abs_position->state = WAIT_PERIOD_DONE; |
| } |
| break; |
| |
| case WAIT_PERIOD_DONE: |
| initial = *channel_val0; |
| final = *channel_val1; |
| if ((*stat_ctrl0 & FTM_CSC_CHF) && (*stat_ctrl1 & FTM_CSC_CHF)) { |
| // Add 0x10000 to correct for rollover |
| abs_position->period = (final - initial) & 0xFFFF; |
| |
| *stat_ctrl0 &= ~FTM_CSC_CHF; |
| *stat_ctrl1 &= ~FTM_CSC_CHF; |
| |
| abs_position->state = START_WIDTH; |
| } |
| break; |
| |
| case START_WIDTH: |
| if ((ftm->COMBINE & decap_mask) != decap_mask) { |
| ftm->COMBINE = decap_enable | decap_mask; |
| *stat_ctrl0 = FTM_CSC_ELSA; |
| *stat_ctrl1 = FTM_CSC_ELSB; |
| abs_position->state = WAIT_WIDTH_DONE; |
| } |
| break; |
| |
| case WAIT_WIDTH_DONE: |
| initial = *channel_val0; |
| final = *channel_val1; |
| if ((*stat_ctrl0 & FTM_CSC_CHF) && (*stat_ctrl1 & FTM_CSC_CHF)) { |
| // Add 0x10000 to correct for rollover |
| abs_position->width = (final - initial) & 0xFFFF; |
| |
| *stat_ctrl0 &= ~FTM_CSC_CHF; |
| *stat_ctrl1 &= ~FTM_CSC_CHF; |
| |
| if (abs_position->period != 0) { |
| if (((abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period) > ENCODER_MOD) { |
| abs_position->state = START_PERIOD; |
| break; |
| } |
| |
| // Handle duty cycle out of range. Reset at next reasonable |
| // measurement. |
| if (abs_position->last_erronious_dutycycle == |
| (abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period) { |
| abs_position->dutycycle = |
| (abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period; |
| |
| abs_position->state = START_PERIOD; |
| break; |
| } |
| |
| if (abs((int)((abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period) - |
| (int)abs_position->dutycycle) > MAX_DUTY_CYCLE_DELTA) { |
| abs_position->last_erronious_dutycycle = |
| (abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period; |
| |
| abs_position->state = START_PERIOD; |
| break; |
| } |
| |
| abs_position->dutycycle = |
| (abs_position->width * ENCODER_COUNTS_PER_REV) / |
| abs_position->period; |
| abs_position->intialized = true; |
| } else { |
| abs_position->period = 0xFFFF; |
| } |
| |
| abs_position->state = START_PERIOD; |
| } |
| break; |
| |
| default: |
| return -1; |
| break; |
| } |
| |
| return 0; |
| } |
| |
| std::array<ENCODER_DATA_S, 2> MakeEncoderData(uint32_t processor_index) { |
| switch (processor_index) { |
| case 0: |
| default: |
| return {ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0, |
| .enc_min = 0, |
| .enc_max = 0, |
| }, |
| ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0, |
| .enc_min = 0, |
| .enc_max = 0, |
| }}; |
| case 1: |
| // Both enc_min and enc_max should come from the "trimmed" enc print |
| // enc_min should be < 0x7FF and enc_max > 0x800 |
| // for encoder 0 they should be ~ +/- 0x050 and encoder 1 ~ +/- 0x700 from |
| // center |
| return {ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0x0568, |
| .enc_min = 0x0741, |
| .enc_max = 0x08EB, |
| }, |
| ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0x0987, |
| .enc_min = 0x00EA, |
| .enc_max = 0x0FD5, |
| }}; |
| case 2: |
| return {ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0x02CD, |
| .enc_min = 0x0746, |
| .enc_max = 0x0900, |
| }, |
| ENCODER_DATA_S{ |
| .angle = 0, |
| .offset = 0, |
| .resetTimer_ms = 0, |
| .filtered_duty_cycle = 0, |
| .angle_filter = 0, |
| .enc_trim = 0x0589, |
| .enc_min = 0x00FF, |
| .enc_max = 0x0FC0, |
| }}; |
| } |
| } |
| |
| void DriverStation::SendJoystickData(teensy::HidFunction *joystick0, |
| teensy::HidFunction *joystick1, |
| teensy::HidFunction *joystick2, |
| uint32_t processor_index) { |
| std::array<ENCODER_DATA_S, NUM_ENCODERS> encoder_data = |
| MakeEncoderData(processor_index); |
| |
| static ABS_POSITION_S abs_position[NUM_ENCODERS]; |
| memset(abs_position, 0, NUM_ENCODERS * sizeof(ABS_POSITION_S)); |
| |
| uint8_t can_data_out[8] = {0, 0, 0, 0, 0, 0, 0, 0}; |
| |
| uint8_t can_data_in[8] = {0, 0, 0, 0, 0, 0, 0, 0}; |
| static int length = 0; |
| |
| MEASUREMENT_DATA_S measurements[BUTTON_BOARD_COUNT]; |
| |
| uint32_t start = micros(); |
| uint16_t can_timer_1 = 0; |
| uint16_t can_timer_2 = 0; |
| while (true) { |
| JoystickAdcReadings adc; |
| char report[3][kReportSize]; |
| { |
| DisableInterrupts disable_interrupts; |
| adc = AdcReadJoystick(disable_interrupts); |
| } |
| |
| uint16_t tared_encoders[NUM_ENCODERS]; |
| for (int i = 0; i < NUM_ENCODERS; i++) { |
| (void)DetermineEncoderValues(&encoder_data[i], &abs_position[i], i, |
| ENCODER_RESET_TIME_MS); |
| tared_encoders[i] = TareEncoder(&encoder_data[i]); |
| } |
| |
| measurements[board_config.board_id - 1].buttons = ReadButtons(); |
| measurements[board_config.board_id - 1].abs0 = adc.analog0; |
| measurements[board_config.board_id - 1].abs1 = adc.analog1; |
| measurements[board_config.board_id - 1].abs2 = adc.analog2; |
| measurements[board_config.board_id - 1].abs3 = adc.analog3; |
| measurements[board_config.board_id - 1].enc0 = |
| ScaleEncoder(&encoder_data[0], tared_encoders[0]); |
| measurements[board_config.board_id - 1].enc1 = |
| ScaleEncoder(&encoder_data[1], tared_encoders[1]); |
| |
| #if PRINT_OFFSETS |
| static int counter = 0; |
| counter++; |
| |
| if (counter % 100 == 0) { |
| printf( |
| "Processor_ID: 0x%08lX 0x%08lX 0x%08lX 0x%08lX, ENC0 Max: 0x%03X, " |
| "ENC0 Angle: 0x%03X, " |
| "ENC0 Tared: 0x%03X, ENC0 Result: 0x%04X, ENC1 Max: 0x%03X, ENC1 " |
| "Angle: 0x%03X, ENC1 " |
| "Tared: 0x%03X, ENC1 Result: 0x%04X\n\r", |
| SIM_UIDH, SIM_UIDMH, SIM_UIDML, SIM_UIDL, encoder_data[0].enc_max, |
| encoder_data[0].angle, tared_encoders[0], measurements[2].enc0, |
| encoder_data[1].enc_max, encoder_data[1].angle, tared_encoders[1], |
| measurements[2].enc1); |
| } |
| #endif |
| |
| can_receive(can_data_in, &length, 0); |
| |
| if (length == -1) { |
| if (can_timer_1 < 2000) { |
| can_timer_1++; |
| } |
| } |
| |
| if (can_timer_1 >= 2000) { |
| ZeroMeasurements(measurements, board_config.can_id0 - 1); |
| } |
| |
| if (length == 8) { |
| UpdateMeasurementsFromCAN(measurements, can_data_in); |
| can_timer_1 = 0; |
| } |
| |
| can_receive(can_data_in, &length, 1); |
| |
| if (length == -1) { |
| if (can_timer_2 < 2000) { |
| can_timer_2++; |
| } |
| } |
| |
| if (can_timer_2 >= 2000) { |
| ZeroMeasurements(measurements, board_config.can_id1 - 1); |
| } |
| |
| if (length == 8) { |
| UpdateMeasurementsFromCAN(measurements, can_data_in); |
| can_timer_2 = 0; |
| } |
| |
| static int counter = 0; |
| counter++; |
| |
| if (counter % 100 == 0) { |
| printf("CAN Timer 1: %d, from BB%ld + CAN Timer 2: %d, from BB%ld\n\r", |
| can_timer_1, board_config.can_id0, can_timer_2, |
| board_config.can_id1); |
| printf("Report 1: 0x%04X + Report 2: 0x%04X + Report 3: 0x%04X\n\r", |
| report[0][4], report[1][4], report[2][4]); |
| } |
| |
| PackMeasurementsToCAN(&measurements[board_config.board_id - 1], |
| can_data_out); |
| |
| can_send(board_config.board_id, can_data_out, sizeof(can_data_out), 2); |
| |
| ComposeReport(report, measurements, board_config.board_id, |
| (can_timer_1 >= 2000) ? -1 : board_config.can_id0 - 1, |
| (can_timer_2 >= 2000) ? -1 : board_config.can_id1 - 1); |
| |
| { |
| DisableInterrupts disable_interrupts; |
| joystick0->UpdateReport(report[0], sizeof(report[0]), disable_interrupts); |
| joystick1->UpdateReport(report[1], sizeof(report[1]), disable_interrupts); |
| joystick2->UpdateReport(report[2], sizeof(report[2]), disable_interrupts); |
| } |
| |
| start = delay_from(start, 1); |
| } |
| } |
| |
| void DriverStation::ZeroMeasurements(MEASUREMENT_DATA_S *bbMeasurements, |
| uint32_t board) { |
| bbMeasurements[board].buttons = 0; |
| bbMeasurements[board].abs0 = 0; |
| bbMeasurements[board].abs1 = 0; |
| bbMeasurements[board].abs2 = 0; |
| bbMeasurements[board].abs3 = 0; |
| bbMeasurements[board].enc0 = 0; |
| bbMeasurements[board].enc1 = 0; |
| } |
| |
| // clang-format off |
| // CAN Packet Format |
| // | Bit | |
| // Byte 7 6 5 4 3 2 1 0 |
| // 0 BTN7 BTN6 BTN5 BTN4 BTN3 BTN2 BTN1 BTN0 |
| // 1 BTN15 BTN14 BTN13 BTN12 BTN11 BTN10 BTN9 BTN8 |
| // 2 - - - - BTN19 BTN18 BTN17 BTN16 |
| // 3 ENC0:7 ENC0:6 ENC0:5 ENC0:4 ENC0:3 ENC0_2 ENC0:1 ENC0:0 |
| // 4 ENC1:3 ENC1:2 ENC1:1 ENC1:0 ENC0:11 ENC0:10 ENC0:9 ENC0:8 |
| // 5 ENC1:11 ENC1:10 ENC1:9 ENC1:8 ENC1:7 ENC1:6 ENC1:5 ENC1:4 |
| // 6 ABS2:7 ABS2:6 ABS2:5 ABS2:4 ABS2:3 ABS2:2 ABS2:1 ABS2:0 |
| // 7 ABS3:7 ABS3:6 ABS3:5 ABS3:4 ABS3:3 ABS3:2 ABS3:1 ABS3:0 |
| // clang-format on |
| int DriverStation::UpdateMeasurementsFromCAN(MEASUREMENT_DATA_S *bbMeasurements, |
| uint8_t *canRX_data) { |
| int bb_id = 0; |
| uint32_t buttons = 0; |
| uint16_t enc0 = 0; |
| uint16_t enc1 = 0; |
| uint16_t abs2 = 0; |
| uint16_t abs3 = 0; |
| |
| // Extract BB_id |
| bb_id = (int)((canRX_data[2] >> 2) & 0x03); |
| bb_id--; |
| |
| if (bb_id == -1) { |
| return -1; |
| } |
| |
| buttons = (uint32_t)canRX_data[0]; |
| buttons |= (uint32_t)canRX_data[1] << 8; |
| buttons |= ((uint32_t)canRX_data[2] & 0x0F) << 16; |
| |
| bbMeasurements[bb_id].buttons = buttons; |
| |
| enc0 = (uint16_t)canRX_data[3]; |
| enc0 |= ((uint16_t)canRX_data[4] & 0x0F) << 8; |
| |
| bbMeasurements[bb_id].enc0 = enc0 << 4; |
| |
| enc1 = ((uint16_t)canRX_data[4] & 0xF0) >> 4; |
| enc1 |= ((uint16_t)canRX_data[5]) << 4; |
| |
| bbMeasurements[bb_id].enc1 = enc1 << 4; |
| |
| abs2 = ((uint16_t)canRX_data[6]) << 8; |
| |
| bbMeasurements[bb_id].abs2 = abs2; |
| |
| abs3 = ((uint16_t)canRX_data[7]) << 8; |
| |
| bbMeasurements[bb_id].abs3 = abs3; |
| |
| return bb_id; |
| } |
| |
| void DriverStation::PackMeasurementsToCAN(MEASUREMENT_DATA_S *bbMeasurements, |
| uint8_t *canTX_data) { |
| uint16_t encoder_measurements_0 = bbMeasurements->enc0 >> 4; |
| uint16_t encoder_measurements_1 = bbMeasurements->enc1 >> 4; |
| |
| // taking button data |
| canTX_data[2] = (uint8_t)((bbMeasurements->buttons >> 16) & 0x0F); |
| canTX_data[1] = (uint8_t)((bbMeasurements->buttons >> 8) & 0xFF); |
| canTX_data[0] = (uint8_t)(bbMeasurements->buttons & 0xFF); |
| |
| // taking encdoer data |
| canTX_data[3] = (uint8_t)(encoder_measurements_0 & 0xFF); |
| canTX_data[4] = (uint8_t)((encoder_measurements_0 >> 8) & 0x0F); |
| |
| canTX_data[4] |= (uint8_t)((encoder_measurements_1 & 0x0F) << 4); |
| canTX_data[5] = (uint8_t)((encoder_measurements_1 >> 4) & 0xFF); |
| |
| // taking abs data |
| canTX_data[6] = (uint8_t)(bbMeasurements->abs2 >> 8); |
| canTX_data[7] = (uint8_t)(bbMeasurements->abs3 >> 8); |
| } |
| |
| 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 "C" |
| |
| // clang-format off |
| |
| // BB1/BB3 HID BB2 HID |
| // HID Word JS0 JS1 JS2 JS3 JS4 JS5 |
| // 0 AXIS0 BB3:ENC0[15:8] BB3:ENC1[15:8] BB2:ADC0 BB3:ENC0[15:8] BB3:ENC1[15:8] BB2:ADC0 |
| // 1 AXIS1 0x7F 0x7F BB2:ADC1 0x7F 0x7F BB2:ADC1 |
| // 2 AXIS2 0x7F 0x7F BB2:ADC2 0x7F 0x7F BB2:ADC2 |
| // 3 AXIS3 BB3:ENC0[7:0] BB3:ENC1[7:0] BB2:ADC3 BB3:ENC0[7:0] BB3:ENC1[7:0] BB2:ADC3 |
| // 4 AXIS4[0] BB1 _OK BB1 _OK BB1 _OK BB1 _OK BB1 _OK BB1 _OK |
| // 4 AXIS4[1] BB2 _OK BB2 _OK BB2 _OK BB2 _OK BB2 _OK BB2 _OK |
| // 4 AXIS4[2] BB3_OK BB3_OK BB3_OK BB3_OK BB3_OK BB3_OK |
| // 4 AXIS4[3:4] 0b01 0b10 0b11 0b01 0b10 0b11 |
| // 4 AXIS4[5] 0 0 0 1 1 1 |
| // 4 AXIS4[6:7] 0 0 0 0 0 0 |
| // 5 B0 BB3:B0 BB1:B8 BB2:B4 BB3:B0 BB1:B8 BB2:B4 |
| // 5 B1 BB3:B1 BB1:B9 BB2:B5 BB3:B1 BB1:B9 BB2:B5 |
| // 5 B2 BB3:B2 BB1:B10 BB2:B6 BB3:B2 BB1:B10 BB2:B6 |
| // 5 B3 BB3:B3 BB1:B11 BB2:B7 BB3:B3 BB1:B11 BB2:B7 |
| // 5 B4 BB3:B4 BB1:B12 BB2:B8 BB3:B4 BB1:B12 BB2:B8 |
| // 5 B5 BB1:B0 BB1:B13 BB2:B9 BB1:B0 BB1:B13 BB2:B9 |
| // 5 B6 BB1:B1 BB1:B14 BB2:B10 BB1:B1 BB1:B14 BB2:B10 |
| // 5 B7 BB1:B2 BB1:B15 BB2:B11 BB1:B2 BB1:B15 BB2:B11 |
| // 6 B8 BB1:B3 BB1:B16 BB2:B12 BB1:B3 BB1:B16 BB2:B12 |
| // 6 B9 BB1:B4 BB2:B0 BB2:B13 BB1:B4 BB2:B0 BB2:B13 |
| // 6 B10 BB1:B5 BB2:B1 BB2:B14 BB1:B5 BB2:B1 BB2:B14 |
| // 6 B11 BB1:B6 BB2:B2 BB2:B15 BB1:B6 BB2:B2 BB2:B15 |
| // 6 B12 BB1:B7 BB2:B3 BB2:B16 BB1:B7 BB2:B3 BB2:B16 |
| // 6 B[14:13] 0b01 0b10 0b11 0b01 0b10 0b11 |
| // 6 B15 0 0 0 1 1 1 |
| |
| // clang-format on |
| void DriverStation::ComposeReport(char report[][kReportSize], |
| MEASUREMENT_DATA_S *bbMeasurements, |
| uint8_t board_id, int can_1_board, |
| int can_2_board) { |
| memset(report, 0, 3 * sizeof(*report)); |
| |
| report[0][0] = (char)(bbMeasurements[2].enc0 >> 8 & 0xFF); |
| report[0][1] = (char)((0x7F) & 0xFF); |
| report[0][2] = (char)((0x7F) & 0xFF); |
| report[0][3] = (char)(bbMeasurements[2].enc0 & 0xFF); |
| report[0][4] |= (char)(1 << (board_id - 1)); |
| if (can_1_board != -1) { |
| report[0][4] |= (char)(1 << (can_1_board)); |
| } |
| if (can_2_board != -1) { |
| report[0][4] |= (char)(1 << (can_2_board)); |
| } |
| report[0][4] |= (char)(1 << 3); |
| report[0][5] = (char)(bbMeasurements[2].buttons & 0x1F); // BB1 BTN[7:0] |
| report[0][5] |= (char)((bbMeasurements[0].buttons << 5) & 0xFF); |
| report[0][6] = |
| (char)((bbMeasurements[0].buttons >> 3) & 0x1F); // BB1 BTN[12:8] |
| report[0][6] |= (char)(1 << 5); // BB1 BTN[14:13] |
| |
| report[1][0] = (char)(((bbMeasurements[2].enc1) >> 8) & 0xFF); |
| report[1][1] = (char)((0x7F) & 0xFF); |
| report[1][2] = (char)((0x7F) & 0xFF); |
| report[1][3] = (char)((bbMeasurements[2].enc1) & 0xFF); |
| report[1][4] |= (char)(1 << (board_id - 1)); |
| if (can_1_board != -1) { |
| report[1][4] |= (char)(1 << (can_1_board)); |
| } |
| if (can_2_board != -1) { |
| report[1][4] |= (char)(1 << (can_2_board)); |
| } |
| report[1][4] |= (char)(2 << 3); |
| report[1][5] = |
| (char)((bbMeasurements[0].buttons >> 8) & 0xFF); // BB1 BTN[16:13] |
| report[1][6] = (char)((bbMeasurements[1].buttons) & 0x1F); // BB2 BTN[3:0] |
| report[1][6] |= (char)(2 << 5); // BB2 BTN[14:13] |
| |
| report[2][0] = (char)((bbMeasurements[1].abs0 >> 8) & 0xFF); |
| report[2][1] = (char)((bbMeasurements[1].abs1 >> 8) & 0xFF); |
| report[2][2] = (char)((bbMeasurements[1].abs2 >> 8) & 0xFF); |
| report[2][3] = (char)((bbMeasurements[1].abs3 >> 8) & 0xFF); |
| report[2][4] |= (char)(1 << (board_id - 1)); |
| if (can_1_board != -1) { |
| report[2][4] |= (char)(1 << (can_1_board)); |
| } |
| if (can_2_board != -1) { |
| report[2][4] |= (char)(1 << (can_2_board)); |
| } |
| report[2][4] |= (char)(3 << 3); |
| report[2][5] = |
| (char)((bbMeasurements[1].buttons >> 5) & 0xFF); // BB2 BTN[8:4] |
| report[2][6] = |
| (char)((bbMeasurements[1].buttons >> 13) & 0x1F); // BB2 BTN[16:9] |
| // report[2][5] = (char)(bbMeasurements[2].buttons & 0x1F); // BB3 BTN[4:0] |
| report[2][6] |= (char)(3 << 5); // BB3 BTN[14:13] |
| |
| if (board_id == 2) { |
| report[0][4] |= (char)(1 << 5); |
| report[1][4] |= (char)(1 << 5); |
| report[2][4] |= (char)(1 << 5); |
| |
| report[0][6] |= (char)(1 << 7); // BB1 BTN[15] |
| report[1][6] |= (char)(1 << 7); // BB2 BTN[15] |
| report[2][6] |= (char)(1 << 7); // BB3 BTN[15] |
| } |
| |
| /*for ( int i = 0; i < 3; i++ ) { |
| printf("Report %d: 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X ", i, |
| report[i][0], report[i][1], report[i][2], report[i][3], report[i][4], |
| report[i][5]); |
| } |
| printf("\n\r\n\r");*/ |
| } |
| |
| uint32_t DriverStation::ReadButtons() { |
| uint32_t buttons = 0; |
| |
| buttons = ((PERIPHERAL_BITBAND(GPIOD_PDIR, 5) << 0) | // BTN0 PTD5 |
| (PERIPHERAL_BITBAND(GPIOC_PDIR, 0) << 1) | // BTN1 PTC0 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 3) << 2) | // BTN2 PTB3 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 2) << 3) | // BTN3 PTB2 |
| (PERIPHERAL_BITBAND(GPIOA_PDIR, 14) << 4) | // BTN4 PTA14 |
| (PERIPHERAL_BITBAND(GPIOE_PDIR, 26) << 5) | // BTN5 PTE26 |
| (PERIPHERAL_BITBAND(GPIOA_PDIR, 16) << 6) | // BTN6 PTA16 |
| (PERIPHERAL_BITBAND(GPIOA_PDIR, 15) << 7) | // BTN7 PTA15 |
| (PERIPHERAL_BITBAND(GPIOE_PDIR, 25) << 8) | // BTN8 PTE25 |
| (PERIPHERAL_BITBAND(GPIOA_PDIR, 5) << 9) | // BTN9 PTA5 |
| (PERIPHERAL_BITBAND(GPIOC_PDIR, 3) << 10) | // BTN10 PTC3 |
| (PERIPHERAL_BITBAND(GPIOC_PDIR, 7) << 11) | // BTN11 PTC7 |
| (PERIPHERAL_BITBAND(GPIOD_PDIR, 3) << 12) | // BTN12 PTD3 |
| (PERIPHERAL_BITBAND(GPIOD_PDIR, 2) << 13) | // BTN13 PTD2 |
| (PERIPHERAL_BITBAND(GPIOD_PDIR, 7) << 14) | // BTN14 PTD7 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 10) << 15) | // BTN15 PTB10 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 11) << 16) | // BTN16 PTB11 |
| (PERIPHERAL_BITBAND(GPIOD_PDIR, 4) << 17) | // BTN17 PTD4 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 17) << 18) | // BTN18 PTB17 |
| (PERIPHERAL_BITBAND(GPIOB_PDIR, 16) << 19)) ^ // BTN19 PTB16 |
| 0x000FFFFF; |
| |
| return buttons; |
| } |
| |
| JoystickAdcReadings DriverStation::AdcReadJoystick(const DisableInterrupts &) { |
| JoystickAdcReadings r; |
| |
| // ENC1_ABS_ADC (PTE24) ADC0_SE17 |
| ADC0_SC1A = 17; |
| while (!(ADC0_SC1A & ADC_SC1_COCO)) { |
| } |
| // ABS2_ADC (PTC1) ADC0_SE15 |
| ADC0_SC1A = 15; |
| r.analog1 = ADC0_RA << 4; |
| while (!(ADC0_SC1A & ADC_SC1_COCO)) { |
| } |
| // ABS3_ADC (PTC2) ADC0_SE4b |
| ADC0_SC1A = 4; |
| r.analog2 = ADC0_RA << 4; |
| while (!(ADC0_SC1A & ADC_SC1_COCO)) { |
| } |
| // ENC0_ABS_ADC (PTD1) ADC0_SE5b |
| ADC0_SC1A = 5; |
| r.analog3 = ADC0_RA << 4; |
| while (!(ADC0_SC1A & ADC_SC1_COCO)) { |
| } |
| r.analog0 = ADC0_RA << 4; |
| |
| return r; |
| } |
| |
| void DriverStation::AdcInitJoystick() { |
| AdcInitCommon(); |
| // ENC1_ABS_ADC (PTE24) ADC0_SE17 |
| PORTE_PCR24 = PORT_PCR_MUX(0); |
| // ABS2_ADC (PTC1) ADC0_SE15 |
| PORTC_PCR1 = PORT_PCR_MUX(0); |
| // ABS3_ADC (PTC2) ADC0_SE4b |
| PORTC_PCR2 = PORT_PCR_MUX(0); |
| // ENC0_ABS_ADC (PTD1) ADC0_SE5b |
| PORTD_PCR1 = PORT_PCR_MUX(0); |
| } |
| |
| void DriverStation::EnableLeds() { |
| // Set all the LED pins to output, drive strength enable (high drive since its |
| // an output), slew rate enable (slow since output) LED (on board) PTC5 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1; |
| PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1; |
| // LED0R PTC6 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 1; |
| PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 6) = 1; |
| // LED0Y PTA17 |
| PERIPHERAL_BITBAND(GPIOA_PDOR, 17) = 1; |
| PORTA_PCR17 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOA_PDDR, 17) = 1; |
| // LED0G PTC11 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 1; |
| PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 11) = 1; |
| // LED1R PTC10 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1; |
| PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1; |
| // LED1Y PTC4 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 1; |
| PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 4) = 1; |
| // LED1G PTC8 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1; |
| PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1; |
| } |
| |
| void DriverStation::EnableCan() { |
| // Set up the CAN pins. |
| // (PTA12) CAN0_TX |
| PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| // (PTA13) CAN0_RX |
| PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| } |
| |
| void DriverStation::EnableEncoders() { |
| // Set up the encoder inputs |
| // ENC0_A (PTB18) FTM2_QD_PHA |
| PORTB_PCR18 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| |
| // ENC0_B (PTB19) FTM2_QD_PHB |
| PORTB_PCR19 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| |
| // ENC0_ABS (PTD0) FTM3_CH0 |
| PORTD_PCR0 = PORT_PCR_MUX(4) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| |
| // ENC1_A (PTB1) FTM1_QD_PHB |
| PORTB_PCR1 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| |
| // ENC1_B (PTB0) FTM1_QD_PHA |
| PORTB_PCR0 = PORT_PCR_MUX(6) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| |
| // ENC1_ABS (PTD6) FTM0_CH6 |
| PORTD_PCR6 = PORT_PCR_MUX(4) /*| PORT_PCR_PE | PORT_PCR_PS*/; |
| } |
| |
| // Enable quadrature encoding. |
| void DriverStation::EnableQD(LittleFTM *ftm, int encoder) { |
| ftm->MODE = FTM_MODE_WPDIS; |
| ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; |
| |
| ftm->CNTIN = 0; |
| ftm->CNT = 0; |
| ftm->MOD = ENCODER_COUNTS_PER_REV; |
| |
| if (encoder == 1) { |
| ftm->QDCTRL = FTM_QDCTRL_PHBPOL; |
| } |
| |
| ftm->C0SC = FTM_CSC_ELSA; |
| ftm->C1SC = FTM_CSC_ELSA; |
| |
| // Disable filters |
| ftm->FILTER = FTM_FILTER_CH0FVAL(0) | FTM_FILTER_CH1FVAL(0) | |
| FTM_FILTER_CH2FVAL(0) | FTM_FILTER_CH3FVAL(0); |
| |
| // Use Quadrature Encoding. |
| ftm->QDCTRL |= FTM_QDCTRL_QUADEN; |
| |
| ftm->SYNCONF = FTM_SYNCONF_SWWRBUF /* Software trigger flushes MOD */ | |
| FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ | |
| FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */; |
| |
| ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */; |
| |
| // Wait for the software synchronization to finish. |
| while (ftm->SYNC & FTM_SYNC_SWSYNC) { |
| } |
| |
| ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ | |
| FTM_SC_PS(0) /* Prescaler=64 */; |
| |
| ftm->MODE &= ~FTM_MODE_WPDIS; |
| } |
| |
| int DriverStation::ReadQuadrature(int encoderNum, uint16_t *encoderAngle) { |
| switch (encoderNum) { |
| case 0: |
| *encoderAngle = FTM2_CNT; |
| break; |
| |
| case 1: |
| *encoderAngle = FTM1_CNT; |
| break; |
| |
| default: |
| return -1; |
| break; |
| } |
| return 0; |
| } |
| |
| void DriverStation::EnableGlitchFilter() { |
| // Enable 1KHz filter clock |
| PORTA_DFCR = 1; |
| PORTB_DFCR = 1; |
| PORTC_DFCR = 1; |
| PORTD_DFCR = 1; |
| PORTE_DFCR = 1; |
| // 10ms filter time |
| PORTA_DFWR = 10; |
| PORTB_DFWR = 10; |
| PORTC_DFWR = 10; |
| PORTD_DFWR = 10; |
| PORTE_DFWR = 10; |
| // TODO: validate the 10ms gitch filter, seems to work |
| } |
| |
| void DriverStation::EnableButtons() { |
| // Set up the buttons. The LEDs pull them up to 5V, so the Teensy needs to not |
| // be set to pull up. |
| // BTN0 PTD5 |
| PORTD_PCR5 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTD_DFER |= 1 << 5; |
| |
| // BTN1 PTC0 |
| PORTC_PCR0 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTC_DFER |= 1 << 0; |
| |
| // BTN2 PTB3 |
| PORTB_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 3; |
| |
| // BTN3 PTB2 |
| PORTB_PCR2 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 2; |
| |
| // BTN4 PTA14 |
| PORTA_PCR14 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTA_DFER |= 1 << 14; |
| |
| // BTN5 PTE26 |
| PORTE_PCR26 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTE_DFER |= 1 << 26; |
| |
| // BTN6 PTA16 |
| PORTA_PCR16 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTA_DFER |= 1 << 16; |
| |
| // BTN7 PTA15 |
| PORTA_PCR15 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTA_DFER |= 1 << 15; |
| |
| // BTN8 PTE25 |
| PORTE_PCR25 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTE_DFER |= 1 << 25; |
| |
| // BTN9 PTA5 |
| PORTA_PCR5 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTA_DFER |= 1 << 5; |
| |
| // BTN10 PTC3 |
| PORTC_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTC_DFER |= 1 << 3; |
| |
| // BTN11 PTC7 |
| PORTC_PCR7 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTC_DFER |= 1 << 7; |
| |
| // BTN12 PTD3 |
| PORTD_PCR3 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTD_DFER |= 1 << 3; |
| |
| // BTN13 PTD2 |
| PORTD_PCR2 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTD_DFER |= 1 << 2; |
| |
| // BTN14 PTD7 |
| PORTD_PCR7 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTD_DFER |= 1 << 7; |
| |
| // BTN15 PTB10 |
| PORTB_PCR10 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 10; |
| |
| // BTN16 PTB11 |
| PORTB_PCR11 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 11; |
| |
| // BTN17 PTD4 |
| PORTD_PCR4 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTD_DFER |= 1 << 4; |
| (PERIPHERAL_BITBAND(GPIOD_PDIR, 4) = |
| 1); // BTN17 PTD4 is being forced to be an input |
| |
| // BTN18 PTB17 |
| PORTB_PCR17 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 17; |
| |
| // BTN19 PTB16 |
| PORTB_PCR16 = PORT_PCR_MUX(1) | PORT_PCR_PFE; |
| PORTB_DFER |= 1 << 16; |
| } |
| |
| void DriverStation::DisableLeds() { |
| // LED (on board) PTC5 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0; |
| // LED0R PTC6 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 0; |
| // LED0Y PTA17 |
| PERIPHERAL_BITBAND(GPIOA_PDOR, 17) = 0; |
| // LED0G PTC11 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 0; |
| // LED1R PTC10 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0; |
| // LED1Y PTC4 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 4) = 0; |
| // LED1G PTC8 |
| PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1; |
| } |
| |
| int DriverStation::Run() { |
| // for background about this startup delay, please see these conversations |
| // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980 |
| // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273 |
| delay(400); |
| |
| // Set all interrupts to the second-lowest priority to start with. |
| for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD); |
| |
| // Now set priorities for all the ones we care about. They only have meaning |
| // relative to each other, which means centralizing them here makes it a lot |
| // more manageable. |
| NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7); |
| |
| EnableLeds(); |
| |
| EnableGlitchFilter(); |
| |
| EnableCan(); |
| |
| EnableEncoders(); |
| |
| EnableButtons(); |
| |
| delay(100); |
| |
| teensy::UsbDevice usb_device(0, 0x16c0, 0x0492); |
| usb_device.SetManufacturer("FRC 971 Spartan Robotics"); |
| usb_device.SetProduct("Spartan Joystick Board"); |
| |
| teensy::HidFunction joystick0(&usb_device, kReportSize); |
| joystick0.set_report_descriptor( |
| ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1))); |
| |
| teensy::HidFunction joystick1(&usb_device, kReportSize); |
| joystick1.set_report_descriptor( |
| ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1))); |
| |
| teensy::HidFunction joystick2(&usb_device, kReportSize); |
| joystick2.set_report_descriptor( |
| ::std::string(kReportDescriptor1, sizeof(kReportDescriptor1))); |
| |
| teensy::AcmTty tty1(&usb_device); |
| PrintingParameters printing_parameters; |
| printing_parameters.stdout_tty = &tty1; |
| |
| const ::std::unique_ptr<PrintingImplementation> printing = |
| CreatePrinting(printing_parameters); |
| usb_device.Initialize(); |
| printing->Initialize(); |
| |
| AdcInitJoystick(); |
| |
| EnableQD(FTM1, 1); |
| EnableQD(FTM2, 0); |
| |
| // Leave the LEDs on for a bit longer. |
| delay(300); |
| printf("Done starting up\n"); |
| |
| // Done starting up, now turn all the LEDs off. |
| DisableLeds(); |
| |
| board_config.board_id = 0; |
| board_config.processor_index = 0; |
| board_config.can_id0 = 0; |
| board_config.can_id1 = 0; |
| |
| uint32_t button18 = |
| PERIPHERAL_BITBAND(GPIOB_PDIR, 17) ^ 0x1; // BTN18 PTB17 |
| uint32_t button19 = |
| PERIPHERAL_BITBAND(GPIOB_PDIR, 16) ^ 0x1; // BTN19 PTB16 |
| |
| board_config.board_id = (button19 << 1) | button18; |
| |
| board_config.processor_index = ProcessorIndex(); |
| |
| switch (board_config.board_id) { |
| case 1: |
| board_config.can_id0 = 2; |
| board_config.can_id1 = 3; |
| break; |
| |
| case 2: |
| board_config.can_id0 = 1; |
| board_config.can_id1 = 3; |
| break; |
| |
| case 3: |
| board_config.can_id0 = 2; |
| board_config.can_id1 = 1; |
| break; |
| |
| default: |
| board_config.board_id = 0; |
| break; |
| } |
| |
| can_init(board_config.can_id0, board_config.can_id1); |
| |
| SendJoystickData(&joystick0, &joystick1, &joystick2, |
| board_config.processor_index); |
| |
| return 0; |
| } |
| |
| extern "C" int main(void) { |
| frc971::motors::DriverStation driverStation; |
| return driverStation.Run(); |
| } |
| |
| } // namespace motors |
| } // namespace frc971 |