THC Velocity lock and void lock

pin in  s32 num_readings = 100             "Number of readings for average volts (3-1000)"; #define BUFSIZE 1000 // maximum number of readings to average torch volts double buf[BUFSIZE]; int reading_count; float avgarcvolts(double tvolts, int iscutting, int buffersize) {     // Calculates the moving average of buffersize readings          static double *b = &buf[0], *p = &buf[0];        // pointers for beginning of buffer and current position     static double *e;   // pointer for end of buffer     static int   wascutting = 0;        // cutting state last time     static double sumvolts = 0.0;        // Sum of readings in buffer     static int numreadings = 0;            // number of readings in buffer     static int last_thc_active = 0;     // Save last state     e =  &buf[buffersize];              // Initialise end pointer     if(!iscutting && wascutting){         // Torch just turned off, so reset the variables         p = b;         sumvolts = 0.0;         numreadings = 0;         wascutting = iscutting;         //and save the state         return(0.0);     }     if(iscutting){                        // Arc_OK is on so lets start gathering data to average         *p++ = tvolts;                    // Save volts to the buffer and increment pointer         if(numreadings < buffersize)             numreadings++;         if(p > e)                        // if we've gone past the end of the buffer, wrap to the beginning             p = b;         sumvolts += tvolts;                // add the new reading         if(numreadings >= buffersize)             sumvolts -= *p;                // subtract the oldest reading (which is the one we are pointing at now)     }     wascutting = iscutting;             //and save the state     if(numreadings)                             return(sumvolts/(double)numreadings);  // return Average volts     else         return (0.0);                    // catch divide by zero errors }

     // Lets average the arc volts          avgvolts =    avgarcvolts(tvolts, (int)arcokin, (int)reading_count);         average_volts = avgvolts;           torch_volts = tvolts;       dvdt = (tvolts - avgvolts)/fperiod;       // Calculate change in volts for this servo cycle.       if(!thc_is_active)         void_hold = 0;             movingup   = (last_dvdt <= dvdt ? 1:0);   // set flag if volts are moving up       movingdown = (last_dvdt >  dvdt ? 1:0);   // set flag if volts are moving down       if(movingup && dvdt > dvdt_threshold  && !void_hold  && thc_is_active){           first_dvdt = dvdt;   // record the level the dvdt was at when activated           trigger_volts = torch_volts;  // and save the trigger volts                     void_hold = 1;         // hold the THC       }       else if(!gone_low && void_hold && (dvdt < (0.0 - dvdt_threshold)) && thc_is_active){           gone_low = 1;       }       else if(void_hold && gone_low && torch_volts <= trigger_volts && thc_is_active){ // holding and on our way up           void_hold = 0;         first_dvdt = 0.0;         trigger_volts = 0.0;         gone_low = 0;                    }       last_dvdt = dvdt;                // Save the last dvdt         

    typedef enum { WAIITNG, THC_ACTIVE, VOID_FOUND, VOID_LOW, VOID_END} State_V;     State_V vstate = WAITING;

    dvdt = (tvolts - avgvolts)/fperiod;       // Calculate change in volts for this servo cycle.        if(torch_on){         //movingup   = (last_dvdt <= dvdt ? 1:0);   // set flag if volts are moving up           switch(vstate){             case WAIITNG:                 if(thc_is_active){                     vstate = THC_ACTIVE;                 }                 else{                     break;                 }             case THC_ACTIVE:                 //if(movingup && dvdt > dvdt_threshold)                 if(dvdt > dvdt_threshold){                     first_dvdt = dvdt;                     trigger_volts = torch_volts;                               void_hold = 1;                                    vstate = VOID_FOUND;                 }                 break;             case VOID_FOUND:                 if(dvdt < (0.0 - dvdt_threshold)){                      vstate = VOID_LOW;                 }                 else{                     break;                 }             case VOID_LOW:                 if(torch_volts <= trigger_volts){                     elapsed_time += fperiod;                     if(elapsed_time >= final_delay){                         void_hold = 0;                         first_dvdt = 0.0;                         trigger_volts = 0.0;                         gone_low = 0;                         elapsed_time = 0.0;                         vstate = VOID_END;                     }                 }                 break;             case VOID_END:                 if(thc_is_active){                     vstate = THC_ACTIVE;                 }                 else{                     vstate = WAITING;                 }                 break;          }     }     else{         void_hold = 0;         first_dvdt = 0.0;         trigger_volts = 0.0;         gone_low = 0;         elapsed_time = 0.0;         vstate = VOID_END;     }     last_dvdt = dvdt;                // Save the last dvdt