1

#include <SimpleFOC.h>

#define LASERSENSOR_PIN 14

BLDCMotor motor1 = BLDCMotor(11, 5.6, 120); //5.6, 120
BLDCMotor motor2 = BLDCMotor(14, 32, 9); //32, 9

float filmTension = 1;          //film tension in (N)
float init_Spool1D = 80.52; //80.52 - DimaSpool / 64.75 - ReelSpool
float init_Spool2D = 30.2;  //30.8
float fScanSpeed = 2; // rad/sec

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

long frameCount = 1;
float scannerState = 0;
float lastState = -1;

float motor1Target; 
float motor2Target; 

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

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

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

float target1 = .5;
float target2 = 0.13;

volatile bool sensorStopState = false; //Interrupt
unsigned long sensorInterval = 250; //Interrupt

void setup() {
  Serial.begin(115200);  

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

  delay(1000);

  float m1InitTorque = tensionM1TargetSpoolD(filmTension/2);
  float m2InitTorque = tensionM2TargetSpoolD(filmTension/2);
  initialTension(m1InitTorque, m2InitTorque, 3000); //Init tension
}

void loop() {
  //ON STATE SWITCH
  if (scannerState != lastState && ledCamState == false) {
    //motorRewind(false);
    switch(int(scannerState)) {
      case 0: // PAUSE MODE
        Serial.println("PAUSE MODE");
        LED_Control(R_value, G_value, B_value);  
        motorsStandbyTorque();  
      break;

      case 1: // SCAN MODE
        Serial.println("SCAN MODE");
        //motorsStartMoveSimple();
        motorsStartMoveSpoolD();
      break;

      case 2: // LED OFF / PAUSE MODE
        Serial.println("SCAN STOP MODE");
        LED_Control(0, 0, 0);
        motorsStandbyTorque();
      break;

      case 3: //RADIUS RESET
        frameCount = 0;
        Serial.println("Framecount Reset / Please specify new m1D & m2D");
        
        requestRadi();
        resetSpoolD();
        scannerState = 0; //EXIT
      break;

      case 4: // REWIND
        Serial.println("REWIND");
        //motorsRewindSimple(true, 10);
      break;

      case 5: // WIND FORWARD
        //motorsRewindSimple(false, 10);
      break;

      case 6: //TEST MODE
        motor1Target = -tensionM1TargetSpoolD(filmTension/2);
        motor2.controller = MotionControlType::velocity; 
        motor2Target = .5;
      break;
    }

    lastState = scannerState;
  } 

  //STATE CALL
  switch(int(scannerState)) {
      case 0:
         
      break;
      
      case 1: // SCAN MODE
        sensorCall(); //OLD SENSOR CALL
        //if (sensorStopState) sensorCallInterrupt(); //Interrupt FLAG
        //sensorInterval = target1;
       
      
        //led_cam_call_BW(true);
      break;

      case 3: //SensorSetup
        sensorDebug();
      break;

      case 6: //TEST
        if(sensorStopState) {
          frameCount++;
          Serial.println("Perf CATCH! Frame: " + String(frameCount));
          sensorStopState = false;
        }
        //Serial.println(" Val: " + String(analogRead(LASERSENSOR_PIN)));
        //delay(500);
      break;
  }

  //if(frameCount == 50+1) scannerState = 0;

  //led_cam_call_Color();
  //led_cam_call_BWONLY();
  led_cam_call_BW(true);
  camTriggerOffCall();
  motorLoop();
  callCommand();
}

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

  if (ledCamState) {
    switch (ledCamMode) {
      case 0: //WAIT FOR FILM TO SETTLE / LED ON
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(R_value,G_value,B_value);

          if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime));
        break;
      //WAITING
      case 1: //CAM TRIGGER 1
        if (currentTime - ledCamLastTime >= filmSettleInterval) {
          ledCamLastTime = currentTime;
          ledCamMode++;

          Serial.println("Prepare to CamTrigger");
          camTriggerON();

          if(stateDebug) Serial.println("Frame Capture / CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE R
      case 2: //CAM FALLS TRIGGER
        if (currentTime - ledCamLastTime >= 100) {
            ledCamLastTime = currentTime;
            ledCamMode++;
            LED_Control(0,0,0);
            camTriggerON();

            if(stateDebug) Serial.println("FALLSFRAME 1 / CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE G(0)
      case 3: //CAM FALLS TRIGGER
        if (currentTime - ledCamLastTime >= 100) {
            ledCamLastTime = currentTime;
            ledCamMode++;

            camTriggerON();

            if(stateDebug) Serial.println("FALLSFRAME 2 / CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE B(0)
      case 4: //EXIT
        if (currentTime - ledCamLastTime >= 100) {
            ledCamLastTime = currentTime;
            ledCamMode = 0;
            ledCamState = false;
            
            frameCount++;
            if(stateDebug) {
              Serial.println("Frame: " + String(frameCount) + " / MOVE: " + String(ledCamLastTime));
              Serial.println("///////////////////////////////");
            }

            filmLenghtCall(); 
            motorsStartMoveSpoolD();
            torqueControlDebug();
          }
        break;
    }
  }
}

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

  if (ledCamState) {
    switch (ledCamMode) {
      case 0: //WAIT FOR FILM TO SETTLE / LED ON
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(R_value,G_value,B_value);

          Serial.println("///////////////////////////////");
          Serial.println("Frame: " + String(frameCount) + " / MOVE: " + String(ledCamLastTime));
          
          if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime));
        break;
      //WAITING
      case 1: //CAM TRIGGER 1
        if (currentTime - ledCamLastTime >= filmSettleInterval) {
          ledCamLastTime = currentTime;
          if(!hdr) ledCamMode = 4;
          else ledCamMode++;

          camTriggerON();

          if(stateDebug) Serial.println("Frame Capture / CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE 1
      case 2: //CAM TRIGGER 2
        if (currentTime - ledCamLastTime >= ledCamInterval/3) {
            ledCamLastTime = currentTime;
            ledCamMode++;

            camTriggerON();

            if(stateDebug) Serial.println("Frame UnderExposure / CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE 2 HDR
      case 3: //CAM TRIGGER 3
        if (currentTime - ledCamLastTime >= ledCamInterval/3) {
            ledCamLastTime = currentTime;
            ledCamMode++;

            camTriggerON();

            if(stateDebug) Serial.println("OverExposure/ CAM TRIGGER: " + String(ledCamLastTime));
          }
        break;
      //CAM TAKING PICTURE 3 HDR
      case 4: //EXIT
        if (currentTime - ledCamLastTime >= ledCamInterval/3) {
            ledCamLastTime = currentTime;
            ledCamMode = 0;
            ledCamState = false;
            
            frameCount++;
            if(stateDebug) {
              
            }

            filmLenghtCall(); 
            motorsStartMoveSpoolD();
            torqueControlDebug();
          }
        break;
    }
  }
}

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

  if (ledCamState) {
    switch (ledCamMode) {
      case 0: //WAIT FOR FILM TO SETTLE / LED ON
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(0, 0, 0);
          
          Serial.println("////////////////////////");
          Serial.println("Frame: " + String(frameCount));
          if(stateDebug) Serial.println("WAIT / LED ON: " + String(ledCamLastTime));
        break;

      case 1: //CAM TRIGGER R / LED R
        if (currentTime - ledCamLastTime >= filmSettleInterval) {
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(R_value, 0, 0);
          camTriggerON();

          if(stateDebug) Serial.println("TRIGGER R: " + String(ledCamLastTime));
          }
        break;
      //TAKING R CHANNEL
      case 2: //CAM TRIGGER G / LED G
        if (currentTime - ledCamLastTime >= ledCamInterval/3) {
          ledCamLastTime = currentTime;
          ledCamMode++;

          LED_Control(0, G_value, 0);
          camTriggerON();

          if(stateDebug) Serial.println("TRIGGER G: " + String(ledCamLastTime));
          }
        break;
      //TAKING G CHANNEL
      case 3: //CAM TRIGGER B / LED B
      if (currentTime - ledCamLastTime >= ledCamInterval/3) {
        ledCamLastTime = currentTime;
        ledCamMode++;

        LED_Control(0, 0, B_value);
        camTriggerON();

        if(stateDebug) Serial.println("TRIGGER B: " + String(ledCamLastTime));
        }
      break;
      //TAKING B CHANNEL  
      case 4: //RADI CALC / LED OFF / MOVE
        if (currentTime - ledCamLastTime >= ledCamInterval/3) {
          ledCamLastTime = currentTime;
          ledCamMode = 0; 
          ledCamState = false;
          
          LED_Control(0,0,0);

          frameCount++;

          //motorsStartMoveSimple();
          filmLenghtCall();
          motorsStartMoveSpoolD();
          torqueControlDebug();

          if(stateDebug) Serial.println("MOVE: " + String(ledCamLastTime));
          }
        break;
    }
  }
}

bool firstValueReceived = false;
void requestRadi() {
  Serial.println("Type TakeOUT Spool Rad: ");
  
  // Ждем и получаем первое значение
  while (!firstValueReceived) {
    if (Serial.available() > 0) {
      init_Spool1D = Serial.parseFloat();
      
      // Очищаем буфер
      while (Serial.available()) {
        Serial.read();
      }
      
      firstValueReceived = true;
      Serial.print("m1R: ");
      Serial.println(init_Spool1D, 2); // 6 знаков после запятой
      
      // Запрашиваем второе значение
      Serial.println("Type TakeIN Spool Rad: ");
    }
    delay(100);
  }
  
  // Ждем и получаем второе значение
  boolean secondValueReceived = false;
  while (!secondValueReceived) {
    if (Serial.available() > 0) {
      init_Spool2D = Serial.parseFloat();
      secondValueReceived = true;
      
      Serial.print("m2R: ");
      Serial.println(init_Spool2D, 2);
    }
    delay(100);
  }

  firstValueReceived = false;
}