79 lines
2.3 KiB
C++
79 lines
2.3 KiB
C++
#include <regex>
|
|
|
|
#include "MBDynDrive.h"
|
|
|
|
using namespace MbD;
|
|
|
|
void MbD::MBDynDrive::parseMBDyn(std::string line)
|
|
{
|
|
driveString = line;
|
|
size_t previousPos = 0;
|
|
auto pos = line.find(":");
|
|
auto front = line.substr(previousPos, pos - previousPos);
|
|
assert(front.find("drive caller") != std::string::npos);
|
|
auto arguments = std::vector<std::string>();
|
|
std::string argument;
|
|
while (true) {
|
|
previousPos = pos;
|
|
pos = line.find(",", pos + 1);
|
|
if (pos != std::string::npos) {
|
|
argument = line.substr(previousPos + 1, pos - previousPos - 1);
|
|
arguments.push_back(argument);
|
|
}
|
|
else {
|
|
argument = line.substr(previousPos + 1);
|
|
arguments.push_back(argument);
|
|
break;
|
|
}
|
|
}
|
|
auto iss = std::istringstream(arguments.at(0));
|
|
iss >> name;
|
|
arguments.erase(arguments.begin());
|
|
assert(arguments.at(0).find("name") != std::string::npos);
|
|
arguments.erase(arguments.begin());
|
|
iss = std::istringstream(arguments.at(0));
|
|
iss >> driveName;
|
|
driveName = std::regex_replace(driveName, std::regex("\""), "");
|
|
arguments.erase(arguments.begin());
|
|
readFunction(arguments);
|
|
}
|
|
|
|
void MbD::MBDynDrive::readFunction(std::vector<std::string>& args)
|
|
{
|
|
if (args.empty()) return;
|
|
std::string str;
|
|
auto iss = std::istringstream(args.at(0));
|
|
iss >> str;
|
|
if (str.find("ramp") != std::string::npos) {
|
|
args.erase(args.begin());
|
|
std::string slope, initValue, initTime, finalTime;
|
|
slope = popOffTop(args);
|
|
initTime = popOffTop(args);
|
|
finalTime = popOffTop(args);
|
|
initValue = popOffTop(args);
|
|
slope.erase(remove_if(slope.begin(), slope.end(), isspace), slope.end());
|
|
initTime.erase(remove_if(initTime.begin(), initTime.end(), isspace), initTime.end());
|
|
finalTime.erase(remove_if(finalTime.begin(), finalTime.end(), isspace), finalTime.end());
|
|
initValue.erase(remove_if(initValue.begin(), initValue.end(), isspace), initValue.end());
|
|
|
|
//f = slope*(time - t0) + f0
|
|
//rampstep(time, t0, f0, t1, f1)
|
|
//t0 = initTime
|
|
//f0 = initValue
|
|
//t1 = finalTime
|
|
//f1 = initValue + slope * (finalTime - initTime)
|
|
auto ss = std::stringstream();
|
|
ss << "rampstep(time, " << initTime << ", " << initValue << ", " << finalTime << ", "
|
|
<< initValue << " + " << slope << "*(" << finalTime << " - " << initTime << "))";
|
|
formula = ss.str();
|
|
}
|
|
else {
|
|
assert(false);
|
|
}
|
|
}
|
|
|
|
void MbD::MBDynDrive::createASMT()
|
|
{
|
|
assert(false);
|
|
}
|