Build your own Servo based on a BLDC

#include <Tle5012b.h>
#include <Tle5012b_reg.h>

#define POLEPAIRS 21

//SPI on X1
#define SS 38 // P0.3
#define MOSI 39 // P0.1 -----> connect 0.1 and 0.0 on the board! (3-Wire SPI)
#define MISO 65 // P0.0
#define SCK 40 // P0.10

//#define PI              (double)  3.14159265359
#define PHASE_DELAY_1     (double)  2.094395102 //120°
#define PHASE_DELAY_2     (double)  4.188790205 //240°

Tle5012b_SPI Spi1 = Tle5012b_SPI(1);
Tle5012b_reg Tle5012MagneticAngleSensor = Tle5012b_reg();

const int U = 11;
const int V = 10; 
const int W = 9;
const int EN_U = 6;
const int EN_V = 5;
const int EN_W = 3;
 
int i=0;
int duty = 40;

int pwmOne = 0;
int pwmTwo = 0;
int pwmThree = 0;

double amplitudeOne;
double amplitudeTwo;
double amplitudeThree;

double angle = 0.0;
double last_angle = 0.0;
double intent_angle = 168.0; //(upright position)
double integrator = 0; //for PID - I controller

double speed;
double timestamp = 0;

float angle_rad = 0.0;

int myPWM_U_values[3600];
int myPWM_V_values[3600];
int myPWM_W_values[3600];

//change these parameters according to motor
int offset = 53;
int PhaseShift = 130;
float speed_init = 0.003;


extern "C" {
  void CCU40_0_IRQHandler(void)
  { 
  //driving motor uncontrolled
    if(millis()<5000){ 
     angle_rad += speed_init;
     pwmOne =      100*sin(angle_rad);                      
     pwmTwo =      100*sin(angle_rad + PHASE_DELAY_1);      
     pwmThree =    100*sin(angle_rad + PHASE_DELAY_2); 
     analogWrite(U, 127 + duty*pwmOne/100); 
     analogWrite(V, 127 + duty*pwmTwo/100); 
     analogWrite(W, 127 + duty*pwmThree/100);   

     Tle5012MagneticAngleSensor.getAngleValue(angle);
     int angle_out = (angle+180)*10; //0-3600 
     int pwm_U_out = pwmOne;     //-100-100
     int pwm_V_out = pwmTwo;     //-100-100
     int pwm_W_out = pwmThree;   //-100-100
     
  //save mechanical angle and PWM values
    if(millis()>2000){      
     myPWM_U_values[angle_out] = pwm_U_out;
     myPWM_V_values[angle_out] = pwm_V_out;
     myPWM_W_values[angle_out] = pwm_W_out;
     
    }
   }
  //driving motor controlled 
    if(millis()> 5000){
       Tle5012MagneticAngleSensor.getAngleValue(angle);
       int angle_table = (angle+180)*10+PhaseShift+offset;
         if (angle_table >= 3600) {angle_table -=3600;} 
         if (angle_table < 0000) {angle_table +=3600;} 
  
       pwmOne   = myPWM_U_values[angle_table];
       pwmTwo   = myPWM_V_values[angle_table];
       pwmThree = myPWM_W_values[angle_table];
  
       analogWrite(U, 127 + duty*pwmOne/100); 
       analogWrite(V, 127 + duty*pwmTwo/100); 
       analogWrite(W, 127 + duty*pwmThree/100);  
      }
  }
}


void setup() { 
  Serial.begin(115200);
  Tle5012MagneticAngleSensor.begin(Spi1, MISO, MOSI, SCK, SS, Tle5012b::TLE5012B_S0);

  pinMode(U, OUTPUT);  setAnalogWriteFrequency(U,20000);
  pinMode(V, OUTPUT);  setAnalogWriteFrequency(V,20000);
  pinMode(W, OUTPUT);  setAnalogWriteFrequency(W,20000);

  pinMode(EN_U, OUTPUT);  digitalWrite(EN_U, HIGH);
  pinMode(EN_V, OUTPUT);  digitalWrite(EN_V, HIGH);
  pinMode(EN_W, OUTPUT);  digitalWrite(EN_W, HIGH);

 
//Setup Interrupt settings
  XMC_CCU4_SLICE_COMPARE_CONFIG_t pwm_config = {0};
    pwm_config.passive_level = XMC_CCU4_SLICE_OUTPUT_PASSIVE_LEVEL_HIGH;
    pwm_config.prescaler_initval = XMC_CCU4_SLICE_PRESCALER_128;
//Setup interrupt1
    XMC_CCU4_Init(CCU40, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);

    XMC_CCU4_SLICE_CompareInit(CCU40_CC43, &pwm_config);

    XMC_CCU4_EnableClock(CCU40, 3);

    XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU40_CC43, 200); // Adjust last Value or Prescaler

    /* Enable compare match and period match events */
    XMC_CCU4_SLICE_EnableEvent(CCU40_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);

    /* Connect period match event to SR0 */
    XMC_CCU4_SLICE_SetInterruptNode(CCU40_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
    
    /* Configure NVIC */
    /* Set priority */
    NVIC_SetPriority(CCU40_0_IRQn, 10);

    /* Enable IRQ */
    NVIC_EnableIRQ(CCU40_0_IRQn); 
    XMC_CCU4_EnableShadowTransfer(CCU40, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
    XMC_CCU4_SLICE_StartTimer(CCU40_CC43);
}

void loop() {
  while(millis()<5000){} //wait init of Lookup table
  speed = last_angle-angle; //calculate speed
  last_angle = angle; //update "last angle"
  
  //Position request
  if (millis()-timestamp >  500){intent_angle = 168.0-120;}
  if (millis()-timestamp > 1000){intent_angle = 168.0; timestamp=millis();}
  //Position Control (PID)
  double error = intent_angle-angle;
  int P = 8;
  double I = 0.3;
  double D = 80;
  integrator = constrain(integrator + error,-100, 100);
    double control = error * P + integrator * I + speed * D; 
    if (control > 0){PhaseShift = 130;} //turn clockwise
    if (control < 0){PhaseShift =-130;} //turn counterclockwise
    duty = constrain(abs(control),0,255); //Limit output to meaningful range

//Monitor
//  Serial.print("0"); Serial.print("\t"); Serial.print(intent_angle); Serial.print("\t"); Serial.println(angle);
  

  //Setting Phaseshift
  while (Serial.available() != 0)
    {
        int input = 0;
        input = Serial.read();
        if(input == 43)
        {
            PhaseShift+=1;
            Serial.print("\t\t");
            Serial.println(PhaseShift);
        }
        if(input == 45)
        {
            PhaseShift-=1;
            Serial.print("\t\t");
            Serial.println(PhaseShift);
        }
    }
}