blob: 61562beaca7e994db9e68a7acbe823783489a861 [file] [log] [blame]
Brian Silvermand9566392018-06-10 15:02:03 -07001#include <inttypes.h>
2#include <stdio.h>
3
4#include <atomic>
5
Philipp Schrader790cb542023-07-05 21:06:52 -07006#include "motors/core/kinetis.h"
Brian Silvermand9566392018-06-10 15:02:03 -07007#include "motors/core/time.h"
8#include "motors/fet12/motor_controls.h"
9#include "motors/motor.h"
10#include "motors/peripheral/adc.h"
11#include "motors/peripheral/can.h"
Brian Silverman4787a6e2018-10-06 16:00:54 -070012#include "motors/print/print.h"
Brian Silvermand9566392018-06-10 15:02:03 -070013#include "motors/util.h"
14
Stephan Pleinesf63bde82024-01-13 15:59:33 -080015namespace frc971::motors {
Brian Silvermand9566392018-06-10 15:02:03 -070016namespace {
17
18struct Fet12AdcReadings {
19 uint16_t motor_currents[3];
20};
21
22void AdcInitFet12() {
23 AdcInitCommon();
24
25 // M_CH0V ADC1_SE13
26 PORTB_PCR7 = PORT_PCR_MUX(0);
27
28 // M_CH1V ADC1_SE14
29 PORTB_PCR10 = PORT_PCR_MUX(0);
30
31 // M_CH2V ADC1_SE4b
32 PORTC_PCR10 = PORT_PCR_MUX(0);
33
34 // M_CH0F ADC0_SE16
35 // dedicated
36
37 // M_CH1F ADC0_SE18
38 PORTE_PCR24 = PORT_PCR_MUX(0);
39
40 // M_CH2F ADC0_SE23
41 // dedicated
42}
43
44Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
45 Fet12AdcReadings r;
46
47 ADC0_SC1A = 16;
48 while (!(ADC0_SC1A & ADC_SC1_COCO)) {
49 }
50 ADC0_SC1A = 18;
51 r.motor_currents[0] = ADC0_RA;
52 while (!(ADC0_SC1A & ADC_SC1_COCO)) {
53 }
54 ADC0_SC1A = 23;
55 r.motor_currents[1] = ADC0_RA;
56 while (!(ADC0_SC1A & ADC_SC1_COCO)) {
57 }
58 r.motor_currents[2] = ADC0_RA;
59
60 return r;
61}
62
63::std::atomic<Motor *> global_motor{nullptr};
Brian Silvermand9566392018-06-10 15:02:03 -070064
65extern "C" {
66
67void *__stack_chk_guard = (void *)0x67111971;
68void __stack_chk_fail(void) {
69 while (true) {
70 GPIOC_PSOR = (1 << 5);
71 printf("Stack corruption detected\n");
72 delay(1000);
73 GPIOC_PCOR = (1 << 5);
74 delay(1000);
75 }
76}
77
Brian Silvermand9566392018-06-10 15:02:03 -070078extern char *__brkval;
79extern uint32_t __bss_ram_start__[];
80extern uint32_t __heap_start__[];
81extern uint32_t __stack_end__[];
82
83void ftm0_isr(void) {
84 Fet12AdcReadings adc_readings;
85 {
86 DisableInterrupts disable_interrupts;
87 adc_readings = AdcReadFet12(disable_interrupts);
88 }
89#if 1
90 printf("%" PRIu16 " %" PRIu16 " %" PRIu16 "\n",
91 adc_readings.motor_currents[0], adc_readings.motor_currents[1],
92 adc_readings.motor_currents[2]);
93#endif
94 const BalancedReadings balanced =
95 BalanceSimpleReadings(adc_readings.motor_currents);
96
97#if 0
98 global_motor.load(::std::memory_order_relaxed)->HandleInterrupt(
99 balanced,
100 global_motor.load(::std::memory_order_relaxed)->wrapped_encoder());
101#else
102 (void)balanced;
103#endif
104 FTM0->SC &= ~FTM_SC_TOF;
105 FTM0->C0V = 0;
106 FTM0->C1V = 0;
107 FTM0->C2V = 0;
108 FTM0->C3V = 0;
109 FTM0->C4V = 0;
110 FTM0->C5V = 80;
111 FTM0->PWMLOAD = FTM_PWMLOAD_LDOK;
112}
113
114} // extern "C"
115
116void ConfigurePwmFtm(BigFTM *pwm_ftm) {
117 // Put them all into combine active-high mode, and all the low ones staying on
118 // all the time by default.
119 pwm_ftm->C0SC = FTM_CSC_ELSA;
120 pwm_ftm->C0V = 0;
121 pwm_ftm->C1SC = FTM_CSC_ELSA;
122 pwm_ftm->C1V = 0;
123 pwm_ftm->C2SC = FTM_CSC_ELSA;
124 pwm_ftm->C2V = 0;
125 pwm_ftm->C3SC = FTM_CSC_ELSA;
126 pwm_ftm->C3V = 0;
127 pwm_ftm->C4SC = FTM_CSC_ELSA;
128 pwm_ftm->C4V = 0;
129 pwm_ftm->C5SC = FTM_CSC_ELSA;
130 pwm_ftm->C5V = 0;
131 pwm_ftm->C6SC = FTM_CSC_ELSA;
132 pwm_ftm->C6V = 0;
133 pwm_ftm->C7SC = FTM_CSC_ELSA;
134 pwm_ftm->C7V = 0;
135
136 pwm_ftm->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
137 FTM_COMBINE_DTEN3 /* Enable deadtime */ |
138 FTM_COMBINE_COMP3 /* Make them complementary */ |
139 FTM_COMBINE_COMBINE3 /* Combine the channels */ |
140 FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
141 FTM_COMBINE_DTEN2 /* Enable deadtime */ |
142 FTM_COMBINE_COMP2 /* Make them complementary */ |
143 FTM_COMBINE_COMBINE2 /* Combine the channels */ |
144 FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
145 FTM_COMBINE_DTEN1 /* Enable deadtime */ |
146 FTM_COMBINE_COMP1 /* Make them complementary */ |
147 FTM_COMBINE_COMBINE1 /* Combine the channels */ |
148 FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */ |
149 FTM_COMBINE_DTEN0 /* Enable deadtime */ |
150 FTM_COMBINE_COMP0 /* Make them complementary */ |
151 FTM_COMBINE_COMBINE0 /* Combine the channels */;
152
153 // Set the deadtime.
154 pwm_ftm->DEADTIME =
155 FTM_DEADTIME_DTPS(0) /* Prescaler of 1 */ | FTM_DEADTIME_DTVAL(9);
156}
157
158// Zeros the encoder. This involves blocking for an arbitrary length of time
159// with interrupts disabled.
160void ZeroMotor() {
161#if 0
162 while (true) {
163 if (PERIPHERAL_BITBAND(GPIOA_PDIR, 7)) {
164 encoder_ftm_->CNT = 0;
165 break;
166 }
167 }
168#else
169 uint32_t scratch;
170 __disable_irq();
171 // Stuff all of this in an inline assembly statement so we can make sure the
172 // compiler doesn't decide sticking constant loads etc in the middle of
173 // the loop is a good idea, because that increases the latency of recognizing
174 // the index pulse edge which makes velocity affect the zeroing accuracy.
175 __asm__ __volatile__(
176 // A label to restart the loop.
177 "0:\n"
178 // Load the current PDIR value for the pin we care about.
179 "ldr %[scratch], [%[pdir_word]]\n"
180 // Terminate the loop if it's non-0.
181 "cbnz %[scratch], 1f\n"
182 // Go back around again.
183 "b 0b\n"
184 // A label to finish the loop.
185 "1:\n"
186 // Reset the count once we're down here. It doesn't actually matter what
187 // value we store because writing anything resets it to CNTIN (ie 0).
188 "str %[scratch], [%[cnt]]\n"
189 : [scratch] "=&l"(scratch)
Philipp Schrader790cb542023-07-05 21:06:52 -0700190 : [pdir_word] "l"(&PERIPHERAL_BITBAND(GPIOA_PDIR, 7)), [cnt] "l"(
191 &FTM1->CNT));
Brian Silvermand9566392018-06-10 15:02:03 -0700192 __enable_irq();
193#endif
194}
195
196} // namespace
197
198extern "C" int main(void) {
199 // for background about this startup delay, please see these conversations
200 // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
201 // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
202 delay(400);
203
204 // Set all interrupts to the second-lowest priority to start with.
205 for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
206
207 // Now set priorities for all the ones we care about. They only have meaning
208 // relative to each other, which means centralizing them here makes it a lot
209 // more manageable.
210 NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
211 NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
212
213 // Set the LED's pin to output mode.
214 PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
215 PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
216
217#if 0
218 PERIPHERAL_BITBAND(GPIOA_PDDR, 15) = 1;
219 PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
220#endif
221
222 // Set up the CAN pins.
223 PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
224 PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
225
Brian Silverman45564a82018-09-02 16:35:22 -0700226 DMA.CR = M_DMA_EMLM;
Brian Silvermand9566392018-06-10 15:02:03 -0700227
Brian Silverman4787a6e2018-10-06 16:00:54 -0700228 PrintingParameters printing_parameters;
229 printing_parameters.dedicated_usb = true;
230 const ::std::unique_ptr<PrintingImplementation> printing =
231 CreatePrinting(printing_parameters);
232 printing->Initialize();
Brian Silvermand9566392018-06-10 15:02:03 -0700233
234 AdcInitFet12();
235 MathInit();
236 delay(1000);
237 can_init(0, 1);
238
239#if 0
240 GPIOD_PCOR = 1 << 3;
241 PERIPHERAL_BITBAND(GPIOD_PDDR, 3) = 1;
242 PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
243 delay(1000);
244 GPIOD_PSOR = 1 << 3;
245 delay(1000);
246 GPIOD_PCOR = 1 << 3;
247 delay(1000);
248#endif
249
250 MotorControlsImplementation controls;
251
252 delay(1000);
253
254 // Index pin
255 PORTA_PCR7 = PORT_PCR_MUX(1);
256 // FTM1_QD_PH{A,B}
257 PORTB_PCR0 = PORT_PCR_MUX(6);
258 PORTB_PCR1 = PORT_PCR_MUX(6);
259
260 // FTM0_CH[0-5]
261 PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
262 PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
263 PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(4);
264 PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
265 PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
266 PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
267
268 Motor motor(FTM0, FTM1, &controls, {&FTM0->C0V, &FTM0->C2V, &FTM0->C4V});
269 motor.set_encoder_offset(810);
270 motor.set_deadtime_compensation(9);
271 ConfigurePwmFtm(FTM0);
272 motor.Init();
273 global_motor.store(&motor, ::std::memory_order_relaxed);
274 // Output triggers to things like the PDBs on initialization.
275 FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
276 // Don't let any memory accesses sneak past here, because we actually
277 // need everything to be starting up.
Philipp Schrader790cb542023-07-05 21:06:52 -0700278 __asm__("" ::: "memory");
Brian Silvermand9566392018-06-10 15:02:03 -0700279
280 // Give everything a chance to get going.
281 delay(100);
282
283 printf("Ram start: %p\n", __bss_ram_start__);
284 printf("Heap start: %p\n", __heap_start__);
285 printf("Heap end: %p\n", __brkval);
286 printf("Stack start: %p\n", __stack_end__);
287
288 printf("Going silent to zero motors...\n");
289 // Give the print a chance to make it out.
290 delay(1000);
291#if 0
292 ZeroMotor();
Brian Silverman6c8b88b2018-09-03 18:17:02 -0700293#else
294 (void)ZeroMotor;
Brian Silvermand9566392018-06-10 15:02:03 -0700295#endif
296
297 printf("Zeroed motor!\n");
298 // Give stuff a chance to recover from interrupts-disabled.
299 delay(100);
300 motor.Start();
301 NVIC_ENABLE_IRQ(IRQ_FTM0);
302 GPIOC_PSOR = 1 << 5;
303
304 float current_command = 0;
305 while (true) {
306 unsigned char command_data[8];
307 int command_length;
308 can_receive(command_data, &command_length, 0);
309 if (command_length == 4) {
310 uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
311 command_data[2] << 8 | command_data[3];
312 float current = static_cast<float>(result) / 1000.0f;
313
314 static bool high_gear = false;
315 if (controls.estimated_velocity() < -2015) {
316 high_gear = true;
317 }
318 if (current < 1) {
319 high_gear = false;
320 }
321 if (!high_gear) {
322 current = current_command * -120.0f / 120.0f;
323 } else {
324 current = current_command * 115.0f / 120.0f;
325 }
326 motor.SetGoalCurrent(current);
327 current_command = current;
328 }
329 }
330
331 return 0;
332}
333
Stephan Pleinesf63bde82024-01-13 15:59:33 -0800334} // namespace frc971::motors