1 files changed, 27 insertions, 0 deletions
diff --git a/main.cpp b/main.cpp
index 08072f7..392ee2d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -22,6 +22,11 @@ Pin mag_cs = GPIOC[14];
 Pin mpu_cs = GPIOC[15];
 Pin flash_cs = GPIOB[2];
 
+Pin pwm1 = GPIOA[0];
+Pin pwm2 = GPIOA[1];
+Pin pwm3 = GPIOA[2];
+Pin pwm4 = GPIOA[3];
+
 Pin s1 = GPIOA[0];
 Pin s2 = GPIOA[1];
 
@@ -97,6 +102,28 @@ int main() {
 	flash_cs.set_mode(Pin::Output);
 	flash_cs.on();
 	
+	pwm1.set_mode(Pin::AF);
+	pwm1.set_af(1);
+	pwm2.set_mode(Pin::AF);
+	pwm2.set_af(1);
+	pwm3.set_mode(Pin::AF);
+	pwm3.set_af(1);
+	pwm4.set_mode(Pin::AF);
+	pwm4.set_af(1);
+	
+	RCC.enable(RCC.TIM2);
+	
+	TIM2.PSC = 71;
+	TIM2.ARR = 2500;
+	TIM2.CCER = (1 << 12) | (1 << 8) | (1 << 4) | (1 << 0);
+	TIM2.CCMR1 = (6 << 12) | (1 << 11) | (6 << 4) | (1 << 3);
+	TIM2.CCMR2 = (6 << 12) | (1 << 11) | (6 << 4) | (1 << 3);
+	TIM2.CCR1 = 1000;
+	TIM2.CCR2 = 1000;
+	TIM2.CCR3 = 1000;
+	TIM2.CCR4 = 1000;
+	TIM2.CR1 = (1 << 0);
+	
 	magn.init();
 	mpu.init();
 	float buf[9] = {0,0,0,0,0,0,0,0,0};