seklyuts/MotorControlModuleSDFM_TMS320F28388D
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 4 Wiki Activity
44cfb121d4
MotorControlModuleSDFM_TMS3.../Projects/EFC_Application/UMLibrary/control/Vector.hh

347 lines
11 KiB
C++
Raw Normal View History

Модификация проекта EFC_Application с добавлением запуска мотора на CPU2
2024-05-22 09:42:45 +03:00
/*
* Vector.h
*
* Created on: 7 <EFBFBD><EFBFBD><EFBFBD> 2017 <EFBFBD>.
* Author: titov
*/
#ifndef SOURCES_CONTROL_VECTOR_H_
#define SOURCES_CONTROL_VECTOR_H_
#include "../math/math_inc.hh"
namespace control {
typedef float Value;
struct IValueTransform {
virtual Value operator()(Value value) const = 0;
};
struct NaturalCoordinate {
float u;
float v;
float w;
NaturalCoordinate() noexcept : u(NAN), v(NAN), w(NAN) {}
NaturalCoordinate( float _u, float _v, float _w) noexcept : u(_u), v(_v), w(_w) {}
NaturalCoordinate( const NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
NaturalCoordinate & operator=( const NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }
NaturalCoordinate( const volatile NaturalCoordinate & rh ) noexcept : u(rh.u), v(rh.v), w(rh.w) {}
NaturalCoordinate & operator=( const volatile NaturalCoordinate & rh ) { u = rh.u, v = rh.v, w = rh.w; return *this; }
bool operator==( const NaturalCoordinate & rh ) const { return u == rh.u and v == rh.v and w == rh.w; }
bool operator!=( const NaturalCoordinate & rh ) const { return not rh.operator ==(*this); }
operator bool() const {
using namespace std;
return not( isnan(u) or isnan(v) or isnan(w) );
}
};
struct StandingVector {
float alfa;
float beta;
StandingVector() noexcept : alfa(NAN), beta(NAN) {}
StandingVector(float _alfa, float _beta) noexcept : alfa(_alfa), beta(_beta) {}
StandingVector( const volatile StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
StandingVector( const StandingVector & rh ) noexcept : alfa(rh.alfa), beta(rh.beta) {}
StandingVector & operator=( const volatile StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }
StandingVector & operator=( const StandingVector & rh ) noexcept { alfa = rh.alfa, beta = rh.beta; return *this; }
float sqrlen() const { return alfa * alfa + beta * beta; }
float len() const { return std::sqrt( sqrlen() ); }
StandingVector & operator*=(float mul) { alfa *= mul; beta *= mul; return *this; }
StandingVector operator*(float mul) {
return StandingVector( alfa * mul, beta * mul );
}
bool operator==( const StandingVector & rh ) const { return alfa == rh.alfa and beta == rh.beta; }
bool operator!=( const StandingVector & rh ) const { return not rh.operator ==(*this); }
};
struct IStandingVectorFactorTransform {
virtual StandingVector operator()( StandingVector value, Value factor ) const = 0;
};
struct RotatingVector {
float q;
float d;
RotatingVector() noexcept : q(NAN), d(NAN) {}
RotatingVector(float _q, float _d) : q(_q), d(_d) {}
RotatingVector( const volatile RotatingVector & rh ) : q(rh.q), d(rh.d) {}
RotatingVector & operator=( const volatile RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}
RotatingVector( const RotatingVector & rh ) : q(rh.q), d(rh.d) {}
RotatingVector & operator=( const RotatingVector & rh ) { q = rh.q, d = rh.d; return *this;}
float sqrlen() const { return q * q + d * d; }
float len() const { return std::sqrt( sqrlen() ); }
RotatingVector & operator*=(float mul) { q *= mul; d *= mul; return *this; }
RotatingVector operator*(float mul) { return RotatingVector( q * mul, d * mul ); }
RotatingVector operator-( const RotatingVector & rv ) {
return RotatingVector( q - rv.q, d - rv.d );
}
RotatingVector operator+( const RotatingVector & rv ) {
return RotatingVector( q + rv.q, d + rv.d );
}
RotatingVector & operator+=( const RotatingVector & rv ) {
q += rv.q, d += rv.d;
return *this;
}
bool operator==( const RotatingVector & rh ) const { return q == rh.q and d == rh.d; }
bool operator!=( const RotatingVector & rh ) const { return not rh.operator ==(*this); }
};
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 120 <20><>.
/*!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 120 <20><>.
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
*
*/
NaturalCoordinate normalize( const NaturalCoordinate & input );
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
float calc_offset(const NaturalCoordinate & input);
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
float flength(const StandingVector & vector);
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
float flength(const RotatingVector & vector);
StandingVector tf_clark( const NaturalCoordinate & );
RotatingVector tf_park( const StandingVector &, float );
NaturalCoordinate itf_clark( const StandingVector & );
StandingVector itf_park( const RotatingVector &, float );
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
/*!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 5%.
* \param[in,out] vector <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* \param[in] limit <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* \return <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
*/
bool flimitation( RotatingVector & vector, float limit );
//!\copydoc flimitation( RotatingVector & vector, float limit )
bool flimitation( StandingVector & vector, float limit );
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
/*!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* \param[in,out] vector <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* \param[in] limit <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
* \return <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
*/
bool limitation( RotatingVector & vector, float limit );
//!\copydoc flimitation( RotatingVector & vector, float limit )
bool limitation( StandingVector & vector, float limit );
template<typename Type>
struct PhaseSpacePoint {
typedef Type Value;
Type value;
Type d_value;
PhaseSpacePoint() noexcept : value(), d_value() {}
PhaseSpacePoint( Type val, Type d_val ) noexcept : value(val), d_value(d_val) {}
PhaseSpacePoint( const PhaseSpacePoint & rh ) noexcept : value(rh.value), d_value(rh.d_value) {}
PhaseSpacePoint operator*( const Type & mul ) { return PhaseSpacePoint( value * mul, d_value * mul ); }
PhaseSpacePoint & operator=( const PhaseSpacePoint & rh ) { value = rh.value, d_value = rh.d_value; return *this;}
bool operator==( const PhaseSpacePoint & rh ) const { return value == rh.value and d_value == rh.d_value; }
bool operator!=( const PhaseSpacePoint & rh ) const { return not rh.operator ==(*this); }
};
typedef PhaseSpacePoint<float> PhaseSpaceValue;
bool limitation( PhaseSpaceValue & value, float limit );
float next( const PhaseSpaceValue & val, float time );
template<typename Type, typename Time = float>
Type next( const PhaseSpacePoint<Type> & val, Time time );
bool limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit );
}
inline float control::flength(const StandingVector & vector) {
const float amax = 0.960433870f;
const float bmin = 0.397824735f;
const float qa = std::fabs(vector.alfa);
const float qb = std::fabs(vector.beta);
if(qa > qb)
return qa * amax + qb * bmin;
else
return qa * bmin + qb * amax;
}
inline float control::flength(const RotatingVector & vector) {
const float amax = 0.960433870f;
const float bmin = 0.397824735f;
const float qa = std::fabs(vector.q);
const float qb = std::fabs(vector.d);
if(qa > qb)
return qa * amax + qb * bmin;
else
return qa * bmin + qb * amax;
}
inline control::NaturalCoordinate control::normalize(const NaturalCoordinate & input) {
return NaturalCoordinate(
( input.u * 2 - input.v - input.w ) * 0.333333333f,
( input.v * 2 - input.w - input.u ) * 0.333333333f,
( input.w * 2 - input.u - input.v ) * 0.333333333f
);
}
inline float control::calc_offset(const NaturalCoordinate & input) {
return ( input.u + input.v + input.w ) * 0.333333333f;
}
inline control::StandingVector control::tf_clark( const NaturalCoordinate & value ) {
return StandingVector(
value.u,
( value.u + 2 * value.v ) * 0.577350269f
);
}
inline control::RotatingVector control::tf_park( const StandingVector & value, float iota ) {
float sin_val;
float cos_val;
math::function::sincos(iota, &sin_val, &cos_val);
return RotatingVector(
- value.alfa * sin_val + value.beta * cos_val,
value.alfa * cos_val + value.beta * sin_val
);
}
inline control::NaturalCoordinate control::itf_clark( const StandingVector & value ) {
return NaturalCoordinate(
value.alfa,
(math::constants::sqrt3 * value.beta - value.alfa) * 0.5f,
- value.alfa - (math::constants::sqrt3 * value.beta - value.alfa) * 0.5f
);
}
inline control::StandingVector control::itf_park( const RotatingVector & value, float iota ) {
float sin_val;
float cos_val;
math::function::sincos(iota, &sin_val, &cos_val);
return StandingVector(
-value.q * sin_val + value.d * cos_val,
value.q * cos_val + value.d * sin_val
);
}
inline float control::next( const PhaseSpaceValue & val, float time ) {
return val.value + val.d_value * time;
}
template<typename Type, typename Time>
inline Type control::next( const PhaseSpacePoint<Type> & val, Time time ) {
return val.value + val.d_value * time;
}
inline bool control::flimitation( RotatingVector & vector, float limit ) {
return limitation( vector, limit );
}
inline bool control::flimitation( StandingVector & vector, float limit ) {
return limitation( reinterpret_cast<RotatingVector &>(vector), limit );
}
inline bool control::limitation( RotatingVector & vector, float limit ) {
bool is_limited;
float a_sqrt = vector.sqrlen();
float inv_sqrt = math::function::q_rsqrt( a_sqrt );
if( is_limited = ( a_sqrt > limit*limit ) )
vector = vector * ( limit * inv_sqrt );
return is_limited;
}
inline bool control::limitation( StandingVector & vector, float limit ) {
return limitation( reinterpret_cast<RotatingVector &>(vector), limit );
}
inline bool control::limitation( PhaseSpaceValue & value, float limit ) {
bool is_limited = false;
using namespace std;
if( value.value > limit ) {
value = PhaseSpaceValue( limit, 0.0f );
is_limited = true;
}
if( value.value < - limit ) {
value = PhaseSpaceValue( - limit, 0.0f );
is_limited = true;
}
return is_limited;
}
inline bool control::limitation( PhaseSpaceValue & value, float upper_limit, float lower_limit ) {
bool is_limited = false;
using namespace std;
if( value.value > upper_limit ) {
value = PhaseSpaceValue( upper_limit, 0.0f );
is_limited = true;
}
if( value.value < lower_limit ) {
value = PhaseSpaceValue( lower_limit, 0.0f );
is_limited = true;
}
return is_limited;
}
#endif /* SOURCES_CONTROL_VECTOR_H_ */
Reference in New Issue Copy Permalink