//
// Tyrone Crawler & Engine Start Code
//
// This sketch contains a combination of the crawler and engine start code in a single sketch
//

// Notes from crawler code ...
//
// Some of this code needs to be eliminated or integrated into the other codes where the chute comes into play.
// ENB,IN3 and IN4 are not gonna be used. I installed another BTS7960 to carry the load for the chute actuator.
//
// Chute with directional memory: max 3.5s CW or CCW from center, total 7s range
// Actuator lift (CH7 only)
// Big Ben Crawler + Blower integration with persistent lift in reverse and persistent lower in forward
//

// Defines to control serial debug output statements (0 = disable, 1 = enable)
#define DEBUG_PRINT_FLYSKY_INPUTS                 1
#define DEBUG_PRINT_ENGINE_STATE_INFO             0
#define DEBUG_PRINT_CLUTCH_STATE_INFO             0
#define DEBUG_PRINT_CRAWLER_MOVEMENT_INFO         0
#define DEBUG_PRINT_BLOWER_UP_DOWN_INFO           0
#define DEBUG_PRINT_CHUTE_ROTATION_INFO           1
#define DEBUG_PRINT_CHUTE_DEFLECTOR_INFO          1

// Define to activate Wokwi.com simulation code (0 = disable, 1 = enable)
// See https://wokwi.com/projects/433580030820482049
#define WOKWI_SIMULATION                          0

// Include needed libraries
#include <SoftwareSerial.h>
#include "SabertoothSimplified.h"

// Sabertooth S1 Motor Driver
#define S1_PIN 5                       // Sabertooth S1

// Flysky FS-i6X 10 Channel RC Transmitter and FS-iA6B Receiver
#define INPUT_CH1_PIN 2                // Flysky Right Gimbal (forward-reverse & left-right control)
#define INPUT_CH2_PIN 3                // Flysky Right Gimbal (forward-reverse & left-right control)
// CH3 is connected directly to engine throttle (Flysky Left Gimbal)
// CH4 is connected directly to engine choke (Flysky Left Gimbal)
#define INPUT_CH5_PIN 35               // Flysky Switch 5 (Engine start/stop)
#define INPUT_CH6_PIN 36               // Flysky Switch 6 (Clutch control)
#define INPUT_CH7_PIN A6               // Flysky Var7 knob (Chute deflection angle control)
#define INPUT_CH8_PIN A7               // Flysky Var8 knob (Chute rotation control)

#define FLYSKY_TIMEOUT_VALUE 35000     // Timeout is in microseconds

// 100 amp alternator tells the Arduino engine is running and kicks the starter off.
#define CURRENT_SENSOR_PIN A5   // ACS758 current sensor
#define STARTER_RELAY_PIN 22    // Starter relay pin
#define KILL_RELAY_PIN 23       // Kill relay pin
#define CLUTCH_RELAY_PIN 24     // Clutch relay pin

// BTS7960 Blower Actuator (Raises and lowers the blower assembly)
#define BLOWER_RPWM 6
#define BLOWER_LPWM 9
#define BLOWER_REN 11
#define BLOWER_LEN 12

// BTS7960 Chute Rotation Motor (Rotates blower chute left and right)
#define CHUTE_RPWM 30
#define CHUTE_LPWM 31
#define CHUTE_REN 26
#define CHUTE_LEN 27

// Chute Rotation Motor RS/LS current sense
#define CHUTE_RS_PIN A2   // Right Sense
#define CHUTE_LS_PIN A3   // Left Sense

// BTS7960 Chute Deflector Actuator (Adjusts chute deflector angle)
#define DEFLECTOR_RPWM 44
#define DEFLECTOR_LPWM 45
#define DEFLECTOR_REN 28
#define DEFLECTOR_LEN 29

// Create the Sabertooth object ST
SoftwareSerial SWSerial(NOT_A_PIN, S1_PIN);
SabertoothSimplified ST(SWSerial);

//
// Global variable declarations and initialization
//

// Engine variables
bool starterActive = false;
bool killActive = false;
bool engineRunning = false;
bool engineWasRunning = false;

