Laser

//LASER SENSOR AVG
#include <SimpleKalmanFilter.h>

SimpleKalmanFilter SKF(2, 2, 0.1);

const int sensorPin = 14;  
uint32_t frame = 0;
//int sensorMinVal = 200; //Срабатываение сенсора на перфорацию

float sensorMaxValue = 0;
float sensorMinValue = 4095;

unsigned long sensorTime = 0; 
unsigned long sensorLastTime = 0; 
unsigned long sensorStopInterval = 100;  // Интервал остановки работы снесора после начала движения (мс)

void IRAM_ATTR isr() {
    sensorTime = millis();
    sensorStopInterval = target1;
    
    if(sensorTime - ledCamLastTime >= sensorStopInterval && move == 1)
    {
      frame++;
      Serial.print("Perf Catch: ");
      Serial.println(frame);

      sensorLastTime = sensorTime;
      ledCamLastTime = sensorTime;

      motor2ControlTorque(true);
      motor2Target = -motor1Target*target2;
      
      move = false;
      ledCamState = true;

      Serial.print("STOP: ");
      Serial.println(sensorTime);
    }
}

void initLaser() {
  pinMode(sensorPin, INPUT);
  //attachInterrupt(sensorPin, isr, HIGH);
}

void sensorDeltaZero() {
  sensorMaxValue = 0;
  sensorMinValue = 4095; 
}

void updateKalman() {
  //SKF.updateEstimate(analogRead(LASERSENSOR_PIN));
  
  float sensor_value_ish = analogRead(LASERSENSOR_PIN);
  float sensor_value = SKF.updateEstimate(sensor_value_ish);

  Serial.print(sensor_value_ish);
  Serial.print("\t");
  Serial.print(sensor_value);
  Serial.println("\t");
}

float readSensorKalman() {
  return SKF.updateEstimate(analogRead(LASERSENSOR_PIN));
}

void sensorDelta() {
  unsigned long currentSensorTime = millis();
      
  if (currentSensorTime - sensorlastTime >= sensorInterval) {
    sensorlastTime = currentSensorTime; 

    float sensor_value_ish = analogRead(LASERSENSOR_PIN);
    float sensor_value = SKF.updateEstimate(sensor_value_ish);

    if (sensor_value > sensorMaxValue) sensorMaxValue = sensor_value;
    if (sensor_value < sensorMinValue) sensorMinValue = sensor_value;
    float del = sensorMaxValue - sensorMinValue;

    if (currentSensorTime - lastDebugTime >= debugInterval) {
      lastDebugTime = currentSensorTime;
      
      /*
      Serial.print(sensor_value_ish,0);
      Serial.print(" / ");
      Serial.print(sensor_value,0);
      Serial.print("/ Max: ");
      Serial.print(sensorMaxValue,0);
      Serial.print("/ Min>: ");
      Serial.print(sensorMinValue,0);
      Serial.print("/ Del: ");
      Serial.println(del,1);
      */
    }
    
    
    Serial.print(sensor_value_ish,0);
    Serial.print("\t");
    Serial.print(sensor_value,0);
    Serial.println("\t");
    
  }
}