2018-11-08 13:31:28 +01:00
|
|
|
/*******************************************************************************
|
|
|
|
* Copyright (C) 2018 MINRES Technologies GmbH
|
|
|
|
* All rights reserved.
|
2018-07-28 09:45:49 +02:00
|
|
|
*
|
2018-11-08 13:31:28 +01:00
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
* modification, are permitted provided that the following conditions are met:
|
|
|
|
*
|
|
|
|
* 1. Redistributions of source code must retain the above copyright notice,
|
|
|
|
* this list of conditions and the following disclaimer.
|
|
|
|
*
|
|
|
|
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
|
|
|
* this list of conditions and the following disclaimer in the documentation
|
|
|
|
* and/or other materials provided with the distribution.
|
|
|
|
*
|
|
|
|
* 3. Neither the name of the copyright holder nor the names of its contributors
|
|
|
|
* may be used to endorse or promote products derived from this software
|
|
|
|
* without specific prior written permission.
|
|
|
|
*
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*
|
|
|
|
*******************************************************************************/
|
2018-07-28 09:45:49 +02:00
|
|
|
|
|
|
|
#include "sysc/top/BLDC.h"
|
|
|
|
|
|
|
|
// implementation according to Modeling of BLDC Motor with Ideal Back-EMF for Automotive Applications
|
|
|
|
// Proceedings of the World Congress on Engineering 2011 Vol II WCE 2011, July 6 - 8, 2011, London, U.K.
|
|
|
|
BLDC::BLDC(const Config config)
|
|
|
|
: config(config)
|
|
|
|
, stateVector({{0.0, 0.0, 0.0, 0.0, 0.0}})
|
|
|
|
, state(stateVector)
|
|
|
|
, vin({{0.0, 0.0, 0.0}})
|
2018-11-08 13:31:28 +01:00
|
|
|
, voltages({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}) {
|
2018-07-28 09:45:49 +02:00
|
|
|
state.init();
|
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
BLDC::~BLDC() = default;
|
2018-07-28 09:45:49 +02:00
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
double BLDC::calc_bemf_factor(const State &x, double theta) {
|
|
|
|
if (theta >= 0 && theta < 2. / 3. * M_PI) {
|
2018-07-28 09:45:49 +02:00
|
|
|
return 1;
|
2018-11-08 13:31:28 +01:00
|
|
|
} else if (theta >= 2. / 3. * M_PI && theta < M_PI) {
|
|
|
|
return 1 - 6 / M_PI * (theta - 2. / 3. * M_PI);
|
|
|
|
} else if (theta >= M_PI && theta < 5. / 3. * M_PI) {
|
2018-07-28 09:45:49 +02:00
|
|
|
return -1;
|
2018-11-08 13:31:28 +01:00
|
|
|
} else if (theta >= 5. / 3. * M_PI && theta <= 2. * M_PI) {
|
|
|
|
return -1 + 6 / M_PI * (theta - 5. / 3. * M_PI);
|
2018-07-28 09:45:49 +02:00
|
|
|
} else {
|
|
|
|
fprintf(stderr, "ERROR: angle out of bounds can not calculate bemf %f\n", theta);
|
|
|
|
throw std::runtime_error("angle out of bounds can not calculate bemf");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
void BLDC::calc_back_emf(const State &state, double theta_e) {
|
2018-07-28 09:45:49 +02:00
|
|
|
double max_bemf = config.Ke * state.omega;
|
2018-11-08 13:31:28 +01:00
|
|
|
theta_e-=M_PI * (1. / 3.);
|
|
|
|
voltages[EA] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e));
|
|
|
|
voltages[EB] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e + M_PI * (2. / 3.)));
|
|
|
|
voltages[EC] = max_bemf * calc_bemf_factor(state, norm_angle(theta_e + M_PI * (4. / 3.)));
|
2018-07-28 09:45:49 +02:00
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
void BLDC::calc_voltages() {
|
2018-07-28 09:45:49 +02:00
|
|
|
const double NaN = nan("");
|
|
|
|
/* Check which phases are excited. */
|
2018-11-08 13:31:28 +01:00
|
|
|
bool pa = isnan(vin[0]) ? false : true;
|
|
|
|
bool pb = isnan(vin[1]) ? false : true;
|
|
|
|
bool pc = isnan(vin[2]) ? false : true;
|
2018-07-28 09:45:49 +02:00
|
|
|
|
|
|
|
if (pa && pb && pc) {
|
|
|
|
voltages[VA] = vin[0];
|
|
|
|
voltages[VB] = vin[1];
|
|
|
|
voltages[VC] = vin[2];
|
2018-11-08 13:31:28 +01:00
|
|
|
voltages[VCENTER] =
|
|
|
|
(voltages[VA] + voltages[VB] + voltages[VC] - voltages[EA] - voltages[EB] - voltages[EC]) / 3.;
|
2018-07-28 09:45:49 +02:00
|
|
|
} else if (pa && pb) {
|
|
|
|
voltages[VA] = vin[0];
|
|
|
|
voltages[VB] = vin[1];
|
|
|
|
voltages[VCENTER] = (voltages[VA] + voltages[VB] - voltages[EA] - voltages[EB]) / 2.;
|
|
|
|
voltages[VC] = voltages[EC] + voltages[VCENTER];
|
|
|
|
} else if (pa && pc) {
|
|
|
|
voltages[VA] = vin[0];
|
|
|
|
voltages[VC] = vin[2];
|
|
|
|
voltages[VCENTER] = (voltages[VA] + voltages[VC] - voltages[EA] - voltages[EC]) / 2.;
|
|
|
|
voltages[VB] = voltages[EB] + voltages[VCENTER];
|
|
|
|
} else if (pb && pc) {
|
|
|
|
voltages[VB] = vin[1];
|
|
|
|
voltages[VC] = vin[2];
|
|
|
|
voltages[VCENTER] = (voltages[VB] + voltages[VC] - voltages[EB] - voltages[EC]) / 2.;
|
|
|
|
voltages[VA] = voltages[EA] + voltages[VCENTER];
|
|
|
|
} else if (pa) {
|
|
|
|
voltages[VA] = vin[0];
|
|
|
|
voltages[VCENTER] = (voltages[VA] - voltages[EA]);
|
|
|
|
voltages[VB] = voltages[EB] + voltages[VCENTER];
|
|
|
|
voltages[VC] = voltages[EC] + voltages[VCENTER];
|
|
|
|
} else if (pb) {
|
|
|
|
voltages[VB] = vin[1];
|
|
|
|
voltages[VCENTER] = (voltages[VB] - voltages[EB]);
|
|
|
|
voltages[VA] = voltages[EA] + voltages[VCENTER];
|
|
|
|
voltages[VC] = voltages[EC] + voltages[VCENTER];
|
|
|
|
} else if (pc) {
|
|
|
|
voltages[VC] = vin[0];
|
|
|
|
voltages[VCENTER] = (voltages[VC] - voltages[EC]);
|
|
|
|
voltages[VA] = voltages[EA] + voltages[VCENTER];
|
|
|
|
voltages[VB] = voltages[EB] + voltages[VCENTER];
|
|
|
|
} else {
|
|
|
|
voltages[VA] = voltages[EA];
|
|
|
|
voltages[VB] = voltages[EB];
|
|
|
|
voltages[VC] = voltages[EC];
|
|
|
|
voltages[VCENTER] = 0;
|
2018-11-08 13:31:28 +01:00
|
|
|
// return;
|
2018-07-28 09:45:49 +02:00
|
|
|
}
|
2018-11-08 13:31:28 +01:00
|
|
|
auto vmax = std::max({pa ? vin[0] : 0, pb ? vin[1] : 0, pc ? vin[2] : 0});
|
|
|
|
voltages[VCENTER] = vmax / 2;
|
2018-07-28 09:45:49 +02:00
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
void BLDC::printToStream(std::ostream &os) const {
|
|
|
|
os << state.omega << ";" << state.theta << ";" << state.ia << ";" << state.ib << ";" << state.ic << ";"
|
|
|
|
<< voltages[VA] << ";" << voltages[VB] << ";" << voltages[VC] << ";" << voltages[EA] << ";" << voltages[EB]
|
|
|
|
<< ";" << voltages[EC] << ";" << voltages[VCENTER] << ";" << vin[0] << ";" << vin[1] << ";" << vin[2] << ";"
|
|
|
|
<< etorque;
|
2018-07-28 09:45:49 +02:00
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
void BLDC::rotor_dyn(const StateVector &x_, StateVector &dxdt_, const double t) {
|
|
|
|
const State x(const_cast<StateVector &>(x_));
|
2018-07-28 09:45:49 +02:00
|
|
|
State dxdt(dxdt_);
|
|
|
|
double theta_e = state.theta * (config.NbPoles / 2.);
|
|
|
|
/* Calculate backemf voltages. */
|
|
|
|
calc_back_emf(x, theta_e);
|
|
|
|
/* Calculate voltages. */
|
|
|
|
calc_voltages();
|
|
|
|
/* Electromagnetic torque. */
|
2018-11-08 13:31:28 +01:00
|
|
|
// if (x.omega == 0) {
|
|
|
|
// printf("ERROR: input state vector omega equals 0!!!\n");
|
|
|
|
// throw std::runtime_error("input state vector omega equals 0");
|
|
|
|
// }
|
2018-07-28 09:45:49 +02:00
|
|
|
/* electrical torque */
|
2018-11-08 13:31:28 +01:00
|
|
|
// etorque = ((voltages[EA] * x.ia) + (voltages[EB] * x.ib) + (voltages[EC] * x.ic)) / x.omega;
|
2018-07-28 09:45:49 +02:00
|
|
|
// which is equivalent to:
|
2018-11-08 13:31:28 +01:00
|
|
|
etorque = config.Ke * (x.ia * (calc_bemf_factor(state, norm_angle(theta_e))) +
|
|
|
|
x.ib * (calc_bemf_factor(state, norm_angle(theta_e + M_PI * (2. / 3.)))) +
|
|
|
|
x.ic * (calc_bemf_factor(state, norm_angle(theta_e + M_PI * (4. / 3.)))));
|
2018-07-28 09:45:49 +02:00
|
|
|
/* Mechanical torque. */
|
|
|
|
mtorque = ((etorque * (config.NbPoles / 2)) - (config.damping * x.omega) - torque_load);
|
|
|
|
|
|
|
|
if ((mtorque > 0) && (mtorque <= config.static_friction)) {
|
|
|
|
mtorque = 0;
|
|
|
|
} else if (mtorque > config.static_friction) {
|
|
|
|
mtorque -= config.static_friction;
|
|
|
|
} else if ((mtorque < 0) && (mtorque >= -(config.static_friction))) {
|
|
|
|
mtorque = 0;
|
|
|
|
} else if (mtorque < -(config.static_friction)) {
|
|
|
|
mtorque += config.static_friction;
|
|
|
|
}
|
|
|
|
/* Position of the rotor */
|
|
|
|
dxdt.theta = x.omega;
|
|
|
|
/* Acceleration of the rotor. (omega_dot) */
|
|
|
|
// a=M/J with M->torque, J->Inertia, a->angular acceleration
|
|
|
|
dxdt.omega = mtorque / config.inertia;
|
|
|
|
/* Calculate dot currents. */
|
|
|
|
dxdt.ia = (voltages[VA] - (config.R * x.ia) - voltages[EA] - voltages[VCENTER]) / (config.L - config.M);
|
|
|
|
dxdt.ib = (voltages[VB] - (config.R * x.ib) - voltages[EB] - voltages[VCENTER]) / (config.L - config.M);
|
|
|
|
dxdt.ic = (voltages[VC] - (config.R * x.ic) - voltages[EC] - voltages[VCENTER]) / (config.L - config.M);
|
|
|
|
}
|
|
|
|
|
|
|
|
void BLDC::run(double incr) {
|
2018-11-08 13:31:28 +01:00
|
|
|
if (dt > incr) throw std::runtime_error("incr needs to be larger than dt");
|
|
|
|
double next_time = current_time + incr;
|
|
|
|
odeint::integrate_adaptive(
|
|
|
|
make_controlled(1.0e-10, 1.0e-6, stepper_type()),
|
|
|
|
[this](const StateVector &x, StateVector &dxdt, double t) { this->rotor_dyn(x, dxdt, t); }, stateVector,
|
|
|
|
current_time, next_time, dt);
|
|
|
|
current_time = next_time;
|
|
|
|
state.theta = norm_angle(state.theta);
|
2018-07-28 09:45:49 +02:00
|
|
|
}
|
|
|
|
|
2018-11-08 13:31:28 +01:00
|
|
|
std::ostream &operator<<(std::ostream &os, const BLDC &bldc) {
|
2018-07-28 09:45:49 +02:00
|
|
|
bldc.printToStream(os);
|
|
|
|
return os;
|
|
|
|
}
|