BLDC

#define LASERSENSOR_PIN 14
#define CAMTRIGGER_PIN 21

float filmTorque = 1;
float motorDiff = 0.75; 
float filmScanSpeed = 1;

const int R_value = 100; //R_Color 220 Mono 100
const int G_value = 180; //G_Color 100 Mono 180 
const int B_value = 230; //B_Color 200 Mono 230

float motor1Target; 
float motor2Target; 

// Переменные для отладочного вывода
unsigned long lastDebugTime = 0;
const unsigned long debugInterval = 120;  // Интервал вывода отладочной информации (мс)

unsigned long stopLastTime = 0;
unsigned long moveInterval = 175;  // Интервал движения мотора перед сенсором (мс) 175
const unsigned long stopInterval = 1250;  // Интервал остановки мотора (мс)
bool move = false;

unsigned long sensorlastTime = 0;
unsigned long sensorInterval = 1;  //Задержка в срабатывания сенсора (мс)
int sensorMinVal = 250;

unsigned long ledCamLastTime = 0;
unsigned long ledCamInterval = stopInterval/5;  //Интервал переключения между R/G/B (мс)
int ledCamMode = 0; //Режим работы камеры и светодиодов
bool ledCamState = false; //Блок работы камеры (ON/OFF)

unsigned long camTriggerLastTime = 0;
unsigned long camTriggerInterval = 10;  //Задержка сигнала триггера (ON/OFF)
bool camTriggerState = false; //Режим сигнала триггера камеры (ON/OFF)

bool stateDebug = true;
float scannerState = 0;
float lastState = -1;

float target1 = 200;
float target2 = 175;
bool change = false;


long frameCount = 0;

void setup() {
  // monitoring port
  Serial.begin(115200);

  pinMode(CAMTRIGGER_PIN, OUTPUT);
  digitalWrite(CAMTRIGGER_PIN, LOW);

  initMotors();
  initLaser();
  initLED();

  delay(1000);

  initialTension(filmTorque,2000);
}

void loop() {
  if (scannerState != lastState) {
    //motorRewind(false);
    switch(int(scannerState)) {
      case 0:
        LED_Control(R_value, G_value, B_value);
        
        motor2ControlTorque(true);
        motorsFilmTorque();
        
        frameCount = 0;
      break;

      case 1:
        //motor2ControlTorque(true);
        motorsStartMove();
      break;

      case 2:
        LED_Control(0, 0, 0);
        motorsFilmTorque();
      break;

      case 3:
        //motorsFilmTorque();
        motor2ControlTorque(true);
    
        motor1Target = 0;
        motor2Target = 1.3;

        sensorDeltaZero();
      break;
      case 4:
        motorsRewind(true);
      break;

      case 5:
        motorsRewind(false);
      break;
    }

    lastState = scannerState;
  }

  if(scannerState == 3) {
    sensorDelta();
  }

  if(scannerState == 1) sensorCall();
  if(scannerState == 1) led_cam_call_BW();
  //if(scannerState == 1) led_cam_call_Color();

  if(scannerState == 6) motorsTest();
  if(frameCount == 1500) scannerState = 0;

  //sensorMinVal = target1;
  //moveInterval = target2;

  motorLoop();
  callCommand();
}

void sensorCall() {
  unsigned long currentTime = millis();

  //Проверяем перфорация с задержкой после старта
  if (currentTime - stopLastTime >= moveInterval && move) {

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

      //float sensor_value = readSensorKalman();
      float sensor_value = analogRead(LASERSENSOR_PIN);

      if (sensor_value < sensorMinVal) {
        
        motorsFilmTorqueMove();
        //motorsFilmTorqueAngle();

        move = false;
        ledCamState = true;

        if(stateDebug) {
          Serial.print("STOP: ");
          Serial.print(currentTime);
          Serial.print(" / Move Time: ");
          Serial.print(currentTime - stopLastTime);
          Serial.print(" / Sen Val: ");
          Serial.println(sensor_value,0);
        }
      }
    //}   
  }
  /*
  else {
    if (currentTime - sensorlastTime >= sensorInterval) {
      sensorlastTime = currentTime;
      updateKalman(); 
    }
  }
  */
}

