successful FOC implementation

.gitignore

@@ -1,3 +1,4 @@
 *.o
 *.bin
 *.elf
+main.s

generate_table.rb

@@ -5,21 +5,21 @@
   v2 = Math.sin(angle + (Math::PI * 2) / 3)
   v3 = Math.sin(angle + (Math::PI * 4) / 3)
   min = [v1,v2,v3].min
-  values[0] << ((v1 - min) / 1.732050808 * 65535).round
-  values[1] << ((v2 - min) / 1.732050808 * 65535).round
-  values[2] << ((v3 - min) / 1.732050808 * 65535).round
+  values[0] << (v1 - min) / 1.732050808
+  values[1] << (v2 - min) / 1.732050808
+  values[2] << (v3 - min) / 1.732050808
 end
 
 f = File.open('table.h', 'w')
 
-f.write("static uint16_t table1[] = { ")
+f.write("static float table1[] = { ")
 f.write(values[0].join(", "))
 f.write(" };\n")
 
-f.write("static uint16_t table2[] = { ")
+f.write("static float table2[] = { ")
 f.write(values[1].join(", "))
 f.write(" };\n")
 
-f.write("static uint16_t table3[] = { ")
+f.write("static float table3[] = { ")
 f.write(values[2].join(", "))
 f.write(" };\n")

main.c

@@ -5,37 +5,113 @@
 #include <math.h>
 
 #define nop()  __asm__ __volatile__ ("nop" ::)
+#define PI 3.14159265f
 
+// Performance metrics
+int start_time, end_time;
+
+// Debug
+int id_dbg, iq_dbg;
+int flux_angle_dbg;
+
+// Analog to digital converter DMA data
 uint16_t adc_data[4];
 
+// For hard coded run loop
 int angle = 0;
-int frequency = 0;
-int voltage = 0;
 
-int i1, i2;
+// For flux estimator
+float imag;
+float flux_angle;
+int rotor_direction;
+uint32_t encoder_position_previous;
+const float loop_period = 0.0001024f;
+const float time_constant = 0.05f;
+const float pole_pairs = 2.0f;
 
