diff --git a/main.c b/main.c
index 6999551..ab25df6 100644
--- a/main.c
+++ b/main.c
@@ -9,17 +9,8 @@ uint16_t adc_data[4];
 
 int sine_angle = 0;
 int frequency = 1;
-int voltage = 0;
 
-int v1, v2;
-int va, vb;
-
-int i1, i2;
-int ia, ib;
-
-int id, iq;
-
-float vangle;
+float i1, i2, total_current;
 
 void uart_write_string(char* str);
 void uart_write_int(int32_t i);
@@ -44,84 +35,63 @@ void update_svm(uint32_t phase, uint32_t voltage)
   switch(sine_segment) {
   case 0:
     // 100 -> 110
-    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     TIM1->CCR2 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
-    TIM1->CCR3 = 0;
+    TIM1->CCR1 = 0;
     break;
 
   case 1:
     // 110 -> 010
-    TIM1->CCR1 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR3 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
     TIM1->CCR2 = ((voltage+1) * table[sine_angle] - 1) >> 9;
-    TIM1->CCR3 = 0;
+    TIM1->CCR1 = 0;
     break;
 
   case 2:
     // 010 -> 011
-    TIM1->CCR1 = 0;
+    TIM1->CCR3 = 0;
     TIM1->CCR2 = ((voltage+1) * table[sine_angle] - 1) >> 9;
-    TIM1->CCR3 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR1 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
     break;
 
   case 3:
     // 011 -> 001
-    TIM1->CCR1 = 0;
+    TIM1->CCR3 = 0;
     TIM1->CCR2 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
-    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     break;
 
   case 4:
     // 001 -> 101
-    TIM1->CCR1 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR3 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
     TIM1->CCR2 = 0;
-    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     break;
 
   case 5:
     // 101 -> 100
-    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     TIM1->CCR2 = 0;
-    TIM1->CCR3 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR1 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
     break;
   }
 }
 
 // This runs at 80000000/8192 = 9765.625Hz
 void DMA1_Channel1_IRQHandler(void) {
-  v1 = TIM1->CCR1 + TIM1->CCR1 - TIM1->CCR2 - TIM1->CCR3;
-  v2 = TIM1->CCR2 + TIM1->CCR2 - TIM1->CCR1 - TIM1->CCR3;
-
-  va = v1;
-  vb = 0.577350269f * (float)v1 + 1.154700538 * (float)v2;
-
-  i1 = adc_data[0] - 32840;
-  i2 = adc_data[1] - 32840;
-  //i1 = adc_data[0] - 32845;
-  //i2 = adc_data[1] - 32770;
-
-  ia = i1;
-  ib = 0.577350269f * (float)i1 + 1.154700538 * (float)i2;
+  i1 = (adc_data[0] - 31710)/5242.88;
+  i2 = (adc_data[1] - 31795)/5242.88;
 
-  vangle = atan2f(vb, va);
+  // Calculate instantaneous total current
+  total_current = sqrtf(2*(i1*i1+i2*i2+i1*i2));
 
-  id =  ia * cosf(vangle) + ib * sinf(vangle);
-  iq =  ia * sinf(vangle) - ib * cosf(vangle);
+  // Calculate fixed voltage drop based on current and known resistance (8 ohm)
+  // Scale RMS to peak voltage
+  float resistive_waste = (total_current / 3 * sqrt(2)) * 8;
 
   // 10308 = 1Hz
-  // adc_data[3] = 0-65535
-  // fMax = 25.43Hz
-  //frequency = adc_data[3]*4;
-
-  if(iq < 800) frequency -= 50;
-  if(iq > 800) frequency += 50;
-  if(frequency < 40000) frequency = 40000;
-
-  if(id < 2000) voltage += 10;
-  if(id > 2000) voltage -= 10;
-  if(voltage > 4000) voltage = 4000;
-
-  //int voltage = 4000; //frequency / 12.885f;
-  //if(voltage > 4000) voltage = 4000;
+  frequency = 10308 * 5;
+  int voltage = 4000;
 
   // Increment angle
   sine_angle += frequency;
@@ -142,14 +112,22 @@ void TIM1_UP_TIM16_IRQHandler(void) {
 int main() {
   while(1) {
     // Debug output
-     uart_write_int(id);
+     uart_write_int(i1*1000);
      uart_write_string(",");
-     uart_write_int(iq);
+     uart_write_int(i2*1000);
      uart_write_string(",");
-     uart_write_int(frequency);
-
+     uart_write_int(total_current*1000);
+     //uart_write_string(",");
+     //uart_write_int(adc_data[3]);
+    // uart_write_int(i3);
+    // uart_write_string(",");
+    // uart_write_int(iangle);
+    // uart_write_string(",");
+    // uart_write_int(vangle);
+    // uart_write_string(",");
+    // uart_write_int(i3);
      uart_write_nl();
-     int n; for(n=0;n<10000;n++) nop();
+     int n; for(n=0;n<1000000;n++) nop();
 
   }
 }
diff --git a/system.c b/system.c
index 2c0f405..d73554f 100644
--- a/system.c
+++ b/system.c
@@ -73,8 +73,8 @@ void SystemInit() {
   // Disable USART2
   USART2->CR1 = 0;
   // Set data rate
-  //USART2->BRR = 694; //115200
-  USART2->BRR = 80; //1M
+  USART2->BRR = 694; //115200
+  //USART2->BRR = 80; //1M
   // Enable USART2
   USART2->CR1 |= USART_CR1_UE;
   // Enable transmit
@@ -114,12 +114,12 @@ void SystemInit() {
   //ADC2->DIFSEL |= (1<<5);
 
   // Calibrate
-  ADC1->CR |= ADC_CR_ADCAL;
-  ADC2->CR |= ADC_CR_ADCAL;
-  while(ADC1->CR & ADC_CR_ADCAL);
-  while(ADC2->CR & ADC_CR_ADCAL);
-  // Wait a bit
-  for(n=0;n<100000;n++) nop();
+  // ADC1->CR |= ADC_CR_ADCAL;
+  // ADC2->CR |= ADC_CR_ADCAL;
+  // while(ADC1->CR & ADC_CR_ADCAL);
+  // while(ADC2->CR & ADC_CR_ADCAL);
+  // // Wait a bit
+  // for(n=0;n<100000;n++) nop();
 
   // Enable procedure
   ADC1->ISR |= ADC_ISR_ADRDY;