Precizna kontrola Servo Motora Pomocu SimpleFOC biblioteke

[elektropionir]

SimpleFOC je trenutno najbolji framework za kontrolu elektricnih motora pomocu FOC metode.
Ovo je open source projekat koji me je zainteresirao jako davno. Mislim da je projekat zapoceo inzinjer koji se zove Antun Skuric sa nasih prostora, ali ovo je sada jako veliki framework kome doprinosi mnogo developera.
Razviti citav kontrolni algoritam je jako komplikovan zadatak. Srecom SimpleFOC je jako modularan i fleksibilan framework.
Podrzava mnogo tipova motora, tipova kontrole obrtnog momenta, brzine ili pozicije u open loop ili closed loop modu, kao i mnogo razlicitih senzora za detekciju pozicije.

Ovdje cu opisati svoj test setup. Namjerno sam koristio jako jednostavne i jeftine alate da bi ovaj primjer bio laksi za reproducirati.

Motor koji koristim je mali servo motor, nema17 RS broj 536-6024. Napon 24V, peak struje 6A sa 4 para polova i 3 hall senzora. Bez obzira na tip motora, ovi primjeri i metodi rade za bilo koju masinu.



Naravno protrebna nam je i kontrolna ploca sa 3fanzim mostom kao servo pojacalom. Eksperimentisanje sam zapoceo sa steval-spin3202.


Medjutim, ova ploca nema shunt u svakoj grani 3faznog mosta, tako da sam odlucio da nabavim mnogo popularniju b-g431b-esc1. Ovo je plocica koja je namjenjena za kontrolu motora u RC avionima i dronovima. Ima jako brz G4 microcontroller, ali je nocna mora za bilo kakvo testiranje. Definitivno cu se vratiti Prvoj opciji kada budem morao ustvari primjeniti ovaj motor i dodati svoju elektroniku i mehaniku na uredjaj koji pravim.



Posto je ESC1 popularna razvojna plocica, svi pinovi mikrokontrolera kao i ostali hardware su vec definisani u SimpleFOC.
U ini file platformio mozemo da stavimo osnovne podatke o projektu, ovo je moj platformio.ini fajl:

Code:

[env:disco_b_g431b_esc1]
platform = ststm32
board = disco_b_g431b_esc1
framework = arduino
monitor_speed = 115200
upload_protocol = stlink
lib_archive = no
build_flags =
    -D PIO_FRAMEWORK_ARDUINO_NANOLIB_FLOAT_PRINTF
    -D HAL_OPAMP_MODULE_ENABLED
    -D HAL_TIM_MODULE_ENABLED
lib_deps =
    https://github.com/simplefoc/Arduino-FOC
    SPI
    Wire

Da bi skratio ovu temu, ovdje cu postaviti citav main.cpp code:
Cilj ovog koda je testirati regulator brzine. Brzina se mjeri pomocu hall senzora. Trenutno samo regulator brzine je aktivan, regulator momenta i sve ostalo je ugaseno-
Prvi dio koda definise senzor i motor objekat i neke osnovne parametre o motoru.
Commander command je objekat je objekat koji uzima ulaz sa serijskog porta i mjenja reaguje na odredjene ulaze koje cemo kasnije definisati.
void onTarget ili onP I D kao sto vidite su method funkcije koje ce pozvati komander objekat. Ove funkcije samo mjenjaju koeficjent u PID kontroleru brzine kao sto je ocito.
U init dijelu ovi objekti se iniciraju sa parametrima i opisom koji smo zadali.
Init metod po paljenju uredjaja pokrene motor u obje strane za jedno 30 stepeni, provjeri prirodni smijer obrtanja kao i broj pari polova. Postoji nacin da se ovo preskoci i da se zadaju tacni parametri jer inicijalizacija traje 10tak sekundi.

Code:

#include <Arduino.h>
#include <SimpleFOC.h>

HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3, 4);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}
// put function declarations here:


// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number);
BLDCMotor motor = BLDCMotor(4);
// BLDCDriver3PWM driver = BLDCDriver3PWM(pwmA, pwmB, pwmC, Enable(optional));
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);

Commander command = Commander(Serial);
float target_velocity=0;

void onTarget(char* cmd){
  command.scalar(&target_velocity, cmd);
  }

void onP(char* cmd){
  command.scalar(&motor.PID_velocity.P, cmd);
  }
void onI(char* cmd){
  command.scalar(&motor.PID_velocity.I, cmd);
  }
void onD(char* cmd){
  command.scalar(&motor.PID_velocity.D, cmd);
  }
void onT(char* cmd){
  command.scalar(&motor.LPF_velocity.Tf, cmd);
  }
float target = 0.0;

void SerialLoop(){
  static String received_chars;

  while(Serial.available()){
    char inChar = (char) Serial.read();
    received_chars += inChar;
    if (inChar=='\n')
    {
      target = received_chars.toFloat();
      Serial.print("Target = ");
      Serial.println(target);
      received_chars="";
    }
   
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(1000);

  sensor.init();
  // hardware interrupt enable
  sensor.enableInterrupts(doA, doB, doC);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // limit the maximal dc voltage the driver can set
  // as a protection measure for the low-resistance motors
  // this value is fixed on startup
  driver.voltage_limit = 8;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

  // limiting motor movements
  motor.voltage_limit = 9;    // [V]
  motor.velocity_limit = 500;  // [rad/sec]
  motor.voltage_sensor_align=2;
  // limit the voltage to be set to the motor
  // start very low for high resistance motors
  // current = voltage / resistance, so try to be well under 1Amp
  motor.phase_resistance = 1.6; // [Ohm]
  motor.current_limit = 6;   // [Amps] - if phase resistance defined

  motor.useMonitoring(Serial);
  motor.monitor_downsample = 5;
  motor.monitor_variables = _MON_TARGET | _MON_VEL| _MON_VOLT_Q;

  motor.linkSensor(&sensor);

  motor.PID_velocity.P=0.03;
  motor.PID_velocity.I=0.2;
  motor.PID_velocity.D=0.002;
  motor.LPF_velocity.Tf=0.09;

  // open loop control config
  //motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::velocity;

  // init motor hardware
  motor.init();
  motor.initFOC();
  Serial.println("Setup Done!");

  command.add('v', onTarget, "target velocity");
  command.add('p', onP, "Proportional Regulator");
  command.add('i', onI, "Integral Regulator");
  command.add('d', onD, "Differential Regulator");
  command.add('t', onT, "loop speed Regulator");
}

void loop() {

  motor.loopFOC();

  motor.move(target_velocity);
  //motor.monitor();
  command.run();
 
}

// put function definitions here:

Glavna petlja je jako kratka i samo mjenja zadatu brzinu, i poziva glavni FOC kontrolni algoritam.
motor.monitor je funkcija koja u terminalu moze da ispise trenutnu poziciju, ali ova funkcija je uzasno spora. Citav 1 dan sam pokusao da skontam zasto algoritam ne funkcionise. Imao sam cak 0.5A na 24V praznu potrosnju, i kontrolni parametri su bili potpuno besmisleni. Motor je bio nemoguc za kontrolisati. Ukidanjem ove funkcije algoritam radi kako treba

Sada dolazimo do glavnog problem. Podesavanje PID parametara, sto nebi trebao biti problem za nekoga ko je zavrsio Automatiku, ali mi je i dalje trebalo 2 dana da pripremim sve
Iz nekog razloga, sva dokumentacija, primjeri i demonstracije ove biblioteke, cak i od strane developera su poprilicno besmislene. Svaki control loop primjer je ustvari PI kontroler.
Svima samo preporucuju da ne koriste D, jer to cini motor nestabilnim. Meni to nema nikakvog smisla, vidio sam source code ovih regulatora, sve je savrseno programirano.
Ali zaista, poceo sam sa malim parametrima i cim dodam D koji nije 0, motor bi poceo da skace, tako da sam odlucio da vidim sta je problem.
Osim toga, svakaki konverter koji sam ikada napravio obavezno koristi D regulator.

----------------------- OFFTOPIC----------------------
Upravo nedavno sam morao da napravim konverter za napajanje gate drajvera, sto zvuci kao maciji kasalj za mnoge od Vas. Medjutim drajver je za 4.5kV IGBT modul, konverter je 2MW, i sav elektricni naboj koji se potrosi za paljenje modula, mora biti zamjenjen u periodu od 5 mikrosekundi. Paljenje velikog modula uzrokuje da gate napon blago opadne,za oko 1V cak i uz ogroman bulk cap. Manji gate napon povecava disipaciju modula, sto na 2kA nije zezancija. Uglavnom srednja snaga tog jako malog konvertera je samo oko 5w ali peak snaga je 120W, i pored toga udaljen je 2 PCB plocice i 20CM kabla od bulk-a i gate drajvera. HAH! Napravi taj brz i stabilan regulator bez mnogo Dif. komponenti.
----------------------------------------------------------

Prvi zadatak je definitivno napraviti neovisno mjerenje brzine obrtaja.
3D printao sam jako jednostavan enkoder, sa samo 6 pozicija. On se vrti kroz jedan opticki endstop. Ovo sam imao viska kada sam upgrade-ova svoj 3d printer.
Ovi endstopi nisu dobri za brz odziv enkodera. Potrebno je smanjiti intenzitet svjetla diode za brzi odziv. Takodjer fototranzistor sluzi samo da ugasi 2N7002 mosfet, uz malu modifikaciju sheme, uklanjanje mosfeta, pomocu PNP-a pojacati signal da se odziv jos ubrza.



Pravi izazov je snimiti ove signale. Naravno to mozemo pomocu osciloskopa ali to je mukotrpan proces, preracunavanje tih analognih signala u brzinu, to mi se definitivno nije radilo.
Tako da mi je palo na pamet da iskoristim jedan od ovih logickih analizatora koje imam. Ovo je najjeftinija kopija starog Saleae Logic8 analizatora ali za ovu svrhu vise nego dovoljan!
Kada radim testove uvjek koristim jedan od DreamSourceLab jako brzih logickih analizatora tako da sam zaboravio da imam ovo, ali vjerujem da skoro svako ima bar jedan negdje.
Dovoljno je samo spojiti masu i jedan od ulaza, na izlaz iz ovog optickog prekidaca.

Mozemo koristiti LogicView Sigrok open source software za snimanje ovih signala, medjutim, sigrok takodjer ima comand line client koji sam uvjek zelio da naucim koristiti tako da je ovo bila dobra prilika da to provjerimo

Ovdje cu postaviti citavu python skriptu, ja sam je koristio u jupyter notebook:

Ovo su osnovne funkcije:

Code:

import subprocess

SIGROK_CLI_PATH = 'C:/Program Files/sigrok/sigrok-cli/sigrok-cli.exe'

def find_transitions(data):
    states = [data[0]]
    durations = [0]
    for bit_possition, bit_value in enumerate(data):
        if bit_value != states[-1]:
            states.append(bit_value)
            durations.append(bit_possition-durations[-1])
    return states, durations

def calculate_speed(sample_rate, encoder_resolution, durations):
    speeds = [sample_rate*3.14/((durations[i+2]-durations[i])*encoder_resolution) for i in range(len(durations)-2) ]
    timescale = [0]
   
    for index in range(1,len(durations)-2,2):
        temp1=durations[index+2]-durations[index]
        temp2=durations[index+1]-durations[index-1]
        timescale.append(timescale[-1]+temp2)
        timescale.append(timescale[-1]+temp1)
     
    time = [timescale[i]/sample_rate for i in range(1,len(timescale))]
    return speeds, time


def plot_speed(in_samplerate):
    samplerate = {   
    20000   :'20 kHz',
    25000   :'25 kHz',
    50000   :'50 kHz',
    100000  :'100 kHz',
    200000  :'200 kHz',
    250000  :'250 kHz',
    500000  :'500 kHz',
    1000000 :'1 MHz',
    2000000 :'2 MHz',
    3000000 :'3 MHz',
    4000000 :'4 MHz',
    6000000 :'6 MHz',
    8000000 :'8 MHz',
    12000000:'12 MHz',
    16000000:'16 MHz',
    24000000:'24 MHz',
    48000000:'48 MHz'
    }

    SAMPLES = f'{8*10000}'
    TRIGGER = 'D0=1'
    SAMPLE_RATE = f'{samplerate[in_samplerate]}'

    capture = subprocess.run(
        [SIGROK_CLI_PATH, '--driver', DEVICE,
         '--samples', SAMPLES,
         '--triggers', TRIGGER,
         '--output-format' , 'bits:width=8',
         '--channels', 'D0',
         #'--output-file', 'test.csv',
         '--config', f'samplerate={SAMPLE_RATE}'],
        shell=True,
        stdout=subprocess.PIPE,
        stderr=subprocess.STDOUT)

    datastream=capture.stdout.decode('ascii')
    datastream = datastream.replace('FRAME-END','').replace('FRAME-BEGIN','').replace('T:^ 0','').replace('D0:','')

    test = datastream.split('\n')
    to_delete = []
    for index,value in enumerate(test):
        if(len(value)!=8):
            to_delete.append(index)

    to_delete.reverse()

    for index in to_delete:
        test.pop(index)

    waveform = ""
    for bits in test:
        waveform+=bits

    s,d = find_transitions(waveform)
    #print(waveform)
    speed, time = calculate_speed(in_samplerate,6,d)

    return speed, time

I ovo napokon iscrtava grafove koje zelimo vidjeti

Code:

from time import sleep
import plotly.graph_objs as go
speed, time = plot_speed(20000)
fig = go.FigureWidget()
fig.add_scatter(y=speed)
fig.update_layout(xaxis_title="samples x 50us", yaxis_title="rad/s")
fig['data'][0]['name']='speed'
fig['data'][0]['showlegend']=True
fig.update_traces(marker=dict(size=24, color="blue"), selector = dict(name="first_trace"))
fig

Sada dolazi zabavan dio.
U VS code terminalu mozemo da saljemo komande. Kao sto smo programirali "v20" komanda bi promjenila brzinu obrtaja na 20rad/sec. Komanda i5.4 bi promjenila koeficjent kI na 5.4 u PID regulatoru. Sada imamo sve da lako testiramo regulator brzine.
Korak 1.
I = 0 D = 0. Mjenjamo P od 0 to broja koji nam daje relativno glatku opraciju na srednjem broju obrtaja, 50rad/sekundi i ne previse skakanja na 0-5rad/s. 0 je obicno jako nestabilna.
Ukoliko regulator ne reaguje na zadatu vrijednost, P je premali broj. Za moj motor P je bio ok od 0.03 do 0.1 medjutim na vecem broju obrtaja veci P je bio nestabilan. Izabrao sam manju vrijednost.
Nakon nekoliko pokusaja dolazimo do prvog koraka:

Kao sto se vidi iz prvog grafa sam P regulator ima staticku gresu od oko 20rad/s. To je prihvatljivo za ovaj korak.
Sljedeci korak je podesavanje I regulatora. Cilj je imati manji I regulator da dovoljno brzo popravi staticku gresku. Nakon podesavanja I regulatora motor ce raditi ok, ali step odziv je uzasan. Motor radi ok u stacionarnom stanju ali... Dinamika je katastrofa. Na zalost sva dokumentacija i savjeti koje daju pocetnicima na razlicitim platformama kaze da je ovo dovoljno dobro. Za moj cilj na zalost nije. Nakon malo igranja sa parametrima, dajem kratak sazetak rezultata ispod:


Ovo je sada vec mnogo respektabilnije.
Naredni korak bi bio ukljucivanje regulatora momenta. Ali to cu ostaviti za buduci trenutak.