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