/* $Id: gtoisel.cpp 2.5 2002/05/23 14:49:57 leon Exp $
M-ukazi ne delujejo, ce so kot argumenti
*/
#include <iostream.h>
#include <ctype.h> // tolower()
#include <fstream.h>
#include <math.h>
#include <stdlib.h>
#include "parse.h"
#include "machinin.h"
#define LEFT_HANDED 0x04
long TotalLines = 0;
Machining Machining;
#ifdef CMDLINE
typedef unsigned short ushort;
typedef struct settingsRec {
long spuX; //TInputLong
long spuY; //TInputLong
long spuZ; //TInputLong
long speedX; //TInputLong
long speedY; //TInputLong
long speedZ; //TInputLong
long device; //TInputLong
ushort ack; //TRadioButtons
long stepSize; //TInputLong
long ratioZ; //TInputLong
ushort option; //TCheckBoxes LeftHanded is default for cmdline
char logFileName[57]; //TInputLine
} SettingsRec;
typedef struct homingSpeedRec {
long x; //TInputLong
long y; //TInputLong
long z; //TInputLong
} HomingSpeedRec;
SettingsRec localSettings = { 80, 80, 80, 500, 500, 500, 0, 0, 0, 0,
LEFT_HANDED};
SettingsRec *Settings = &localSettings;
int main (int argc, char *argv[])
{
extern FILE *yyin;
TotalLines = 0;
if(argc > 1 && (yyin = fopen(argv[1], "r")) == NULL)
{
perror(argv[1]);
exit(1);
}
Machining.ReadINI("gisel.ini");
if(!yyparse ())
clog << "G-CODE parse OK" << endl;
else
clog << "G-CODE parse NOT OK" << endl;
cout.flush();
return 0;
}
int yyerror(char *str) // C function
{
extern long int lineno;
extern char linebuf[200];
clog << "Error: '" << str << "' at line " << lineno << '\n';
clog << linebuf << endl;
return 0;
}
#else /* CMDLINE */
# include "gisel.h"
extern SettingsRec *Settings;
#endif /* CMDLINE */
// Some usefull macros
#define SQR(x) ((x)*(x))
#define SGN(x) ((x) >= 0 ? 1 : -1)
#define QUADRANT(x, y) ((((x) * (y)) < 0 ? 1 : 0) + ((y) < 0 ? 2 : 0)\
+ ((y) == 0 ? (((x) > 0) ? 3 : 1) : 0))
#define CUTOFF(x) ( (x) > 1.0 ? 1.0 : ((x) < -1.0 ? -1.0 : (x)))
#define LENGTH(x,y) sqrt((x)*(x)+(y)*(y))
#define ANGLE(x,y) ( (y) < 0.0 ? 2.0*M_PI - acos(CUTOFF((x)/LENGTH(x,y))) \
: acos(CUTOFF((x)/LENGTH(x,y))) )
#define DEG(rad) (180.0*(rad)/M_PI)
#define RAD(deg) (M_PI*(deg)/180.0)
#define IVAL node->arg.ival
#define DVAL node->arg.dval
#define COORD(coord, spu) long((coord) * Settings->spu)
#define SPEED(spu) long(Fedrate * Settings->spu / 60.0)
#define ALMOSTZERO(x) (((x) < 1e-2) && ((x) > -1e-2))
#define IMMEDIATE(c) '@' << Settings->device << acknowledge(c)
/**
** Returns upper or lower case letter
**/
inline char acknowledge(char c)
{
return Settings->ack ? toupper(c) : tolower(c);
}
/**
** Position handling
**/
class Position
{
public:
double x, y, z; // position
Position(double x, double y, double z);
Position(const Position& p);
void clear() { x = y = z = 0.0; }
int operator == (const Position& p)
{ return p.x == x && p.y == y && p.z == z; }
};
/**
** Creates default position
**/
Position::Position(const Position& p)
{
x = p.x;
y = p.y;
z = p.z;
}
/** Copy constructor **/
Position::Position(double x, double y, double z)
{
Position::x = x;
Position::y = y;
Position::z = z;
}
typedef struct {
double r; // radius
double a; // starting angle
double b; // ending angle
} CircleArgs;
static Position Pos(0.0, 0.0, 0.0); /* current position */
static Position ToPos(0.0, 0.0, 0.0); /* move To position */
static Position Center(0.0, 0.0, 0.0); /* circle center */
static double Fedrate = 400.0;
static CircleArgs CircleArg = { 0.0, 0.0, 0.0 };
static short Axes = 7;
static int CmdNumber = 0; /* Number for the M, S, T */
enum {ABSOLUTE, RELATIVE} MovingType = ABSOLUTE;
enum {COUNTERCLOCKWISE = -1, CLOCKWISE = 0 } Direction;
enum {RAB, IJK} CircleType;
/**
** Outputs default coordinate for use by absolute/relative move
**/
ostream & operator << (ostream& s, Position &move)
{
s << COORD(move.x, spuX) << "," << SPEED(spuX) << ","
<< COORD(move.y, spuY) << "," << SPEED(spuY) << ","
<< COORD(Settings->option | LEFT_HANDED ? -move.z : move.z, spuZ)
<< "," << SPEED(spuZ) << ",0,30";
return s;
}
/**
** Machining generation for user outputs
**/
static void
executeMachining(char type, int number)
{
char *cmd = Machining.GetCommand(type, number);
if (cmd == NULL)
{
extern long int lineno;
clog << "Warning at line " << lineno << ": Machining type "
<< type << number << " not defined." << endl;
}
else
{
cout << "@" << Settings->device << "i\n";
cout << cmd << "\n9\n";
cout << "@" << Settings->device << "s\n";
TotalLines += 3;
}
}
/**
** Fills arguments into structures
**/
static Node *fillArgs(Node *node)
{
while (node)
{
switch (node->type)
{
#define SINGLE(var, val) var = MovingType == ABSOLUTE ? val : var += val;
case
Node_X:
ToPos.x = MovingType == ABSOLUTE ? DVAL : Pos.x + DVAL;
break;
case
Node_Y:
ToPos.y = MovingType == ABSOLUTE ? DVAL : Pos.y + DVAL;
break;
case
Node_Z:
ToPos.z = MovingType == ABSOLUTE ? DVAL : Pos.z + DVAL;
break;
case
Node_I:
Center.x = MovingType == ABSOLUTE ? DVAL : Pos.x + DVAL;
CircleType = IJK;
break;
case
Node_J:
Center.y = MovingType == ABSOLUTE ? DVAL : Pos.y + DVAL;
CircleType = IJK;
break;
case
Node_K:
Center.z = MovingType == ABSOLUTE ? DVAL : Pos.z + DVAL;
CircleType = IJK;
break;
case
Node_R: /* radius MAHO ?*/
CircleArg.r = DVAL;
CircleType = RAB;
break;
case
Node_A: /* start angle ??? */
CircleArg.a = DVAL;
CircleType = RAB;
break;
case
Node_B: /* end angle ??? */
CircleArg.b = DVAL;
CircleType = RAB;
break;
case
Node_F:
Fedrate = DVAL;
break;
case
Node_MCODE:
executeMachining('M', DVAL);
break;
case
Node_S:
executeMachining('S', DVAL);
break;
case
Node_T:
executeMachining('T', DVAL);
break;
case
Node_EndCmd:
return node->next;
default:
extern long int lineno;
clog << "Warning at line "<< lineno
<< ": Unexpected argument type '"
<< node->type << "'" << endl;
break;
}
node = node->next;
}
return NULL;
}
/** Pause for the green button */
static void m00(Node *node)
{
cout << "@" << Settings->device << "i\n";
cout << "44\n9\n";
cout << "@" << Settings->device << "s\n";
fillArgs(node); // to ni najbolj prav
TotalLines += 3;
}
static void
executeMCODE(Node *node)
{
clog << "M" << node->arg.ival << endl;
switch (node->arg.ival)
{
case 00: m00(node->next); break;
case 30: /* do nothing */; break;
default:
extern long int lineno;
clog << "Warning at line " << lineno << ": M-CODE type M"
<< node->arg.ival << " not defined." << endl;
break;
}
}
/** working move **/
static void g01(Node *node)
{
while (node)
{
node = fillArgs(node);
if (!(Pos == ToPos)) // do not move if no change in position
{
cout << IMMEDIATE('m') << ToPos << '\n';
Pos = ToPos;
}
TotalLines++;
}
}
/** fast move **/
static void g00(Node *node)
{
double oldFeedrate = Fedrate;
Fedrate = 3000.0; // TODO
g01(node);
Fedrate = oldFeedrate;
}
/**
** Error factor for the circular interpolation
**/
static double errorFactor(float Rx, float Ry, double radius, double Xs, double Ys)
{
#if 0
// this is by the ISEL German book, but false
#define SUMME(xx) ( xx > 0 ? (xx)*((xx) + 1) : (xx)*((xx) - 1))
if (Direction == COUNTERCLOCKWISE)
return (Rx * Ry * radius + Rx * Ry * SUMME(radius - 1)
- Rx * SUMME(Xs + (Rx - Ry)/2.0)
+ Ry * SUMME(Ys + (Rx + Ry)/2.0))/2.0;
else
return(- Rx * Ry * radius
- Rx * Ry * SUMME(radius - 1)
- Rx * SUMME(Xs + (Rx + Ry)/2.0)
+ Ry * SUMME(Ys + (Ry - Rx)/2.0))/2.0;
#else
// by the English ISEL book
double error = Rx*Ry*(Xs*(Xs-Rx) + Ys*(Ys-Ry) - SQR(radius))/2;
if (Direction == CLOCKWISE)
return -error;
return error;
#endif
}
/**
** Calculates path absolute length
**/
static double winding(double angle)
{
double qlen = floor(angle/M_PI_2); // quadrant length
double sabs = fabs(sin(angle)); // sinus length
if (short(qlen) % 2) // invert even quadrants
qlen += 1 - sabs;
else
qlen += sabs;
return qlen;
}
/**
** Total number of steps for the circular interpolation
**/
static double circleSteps(double radius, double A, double B)
{
double wx, wy;
// clog << "Circle A:" << A << " B:" << B << endl;
if (Direction == CLOCKWISE)
{
if (ALMOSTZERO((A-B)*radius))
{
A += 2*M_PI;
extern long int lineno;
clog << "Warning: Ambigious circle " << A << " == " << B << " at line "
<< lineno << ". Doing full circle!" << endl;
}
if (A < B)
A += 2 * M_PI;
wy = winding(A) - winding(B);
wx = winding(A + M_PI_2) - winding(B + M_PI_2);
}
else
{
if (ALMOSTZERO((A-B)*radius))
{
B += 2*M_PI;
extern long int lineno;
clog << "Warning: Ambigious circle " << A << " == " << B << " at line "
<< lineno << ". Doing full circle!" << endl;
}
if (B < A)
B += 2 * M_PI;
wy = winding(B) - winding(A);
wx = winding(B + M_PI_2) - winding(A + M_PI_2);
}
return radius * (wx + wy);
}
/**
** Clockwise G02 and Counterclockwise G03 circular interpolation
** XY Plane only -> other planes TODO
**/
static void circle(Node *node)
{
double Xs, Ys; // relative coordinate system starting point
double radius; // circle radius;
double A, B; // Starting and ending angle;
node = fillArgs(node);
if (CircleType == RAB)
{
radius = CircleArg.r * Settings->spuX;
A = RAD(CircleArg.a);
B = RAD(CircleArg.b);
Xs = CircleArg.r * cos(A) * Settings->spuX;
Ys = CircleArg.r * sin(A) * Settings->spuY;
Pos.x = -CircleArg.r * cos(A) + CircleArg.r * cos(B);
Pos.y = -CircleArg.r * sin(A) + CircleArg.r * sin(B);
}
else // IJK
{
double Ex, Ey; // end point
Xs = (Pos.x - Center.x) * Settings->spuX; // Starting point
Ys = (Pos.y - Center.y) * Settings->spuY;
// Ex = ToPos.x * Settings->spuX ; // end point (X, Y)
// Ey = ToPos.y * Settings->spuY ;
radius = sqrt(SQR((Pos.x-Center.x)*Settings->spuX)
+ SQR((Pos.y-Center.y)*Settings->spuY));
if (ALMOSTZERO(radius))
{
extern long int lineno;
clog << "Warning: Circle at line " << lineno
<< " has too small radius. Ignoring!" << endl;
return;
}
if (!ALMOSTZERO(LENGTH(Pos.x-Center.x, Pos.y-Center.y)
- LENGTH(ToPos.x-Center.x, ToPos.y-Center.y)))
{
extern long int lineno;
clog << "Warning: Inconsistent circle at line "
<< lineno << ". Ignoring!" << endl;
return;
}
A = ANGLE(Pos.x - Center.x, Pos.y - Center.y);
B = ANGLE(ToPos.x - Center.x, ToPos.y - Center.y);
}
Xs = floor(Xs+0.5);
Ys = floor(Ys+0.5);
radius = floor(radius+0.5);
// we now have integer values
long Rx;
long Ry;
if (Direction == CLOCKWISE)
{
// clog << "Xs = " << Xs << " Ys = " << Ys << endl;
Rx = ALMOSTZERO(Ys) ? -SGN(Xs) : SGN(Ys);
Ry = ALMOSTZERO(Xs) ? -SGN(Ys) : -SGN(Xs);
// clog << "Rx = " << Rx << " Ry = " << Ry << endl;
}
else
{
Rx = ALMOSTZERO(Ys) ? -SGN(Xs) : -SGN(Ys);
Ry = ALMOSTZERO(Xs) ? -SGN(Ys) : SGN(Xs);
}
cout << IMMEDIATE('f') << Direction << '\n';
cout << IMMEDIATE('y')
<< long(circleSteps(radius, A, B)) << ","
<< SPEED(spuX) << ","
<< long(errorFactor(Rx, Ry, radius, Xs, Ys)) << ","
<< long(Xs) << "," << long(Ys) << ","
<< Rx << "," << Ry << '\n';
Pos = ToPos;
TotalLines += 2;
}
/** Set 3D interpolation ON **/
static void g20(Node *node)
{
cout << IMMEDIATE('z') << "1\n";
fillArgs(node);
TotalLines++;
}
/** Set Absolute Zero **/
static void g52(Node *node)
{
cout << IMMEDIATE('n') << Axes << '\n';
Pos.clear();
fillArgs(node);
TotalLines++;
}
/** Home motors **/
static void g74(Node *node)
{
cout << IMMEDIATE('r') << 7 << '\n'; // TODO which motors
fillArgs(node);
TotalLines++;
}
/** Set Absolute moving **/
static void g90(void)
{
MovingType = ABSOLUTE;
}
/** Set Relative moving **/
static void g91()
{
MovingType = RELATIVE;
}
/** Set Homing speed **/
static void g94(Node *node)
{
long hs = long(DVAL * Settings->spuX / 60.0);
cout << IMMEDIATE('d')
<< hs << "," << hs << "," << hs << '\n';
fillArgs(node);
TotalLines++;
}
/** This is Callback function called by yyparse() **/
void ExecCommandCb(Node *node)
{
// #define CHECKONLY
#ifndef CHECKONLY
// #define LISTNODES
#ifdef LISTNODES
while (node)
{
if (node->type >= 100)
cout << "Node :" << node->type << " Ival:" << node->arg.ival << endl;
else
cout << "Node :" << node->type << " Dval:" << node->arg.dval << endl;
node = node->next;
}
cout << "---------------" << endl;
#else
switch (node->type)
{
case Node_GCODE:
switch (node->arg.ival)
{
case 00: g00(node->next); break;
case 01: g01(node->next); break;
case 02:
Direction = CLOCKWISE;
circle(node->next);
break;
case 03:
Direction = COUNTERCLOCKWISE;
circle(node->next);
break;
case 20: g20(node->next); break;
case 52: g52(node->next); break;
case 74: g74(node->next); break;
case 90: g90(); break;
case 91: g91(); break;
case 94: g94(node->next); break;
default:
extern long int lineno;
clog << "Warning at line " << lineno
<< ": G-CODE type G" <<
node->arg.ival << " not yet implemented." << endl;
break;
}
break;
case Node_MCODE:
executeMCODE(node);
break;
}
#endif // LISTNODES
#endif // CHECKONLY
}
syntax highlighted by Code2HTML, v. 0.9.1