Helix Berechnung G-Code Interpreter

[Che Guevara]

Hi,

ich arbeite gerade an einem G-Code Interpreter auf Basis eines ATXMEGA32A4, programmiert wird in C (AVR Studio). Der µC soll nur Takt / Richtungssignale ausgeben und soll in Zukunft bis zu 6 Achsen "gleichzeitig" handeln können. Ich bin gerade dabei, die Helix-Berechnung zu implementieren. Hab mir dazu einige Gedanken gemacht (auch speziell zum Thema FeedRate) und möchte wissen, was ihr davon haltet bzw. obs funktionieren KANN.
Input sieht z.b. so aus:


G00 X0 Y0 Z0 //Gehe auf (0/0/0)
G02 X20 Y0 Z0 I10 J0 K0 F600 //Halbkreis von (0/0/0) auf (20/0/0) um (10/0/0) mit Feedrate = 600(mm/min)


Hier mal meine Routine:


void ProcessArc(PunkteVektor StartPunkt , PunkteVektor EndPunkt , ArcParams Vars , Ebene ArbeitsEbene , uint16_t Feedrate)
{
//
float StartAngle;
float EndAngle;
float Angle;
float AngleStep;

//Feedrate
float WegArc;
float FeedrateMMS;
float TimeArc;
float AngleVelocityArc;
float FeedrateTime;

//
uint8_t StepMade;
float TmpSingle;

//
uint16_t TimeHigh = 20;


//the following is only correct for XY-Arbeitsebene


//Calculate Radius for later use
if(Vars.R==0) {Vars.R = sqrt((Vars.I*Vars.I)+(Vars.J*Vars.J));}

//Calculate IJK for later use
if((Vars.I==0) && (Vars.J==0) && (Vars.K==0)) {Vars.I = Vars.J = (sqrt(2)/2)*Vars.R;}

//Calculate StartingAngle
StartAngle = atan2f(Vars.I,Vars.J) * 57.2957795; //evtl. needs some modification
EndAngle = atan2f(-Vars.I,-Vars.J) * 57.2957795;
Angle = StartAngle;

//choose which Axis has the smallest Step
OneStep.Smallest = OneStep.X;
if(OneStep.Smallest<OneStep.Y) {OneStep.Smallest = OneStep.Y;}

//Calculate AngleStep
AngleStep = acos(((Vars.R*Vars.R)+(Vars.R*Vars.R)-((0.707106*OneStep.Smallest)*(0.707106*OneStep.Sma llest)))/(2*Vars.R*Vars.R)); //0.707106 = (sqrt(2)/2)

//DEBUG
AngleStep = 0.05;

//gesamter Weg in Arbeitsebene (XY)
WegArc = 3.14159265359 * Vars.R * ((fabs(EndAngle)-fabs(StartAngle))/180); //evtl. ohne fabs()
FeedrateMMS = Feedrate / 60;
TimeArc = WegArc / FeedrateMMS;
AngleVelocityArc = (fabs(EndAngle)-fabs(StartAngle))/TimeArc; //evtl. ohne fabs()
FeedrateTime = (1/(AngleVelocityArc/AngleStep))*1000000;

//Arc Test Nr. 2
while(fabs(EndAngle-Angle)>=AngleStep)
{
StepMade = 0;

//deside in which direction to go
if(Vars.Dir==0) {Angle -= AngleStep;} //CW
else if(Vars.Dir==1) {Angle += AngleStep;} //CCW
Angle = fmod(Angle,360); //Angle %= 360;

//Calculate X & Y & Z Delta
Delta.X = cos((Angle*0.01745329))*Vars.R - Position.X;
Delta.Y = sin((Angle*0.01745329))*Vars.R - Position.Y;
Delta.Z = (((Angle-StartAngle)/(EndAngle-StartAngle))*(EndPunkt.Z-StartPunkt.Z))-Position.Z; //evtl. VZ beachten

//Generate Stepper-Signals
if(Delta.X>=OneStep.X)
{
Position.X += OneStep.X;
PORTD.OUTSET = 0x02;
PORTD.OUTSET = 0x01;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x01;

StepMade += 1;
}else if(Delta.X<=-OneStep.X)
{
Position.X -= OneStep.X;
PORTD.OUTCLR = 0x02;
PORTD.OUTSET = 0x01;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x01;

StepMade += 1;
}

if(Delta.Y>=OneStep.Y)
{
Position.Y += OneStep.Y;
PORTD.OUTSET = 0x08;
PORTD.OUTSET = 0x04;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x04;

StepMade += 1;
}else if(Delta.Y<=-OneStep.Y)
{
Position.Y -= OneStep.Y;
PORTD.OUTCLR = 0x08;
PORTD.OUTSET = 0x04;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x04;

StepMade += 1;
}

if(Delta.Z>=OneStep.Z)
{
Position.Z += OneStep.Z;
PORTD.OUTSET = 0x20;
PORTD.OUTSET = 0x10;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x10;

StepMade += 1;
}else if(Delta.Z<=-OneStep.Z)
{
Position.Z -= OneStep.Z;
PORTD.OUTCLR = 0x20;
PORTD.OUTSET = 0x10;
WaitUs(TimeHigh); //5
PORTD.OUTCLR = 0x10;

StepMade += 1;
}


//FeedRate
if(StepMade>=1)
{
TmpSingle = FeedrateTime - (StepMade*TimeHigh); //15 <==> High-Time Step-Signal
if(TmpSingle<0) {TmpSingle=0;}
WaitUs((uint16_t)TmpSingle);
}else
{
WaitUs((uint16_t)FeedrateTime);
}

}
}


Wäre nett, wenn jemand mal drüberschauen würde!
Vielen Dank & Gruß
Chris