unsigned long startTime = 0;
unsigned long killTime = 0;

bool lastEngineRunStopSwitch = false;

// Chute variables

int initialCh8 = 1500;
int chutePosition = 0;
unsigned long chuteLastUpdate = 0;
unsigned long lastCh8Pulse = 0;
bool knobMoved = false;
bool chutePositionRiggedStarted = false;
bool chutePositionRigged = false;
bool blowerLiftMode = false;
bool blowerLowerMode = false;

//
// Setup function
//

void setup() {
  Serial.begin(9600);
  SWSerial.begin(9600);

  // Engine pin assignment & setup
  pinMode(STARTER_RELAY_PIN, OUTPUT);
  pinMode(KILL_RELAY_PIN, OUTPUT);
  pinMode(CLUTCH_RELAY_PIN, OUTPUT);

  digitalWrite(STARTER_RELAY_PIN, LOW);
  digitalWrite(KILL_RELAY_PIN, LOW);
  digitalWrite(CLUTCH_RELAY_PIN, LOW);  // Clutch disengaged

  // FS-iA6B Receiver pin assignments
  pinMode(INPUT_CH1_PIN, INPUT);
  pinMode(INPUT_CH2_PIN, INPUT);
  pinMode(INPUT_CH5_PIN, INPUT);
  pinMode(INPUT_CH6_PIN, INPUT);
  pinMode(INPUT_CH7_PIN, INPUT);
  pinMode(INPUT_CH8_PIN, INPUT);

  // BTS7960 blower actuator pin assignment
  pinMode(BLOWER_RPWM, OUTPUT);
  pinMode(BLOWER_LPWM, OUTPUT);
  pinMode(BLOWER_REN, OUTPUT);
  pinMode(BLOWER_LEN, OUTPUT);

  // Enable the blower actuator on powerup
  digitalWrite(BLOWER_REN, HIGH);
  digitalWrite(BLOWER_LEN, HIGH);

  // BTS7960 chute rotation motor pin assignment
  pinMode(CHUTE_RPWM, OUTPUT);
  pinMode(CHUTE_LPWM, OUTPUT);
  pinMode(CHUTE_REN, OUTPUT);
  pinMode(CHUTE_LEN, OUTPUT);

  // Enable the chute rotation motor on powerup
  digitalWrite(CHUTE_REN, HIGH);
  digitalWrite(CHUTE_LEN, HIGH);

  // Chute rotation current sensor pin assignment
  pinMode(CHUTE_RS_PIN, INPUT);
  pinMode(CHUTE_LS_PIN, INPUT);

  // BTS7960 chute deflection actuator
  pinMode(DEFLECTOR_RPWM, OUTPUT);
  pinMode(DEFLECTOR_LPWM, OUTPUT);
  pinMode(DEFLECTOR_REN, OUTPUT);
  pinMode(DEFLECTOR_LEN, OUTPUT);

  digitalWrite(DEFLECTOR_REN, HIGH);
  digitalWrite(DEFLECTOR_LEN, HIGH);

  // Initialize various "last" chute rotation values
  delay(1000);
  chuteLastUpdate = millis();
  lastCh8Pulse = chuteLastUpdate;
  initialCh8 = pulseIn(INPUT_CH8_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  initialCh8 = constrain(initialCh8, 1000, 2000);
  Serial.print("initialCh8="); Serial.println(initialCh8);

  Serial.println("Setup complete - System Ready.");
}


//
// Main Loop function
//

void loop() {

  // Read inputs from Flysky receiver
  long ch1RawValue = pulseIn(INPUT_CH1_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch2RawValue = pulseIn(INPUT_CH2_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch5Value = pulseIn(INPUT_CH5_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch6Value = pulseIn(INPUT_CH6_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch7RawValue = pulseIn(INPUT_CH7_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);
  long ch8Value = pulseIn(INPUT_CH8_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);

  // Wokwi simulated Flysky receiver channel inputs
  #if WOKWI_SIMULATION
    int inputA14 = analogRead(A14);
    ch1RawValue = map(inputA14, 0, 1023, 900, 2100); // This range allows for out of range testing
    int inputA15 = analogRead(A15);
    ch2RawValue = map(inputA15,0 , 1023, 900, 2100); // This range allows for out of range testing
  #endif

  // Adjust Ch1 and Ch2 to a failsafe value of 1500 if raw value is outside expected range.
  // This is done because a value of 1500 for both results in a "stop" movement for the clawler.
  // While the raw value should be in the range of 1000 to 2000, real-world observation found
  // that values are sometime just outside those limits.  So extend the lower and upper limits
  // by 50 when checking for out of range inputs.
  long ch1Value = setToDefaultIfOutOfRange(ch1RawValue, 950, 2050, 1500);
  long ch2Value = setToDefaultIfOutOfRange(ch2RawValue, 950, 2050, 1500);

  // Adjust Ch7 to a failsafe value of 1500 if raw value is outside expected range.
  // This is done because a value of 1500 stops the movement of chute deflector actuator.
  long ch7Value = setToDefaultIfOutOfRange(ch7RawValue, 950, 2050, 1500);


	// ***********************************************************************
	// Consider adding code to condition Ch8 to insure full rotation range.
  // *********************************************************************** 

  // Constrain the value of all channel inputs to 1000 to 2000.  If the transmitter or receiver
  // are off, a value of 0 is see by the Arduino with the pulseIn function. This action will change
  // a 0 or any out of range value to 1000, which is a "safe" value for Ch5, Ch6, Ch7 and Ch8.
  ch1Value = constrain(ch1Value, 1000, 2000);
  ch2Value = constrain(ch2Value, 1000, 2000);
  ch5Value = constrain(ch5Value, 1000, 2000);
  ch6Value = constrain(ch6Value, 1000, 2000);
  ch7Value = constrain(ch7Value, 1000, 2000);
  ch8Value = constrain(ch8Value, 1000, 2000);

  // Debug code to print Flysky channel inputs on serial monitor
  #if DEBUG_PRINT_FLYSKY_INPUTS
    Serial.print("Flysky Receiver:  Ch1="); Serial.print(ch1Value);
    Serial.print(" | Ch1Raw="); Serial.print(ch1RawValue);
    Serial.print(" | Ch2="); Serial.print(ch2Value);
    Serial.print(" | Ch2Raw="); Serial.print(ch2RawValue);
    Serial.print(" | Ch5="); Serial.print(ch5Value);
    Serial.print(" | Ch6="); Serial.print(ch6Value);
    Serial.print(" | Ch7="); Serial.print(ch7Value);
    Serial.print(" | Ch7Raw="); Serial.print(ch7RawValue);
    Serial.print(" | Ch8="); Serial.print(ch8Value);
    Serial.println();
  #endif

  // Crawler chute rotation - do this as soon as possible after reading RC channel inputs
  controlChuteRotation(ch8Value);

  // Crawler movement control
  ////controlCrawlerMovement(ch1Value, ch2Value);

  // Crawler chute deflection angle control
  // controlChuteDeflection(ch7Value);

  // Crawler blower assembly lifting/lowering
  ////controlBlowerLiftingAndLowering(ch1Value, ch2Value);
    
  // Engine and clutch control
  bool engineRunStopSwitch = (ch5Value > 1700);       // Channel 5 is the engine run/stop switch
  bool engageClutchSwitch = (ch6Value > 1700);        // Channel 6 is the clutch engage switch
  ////controlEngine(engineRunStopSwitch);
  ////controlClutch(engageClutchSwitch);

  // Various sketches used different delays.  The engine code had delay(100) and crawler movement code 
  // was delay(10).  For intial testing, we are going with a short delay(5).
  delay(5);
}

//
// Support functions
//

long setToDefaultIfOutOfRange(long val, long minVal, long maxVal, long defaultVal) {
  return (val < minVal || val > maxVal) ? defaultVal : val;
}


//
// Control functions
//

void controlEngine(bool engineRunStopSwitch) {

  // The engineRunStopSwitch is true when this switch is in the on position.  When the engine is not
  // running, moving this switch to the "on" position to engage the starter to crank the engine to start
  // it.  After the engine is running, the operator will move this switch back to the "off" position.
  // When the engine is running, moving this switch to the "on" position will stop the engine.  After
  // the engine has stopped, the operator will move this switch back to the "off" position.

  #define ENGINE_START_DURATION 3000         // 3 seconds
  #define ENGINE_KILL_DURATION 2000          // 2 seconds
  #define ENGINE_RUNNING_THRESHOLD 3.0       // 3 Amps - adjust if needed

  // === Read current sensor to determine is engine is running ===
  int rawValue = analogRead(CURRENT_SENSOR_PIN);
  float voltage = (rawValue / 1023.0) * 5.0;
  float currentAmps = (voltage - 2.5) / 0.066;
  if (abs(currentAmps) < 1.0) currentAmps = 0.0;

  engineRunning = currentAmps >= ENGINE_RUNNING_THRESHOLD;
  if (engineRunning) engineWasRunning = true;

  // === Engine START logic ===
  if (engineRunStopSwitch && !lastEngineRunStopSwitch && !engineWasRunning) {
    digitalWrite(STARTER_RELAY_PIN, HIGH);
    startTime = millis();
    starterActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Cranking engine...");
    #endif
  }

  if (starterActive && (millis() - startTime >= ENGINE_START_DURATION || engineRunning)) {
    digitalWrite(STARTER_RELAY_PIN, LOW);
    starterActive = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Starter OFF");
    #endif
  }

  // === Engine STOP logic ===
  if (!engineRunStopSwitch && lastEngineRunStopSwitch && engineWasRunning) {
    digitalWrite(KILL_RELAY_PIN, HIGH);
    killTime = millis();
    killActive = true;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Killing engine...");
    #endif
  }

  if (killActive && millis() - killTime >= ENGINE_KILL_DURATION) {
    digitalWrite(KILL_RELAY_PIN, LOW);
    killActive = false;
    engineWasRunning = false;
    #if DEBUG_PRINT_ENGINE_STATE_INFO
      Serial.println("Kill relay OFF - engine reset.");
    #endif
  }

  #if DEBUG_PRINT_ENGINE_STATE_INFO
    Serial.print("Switch 5 (Start): "); Serial.print(engineRunStopSwitch ? "ON" : "OFF");
    Serial.print(" | Amps: "); Serial.print(currentAmps, 2);
    Serial.print(" | Engine: "); Serial.println(engineWasRunning ? "ON" : "OFF");
  #endif

  lastEngineRunStopSwitch = engineRunStopSwitch;
}


void controlClutch(bool engageClutch) {

  // === Clutch control ===
  digitalWrite(CLUTCH_RELAY_PIN, engageClutch ? HIGH : LOW);
  #if DEBUG_PRINT_CLUTCH_STATE_INFO
    Serial.print("Clutch: ");
    Serial.println(engageClutch ? "ENGAGED" : "DISENGAGED");
  #endif
}


void controlChuteDeflection(long ch7) {

  #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
    Serial.print("Knob 7: "); Serial.print(ch7);
  #endif

  if (ch7 > 1600) {
    #if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | Raising Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 0);
    analogWrite(DEFLECTOR_LPWM, 255);
  } else if (ch7 < 1400) {
  	#if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | Lower Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 255);
    analogWrite(DEFLECTOR_LPWM, 0);
  } else {
  	#if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
      Serial.print(" | No Change In Deflector Angle");
    #endif
    analogWrite(DEFLECTOR_RPWM, 0);
    analogWrite(DEFLECTOR_LPWM, 0);
  }

 	#if DEBUG_PRINT_CHUTE_DEFLECTOR_INFO
 	 Serial.println("");
  #endif
}


void controlChuteRotation(long ch8) {

  #define CHUTE_MAX_DURATION 3600       // 3500 is 3.5 seconds
  #define CHUTE_MIN_DURATION -3600      // -3500 is 3.5 seconds
  
  #define CH8_MAX_MAPPING_DURATION  (CHUTE_MAX_DURATION - 100)
  #define CH8_MIN_MAPPING_DURATION  (CHUTE_MIN_DURATION + 100)
    
  #define POSITION_STOP_THRESHOLD 250   // Adjust between 200 and 500
  #define CURRENT_THRESHOLD 700         // Adjust if needed (600 - 800)

  #define PRE_AUTORIG_DURATION 4000     // 4000 is 4.0seconds
  #define AUTORIG_DURATION -4000        // -4000 is 4.0 seconds

  int desiredPosition = 0;

  // Calculate time since last call to this function
  unsigned long now = millis();
  float delta = now - chuteLastUpdate;
  chuteLastUpdate = now;
  if (delta > 1000) delta = 1000;

  // Read current sense
  int rsValue = analogRead(CHUTE_RS_PIN);
  int lsValue = analogRead(CHUTE_LS_PIN);

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print("Knob 8: "); Serial.print(ch8);
    Serial.print(" | Position: "); Serial.print(chutePosition);
    Serial.print(" | Delta: "); Serial.print(delta);
    Serial.print(" | RS: "); Serial.print(rsValue);
    Serial.print(" | LS: "); Serial.print(lsValue);
  #endif

  // After power-on, wait for the knob to move a bit before
  // actually using it's value to change the chute rotation
  if (!knobMoved && abs(ch8 - initialCh8) > 100) {
    knobMoved = true;
  }

  // After first movement of knob, start autorig by going all the left until left stop
  // is hit otherwise use position specified by knob
  if (knobMoved & !chutePositionRigged) {
    if(!chutePositionRiggedStarted) {
      chutePositionRiggedStarted = true;
      chutePosition = PRE_AUTORIG_DURATION;
    }
    desiredPosition = AUTORIG_DURATION;
  } else {
    desiredPosition = map(ch8, 1000, 2000, CH8_MIN_MAPPING_DURATION, CH8_MAX_MAPPING_DURATION);
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print(" | Desired Pos: "); Serial.print(desiredPosition);
  #endif

  // If knob has been moved, start chute rotation to desired position
  if (knobMoved) {
    // Stop if within position stop window
    if (abs(desiredPosition - chutePosition) < POSITION_STOP_THRESHOLD) {
      analogWrite(CHUTE_RPWM, 0);
      analogWrite(CHUTE_LPWM, 0);
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Stopped - At desired postion");
      #endif
    }
    // Clockwise (Right)
    else if ((desiredPosition > chutePosition) && (chutePosition <= CHUTE_MAX_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Rotating Clockwise");
      #endif
      if (lsValue < CURRENT_THRESHOLD) {
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 255);
        chutePosition += delta;
        if (chutePosition > CHUTE_MAX_DURATION) chutePosition = CHUTE_MAX_DURATION;
      } else {
        #if DEBUG_PRINT_CHUTE_ROTATION_INFO
          Serial.print(" - STALL");
        #endif
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 0);
      }
    }
    // Counter-clockwise (Left)
    else if ((desiredPosition < chutePosition) && (chutePosition >= CHUTE_MIN_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Rotating Counter-clockwise");
      #endif
      if (rsValue < CURRENT_THRESHOLD) {
        analogWrite(CHUTE_RPWM, 255);
        analogWrite(CHUTE_LPWM, 0);
        chutePosition -= delta;
        if (chutePosition < -CHUTE_MAX_DURATION) chutePosition = CHUTE_MIN_DURATION;
      } else {
        #if DEBUG_PRINT_CHUTE_ROTATION_INFO
          Serial.print(" - STALL");
        #endif
        chutePositionRigged = true;
        chutePosition = CHUTE_MIN_DURATION;
        analogWrite(CHUTE_RPWM, 0);
        analogWrite(CHUTE_LPWM, 0);
      }
    }
    // Stop
    else {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO
        Serial.print(" | Stopped - we should never get here!!!");
      #endif
      analogWrite(CHUTE_RPWM, 0);
      analogWrite(CHUTE_LPWM, 0);
    }
  } else {
    #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print(" | Stopped - Knob has not yet moved.");
      #endif
    analogWrite(CHUTE_RPWM, 0);
    analogWrite(CHUTE_LPWM, 0);
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.println("");
  #endif
}


void controlBlowerLiftingAndLowering(long ch1, long ch2) {
  static bool wasReverse = false;
  static bool wasForward = false;

  if (ch1 < 1460 && ch2 < 1460) {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 255);
    wasReverse = true;
    wasForward = false;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lifting Blower (triggered by reverse)");
    #endif
  } else if (wasReverse) {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 255);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lifting Blower (persisting reverse)");
    #endif
    if (ch1 > 1530 && ch2 > 1530) {
      wasReverse = false;
    }
  } else if (ch1 > 1530 && ch2 > 1530) {
    analogWrite(BLOWER_RPWM, 255);
    analogWrite(BLOWER_LPWM, 0);
    wasForward = true;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (triggered by forward)");
    #endif
  } else if (wasForward) {
    analogWrite(BLOWER_RPWM, 255);
    analogWrite(BLOWER_LPWM, 0);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (persisting forward)");
    #endif
    if (ch1 < 1460 && ch2 < 1460) {
      wasForward = false;
    }
  } else {
    analogWrite(BLOWER_RPWM, 0);
    analogWrite(BLOWER_LPWM, 0);
  }
}


void controlCrawlerMovement(long ch1, long ch2) {
  // controlCrawler has been updated to turn the correct direction
  // instead of the opposite direction as it originally did

  if (ch1 > 1530 && ch2 > 1530) {
    // Move straight forward
    int speedRight = map(ch1, 1530, 2000, 0, 127);
    int speedLeft = map(ch2, 1530, 2000, 0, 127);
    ST.motor(1, min(speedRight, speedLeft));
    ST.motor(2, min(speedRight, speedLeft));
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.print("Crawler Movement: Move straight forward  | Speed="); Serial.println(min(speedRight, speedLeft));
    #endif
  } else if (ch1 > 1530 && ch2 < 1460) {
    // Turn right
    // int speedRight = map(ch1, 1530, 2000, 0, 127);  -- Turns left
    // int speedLeft = map(ch2, 1000, 1460, -127, 0);  -- Turns left
    int speedRight = map(ch1, 1530, 2000, 0, -127);
    int speedLeft = map(ch2, 1000, 1460, 127, 0);
    ST.motor(1, speedRight);
    ST.motor(2, speedLeft);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.print("Crawler Movement: Turning right  | Speed: Left="); Serial.print(speedLeft); Serial.print(" Right=");Serial.println(speedRight);
    #endif
  } else if (ch1 < 1460 && ch2 > 1530) {
    // Turn left
    // int speedRight = map(ch1, 1000, 1460, -127, 0); -- Turns right
    // int speedLeft = map(ch2, 1530, 2000, 0, 127);   -- Turns right
    int speedRight = map(ch1, 1000, 1460, 127, 0);
    int speedLeft = map(ch2, 1530, 2000, 0, -127);
    ST.motor(1, speedRight);
    ST.motor(2, speedLeft);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.print("Crawler Movement: Turning left  | Speed: Left="); Serial.print(speedLeft); Serial.print(" Right=");Serial.println(speedRight);
    #endif
  } else if (ch1 < 1460 && ch2 < 1460) {
    // Move straight back
    int speedRight = map(ch1, 1000, 1460, -127, 0);
    int speedLeft = map(ch2, 1000, 1460, -127, 0);
    ST.motor(1, max(speedRight, speedLeft));
    ST.motor(2, max(speedRight, speedLeft));
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.print("Crawler Movement: Move straight backward  | Speed="); Serial.println(max(speedRight, speedLeft));
    #endif
  } else {
    // Stop
    ST.motor(1, 0);
    ST.motor(2, 0);
    #if DEBUG_PRINT_CRAWLER_MOVEMENT_INFO
      Serial.println("Crawler Movement: Stop  |  Speed=0");
    #endif
  }
}