Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 1 | // This file has the main for the Teensy on the simple receiver board. |
| 2 | |
| 3 | #include <inttypes.h> |
| 4 | #include <stdio.h> |
| 5 | #include <atomic> |
| 6 | #include <cmath> |
| 7 | |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 8 | #include "frc971/control_loops/drivetrain/polydrivetrain.h" |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 9 | #include "motors/core/kinetis.h" |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 10 | #include "motors/core/time.h" |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 11 | #include "motors/peripheral/adc.h" |
| 12 | #include "motors/peripheral/can.h" |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 13 | #include "motors/peripheral/configuration.h" |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 14 | #include "motors/seems_reasonable/drivetrain_dog_motor_plant.h" |
| 15 | #include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h" |
| 16 | #include "motors/seems_reasonable/spring.h" |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 17 | #include "motors/usb/cdc.h" |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 18 | #include "motors/usb/usb.h" |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 19 | #include "motors/util.h" |
| 20 | |
| 21 | namespace frc971 { |
| 22 | namespace motors { |
| 23 | namespace { |
| 24 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 25 | using ::frc971::control_loops::drivetrain::DrivetrainConfig; |
| 26 | using ::frc971::control_loops::drivetrain::PolyDrivetrain; |
| 27 | using ::frc971::constants::ShifterHallEffect; |
| 28 | using ::frc971::control_loops::DrivetrainQueue_Goal; |
| 29 | using ::frc971::control_loops::DrivetrainQueue_Output; |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 30 | using ::motors::seems_reasonable::Spring; |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 31 | |
Brian Silverman | 9ed2cf1 | 2018-05-12 13:06:38 -0700 | [diff] [blame] | 32 | struct SimpleAdcReadings { |
| 33 | uint16_t sin, cos; |
| 34 | }; |
| 35 | |
| 36 | void AdcInitSimple() { |
| 37 | AdcInitCommon(); |
| 38 | |
| 39 | // ENC_SIN ADC0_SE23 |
| 40 | // ENC_COS ADC1_SE23 |
| 41 | } |
| 42 | |
| 43 | SimpleAdcReadings AdcReadSimple(const DisableInterrupts &) { |
| 44 | SimpleAdcReadings r; |
| 45 | |
| 46 | ADC0_SC1A = 23; |
| 47 | ADC1_SC1A = 23; |
| 48 | while (!(ADC0_SC1A & ADC_SC1_COCO)) { |
| 49 | } |
| 50 | while (!(ADC1_SC1A & ADC_SC1_COCO)) { |
| 51 | } |
| 52 | r.sin = ADC0_RA; |
| 53 | r.cos = ADC1_RA; |
| 54 | |
| 55 | return r; |
| 56 | } |
| 57 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 58 | const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75}; |
| 59 | |
| 60 | const DrivetrainConfig<float> &GetDrivetrainConfig() { |
| 61 | static DrivetrainConfig<float> kDrivetrainConfig{ |
| 62 | ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER, |
| 63 | ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP, |
| 64 | ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO, |
| 65 | ::frc971::control_loops::drivetrain::IMUType::IMU_X, |
| 66 | |
| 67 | ::motors::seems_reasonable::MakeDrivetrainLoop, |
| 68 | ::motors::seems_reasonable::MakeVelocityDrivetrainLoop, |
| 69 | ::std::function<StateFeedbackLoop<7, 2, 4, float>()>(), |
| 70 | |
| 71 | ::motors::seems_reasonable::kDt, ::motors::seems_reasonable::kRobotRadius, |
| 72 | ::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV, |
| 73 | |
| 74 | ::motors::seems_reasonable::kHighGearRatio, |
| 75 | ::motors::seems_reasonable::kLowGearRatio, kThreeStateDriveShifter, |
| 76 | kThreeStateDriveShifter, true /* default_high_gear */, |
| 77 | 0 /* down_offset if using constants use |
| 78 | constants::GetValues().down_error */, 0.8 /* wheel_non_linearity */, |
| 79 | 1.2 /* quickturn_wheel_multiplier */, 1.5 /* wheel_multiplier */, |
| 80 | }; |
| 81 | |
| 82 | return kDrivetrainConfig; |
| 83 | }; |
| 84 | |
| 85 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 86 | ::std::atomic<teensy::AcmTty *> global_stdout{nullptr}; |
| 87 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 88 | ::std::atomic<PolyDrivetrain<float> *> global_polydrivetrain{nullptr}; |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 89 | ::std::atomic<Spring *> global_spring{nullptr}; |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 90 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 91 | // Last width we received on each channel. |
Brian Silverman | 7f5f144 | 2018-04-06 13:00:50 -0400 | [diff] [blame] | 92 | uint16_t pwm_input_widths[6]; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 93 | // When we received a pulse on each channel in milliseconds. |
Brian Silverman | 7f5f144 | 2018-04-06 13:00:50 -0400 | [diff] [blame] | 94 | uint32_t pwm_input_times[6]; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 95 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 96 | constexpr int kChannelTimeout = 100; |
| 97 | |
| 98 | bool lost_channel(int channel) { |
| 99 | DisableInterrupts disable_interrupts; |
| 100 | if (time_after(millis(), |
| 101 | time_add(pwm_input_times[channel], kChannelTimeout))) { |
| 102 | return true; |
| 103 | } |
| 104 | return false; |
| 105 | } |
| 106 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 107 | // Returns the most recently captured value for the specified input channel |
| 108 | // scaled from -1 to 1, or 0 if it was captured over 100ms ago. |
| 109 | float convert_input_width(int channel) { |
| 110 | uint16_t width; |
| 111 | { |
| 112 | DisableInterrupts disable_interrupts; |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 113 | if (time_after(millis(), |
| 114 | time_add(pwm_input_times[channel], kChannelTimeout))) { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 115 | return 0; |
| 116 | } |
| 117 | |
| 118 | width = pwm_input_widths[channel]; |
| 119 | } |
| 120 | |
| 121 | // Values measured with a channel mapped to a button. |
| 122 | static constexpr uint16_t kMinWidth = 4133; |
| 123 | static constexpr uint16_t kMaxWidth = 7177; |
| 124 | if (width < kMinWidth) { |
| 125 | width = kMinWidth; |
| 126 | } else if (width > kMaxWidth) { |
| 127 | width = kMaxWidth; |
| 128 | } |
| 129 | return (static_cast<float>(2 * (width - kMinWidth)) / |
| 130 | static_cast<float>(kMaxWidth - kMinWidth)) - |
| 131 | 1.0f; |
| 132 | } |
| 133 | |
| 134 | // Sends a SET_RPM command to the specified VESC. |
| 135 | // Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN |
| 136 | // bandwidth. |
| 137 | void vesc_set_rpm(int vesc_id, float rpm) { |
| 138 | const int32_t rpm_int = rpm; |
| 139 | uint32_t id = CAN_EFF_FLAG; |
| 140 | id |= vesc_id; |
| 141 | id |= (0x03 /* SET_RPM */) << 8; |
| 142 | uint8_t data[4] = { |
| 143 | static_cast<uint8_t>((rpm_int >> 24) & 0xFF), |
| 144 | static_cast<uint8_t>((rpm_int >> 16) & 0xFF), |
| 145 | static_cast<uint8_t>((rpm_int >> 8) & 0xFF), |
| 146 | static_cast<uint8_t>((rpm_int >> 0) & 0xFF), |
| 147 | }; |
| 148 | can_send(id, data, sizeof(data), 2 + vesc_id); |
| 149 | } |
| 150 | |
| 151 | // Sends a SET_CURRENT command to the specified VESC. |
| 152 | // current is in amps. |
| 153 | // Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN |
| 154 | // bandwidth. |
| 155 | void vesc_set_current(int vesc_id, float current) { |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 156 | constexpr float kMaxCurrent = 80.0f; |
| 157 | const int32_t current_int = |
| 158 | ::std::max(-kMaxCurrent, ::std::min(kMaxCurrent, current)) * 1000.0f; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 159 | uint32_t id = CAN_EFF_FLAG; |
| 160 | id |= vesc_id; |
| 161 | id |= (0x01 /* SET_CURRENT */) << 8; |
| 162 | uint8_t data[4] = { |
| 163 | static_cast<uint8_t>((current_int >> 24) & 0xFF), |
| 164 | static_cast<uint8_t>((current_int >> 16) & 0xFF), |
| 165 | static_cast<uint8_t>((current_int >> 8) & 0xFF), |
| 166 | static_cast<uint8_t>((current_int >> 0) & 0xFF), |
| 167 | }; |
| 168 | can_send(id, data, sizeof(data), 2 + vesc_id); |
| 169 | } |
| 170 | |
Brian Silverman | 4d1e527 | 2018-03-26 03:18:42 -0400 | [diff] [blame] | 171 | // Sends a SET_DUTY command to the specified VESC. |
| 172 | // duty is from -1 to 1. |
| 173 | // Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN |
| 174 | // bandwidth. |
| 175 | void vesc_set_duty(int vesc_id, float duty) { |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 176 | constexpr int32_t kMaxDuty = 99999; |
| 177 | const int32_t duty_int = ::std::max( |
| 178 | -kMaxDuty, ::std::min(kMaxDuty, static_cast<int32_t>(duty * 100000.0f))); |
Brian Silverman | 4d1e527 | 2018-03-26 03:18:42 -0400 | [diff] [blame] | 179 | uint32_t id = CAN_EFF_FLAG; |
| 180 | id |= vesc_id; |
| 181 | id |= (0x00 /* SET_DUTY */) << 8; |
| 182 | uint8_t data[4] = { |
| 183 | static_cast<uint8_t>((duty_int >> 24) & 0xFF), |
| 184 | static_cast<uint8_t>((duty_int >> 16) & 0xFF), |
| 185 | static_cast<uint8_t>((duty_int >> 8) & 0xFF), |
| 186 | static_cast<uint8_t>((duty_int >> 0) & 0xFF), |
| 187 | }; |
| 188 | can_send(id, data, sizeof(data), 2 + vesc_id); |
| 189 | } |
| 190 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 191 | void DoVescTest() { |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 192 | uint32_t time = micros(); |
| 193 | while (true) { |
| 194 | for (int i = 0; i < 6; ++i) { |
| 195 | const uint32_t end = time_add(time, 500000); |
| 196 | while (true) { |
| 197 | const bool done = time_after(micros(), end); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 198 | float current; |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 199 | if (done) { |
| 200 | current = -6; |
| 201 | } else { |
| 202 | current = 6; |
| 203 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 204 | vesc_set_current(i, current); |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 205 | if (done) { |
| 206 | break; |
| 207 | } |
| 208 | delay(5); |
| 209 | } |
| 210 | time = end; |
| 211 | } |
| 212 | } |
| 213 | } |
| 214 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 215 | void DoReceiverTest2() { |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 216 | static constexpr float kMaxRpm = 10000.0f; |
| 217 | while (true) { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 218 | const bool flip = convert_input_width(2) > 0; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 219 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 220 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 221 | const float value = convert_input_width(0); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 222 | |
| 223 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 224 | float rpm = ::std::min(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 225 | if (flip) { |
| 226 | rpm *= -1.0f; |
| 227 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 228 | vesc_set_rpm(0, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 229 | } |
| 230 | |
| 231 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 232 | float rpm = ::std::max(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 233 | if (flip) { |
| 234 | rpm *= -1.0f; |
| 235 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 236 | vesc_set_rpm(1, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 237 | } |
| 238 | } |
| 239 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 240 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 241 | const float value = convert_input_width(1); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 242 | |
| 243 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 244 | float rpm = ::std::min(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 245 | if (flip) { |
| 246 | rpm *= -1.0f; |
| 247 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 248 | vesc_set_rpm(2, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 249 | } |
| 250 | |
| 251 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 252 | float rpm = ::std::max(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 253 | if (flip) { |
| 254 | rpm *= -1.0f; |
| 255 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 256 | vesc_set_rpm(3, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 257 | } |
| 258 | } |
| 259 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 260 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 261 | const float value = convert_input_width(4); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 262 | |
| 263 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 264 | float rpm = ::std::min(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 265 | if (flip) { |
| 266 | rpm *= -1.0f; |
| 267 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 268 | vesc_set_rpm(4, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 269 | } |
| 270 | |
| 271 | { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 272 | float rpm = ::std::max(0.0f, value) * kMaxRpm; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 273 | if (flip) { |
| 274 | rpm *= -1.0f; |
| 275 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 276 | vesc_set_rpm(5, rpm); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 277 | } |
| 278 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 279 | // Give the CAN frames a chance to go out. |
| 280 | delay(5); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 281 | } |
| 282 | } |
| 283 | |
| 284 | void SetupPwmFtm(BigFTM *ftm) { |
| 285 | ftm->MODE = FTM_MODE_WPDIS; |
| 286 | ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; |
| 287 | ftm->SC = FTM_SC_CLKS(0) /* Disable counting for now */; |
| 288 | |
| 289 | // Can't change MOD according to the reference manual ("The Dual Edge Capture |
| 290 | // mode must be used with ... the FTM counter in Free running counter."). |
| 291 | ftm->MOD = 0xFFFF; |
| 292 | |
| 293 | // Capturing rising edge. |
| 294 | ftm->C0SC = FTM_CSC_MSA | FTM_CSC_ELSA; |
| 295 | // Capturing falling edge. |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 296 | ftm->C1SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 297 | |
| 298 | // Capturing rising edge. |
| 299 | ftm->C2SC = FTM_CSC_MSA | FTM_CSC_ELSA; |
| 300 | // Capturing falling edge. |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 301 | ftm->C3SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 302 | |
| 303 | // Capturing rising edge. |
| 304 | ftm->C4SC = FTM_CSC_MSA | FTM_CSC_ELSA; |
| 305 | // Capturing falling edge. |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 306 | ftm->C5SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 307 | |
| 308 | // Capturing rising edge. |
| 309 | ftm->C6SC = FTM_CSC_MSA | FTM_CSC_ELSA; |
| 310 | // Capturing falling edge. |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 311 | ftm->C7SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 312 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 313 | (void)ftm->STATUS; |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 314 | ftm->STATUS = 0x00; |
| 315 | |
| 316 | ftm->COMBINE = FTM_COMBINE_DECAP3 | FTM_COMBINE_DECAPEN3 | |
| 317 | FTM_COMBINE_DECAP2 | FTM_COMBINE_DECAPEN2 | |
| 318 | FTM_COMBINE_DECAP1 | FTM_COMBINE_DECAPEN1 | |
| 319 | FTM_COMBINE_DECAP0 | FTM_COMBINE_DECAPEN0; |
| 320 | |
| 321 | // 34.95ms max period before it starts wrapping and being weird. |
| 322 | ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ | |
| 323 | FTM_SC_PS(4) /* Prescaler=32 */; |
| 324 | |
| 325 | ftm->MODE &= ~FTM_MODE_WPDIS; |
| 326 | } |
| 327 | |
Brian Silverman | 2de95d6 | 2018-03-31 12:32:24 -0700 | [diff] [blame] | 328 | struct AccelerometerResult { |
| 329 | uint16_t result; |
| 330 | bool success; |
| 331 | }; |
| 332 | |
| 333 | // Does a transfer on the accelerometer. Returns the resulting frame, or a |
| 334 | // failure if it takes until after end_micros. |
| 335 | AccelerometerResult AccelerometerTransfer(uint16_t data, uint32_t end_micros) { |
| 336 | SPI0_SR = SPI_SR_RFDF; |
| 337 | SPI0_PUSHR = SPI_PUSHR_PCS(1) | data; |
| 338 | |
| 339 | while (!(SPI0_SR & SPI_SR_RFDF)) { |
| 340 | if (time_after(micros(), end_micros)) { |
| 341 | return {0, false}; |
| 342 | } |
| 343 | } |
| 344 | const uint32_t popr = SPI0_POPR; |
| 345 | SPI0_SR = SPI_SR_RFDF; |
| 346 | return {static_cast<uint16_t>(popr & 0xFFFF), true}; |
| 347 | } |
| 348 | |
| 349 | constexpr uint32_t kAccelerometerTimeout = 500; |
| 350 | |
| 351 | bool AccelerometerWrite(uint8_t address, uint8_t data, uint32_t end_micros) { |
| 352 | const AccelerometerResult r = AccelerometerTransfer( |
| 353 | (static_cast<uint16_t>(address) << 8) | static_cast<uint16_t>(data), |
| 354 | end_micros); |
| 355 | return r.success; |
| 356 | } |
| 357 | |
| 358 | AccelerometerResult AccelerometerRead(uint8_t address, uint32_t end_micros) { |
| 359 | AccelerometerResult r = AccelerometerTransfer( |
| 360 | (static_cast<uint16_t>(address) << 8) | UINT16_C(0x8000), end_micros); |
| 361 | r.result = r.result & UINT16_C(0xFF); |
| 362 | return r; |
| 363 | } |
| 364 | |
| 365 | bool accelerometer_inited = false; |
| 366 | |
| 367 | void AccelerometerInit() { |
| 368 | accelerometer_inited = false; |
| 369 | const uint32_t end_micros = time_add(micros(), kAccelerometerTimeout); |
| 370 | { |
| 371 | const auto who_am_i = AccelerometerRead(0xF, end_micros); |
| 372 | if (!who_am_i.success) { |
| 373 | return; |
| 374 | } |
| 375 | if (who_am_i.result != 0x32) { |
| 376 | return; |
| 377 | } |
| 378 | } |
| 379 | if (!AccelerometerWrite( |
| 380 | 0x20, (1 << 5) /* Normal mode */ | (1 << 3) /* 100 Hz */ | |
| 381 | (1 << 2) /* Z enabled */ | (1 << 1) /* Y enabled */ | |
| 382 | (1 << 0) /* X enabled */, |
| 383 | end_micros)) { |
| 384 | } |
| 385 | // If want to read LSB, need to enable BDU to avoid splitting reads. |
| 386 | if (!AccelerometerWrite(0x23, (0 << 6) /* Data LSB at lower address */ | |
| 387 | (3 << 4) /* 400g full scale */ | |
| 388 | (0 << 0) /* 4-wire interface */, |
| 389 | end_micros)) { |
| 390 | } |
| 391 | accelerometer_inited = true; |
| 392 | } |
| 393 | |
| 394 | float AccelerometerConvert(uint16_t value) { |
| 395 | return static_cast<float>(400.0 / 65536.0) * static_cast<float>(value); |
| 396 | } |
| 397 | |
| 398 | // Returns the total acceleration (in any direction) or 0 if there's an error. |
| 399 | float ReadAccelerometer() { |
| 400 | if (!accelerometer_inited) { |
| 401 | AccelerometerInit(); |
| 402 | return 0; |
| 403 | } |
| 404 | |
| 405 | const uint32_t end_micros = time_add(micros(), kAccelerometerTimeout); |
| 406 | const auto x = AccelerometerRead(0x29, end_micros); |
| 407 | const auto y = AccelerometerRead(0x2B, end_micros); |
| 408 | const auto z = AccelerometerRead(0x2D, end_micros); |
| 409 | if (!x.success || !y.success || !z.success) { |
| 410 | accelerometer_inited = false; |
| 411 | return 0; |
| 412 | } |
| 413 | |
| 414 | const float x_g = AccelerometerConvert(x.result); |
| 415 | const float y_g = AccelerometerConvert(y.result); |
| 416 | const float z_g = AccelerometerConvert(z.result); |
| 417 | return ::std::sqrt(x_g * x_g + y_g * y_g + z_g * z_g); |
| 418 | } |
| 419 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 420 | extern "C" void ftm0_isr() { |
| 421 | while (true) { |
| 422 | const uint32_t status = FTM0->STATUS; |
| 423 | if (status == 0) { |
| 424 | return; |
| 425 | } |
| 426 | |
| 427 | if (status & (1 << 1)) { |
| 428 | const uint32_t start = FTM0->C0V; |
| 429 | const uint32_t end = FTM0->C1V; |
| 430 | pwm_input_widths[0] = (end - start) & 0xFFFF; |
| 431 | pwm_input_times[0] = millis(); |
| 432 | } |
| 433 | if (status & (1 << 7)) { |
| 434 | const uint32_t start = FTM0->C6V; |
| 435 | const uint32_t end = FTM0->C7V; |
| 436 | pwm_input_widths[1] = (end - start) & 0xFFFF; |
| 437 | pwm_input_times[1] = millis(); |
| 438 | } |
| 439 | if (status & (1 << 5)) { |
| 440 | const uint32_t start = FTM0->C4V; |
| 441 | const uint32_t end = FTM0->C5V; |
| 442 | pwm_input_widths[2] = (end - start) & 0xFFFF; |
| 443 | pwm_input_times[2] = millis(); |
| 444 | } |
| 445 | if (status & (1 << 3)) { |
| 446 | const uint32_t start = FTM0->C2V; |
| 447 | const uint32_t end = FTM0->C3V; |
| 448 | pwm_input_widths[4] = (end - start) & 0xFFFF; |
| 449 | pwm_input_times[4] = millis(); |
| 450 | } |
| 451 | |
| 452 | FTM0->STATUS = 0; |
| 453 | } |
| 454 | } |
| 455 | |
| 456 | extern "C" void ftm3_isr() { |
| 457 | while (true) { |
| 458 | const uint32_t status = FTM3->STATUS; |
| 459 | if (status == 0) { |
| 460 | return; |
| 461 | } |
| 462 | |
| 463 | if (status & (1 << 3)) { |
| 464 | const uint32_t start = FTM3->C2V; |
| 465 | const uint32_t end = FTM3->C3V; |
| 466 | pwm_input_widths[3] = (end - start) & 0xFFFF; |
| 467 | pwm_input_times[3] = millis(); |
| 468 | } |
Brian Silverman | 7f5f144 | 2018-04-06 13:00:50 -0400 | [diff] [blame] | 469 | if (status & (1 << 7)) { |
| 470 | const uint32_t start = FTM3->C6V; |
| 471 | const uint32_t end = FTM3->C7V; |
| 472 | pwm_input_widths[5] = (end - start) & 0xFFFF; |
| 473 | pwm_input_times[5] = millis(); |
| 474 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 475 | |
| 476 | FTM3->STATUS = 0; |
| 477 | } |
| 478 | } |
| 479 | |
| 480 | float ConvertEncoderChannel(uint16_t reading) { |
| 481 | // Theoretical values based on the datasheet are 931 and 2917. |
| 482 | // With these values, the magnitude ranges from 0.99-1.03, which works fine |
| 483 | // (the encoder's output appears to get less accurate in one quadrant for some |
| 484 | // reason, hence the variation). |
| 485 | static constexpr uint16_t kMin = 802, kMax = 3088; |
| 486 | if (reading < kMin) { |
| 487 | reading = kMin; |
| 488 | } else if (reading > kMax) { |
| 489 | reading = kMax; |
| 490 | } |
| 491 | return (static_cast<float>(2 * (reading - kMin)) / |
| 492 | static_cast<float>(kMax - kMin)) - |
| 493 | 1.0f; |
| 494 | } |
| 495 | |
| 496 | struct EncoderReading { |
Brian Silverman | a96c1a4 | 2018-05-12 12:11:31 -0700 | [diff] [blame] | 497 | EncoderReading(const SimpleAdcReadings &adc_readings) { |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 498 | const float sin = ConvertEncoderChannel(adc_readings.sin); |
| 499 | const float cos = ConvertEncoderChannel(adc_readings.cos); |
| 500 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 501 | const float magnitude = hypot(sin, cos); |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 502 | const float magnitude_error = ::std::abs(magnitude - 1.0f); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 503 | valid = magnitude_error < 0.30f; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 504 | |
| 505 | angle = ::std::atan2(sin, cos); |
| 506 | } |
| 507 | |
| 508 | // Angle in radians, in [-pi, pi]. |
| 509 | float angle; |
| 510 | |
| 511 | bool valid; |
| 512 | }; |
| 513 | |
| 514 | extern "C" void pit3_isr() { |
| 515 | PIT_TFLG3 = 1; |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 516 | PolyDrivetrain<float> *polydrivetrain = |
| 517 | global_polydrivetrain.load(::std::memory_order_acquire); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 518 | Spring *spring = global_spring.load(::std::memory_order_acquire); |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 519 | |
Brian Silverman | a96c1a4 | 2018-05-12 12:11:31 -0700 | [diff] [blame] | 520 | SimpleAdcReadings adc_readings; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 521 | { |
| 522 | DisableInterrupts disable_interrupts; |
Brian Silverman | a96c1a4 | 2018-05-12 12:11:31 -0700 | [diff] [blame] | 523 | adc_readings = AdcReadSimple(disable_interrupts); |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 524 | } |
| 525 | |
| 526 | EncoderReading encoder(adc_readings); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 527 | static float last_good_encoder = 0.0f; |
| 528 | static int invalid_encoder_count = 0; |
| 529 | if (encoder.valid) { |
| 530 | last_good_encoder = encoder.angle; |
| 531 | invalid_encoder_count = 0; |
| 532 | } else { |
| 533 | ++invalid_encoder_count; |
| 534 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 535 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 536 | const bool lost_any_channel = lost_channel(0) || lost_channel(1) || |
| 537 | lost_channel(2) || lost_channel(3) || |
| 538 | lost_channel(4) || |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 539 | (::std::abs(convert_input_width(4)) < 0.5f); |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 540 | |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 541 | if (polydrivetrain != nullptr && spring != nullptr) { |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 542 | DrivetrainQueue_Goal goal; |
| 543 | goal.control_loop_driving = false; |
| 544 | if (lost_any_channel) { |
| 545 | goal.throttle = 0.0f; |
| 546 | goal.wheel = 0.0f; |
| 547 | } else { |
| 548 | goal.throttle = convert_input_width(1); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 549 | goal.wheel = -convert_input_width(0); |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 550 | } |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 551 | goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f; |
| 552 | |
| 553 | DrivetrainQueue_Output output; |
| 554 | |
| 555 | polydrivetrain->SetGoal(goal); |
| 556 | polydrivetrain->Update(); |
| 557 | polydrivetrain->SetOutput(&output); |
| 558 | |
| 559 | vesc_set_duty(0, -output.left_voltage / 12.0f); |
| 560 | vesc_set_duty(1, -output.left_voltage / 12.0f); |
| 561 | |
| 562 | vesc_set_duty(2, output.right_voltage / 12.0f); |
| 563 | vesc_set_duty(3, output.right_voltage / 12.0f); |
| 564 | |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 565 | bool prime = convert_input_width(2) > 0.1f; |
| 566 | bool fire = convert_input_width(3) > 0.1f; |
| 567 | |
| 568 | bool unload = lost_any_channel; |
| 569 | static bool was_lost = true; |
| 570 | bool force_reset = !lost_any_channel && was_lost; |
| 571 | was_lost = lost_any_channel; |
| 572 | |
| 573 | spring->Iterate(unload, prime, fire, force_reset, invalid_encoder_count <= 2, |
| 574 | last_good_encoder); |
| 575 | |
| 576 | float spring_output = spring->output(); |
| 577 | |
| 578 | vesc_set_duty(4, -spring_output); |
| 579 | vesc_set_duty(5, spring_output); |
| 580 | |
Brian Silverman | 2de95d6 | 2018-03-31 12:32:24 -0700 | [diff] [blame] | 581 | const float accelerometer = ReadAccelerometer(); |
| 582 | (void)accelerometer; |
| 583 | |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 584 | /* |
| 585 | // Massive debug. Turn on for useful bits. |
Brian Silverman | 2de95d6 | 2018-03-31 12:32:24 -0700 | [diff] [blame] | 586 | printf("acc %d/1000\n", (int)(accelerometer / 1000)); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 587 | if (!encoder.valid) { |
| 588 | printf("Stuck encoder: ADC %" PRIu16 " %" PRIu16 |
| 589 | " enc %d/1000 %s mag %d\n", |
| 590 | adc_readings.sin, adc_readings.cos, (int)(encoder.angle * 1000), |
| 591 | encoder.valid ? "T" : "f", |
| 592 | (int)(hypot(ConvertEncoderChannel(adc_readings.sin), |
| 593 | ConvertEncoderChannel(adc_readings.cos)) * |
| 594 | 1000)); |
| 595 | } |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 596 | static int i = 0; |
| 597 | ++i; |
| 598 | if (i > 20) { |
| 599 | i = 0; |
| 600 | if (lost_any_channel) { |
| 601 | printf("200Hz loop, disabled %d %d %d %d %d\n", |
| 602 | (int)(convert_input_width(0) * 1000), |
| 603 | (int)(convert_input_width(1) * 1000), |
| 604 | (int)(convert_input_width(2) * 1000), |
| 605 | (int)(convert_input_width(3) * 1000), |
| 606 | (int)(convert_input_width(4) * 1000)); |
| 607 | } else { |
| 608 | printf( |
| 609 | "TODO(Austin): 200Hz loop %d %d %d %d %d, lr, %d, %d velocity %d " |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 610 | " state: %d, near %d angle %d goal %d to: %d ADC %" PRIu16 |
| 611 | " %" PRIu16 " enc %d/1000 %s from %d\n", |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 612 | (int)(convert_input_width(0) * 1000), |
| 613 | (int)(convert_input_width(1) * 1000), |
| 614 | (int)(convert_input_width(2) * 1000), |
| 615 | (int)(convert_input_width(3) * 1000), |
| 616 | (int)(convert_input_width(4) * 1000), |
| 617 | static_cast<int>(output.left_voltage * 100), |
| 618 | static_cast<int>(output.right_voltage * 100), |
| 619 | static_cast<int>(polydrivetrain->velocity() * 100), |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 620 | static_cast<int>(spring->state()), static_cast<int>(spring->Near()), |
| 621 | static_cast<int>(spring->angle() * 1000), |
| 622 | static_cast<int>(spring->goal() * 1000), |
| 623 | static_cast<int>(spring->timeout()), adc_readings.sin, |
| 624 | adc_readings.cos, (int)(encoder.angle * 1000), |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 625 | encoder.valid ? "T" : "f", |
| 626 | (int)(::std::sqrt(ConvertEncoderChannel(adc_readings.sin) * |
| 627 | ConvertEncoderChannel(adc_readings.sin) + |
| 628 | ConvertEncoderChannel(adc_readings.cos) * |
| 629 | ConvertEncoderChannel(adc_readings.cos)) * |
| 630 | 1000)); |
| 631 | } |
| 632 | } |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 633 | */ |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 634 | } |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 635 | } |
| 636 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 637 | } // namespace |
| 638 | |
| 639 | extern "C" { |
| 640 | |
| 641 | void *__stack_chk_guard = (void *)0x67111971; |
| 642 | |
| 643 | int _write(int /*file*/, char *ptr, int len) { |
| 644 | teensy::AcmTty *const tty = global_stdout.load(::std::memory_order_acquire); |
| 645 | if (tty != nullptr) { |
| 646 | return tty->Write(ptr, len); |
| 647 | } |
| 648 | return 0; |
| 649 | } |
| 650 | |
| 651 | void __stack_chk_fail(void); |
| 652 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 653 | } // extern "C" |
| 654 | |
| 655 | extern "C" int main(void) { |
| 656 | // for background about this startup delay, please see these conversations |
| 657 | // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980 |
| 658 | // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273 |
| 659 | delay(400); |
| 660 | |
| 661 | // Set all interrupts to the second-lowest priority to start with. |
| 662 | for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD); |
| 663 | |
| 664 | // Now set priorities for all the ones we care about. They only have meaning |
| 665 | // relative to each other, which means centralizing them here makes it a lot |
| 666 | // more manageable. |
| 667 | NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7); |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 668 | NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0xa); |
| 669 | NVIC_SET_SANE_PRIORITY(IRQ_FTM3, 0xa); |
| 670 | NVIC_SET_SANE_PRIORITY(IRQ_PIT_CH3, 0x5); |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 671 | |
| 672 | // Builtin LED. |
| 673 | PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1; |
| 674 | PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1); |
| 675 | PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1; |
| 676 | |
| 677 | // Set up the CAN pins. |
| 678 | PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 679 | PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 680 | |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 681 | // PWM_IN0 |
| 682 | // FTM0_CH0 |
| 683 | PORTC_PCR1 = PORT_PCR_MUX(4); |
| 684 | |
| 685 | // PWM_IN1 |
| 686 | // FTM0_CH6 |
| 687 | PORTD_PCR6 = PORT_PCR_MUX(4); |
| 688 | |
| 689 | // PWM_IN2 |
| 690 | // FTM0_CH4 |
| 691 | PORTD_PCR4 = PORT_PCR_MUX(4); |
| 692 | |
| 693 | // PWM_IN3 |
| 694 | // FTM3_CH2 |
| 695 | PORTD_PCR2 = PORT_PCR_MUX(4); |
| 696 | |
| 697 | // PWM_IN4 |
| 698 | // FTM0_CH2 |
| 699 | PORTC_PCR3 = PORT_PCR_MUX(4); |
| 700 | |
Brian Silverman | 7f5f144 | 2018-04-06 13:00:50 -0400 | [diff] [blame] | 701 | // PWM_IN5 |
| 702 | // FTM3_CH6 |
| 703 | PORTC_PCR10 = PORT_PCR_MUX(3); |
| 704 | |
Brian Silverman | 2de95d6 | 2018-03-31 12:32:24 -0700 | [diff] [blame] | 705 | // SPI0 |
| 706 | // ACC_CS PCS0 |
| 707 | PORTA_PCR14 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 708 | // SCK |
| 709 | PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 710 | // MOSI |
| 711 | PORTA_PCR16 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 712 | // MISO |
| 713 | PORTA_PCR17 = PORT_PCR_DSE | PORT_PCR_MUX(2); |
| 714 | |
| 715 | SIM_SCGC6 |= SIM_SCGC6_SPI0; |
| 716 | SPI0_MCR = SPI_MCR_MSTR | SPI_MCR_PCSIS(1) | SPI_MCR_CLR_TXF | |
| 717 | SPI_MCR_CLR_RXF | SPI_MCR_HALT; |
| 718 | // 60 MHz "protocol clock" |
| 719 | // 6ns CS setup |
| 720 | // 8ns CS hold |
| 721 | SPI0_CTAR0 = SPI_CTAR_FMSZ(15) | SPI_CTAR_CPOL /* Clock idles high */ | |
| 722 | SPI_CTAR_CPHA /* Data captured on trailing edge */ | |
| 723 | 0 /* !LSBFE MSB first */ | |
| 724 | SPI_CTAR_PCSSCK(0) /* PCS->SCK prescaler = 1 */ | |
| 725 | SPI_CTAR_PASC(0) /* SCK->PCS prescaler = 1 */ | |
| 726 | SPI_CTAR_PDT(0) /* PCS->PCS prescaler = 1 */ | |
| 727 | SPI_CTAR_PBR(0) /* baud prescaler = 1 */ | |
| 728 | SPI_CTAR_CSSCK(0) /* PCS->SCK 2/60MHz = 33.33ns */ | |
| 729 | SPI_CTAR_ASC(0) /* SCK->PCS 2/60MHz = 33.33ns */ | |
| 730 | SPI_CTAR_DT(2) /* PCS->PSC 8/60MHz = 133.33ns */ | |
| 731 | SPI_CTAR_BR(2) /* BR 60MHz/6 = 10MHz */; |
| 732 | |
| 733 | SPI0_MCR &= ~SPI_MCR_HALT; |
| 734 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 735 | delay(100); |
| 736 | |
| 737 | teensy::UsbDevice usb_device(0, 0x16c0, 0x0492); |
| 738 | usb_device.SetManufacturer("Seems Reasonable LLC"); |
| 739 | usb_device.SetProduct("Simple Receiver Board"); |
| 740 | |
| 741 | teensy::AcmTty tty0(&usb_device); |
| 742 | global_stdout.store(&tty0, ::std::memory_order_release); |
| 743 | usb_device.Initialize(); |
| 744 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 745 | SIM_SCGC6 |= SIM_SCGC6_PIT; |
| 746 | // Workaround for errata e7914. |
| 747 | (void)PIT_MCR; |
| 748 | PIT_MCR = 0; |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 749 | PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 200) - 1; |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 750 | PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN; |
| 751 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 752 | can_init(0, 1); |
Brian Silverman | a96c1a4 | 2018-05-12 12:11:31 -0700 | [diff] [blame] | 753 | AdcInitSimple(); |
Brian Silverman | 4f8c6a7 | 2018-03-17 23:12:45 -0700 | [diff] [blame] | 754 | SetupPwmFtm(FTM0); |
| 755 | SetupPwmFtm(FTM3); |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 756 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 757 | PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr); |
| 758 | global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release); |
Austin Schuh | 4fae0fc | 2018-03-27 23:51:42 -0700 | [diff] [blame] | 759 | Spring spring; |
| 760 | global_spring.store(&spring, ::std::memory_order_release); |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 761 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 762 | // Leave the LEDs on for a bit longer. |
| 763 | delay(300); |
| 764 | printf("Done starting up\n"); |
| 765 | |
Brian Silverman | 2de95d6 | 2018-03-31 12:32:24 -0700 | [diff] [blame] | 766 | AccelerometerInit(); |
| 767 | printf("Accelerometer init %s\n", accelerometer_inited ? "success" : "fail"); |
| 768 | |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 769 | // Done starting up, now turn the LED off. |
| 770 | PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0; |
| 771 | |
Brian Silverman | a3a172b | 2018-03-24 03:53:32 -0400 | [diff] [blame] | 772 | NVIC_ENABLE_IRQ(IRQ_FTM0); |
| 773 | NVIC_ENABLE_IRQ(IRQ_FTM3); |
| 774 | NVIC_ENABLE_IRQ(IRQ_PIT_CH3); |
| 775 | |
Austin Schuh | bcce26a | 2018-03-26 23:41:24 -0700 | [diff] [blame] | 776 | while (true) { |
| 777 | } |
Brian Silverman | 54dd2fe | 2018-03-16 23:44:31 -0700 | [diff] [blame] | 778 | |
| 779 | return 0; |
| 780 | } |
| 781 | |
| 782 | void __stack_chk_fail(void) { |
| 783 | while (true) { |
| 784 | GPIOC_PSOR = (1 << 5); |
| 785 | printf("Stack corruption detected\n"); |
| 786 | delay(1000); |
| 787 | GPIOC_PCOR = (1 << 5); |
| 788 | delay(1000); |
| 789 | } |
| 790 | } |
| 791 | |
| 792 | } // namespace motors |
| 793 | } // namespace frc971 |