diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 306 |
1 files changed, 306 insertions, 0 deletions
diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..105d89d --- /dev/null +++ b/main.cpp @@ -0,0 +1,306 @@ +#include <interrupt/interrupt.h> +#include <os/thread.h> +#include <os/time.h> +#include <can/can.h> +#include <usart/usart.h> +#include <gpio/pin.h> +#include <timer/timer.h> +#include <adc/adc.h> + +static Pin& led_ready = PB3; +static Pin& led_error = PB4; + +static Pin& foo = PA9; + +static Pin& a_l = PA7; +static Pin& b_l = PB0; +static Pin& c_l = PB1; + +static Pin& a_h = PB6; +static Pin& b_h = PB7; +static Pin& c_h = PB8; + +static Pin& sense_a = PA1; // ADC IN1 +static Pin& sense_b = PA2; // ADC IN2 +static Pin& sense_c = PA3; // ADC IN3 + +volatile bool sense_rising = 0; + +class Commutator { + private: + int state; + volatile uint32_t* active_pwm; + + public: + Commutator() : state(0), active_pwm(&TIM4.CCR1) {} + + void set_pwm(uint32_t pwm) { + *active_pwm = pwm; + } + + void next() { + uint32_t temp; + + switch(state++) { + case 0: + b_l.off(); + c_l.on(); + ADC1.JSQR = 2 << 15; + sense_rising = true; + break; + + case 1: + temp = *active_pwm; + *active_pwm = 0; + active_pwm = &TIM4.CCR2; + *active_pwm = temp; + ADC1.JSQR = 1 << 15; + sense_rising = false; + break; + + case 2: + c_l.off(); + a_l.on(); + ADC1.JSQR = 3 << 15; + sense_rising = true; + break; + + case 3: + temp = *active_pwm; + *active_pwm = 0; + active_pwm = &TIM4.CCR3; + *active_pwm = temp; + ADC1.JSQR = 2 << 15; + sense_rising = false; + break; + + case 4: + a_l.off(); + b_l.on(); + ADC1.JSQR = 1 << 15; + sense_rising = true; + break; + + case 5: + temp = *active_pwm; + *active_pwm = 0; + active_pwm = &TIM4.CCR1; + *active_pwm = temp; + ADC1.JSQR = 3 << 15; + sense_rising = false; + + state = 0; + break; + } + } +}; + +Commutator commutator; + +volatile uint32_t ctv = 170; + +volatile uint32_t ct = 0; +volatile uint32_t cc = 200; + +volatile bool do_cc_est = 0; +volatile bool done_cc_est = 0; + +template <> +void interrupt<Interrupt::ADC1_2>() { + if(ADC1.SR & 0x04) { + USART1.reg.DR = ADC1.JDR1; + + if(sense_rising ? ADC1.JDR1 > ctv : ADC1.JDR1 < ctv) { + //foo.on(); + if(do_cc_est && !done_cc_est && ct > 10) { + cc = cc * 0.95 + (ct * 2) * 0.05; + done_cc_est = true; + } + } else { + //foo.off(); + } + } + + ADC1.SR = 0; +} + +int main() { + // Initialize system timer. + STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz. + STK.CTRL = 0x03; + + RCC.enable(RCC.AFIO); + RCC.enable(RCC.GPIOA); + RCC.enable(RCC.GPIOB); + + //AFIO.MAPR = 0x02000000; // Free JTAG pins used for LEDs. + + //led_ready.set_mode(Pin::Output); + //led_error.set_mode(Pin::Output); + + RCC.enable(RCC.CAN); + CAN1.enable(); + + PA12.set_mode(Pin::AF); + + foo.set_mode(Pin::AF); + RCC.enable(RCC.USART1); + USART1.set_baudrate(1000000); + USART1.enable(); + USART1.reg.CR1 |= 1 << 12; + + // ADC. + RCC.enable(RCC.ADC1); + + // Calibrate ADC. + ADC1.CR2 = 0x9; + while(ADC1.CR2 & 0x8); + ADC1.CR2 = 0x5; + while(ADC1.CR2 & 0x4); + + sense_a.set_mode(Pin::Analog); + sense_b.set_mode(Pin::Analog); + sense_c.set_mode(Pin::Analog); + + // Motor outputs. + a_l.off(); + a_l.set_mode(Pin::Output); + //b_l.off(); + b_l.on(); + b_l.set_mode(Pin::Output); + c_l.off(); + c_l.set_mode(Pin::Output); + + a_h.set_mode(Pin::AF); + b_h.set_mode(Pin::AF); + c_h.set_mode(Pin::AF); + + RCC.enable(RCC.TIM4); + TIM4.PSC = 0; + TIM4.ARR = 1000; + TIM4.CCER = 0x1111; + TIM4.CCMR1 = 0x6868; + TIM4.CCMR2 = 0x6868; + + TIM4.CR2 = 0x70; + + TIM4.CR1 = 0x40; + TIM4.CR1 = 0x41; + + TIM4.CCR1 = 0; + TIM4.CCR2 = 0; + TIM4.CCR3 = 0; + TIM4.CCR4 = 1; + + Interrupt::enable(Interrupt::ADC1_2); + + ADC1.CR1 |= 0x80; + ADC1.CR2 |= 0xd001; + + ADC1.JSQR = 3 << 15; + + // Give all hardware enough time to initialize. + Time::sleep(200); + + led_ready.on(); + + // Configure CAN. + CAN1.f_reg.FR[0][0] = 0; + CAN1.f_reg.FR[0][1] = 0; + CAN1.f_reg.FS1R[0] = 0x1; + CAN1.f_reg.FA1R[0] = 0x1; + CAN1.f_reg.FMR = 0; + + uint32_t pwm = 0; + + while(1) { + // Check for CAN message. + if(CAN1.reg.RF0R & 0x3) { + switch(CAN1.mb_reg.RDL0R) { + case '1': + led_ready.toggle(); + break; + + case '2': + led_error.toggle(); + break; + + case '+': + pwm += 10; + commutator.set_pwm(pwm); + break; + + case '-': + if(pwm >= 10) { + pwm -= 10; + commutator.set_pwm(pwm); + } + break; + + case 'e': + pwm += 1; + commutator.set_pwm(pwm); + break; + + case 'd': + if(pwm) { + pwm -= 1; + commutator.set_pwm(pwm); + } + break; + + case 'q': + cc += 10; + break; + + case 'a': + cc -= 10; + break; + + case 'w': + cc++; + break; + + case 's': + cc--; + break; + + case 'r': + ctv += 1; + break; + + case 'f': + ctv -= 1; + break; + + case 'y': + do_cc_est = !do_cc_est; + break; + + case '0': + commutator.set_pwm(pwm = 0); + cc = 200; + do_cc_est = false; + break; + } + + // Release mailbox. + CAN1.reg.RF0R = 0x20; + } + + // Check for pwm update. + if(TIM4.SR & 0x01) { + TIM4.SR &= ~0x01; + if(++ct >= cc) { + commutator.next(); + done_cc_est = false; + ct = 0; + } + } + + // Check for pulse center. + if(TIM4.SR & 0x10) { + TIM4.SR &= ~0x10; + } + } +} |