#pragma once
/* This header substitutes all needed ControlSuite ... MotorControl headers.
*
* 1. goal: More C++
* Each „block“ is now a functor object with constructor and initializer.
* The processing function (i.e. the operator()) can be called with
* 0 or more useful arguments. Arguments set the input variable to the new
* value, so subsequent calls with less variables use the last set one.
* All functors are in namespace "mc" = MotorControl
* This is header-only code.
*
* 2. goal: Independency to Texas Instriments changes in ControlSuite
* The IQ datatype is no more used.
* In case of using a less capable processor, one has to replace float
* with a suitable fixpoint class.
* There are much nicer implementations around than the TI proposal!
*
* The „block“ structure with input variables is still not nice
* but saves the compiler to handle a (possibly) large set of stack parameters.
*/
#if 0
// Ursprünglich ...ControlSuite/.../MotorControl/...
#define MATH_TYPE 1 // this will define _iq to float and _IQ(x) to x, so use true float-point registers
#include <IQmathLib.h>
#include <park.h> // Include header for the PARK object
#include <ipark.h> // Include header for the IPARK object
#include <clarke.h> // Include header for the CLARKE object
#include <rampgen.h> // Include header for the RAMPGEN object
#include <rmp_cntl.h> // Include header for the RMPCNTL object
#include <svgen.h> // Include header for the SVGENDQ object
#include <pi.h> // Include header for the PIDREG3 object
#include <speed_fr.h> // Include header for the SPEED_MEAS_QEP object
#include <pid_grando.h>
#undef _iq
// die xxx_DEFAULTS und xxx_MACRO brauchen das Makro noch
#else
#include <string> // std::memset
namespace mc {
struct Range{
float max,min;
Range():max(1),min(-1) {}
float sat(float v) {
if (!(v<=max)) v=max;
if (!(v>=min)) v=min;
return v;
}
bool limit(float&v) {
bool ret=false;
if (!(v<=max)) {v=max; ret=true;}
if (!(v>=min)) {v=min; ret=true;}
return ret;
}
};
/*******************
* Transformations *
*******************/
// Transformations have no state inside, so true functions would do the job …
struct AlphaBeta{
float Alpha,Beta; // stationary d-axis or q-axis stator variable
};
struct Park:public AlphaBeta{
float Angle, // Input: rotating angle
Ds, // Output: rotating d-axis stator variable
Qs, // Output: rotating q-axis stator variable
Sine,
Cosine;
void init() {std::memset(this,0,sizeof*this);}
void operator()() {
Ds = Alpha*Cosine + Beta*Sine;
Qs = Beta*Cosine - Alpha*Sine;
}
void setSinCos(float angle) {
Angle = angle;
Sine = __sinpuf32(angle);
Cosine = __cospuf32(angle);
}
};
struct Ipark:public Park{
// Here, Ds and Qs are Inputs; Alpha and Beta are Outputs
void operator()() { // overload!!
Alpha = Ds*Cosine - Qs*Sine;
Beta = Qs*Cosine + Ds*Sine;
}
void operator()(float d,float q,Park&park) {Ds=d;Qs=q;Sine=park.Sine;Cosine=park.Cosine; operator()();}
};
struct Clarke:public AlphaBeta{
float As, // Input: phase-a stator variable
Bs, // Input: phase-b stator variable
Cs; // Input: phase-c stator variable
static const float ONEbySQRT3=0.57735026918963; // 1/√3
void init() {std::memset(this,0,sizeof*this);}
void run2() { // Clarke transform (with 2 currents)
Alpha = As;
Beta = (As+Bs+Bs)*ONEbySQRT3;
}
void run3() { // Clarke transform (with 3 currents)
Alpha = As;
Beta = (Bs-Cs)*ONEbySQRT3;
}
};
struct Svgen:public AlphaBeta{ // AlphaBeta = reference axis phase voltage
float Ta, // Output: reference phase-a switching function
Tb, // Output: reference phase-b switching function
Tc; // Output: reference phase-c switching function
void init() {std::memset(this,0,sizeof*this);}
void operator()() {
float tmp1= Beta,
tmp2= Beta*0.5 + 0.866*Alpha,
tmp3= tmp2 - tmp1;
Uint16 VecSector=3; // Space vector sector
if (tmp2>0) --VecSector;
if (tmp3>0) --VecSector;
if (tmp1<0) VecSector=7-VecSector;
if (VecSector==1 || VecSector==4) {
Ta= tmp2;
Tb= tmp1-tmp3;
Tc=-tmp2;
}else if (VecSector==2 || VecSector==5) {
Ta= tmp3+tmp2;
Tb= tmp1;
Tc=-tmp1;
}else{
Ta= tmp3;
Tb=-tmp3;
Tc=-(tmp1+tmp2);
}
}
void operator()(Ipark&ipark) {Alpha=ipark.Alpha; Beta=ipark.Beta; operator()();}
};
/********************
* Signal generator *
********************/
struct Rampgen{
float Freq, // Input: Ramp frequency
StepAngleMax, // Parameter: Maximum step angle
Angle, // Variable: Step angle
Gain, // Input: Ramp gain
Out, // Output: Ramp signal
Offset; // Input: Ramp offset
Rampgen():Freq(0),StepAngleMax(0),Angle(0),Gain(1),Out(0),Offset(1) {}
void init() {
std::memset(this,0,sizeof*this);
Gain=Offset=1;
}
float operator()(bool use_gain_offset=false) { // Sawtooth generator
/* Compute the angle rate */
Angle += StepAngleMax*Freq;
/* Saturate the angle rate within (-1,1) */
if (Angle>1) Angle -= 1;
else if (Angle<-1) Angle += 1;
// Use the code snippet below if gain/offset needed.
if (use_gain_offset) {
/* Compute the ramp output */
Out = Angle*Gain + Offset;
/* Saturate the ramp output within (-1,1) */
while (Out>1) Out -= 1;
while (Out<-1) Out += 1;
}else{
Out=Angle;
}
return Out;
}
float operator()(float freq,bool use_gain_offset=false) {Freq=freq; return operator()(use_gain_offset);}
};
struct Rmpcntl {
float TargetValue; // Input: Target input
Uint32 RampDelayMax; // Parameter: Maximum delay rate
Range RampLimit; // Parameter: Limits
Uint32 RampDelayCount;// Variable: Incremental delay
float SetpointValue; // Output: Target output
Uint32 EqualFlag; // Output: Flag output
Rmpcntl():TargetValue(0),RampDelayMax(5) {}
void init() {
std::memset(this,0,sizeof*this);
RampDelayMax=5;
RampLimit.min=-1;
RampLimit.max=1;
}
float operator()() {
float Tmp = TargetValue-SetpointValue;
const float DELTA=0.0000305; // 0.0000305 is resolution of Q15
if (std::fabs(Tmp) >= DELTA) {
RampDelayCount++;
if (RampDelayCount >= RampDelayMax) {
if (TargetValue >= SetpointValue) SetpointValue += DELTA;
else SetpointValue -= DELTA;
RampLimit.limit(SetpointValue);
RampDelayCount = 0;
}
}
else EqualFlag = 0x7FFFFFFF;
return SetpointValue;
}
float operator()(float tv) {TargetValue=tv; return operator()();}
};
/********************************************
* PI & PID controller (PI- und PID-Regler) *
********************************************/
struct PiController {
float Ref, // Input: reference set-point
Fbk, // Input: feedback
Out, // Output: controller output
Kp, // Parameter: proportional loop gain
Ki; // Parameter: integral gain
Range U; // Parameter: saturation limits
float v1, // Data: pre-saturated controller output
i1; // Data: integrator storage: ui(k-1)
PiController():Ref(0),Fbk(0),Out(0),Kp(1),Ki(0),v1(0),i1(0) {} // Default initalisation values for the PI_GRANDO objects
float operator()() {
/* proportional term */
float up = Kp*(Ref-Fbk);
/* integral term */
float ui = Out==v1 ? Ki*up + i1 : i1;
i1 = ui;
/* control output */
return Out = U.sat(v1=up+ui);
}
float operator()(float fbk) {Fbk=fbk; return operator()();}
float operator()(float fbk,float ref) {Ref=ref; return operator()(fbk);}
#if 0
float pos_run() {// This macro works with angles as inputs, hence error is rolled within -pi to +pi
/* proportional term */
float up = Ref - Fbk;
if (up >= 0.5) up -= 1; /* roll in the error */
else if (up <= -0.5) up += 1; /* roll in the error */
up *= Kp;
/* integral term */
float ui = Out==v1 ? Ki*up + i1 : i1;
i1 = ui;
/* control output */
return Out = U.sat(v1=up+ui);
}
#endif
};
/* PID_GRANDO */
struct PidTerminals {
float Ref, // Input: reference set-point
Fbk, // Input: feedback
Out, // Output: controller output
c1, // Internal: derivative filter coefficient 1
c2; // Internal: derivative filter coefficient 2
PidTerminals():Ref(0),Fbk(0),Out(0),c1(0),c2(0) {}
}; // note: c1 & c2 placed here to keep structure size under 8 words
struct PidParameters {
float Kr, // Parameter: reference set-point weighting
Kp, // Parameter: proportional loop gain
Ki, // Parameter: integral gain
Kd, // Parameter: derivative gain
Km; // Parameter: derivative weighting
Range U; // Parameter: saturation limits
PidParameters():Kr(1),Kp(1),Ki(0),Kd(0),Km(1) {}
};
struct PidData {
float up, // Data: proportional term
ui, // Data: integral term
ud, // Data: derivative term
v1, // Data: pre-saturated controller output
i1, // Data: integrator storage: ui(k-1)
d1, // Data: differentiator storage: ud(k-1)
d2, // Data: differentiator storage: d2(k-1)
w1; // Data: saturation record: [u(k-1) - v(k-1)]
PidData():up(0),ui(0),ud(0),v1(0),i1(0),d1(0),d2(0),w1(1) {}
};
struct PidController {
PidTerminals term;
PidParameters param;
PidData data;
float operator()() {
/* proportional term */
data.up = param.Kr*term.Ref - term.Fbk;
/* integral term */
data.ui = param.Ki*data.w1*(term.Ref-term.Fbk) + data.i1;
data.i1 = data.ui;
/* derivative term */
data.d2 = param.Kd*term.c1*(term.Ref*param.Km-term.Fbk) - data.d2;
data.ud = data.d2+data.d1;
data.d1 = data.ud*term.c2;
/* control output */
data.v1 = param.Kp*(data.up + data.ui + data.ud);
data.w1 = (term.Out == data.v1) ? 1 : 0;
return term.Out = param.U.sat(data.v1);
}
float operator()(float fbk) {term.Fbk=fbk; return operator()();}
float operator()(float fbk,float ref) {term.Ref=ref; return operator()(fbk);}
};
/*****************************************************
* Aus SPEED_FR.H: Speed controller (Drehzahlregler) *
*****************************************************/
struct SpeedMeasQep {
float ElecTheta; // Input: Electrical angle
Uint32 DirectionQep; // Variable: Direction of rotation
float OldElecTheta, // History: Electrical angle at previous step
Speed; // Output: Speed in per-unit
Uint32 BaseRpm; // Parameter: Base speed in rpm
float K1, // Parameter: Constant for differentiator
K2, // Parameter: Constant for low-pass filter
K3; // Parameter: Constant for low-pass filter
int32 SpeedRpm; // Output : Speed in rpm
SpeedMeasQep():ElecTheta(0),DirectionQep(1),OldElecTheta(0),Speed(0),BaseRpm(0),K1(0),K2(0),K3(0),SpeedRpm(0) {}
float operator()() { // calculation operator
/* Differentiator */
/* Synchronous speed computation */
float Tmp = ElecTheta - OldElecTheta;
if (Tmp < -0.5) Tmp += 1;
else if (Tmp > 0.5) Tmp -= 1;
Tmp *= K1;
/* Low-pass filter */
Tmp = K2*Speed+K3*Tmp;
/* Saturate the output */
Range r; // construct a range -1..1 on the fly
r.limit(Tmp);
Speed = Tmp;
/* Update the electrical angle */
OldElecTheta = ElecTheta;
/* Change motor speed from pu value to rpm value */
Tmp = BaseRpm*Speed;
SpeedRpm = Tmp; // SpeedRpm ist dusseligerweise int32 — warum auch immer
return Tmp;
}
float operator()(float theta) {ElecTheta=theta; return operator()();}
};
}// namespace mc
#endif
| Detected encoding: ASCII (7 bit) | 8
|