From 4830139eeae3ce92e923c8550de68b98a16ead84 Mon Sep 17 00:00:00 2001
From: Charlie Smurthwaite <charlie@atechmedia.com>
Date: Fri, 20 Jul 2018 02:52:47 +0100
Subject: [PATCH] a working sensorless control using active current to control
 voltage and reactive current to control frequency

---
 main.c   | 92 +++++++++++++++++++++++++++++++++++---------------------
 system.c | 16 +++++-----
 2 files changed, 65 insertions(+), 43 deletions(-)

diff --git a/main.c b/main.c
index ab25df6..6999551 100644
--- a/main.c
+++ b/main.c
@@ -9,8 +9,17 @@ uint16_t adc_data[4];
 
 int sine_angle = 0;
 int frequency = 1;
+int voltage = 0;
 
-float i1, i2, total_current;
+int v1, v2;
+int va, vb;
+
+int i1, i2;
+int ia, ib;
+
+int id, iq;
+
+float vangle;
 
 void uart_write_string(char* str);
 void uart_write_int(int32_t i);
@@ -35,63 +44,84 @@ void update_svm(uint32_t phase, uint32_t voltage)
   switch(sine_segment) {
   case 0:
     // 100 -> 110
-    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     TIM1->CCR2 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
-    TIM1->CCR1 = 0;
+    TIM1->CCR3 = 0;
     break;
 
   case 1:
     // 110 -> 010
-    TIM1->CCR3 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR1 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
     TIM1->CCR2 = ((voltage+1) * table[sine_angle] - 1) >> 9;
-    TIM1->CCR1 = 0;
+    TIM1->CCR3 = 0;
     break;
 
   case 2:
     // 010 -> 011
-    TIM1->CCR3 = 0;
+    TIM1->CCR1 = 0;
     TIM1->CCR2 = ((voltage+1) * table[sine_angle] - 1) >> 9;
-    TIM1->CCR1 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR3 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
     break;
 
   case 3:
     // 011 -> 001
-    TIM1->CCR3 = 0;
+    TIM1->CCR1 = 0;
     TIM1->CCR2 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
-    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     break;
 
   case 4:
     // 001 -> 101
-    TIM1->CCR3 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR1 = ((sine_angle + 1) * (voltage+1) * table[sine_angle] - 1) >> 19;
     TIM1->CCR2 = 0;
-    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     break;
 
   case 5:
     // 101 -> 100
-    TIM1->CCR3 = ((voltage+1) * table[sine_angle] - 1) >> 9;
+    TIM1->CCR1 = ((voltage+1) * table[sine_angle] - 1) >> 9;
     TIM1->CCR2 = 0;
-    TIM1->CCR1 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
+    TIM1->CCR3 = ((1024 - sine_angle) * (voltage+1) * table[sine_angle] - 1) >> 19;
     break;
   }
 }
 
 // This runs at 80000000/8192 = 9765.625Hz
 void DMA1_Channel1_IRQHandler(void) {
-  i1 = (adc_data[0] - 31710)/5242.88;
-  i2 = (adc_data[1] - 31795)/5242.88;
+  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;
 
-  // Calculate instantaneous total current
-  total_current = sqrtf(2*(i1*i1+i2*i2+i1*i2));
+  vangle = atan2f(vb, va);
 
-  // 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;
+  id =  ia * cosf(vangle) + ib * sinf(vangle);
+  iq =  ia * sinf(vangle) - ib * cosf(vangle);
 
   // 10308 = 1Hz
-  frequency = 10308 * 5;
-  int voltage = 4000;
+  // 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;
 
   // Increment angle
   sine_angle += frequency;
@@ -112,22 +142,14 @@ void TIM1_UP_TIM16_IRQHandler(void) {
 int main() {
   while(1) {
     // Debug output
-     uart_write_int(i1*1000);
+     uart_write_int(id);
      uart_write_string(",");
-     uart_write_int(i2*1000);
+     uart_write_int(iq);
      uart_write_string(",");
-     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_int(frequency);
+
      uart_write_nl();
-     int n; for(n=0;n<1000000;n++) nop();
+     int n; for(n=0;n<10000;n++) nop();
 
   }
 }
diff --git a/system.c b/system.c
index d73554f..2c0f405 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;