Helix Berechnung G-Code Interpreter

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:

Code:

---

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:

Code:

---

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.Smallest)))/(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