-// This runs 0.000048s (20,833Hz) after it's triggered
+// For FOC
+float vd, vq;
+
+// This runs at the PWM frequency: 9765.625Hz
+// It runs 0.000048s after it's triggered (approx half the PWM period)
 // This means data is collected during the first half of each
 // pwm cycle ready for calculations in the second half.
 void DMA1_Channel1_IRQHandler(void) {
+  start_time = TIM1 -> CNT;
+
   // Fetch the phase 1 and 2 currents
-  i1 = adc_data[0] - 32840;
-  i2 = adc_data[1] - 32840;
+  int i1 = adc_data[0] - 32840;
+  int i2 = adc_data[1] - 32840;
 
-  // 13744 = 1Hz
-  frequency = 13744 * 5;
-  voltage = 4000;
+  float ia = i1;
+  float ib = 0.577350269f * i1 + 1.154700538f * i2;
+
+  float id = ia * cosf(flux_angle) + ib * sinf(flux_angle);
+  float iq = ib * cosf(flux_angle) - ia * sinf(flux_angle);
+
+  // Calculate encoder position change
+  uint32_t encoder_position_current = TIM2->CNT;
+  int encoder_position_change = encoder_position_current - encoder_position_previous;
+  encoder_position_previous = encoder_position_current;
+  // Convert encoder pulse count to (electrical) radians/second
+  float rotor_velocity = encoder_position_change * 9765.625f / 2400.0f * PI * 2.0f * pole_pairs;
+
+  // Flux angle estimator
+  imag = imag + (loop_period / time_constant) * (id - imag);
+  if(imag < 0) imag = -imag;
+  if(imag != 0.0f) {
+    float flux_slip_speed = (1.0f / time_constant) * (iq / imag);
+    float flux_speed = rotor_velocity + flux_slip_speed;
+    flux_angle = flux_angle + loop_period * flux_speed;
+  }
+  if(flux_angle > PI)  flux_angle -= PI*2.0f;
+  if(flux_angle < -PI) flux_angle += PI*2.0f;
+  //if(id < 0) flux_angle += PI;
+  // PID
+  float iq_target = (adc_data[3]-32768) / 4;
+  //float id_target = 2000;
+  float id_error = id - 1000;
+  float iq_error = iq - iq_target;
+
+  vd -= id_error * 0.1f;
+  vq -= iq_error * 0.1f;
+
+  if(vd >  4000) vd =  4000;
+  if(vd < -4000) vd = -4000;
+  if(vq >  4000) vq =  4000;
+  if(vq < -4000) vq = -4000;
+
+  // Voltage output
+  float va = vd * cosf(flux_angle) - vq * sinf(flux_angle);
+  float vb = vd * sinf(flux_angle) + vq * cosf(flux_angle);
+  float vangle = atan2f(vb, va);
+  float vamplitude = sqrtf(va * va + vb * vb);
+  if(vamplitude > 4000) vamplitude = 4000;
+  int voltage = vamplitude;
+  angle = ((vangle / PI) + 1.0f) * 67108863;
 
+  // Debug output
+  id_dbg = id;
+  iq_dbg = iq;
+  flux_angle_dbg = flux_angle * 1000.0f;
+
+  // Hard coded speed and voltage for testing
+  // 13744 = 1Hz
   // Increment angle and wrap
-  angle += frequency;
-  if(angle > 134217728) angle -= 134217728;
-  if(angle < 0) angle += 100663296;
+  //angle += adc_data[3] * 8;
+
+  // angle -= (adc_data[3]-32768) * 16;
+  // if(angle > 134217727) angle -= 134217728;
+  // if(angle < 0) angle += 134217728;
+  // int voltage = 4000;
 
   // Update PWM outputs from SVM style lookup table
   int phase = angle >> 14;
-  TIM1->CCR1 = (voltage * table1[phase]) >> 13;
-  TIM1->CCR2 = (voltage * table2[phase]) >> 13;
-  TIM1->CCR3 = (voltage * table3[phase]) >> 13;
+  TIM1->CCR3 = (voltage * table1[phase]) >> 15;
+  TIM1->CCR2 = (voltage * table2[phase]) >> 15;
+  TIM1->CCR1 = (voltage * table3[phase]) >> 15;
+
+  end_time = TIM1 -> CNT;
 
   // Clear interrupt
   DMA1->IFCR = 0xFFFFFFFF;
@@ -52,8 +128,13 @@ int main() {
   while(1) {
     // A handy loop for debugging
     // All the real work is interrupt driven
-    uart_write_int(TIM2->CNT);
+    uart_write_int(imag);
+    uart_write_byte(',');
+    uart_write_int(iq_dbg);
+    uart_write_byte(',');
+    uart_write_int(id_dbg);
+    //uart_write_byte(',');
     uart_write_nl();
-    int n; for(n=0;n<10000;n++) nop();
+    int n; for(n=0;n<1000;n++) nop();
   }
 }

make.sh

@@ -7,6 +7,7 @@ arm-none-eabi-gcc $CCOPTS -c startup_stm32l476xx.s -o startup_stm32l476xx.o
 arm-none-eabi-gcc $CCOPTS -c system.c -o system.o
 arm-none-eabi-gcc $CCOPTS -c uart.c -o uart.o
 arm-none-eabi-gcc $CCOPTS -c main.c -o main.o
+#arm-none-eabi-gcc $CCOPTS -S -c main.c -o main.s
 arm-none-eabi-gcc $CCOPTS -T STM32L476RG_FLASH.ld -Wl,--gc-sections *.o -o main.elf -lm
 arm-none-eabi-objcopy -O binary main.elf main.bin
 #st-flash write main.bin 0x8000000

system.c

@@ -78,8 +78,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
@@ -165,7 +165,7 @@ void SystemInit() {
   GPIOA->PUPDR = 1 | 1<<2;
   RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
   TIM2->SMCR = 3; // Encoder mode 3
-  TIM2->CCMR1 = 1 | (1<<8);
+  TIM2->CCMR1 = 1 | (1<<8) | (3<<4) | (3<<12);
   TIM2->CCER  = TIM_CCER_CC1E | TIM_CCER_CC2E;
   TIM2->CR1 |= 1;