Problema con cdi programable

Hola..necesito ayuda

Hace mucho que estoy buscando informacion y no encuentro la solucion...
El tema es simple...necesito que a partir de una señal de entrada, tenga un delay para una señal de salida..

Ej: si el sensor de entrada recoje un pulso, tengo que tener un pulso de salida (el cual dispararia un tiristor), pero las condiciones son las siguientes..

si Rpm<=500; el delay de salida tiene que ser de 2ms
si Rpm>=500 y <=100 el delay sera de 3ms

les dejo lo que tengo hasta el momento..

const int pinSensor = 0;
const int pinLed   = 13; 
int estadoSensor   = 0;
int estadoAnterior = 0; 
int estadoLed      = 0; 
int rpm; 
int LABEL = 1; 
const unsigned long sampleTime=1000; 
float v; 

void setup() 
{    
 Serial.begin(56000);
 pinMode(pinSensor, INPUT);  
 Serial.println("CLEARDATA");
Serial.println("LABEL,Tiempo, RPM,");
}
void loop()
{
 estadoSensor = digitalRead(pinSensor);
 if (estadoSensor != estadoAnterior)
 {
    rpm=getRPM();
    v = (rpm);
   
    Serial.print("DATA,TIME,");
    Serial.println(v*6.9); 
    Serial.print(",");
    
    
   
    Serial.println();    
       }
  
}


int getRPM()
{
 int kount=0;
 boolean kflag=LOW;
 unsigned long currentTime=0;
 unsigned long startTime=millis();
 
 while (currentTime<=sampleTime)
 {
   if (digitalRead(pinSensor)==HIGH)
   {  
     kflag=HIGH;
   }
   if (digitalRead(pinSensor)==LOW && kflag==HIGH)
   {
     kount++;
     kflag=LOW;
    
   }
   currentTime=millis()-startTime;
 }
 
 int kount2rpm = int(float(sampleTime)/999)*kount;
 return kount2rpm;
}

Lee las normas del foro y edita tu post usando etiquetas para código.

Espero que asi este bien...gracias
Podrias ayudarme con este problema?