void motorsStartMove() {
  move = true;
  stopLastTime = millis();

  motor2ControlTorque(false);
  motor2SetVL(4.5);

  motor1Target = -0.25;
  motor2Target = filmScanSpeed;
  //motor2Target = target1;
}

void motorsFilmTorqueAngle() {
  motor1Target = -filmTorque*motorDiff;
  motor2SetVL(6);
  motor2ControlAngle(true);
}

void motorsFilmTorque() {
  motor2ControlTorque(true);
  motor1Target = -filmTorque*motorDiff;
  motor2Target = filmTorque;
}
void motorsFilmTorqueMove() {
  motor1Target = -1.5;
  motor2Target = 0;
}

void motorsRewind(bool dir) {
  motor2ControlTorque(true);
  if(dir) {
    //motor1Target = -10;
    motor1Target = -6;
    motor2Target = 0;
  }
  else {
    motor1Target = 2;
    motor2Target = 6;
  }
}


void led_cam_call_BW() {
  unsigned long currentTime = millis();

  if (ledCamState) {
    switch (ledCamMode) {
      case 0:
        //if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(0,0,0);

          if(stateDebug) {Serial.print("WAIT / LED ON: "); Serial.println(ledCamLastTime);}
          //}
        break;

      case 1:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;

          ledCamMode++;

          LED_Control(R_value,0,0);

          //camTriggerState = true;
          //camTriggerLastTime = millis();
          cam_Trigger();

          if(stateDebug) {Serial.print("RED / CAM TRIGGER: "); Serial.println(ledCamLastTime);}
          }
        break;

      case 2:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
            ledCamLastTime = currentTime;

            ledCamMode++;

            LED_Control(0,G_value,0);

            cam_Trigger();

            if(stateDebug) {Serial.print("GREEN / CAM TRIGGER: "); Serial.println(ledCamLastTime);}
          }
        break;

      case 3:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
            ledCamLastTime = currentTime;

            ledCamMode++;

            LED_Control(0,0,B_value);

            cam_Trigger();

            if(stateDebug) {Serial.print("BLUE / CAM TRIGGER: "); Serial.println(ledCamLastTime);}
          }
        break;

      case 4:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
            ledCamLastTime = currentTime;

            ledCamMode = 0;
            ledCamState = false;

            LED_Control(0,0,0);

            if(stateDebug) {Serial.print("MOVE: "); Serial.println(ledCamLastTime);}

            motorsStartMove();
            frameCount++;
          }
        break;
    }
  }
}
void led_cam_call_Color() {
  unsigned long currentTime = millis();

  if (ledCamState) {
    switch (ledCamMode) {
      case 0:
        //if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;

          LED_Control(R_value, G_value, B_value);
          
          ledCamMode++;

          if(stateDebug) {Serial.print("WAIT / LED ON: "); Serial.println(ledCamLastTime);}
         // }
        break;

      case 1:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;

          ledCamMode++;
          cam_Trigger();

          if(stateDebug) {Serial.print("TRIGGER "); Serial.println(ledCamLastTime);}
          }
        break;
      
      case 2:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;

          ledCamMode++;

          if(stateDebug) {Serial.print("CASE 2: "); Serial.println(ledCamLastTime);}
          }
        break;
      
      case 3:
        if (currentTime - ledCamLastTime >= ledCamInterval) {
          ledCamLastTime = currentTime;

          ledCamMode = 0; 
          ledCamState = false;
          
          //LED_Control(0,0,0);

          if(stateDebug) {Serial.print("MOVE: "); Serial.println(ledCamLastTime);}

          motorsStartMove();
          frameCount++;
          }
        break;
    }
  }
}

//Триггерим камеру через delay 
void cam_Trigger() {
  digitalWrite(CAMTRIGGER_PIN, HIGH);
  delay(camTriggerInterval);
  digitalWrite(CAMTRIGGER_PIN, LOW);
}
//Выключаем ПИН сигнала камеры если прошло время camTriggerInterval
void cam_Trigger_Call() {
  unsigned long currentTime = millis();

  if (currentTime - camTriggerLastTime >= camTriggerInterval && camTriggerState) {
    camTriggerLastTime = currentTime;

    digitalWrite(CAMTRIGGER_PIN, LOW);
    camTriggerState = false;

    Serial.print("Cam Signal OFF: ");
    Serial.println(currentTime);
  }
}