528 lines
15 KiB
C++
528 lines
15 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2014 Yorik van Havre <yorik@uncreated.net> *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#include "PreCompiled.h"
|
|
|
|
#include <App/Application.h>
|
|
#include <Base/Console.h>
|
|
#include <Base/Reader.h>
|
|
#include <Base/Stream.h>
|
|
#include <Base/Writer.h>
|
|
#include <Mod/CAM/App/PathSegmentWalker.h>
|
|
|
|
#include "Path.h"
|
|
|
|
|
|
using namespace Path;
|
|
using namespace Base;
|
|
|
|
TYPESYSTEM_SOURCE(Path::Toolpath, Base::Persistence)
|
|
|
|
Toolpath::Toolpath()
|
|
{}
|
|
|
|
Toolpath::Toolpath(const Toolpath& otherPath)
|
|
: vpcCommands(otherPath.vpcCommands.size())
|
|
, center(otherPath.center)
|
|
{
|
|
*this = otherPath;
|
|
recalculate();
|
|
}
|
|
|
|
Toolpath::~Toolpath()
|
|
{
|
|
clear();
|
|
}
|
|
|
|
Toolpath& Toolpath::operator=(const Toolpath& otherPath)
|
|
{
|
|
if (this == &otherPath) {
|
|
return *this;
|
|
}
|
|
|
|
clear();
|
|
vpcCommands.resize(otherPath.vpcCommands.size());
|
|
int i = 0;
|
|
for (std::vector<Command*>::const_iterator it = otherPath.vpcCommands.begin();
|
|
it != otherPath.vpcCommands.end();
|
|
++it, i++) {
|
|
vpcCommands[i] = new Command(**it);
|
|
}
|
|
center = otherPath.center;
|
|
recalculate();
|
|
return *this;
|
|
}
|
|
|
|
void Toolpath::clear()
|
|
{
|
|
for (std::vector<Command*>::iterator it = vpcCommands.begin(); it != vpcCommands.end(); ++it) {
|
|
delete (*it);
|
|
}
|
|
vpcCommands.clear();
|
|
recalculate();
|
|
}
|
|
|
|
void Toolpath::addCommand(const Command& Cmd)
|
|
{
|
|
Command* tmp = new Command(Cmd);
|
|
vpcCommands.push_back(tmp);
|
|
recalculate();
|
|
}
|
|
|
|
void Toolpath::insertCommand(const Command& Cmd, int pos)
|
|
{
|
|
if (pos == -1) {
|
|
addCommand(Cmd);
|
|
}
|
|
else if (pos <= static_cast<int>(vpcCommands.size())) {
|
|
Command* tmp = new Command(Cmd);
|
|
vpcCommands.insert(vpcCommands.begin() + pos, tmp);
|
|
}
|
|
else {
|
|
throw Base::IndexError("Index not in range");
|
|
}
|
|
recalculate();
|
|
}
|
|
|
|
void Toolpath::deleteCommand(int pos)
|
|
{
|
|
if (pos == -1) {
|
|
// delete(*vpcCommands.rbegin()); // causes crash
|
|
vpcCommands.pop_back();
|
|
}
|
|
else if (pos <= static_cast<int>(vpcCommands.size())) {
|
|
vpcCommands.erase(vpcCommands.begin() + pos);
|
|
}
|
|
else {
|
|
throw Base::IndexError("Index not in range");
|
|
}
|
|
recalculate();
|
|
}
|
|
|
|
double Toolpath::getLength()
|
|
{
|
|
if (vpcCommands.empty()) {
|
|
return 0;
|
|
}
|
|
double l = 0;
|
|
Vector3d last(0, 0, 0);
|
|
Vector3d next;
|
|
for (std::vector<Command*>::const_iterator it = vpcCommands.begin(); it != vpcCommands.end();
|
|
++it) {
|
|
std::string name = (*it)->Name;
|
|
next = (*it)->getPlacement(last).getPosition();
|
|
if ((name == "G0") || (name == "G00") || (name == "G1") || (name == "G01")) {
|
|
// straight line
|
|
l += (next - last).Length();
|
|
last = next;
|
|
}
|
|
else if ((name == "G2") || (name == "G02") || (name == "G3") || (name == "G03")) {
|
|
// arc
|
|
Vector3d center = (*it)->getCenter();
|
|
double radius = (last - center).Length();
|
|
double angle = (next - center).GetAngle(last - center);
|
|
l += angle * radius;
|
|
last = next;
|
|
}
|
|
}
|
|
return l;
|
|
}
|
|
|
|
double Toolpath::getCycleTime(double hFeed, double vFeed, double hRapid, double vRapid)
|
|
{
|
|
// check the feedrates are set
|
|
if ((hFeed == 0) || (vFeed == 0)) {
|
|
ParameterGrp::handle hGrp = App::GetApplication().GetParameterGroupByPath(
|
|
"User parameter:BaseApp/Preferences/Mod/CAM");
|
|
if (!hGrp->GetBool("WarningsSuppressAllSpeeds", true)) {
|
|
Base::Console().warning("Feed Rate Error: Check Tool Controllers have Feed Rates");
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
if (hRapid == 0) {
|
|
hRapid = hFeed;
|
|
}
|
|
|
|
if (vRapid == 0) {
|
|
vRapid = vFeed;
|
|
}
|
|
|
|
if (vpcCommands.empty()) {
|
|
return 0;
|
|
}
|
|
double l = 0;
|
|
double time = 0;
|
|
bool verticalMove = false;
|
|
Vector3d last(0, 0, 0);
|
|
Vector3d next;
|
|
for (std::vector<Command*>::const_iterator it = vpcCommands.begin(); it != vpcCommands.end();
|
|
++it) {
|
|
std::string name = (*it)->Name;
|
|
float feedrate = (*it)->getParam("F");
|
|
|
|
l = 0;
|
|
verticalMove = false;
|
|
feedrate = hFeed;
|
|
next = (*it)->getPlacement(last).getPosition();
|
|
|
|
if (last.z != next.z) {
|
|
verticalMove = true;
|
|
feedrate = vFeed;
|
|
}
|
|
|
|
if ((name == "G0") || (name == "G00")) {
|
|
// Rapid Move
|
|
l += (next - last).Length();
|
|
feedrate = hRapid;
|
|
if (verticalMove) {
|
|
feedrate = vRapid;
|
|
}
|
|
}
|
|
else if ((name == "G1") || (name == "G01")) {
|
|
// Feed Move
|
|
l += (next - last).Length();
|
|
}
|
|
else if ((name == "G2") || (name == "G02") || (name == "G3") || (name == "G03")) {
|
|
// Arc Move
|
|
Vector3d center = (*it)->getCenter();
|
|
double radius = (last - center).Length();
|
|
double angle = (next - center).GetAngle(last - center);
|
|
l += angle * radius;
|
|
}
|
|
|
|
time += l / feedrate;
|
|
last = next;
|
|
}
|
|
return time;
|
|
}
|
|
|
|
class BoundBoxSegmentVisitor: public PathSegmentVisitor
|
|
{
|
|
public:
|
|
BoundBoxSegmentVisitor()
|
|
{}
|
|
|
|
void g0(int id,
|
|
const Base::Vector3d& last,
|
|
const Base::Vector3d& next,
|
|
const std::deque<Base::Vector3d>& pts) override
|
|
{
|
|
(void)id;
|
|
processPt(last);
|
|
processPts(pts);
|
|
processPt(next);
|
|
}
|
|
void g1(int id,
|
|
const Base::Vector3d& last,
|
|
const Base::Vector3d& next,
|
|
const std::deque<Base::Vector3d>& pts) override
|
|
{
|
|
(void)id;
|
|
processPt(last);
|
|
processPts(pts);
|
|
processPt(next);
|
|
}
|
|
void g23(int id,
|
|
const Base::Vector3d& last,
|
|
const Base::Vector3d& next,
|
|
const std::deque<Base::Vector3d>& pts,
|
|
const Base::Vector3d& center) override
|
|
{
|
|
(void)id;
|
|
(void)center;
|
|
processPt(last);
|
|
processPts(pts);
|
|
processPt(next);
|
|
}
|
|
void g8x(int id,
|
|
const Base::Vector3d& last,
|
|
const Base::Vector3d& next,
|
|
const std::deque<Base::Vector3d>& pts,
|
|
const std::deque<Base::Vector3d>& p,
|
|
const std::deque<Base::Vector3d>& q) override
|
|
{
|
|
(void)id;
|
|
(void)q; // always within the bounds of p
|
|
processPt(last);
|
|
processPts(pts);
|
|
processPts(p);
|
|
processPt(next);
|
|
}
|
|
void g38(int id, const Base::Vector3d& last, const Base::Vector3d& next) override
|
|
{
|
|
(void)id;
|
|
processPt(last);
|
|
processPt(next);
|
|
}
|
|
|
|
Base::BoundBox3d bb;
|
|
|
|
private:
|
|
void processPts(const std::deque<Base::Vector3d>& pts)
|
|
{
|
|
for (std::deque<Base::Vector3d>::const_iterator it = pts.begin(); pts.end() != it; ++it) {
|
|
processPt(*it);
|
|
}
|
|
}
|
|
void processPt(const Base::Vector3d& pt)
|
|
{
|
|
bb.MaxX = std::max(bb.MaxX, pt.x);
|
|
bb.MinX = std::min(bb.MinX, pt.x);
|
|
bb.MaxY = std::max(bb.MaxY, pt.y);
|
|
bb.MinY = std::min(bb.MinY, pt.y);
|
|
bb.MaxZ = std::max(bb.MaxZ, pt.z);
|
|
bb.MinZ = std::min(bb.MinZ, pt.z);
|
|
}
|
|
};
|
|
|
|
Base::BoundBox3d Toolpath::getBoundBox() const
|
|
{
|
|
BoundBoxSegmentVisitor visitor;
|
|
PathSegmentWalker walker(*this);
|
|
walker.walk(visitor, Vector3d(0, 0, 0));
|
|
|
|
return visitor.bb;
|
|
}
|
|
|
|
static void
|
|
bulkAddCommand(const std::string& gcodestr, std::vector<Command*>& commands, bool& inches)
|
|
{
|
|
Command* cmd = new Command();
|
|
cmd->setFromGCode(gcodestr);
|
|
if ("G20" == cmd->Name) {
|
|
inches = true;
|
|
delete cmd;
|
|
}
|
|
else if ("G21" == cmd->Name) {
|
|
inches = false;
|
|
delete cmd;
|
|
}
|
|
else {
|
|
if (inches) {
|
|
cmd->scaleBy(25.4);
|
|
}
|
|
commands.push_back(cmd);
|
|
}
|
|
}
|
|
|
|
void Toolpath::setFromGCode(const std::string instr)
|
|
{
|
|
clear();
|
|
|
|
// remove comments
|
|
// boost::regex e("\\(.*?\\)");
|
|
// std::string str = boost::regex_replace(instr, e, "");
|
|
std::string str(instr);
|
|
|
|
// split input string by () or G or M commands
|
|
std::string mode = "command";
|
|
std::size_t found = str.find_first_of("(gGmM");
|
|
int last = -1;
|
|
bool inches = false;
|
|
while (found != std::string::npos) {
|
|
if (str[found] == '(') {
|
|
// start of comment
|
|
if ((last > -1) && (mode == "command")) {
|
|
// before opening a comment, add the last found command
|
|
std::string gcodestr = str.substr(last, found - last);
|
|
bulkAddCommand(gcodestr, vpcCommands, inches);
|
|
}
|
|
mode = "comment";
|
|
last = found;
|
|
found = str.find_first_of(')', found + 1);
|
|
}
|
|
else if (str[found] == ')') {
|
|
// end of comment
|
|
std::string gcodestr = str.substr(last, found - last + 1);
|
|
bulkAddCommand(gcodestr, vpcCommands, inches);
|
|
last = -1;
|
|
found = str.find_first_of("(gGmM", found + 1);
|
|
mode = "command";
|
|
}
|
|
else if (mode == "command") {
|
|
// command
|
|
if (last > -1) {
|
|
std::string gcodestr = str.substr(last, found - last);
|
|
bulkAddCommand(gcodestr, vpcCommands, inches);
|
|
}
|
|
last = found;
|
|
found = str.find_first_of("(gGmM", found + 1);
|
|
}
|
|
}
|
|
// add the last command found, if any
|
|
if (last > -1) {
|
|
if (mode == "command") {
|
|
std::string gcodestr = str.substr(last, std::string::npos);
|
|
bulkAddCommand(gcodestr, vpcCommands, inches);
|
|
}
|
|
}
|
|
recalculate();
|
|
}
|
|
|
|
std::string Toolpath::toGCode() const
|
|
{
|
|
std::string result;
|
|
for (std::vector<Command*>::const_iterator it = vpcCommands.begin(); it != vpcCommands.end();
|
|
++it) {
|
|
result += (*it)->toGCode();
|
|
result += "\n";
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void Toolpath::recalculate() // recalculates the path cache
|
|
{
|
|
|
|
if (vpcCommands.empty()) {
|
|
return;
|
|
}
|
|
|
|
// TODO recalculate the KDL stuff. At the moment, this is unused.
|
|
|
|
#if 0
|
|
// delete the old and create a new one
|
|
if(pcPath)
|
|
delete (pcPath);
|
|
|
|
pcPath = new KDL::Path_Composite();
|
|
|
|
KDL::Path *tempPath;
|
|
KDL::Frame Last;
|
|
|
|
try {
|
|
// handle the first waypoint differently
|
|
bool first=true;
|
|
|
|
for(std::vector<Command*>::const_iterator it = vpcCommands.begin();it!=vpcCommands.end();++it) {
|
|
if(first){
|
|
Last = toFrame((*it)->getPlacement());
|
|
first = false;
|
|
}else{
|
|
Base::Placement p = (*it)->getPlacement();
|
|
KDL::Frame Next = toFrame(p);
|
|
std::string name = (*it)->Name;
|
|
Vector3d zaxis(0,0,1);
|
|
|
|
if ( (name == "G0") || (name == "G1") || (name == "G01") ) {
|
|
// line segment
|
|
tempPath = new KDL::Path_Line(Last, Next, new KDL::RotationalInterpolation_SingleAxis(), 1.0, true);
|
|
pcPath->Add(tempPath);
|
|
Last = Next;
|
|
} else if ( (name == "G2") || (name == "G02") ) {
|
|
// clockwise arc
|
|
Vector3d fcenter = (*it)->getCenter();
|
|
KDL::Vector center(fcenter.x,fcenter.y,fcenter.z);
|
|
Vector3d fnorm;
|
|
p.getRotation().multVec(zaxis,fnorm);
|
|
KDL::Vector norm(fnorm.x,fnorm.y,fnorm.z);
|
|
Vector3d fstart = toPlacement(Last).getPosition();
|
|
Vector3d fend = toPlacement(Last).getPosition();
|
|
Rotation frot(fstart-fcenter,fend-fcenter);
|
|
double q0,q1,q2,q3;
|
|
frot.getValue(q0,q1,q2,q3);
|
|
KDL::Rotation rot;
|
|
rot.Quaternion(q0,q1,q2,q3);
|
|
tempPath = new KDL::Path_Circle(Last, center, norm, rot, 0.0, new KDL::RotationalInterpolation_SingleAxis(), 1.0, true);
|
|
pcPath->Add(tempPath);
|
|
Last = Next;
|
|
}
|
|
}
|
|
}
|
|
} catch (KDL::Error &e) {
|
|
throw Base::RuntimeError(e.Description());
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// reimplemented from base class
|
|
|
|
unsigned int Toolpath::getMemSize() const
|
|
{
|
|
return toGCode().size();
|
|
}
|
|
|
|
void Toolpath::setCenter(const Base::Vector3d& c)
|
|
{
|
|
center = c;
|
|
recalculate();
|
|
}
|
|
|
|
static void saveCenter(Writer& writer, const Base::Vector3d& center)
|
|
{
|
|
writer.Stream() << writer.ind() << "<Center x=\"" << center.x << "\" y=\"" << center.y
|
|
<< "\" z=\"" << center.z << "\"/>" << std::endl;
|
|
}
|
|
|
|
void Toolpath::Save(Writer& writer) const
|
|
{
|
|
if (writer.isForceXML()) {
|
|
writer.Stream() << writer.ind() << "<Path count=\"" << getSize() << "\" version=\""
|
|
<< SchemaVersion << "\">" << std::endl;
|
|
writer.incInd();
|
|
saveCenter(writer, center);
|
|
for (unsigned int i = 0; i < getSize(); i++) {
|
|
vpcCommands[i]->Save(writer);
|
|
}
|
|
writer.decInd();
|
|
}
|
|
else {
|
|
writer.Stream() << writer.ind() << "<Path file=\""
|
|
<< writer.addFile((writer.ObjectName + ".nc").c_str(), this)
|
|
<< "\" version=\"" << SchemaVersion << "\">" << std::endl;
|
|
writer.incInd();
|
|
saveCenter(writer, center);
|
|
writer.decInd();
|
|
}
|
|
writer.Stream() << writer.ind() << "</Path>" << std::endl;
|
|
}
|
|
|
|
void Toolpath::SaveDocFile(Base::Writer& writer) const
|
|
{
|
|
if (toGCode().empty()) {
|
|
return;
|
|
}
|
|
writer.Stream() << toGCode();
|
|
}
|
|
|
|
void Toolpath::Restore(XMLReader& reader)
|
|
{
|
|
reader.readElement("Path");
|
|
std::string file(reader.getAttribute<const char*>("file"));
|
|
|
|
if (!file.empty()) {
|
|
// initiate a file read
|
|
reader.addFile(file.c_str(), this);
|
|
}
|
|
}
|
|
|
|
void Toolpath::RestoreDocFile(Base::Reader& reader)
|
|
{
|
|
std::string gcode;
|
|
std::string line;
|
|
while (reader >> line) {
|
|
gcode += line;
|
|
gcode += " ";
|
|
}
|
|
setFromGCode(gcode);
|
|
}
|