//
// 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             1
#define DEBUG_PRINT_CLUTCH_STATE_INFO             1
#define DEBUG_PRINT_CRAWLER_MOVEMENT_INFO         1
#define DEBUG_PRINT_BLOWER_UP_DOWN_INFO           1
#define DEBUG_PRINT_CHUTE_ROTATION_INFO           1

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

// 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

// L298N pins
#define ENA 13
#define IN1 A0
#define IN2 A1
//#define ENB 7
//#define IN3 A2
//#define IN4 A3

// BTS7960 blower actuator
#define R_PWM 6
#define L_PWM 9
#define R_EN 11
#define L_EN 12

// BTS7960 Chute Motor
const int chuteRPWM = 30;
const int chuteLPWM = 31;
const int chuteREN = 26;
const int chuteLEN = 27;

// RS/LS current sense
const int rsPin = A2; // Right Sense
const int lsPin = A3; // Left Sense


// 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
#define CHUTE_MAX_DURATION 3500 // 3500 is 3.5 seconds
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);

  // L298N pin assignment
  // IN1, IN2 and ENA control the chute motor
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENA, OUTPUT);
  // IN3, IN4 and ENB control the chute rotation motor
  //pinMode(IN3, OUTPUT);
  //pinMode(IN4, OUTPUT);
  //pinMode(ENB, OUTPUT);

  // Disable the chute motor on powerup
  digitalWrite(ENA, LOW);

  // BTS7960 blower actuator pin assignment
  pinMode(R_PWM, OUTPUT);
  pinMode(L_PWM, OUTPUT);
  pinMode(R_EN, OUTPUT);
  pinMode(L_EN, OUTPUT);

  // Enable the blower motor actuator on powerup
  digitalWrite(R_EN, HIGH);
  digitalWrite(L_EN, HIGH);

  // BTS7960 chute rotation motor pin assignment
  pinMode(chuteRPWM, OUTPUT);
  pinMode(chuteLPWM, OUTPUT);
  pinMode(chuteREN, OUTPUT);
  pinMode(chuteLEN, OUTPUT);

  // Enable the chute rotation motor on powerup
  digitalWrite(chuteREN, HIGH);
  digitalWrite(chuteLEN, HIGH);

  // Chute rotation current sensor pin assignment
  pinMode(rsPin, INPUT);
  pinMode(lsPin, INPUT);

  // Initialize various "last" chute rotation values
  delay(500);
  chuteLastUpdate = millis();
  lastCh8Pulse = chuteLastUpdate;
  initialCh8 = pulseIn(INPUT_CH8_PIN, HIGH, FLYSKY_TIMEOUT_VALUE);

  int inputA14 = analogRead(A14);
  initialCh8 = map(inputA14, 0, 1023, 900, 2100);

  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 ch7Value = 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);
    ch8Value = 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

  // Change 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);

  // 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(" | Ch8="); Serial.print(ch8Value);
    Serial.println();
  #endif

  // 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);

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

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

  // Crawler chute rotation and lifting/lowering of blower
  controlChuteRotation(ch8Value);
  ////controlBlowerLiftingAndLowering(ch1Value, ch2Value);

  // The individual test sketches used a couple different delays.  The engine code had delay(100) and
  // crawler code was delay(10).  For intial testing, we will go with the longer delay of 100 milliseconds.
  delay(100);
}

//
// 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 disabled_controlChuteDeflection(long ch7) {
  if (ch7 > 1600) {
    digitalWrite(ENA, HIGH);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (ch7 < 1400) {
    digitalWrite(ENA, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(ENA, LOW);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void controlChuteRotation(long ch8) {

	#define CURRENT_THRESHOLD 700  // Adjust if needed (600–800)

  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(A15);
  int lsValue = analogRead(lsPin);

  #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 = 3500;	
		}
   	desiredPosition = -3500;
  } else {
    desiredPosition = map(ch8, 1000, 2000, -3150, 3150);
  }
  
  #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) {
    // Clockwise (Right)
    if (abs(desiredPosition - chutePosition) < 100) {
      analogWrite(chuteRPWM, 0);
      analogWrite(chuteLPWM, 0);
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      	Serial.print(" | Stopped - At desired postion");
      #endif  
    }
    else if ((desiredPosition > chutePosition) && (chutePosition <= CHUTE_MAX_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      	Serial.print(" | Rotating Clockwise");
      #endif 
      if (lsValue < CURRENT_THRESHOLD) {
        analogWrite(chuteRPWM, 0);
        analogWrite(chuteLPWM, 255);
        chutePosition += delta;
        if (chutePosition > CHUTE_MAX_DURATION) chutePosition = CHUTE_MAX_DURATION;
      } else {
       	#if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      		Serial.print(" - STALL");
      	#endif
        analogWrite(chuteRPWM, 0);
        analogWrite(chuteLPWM, 0);
      }      
    }
    // Counter-clockwise (Left)
    else if ((desiredPosition < chutePosition) && (chutePosition >= -CHUTE_MAX_DURATION)) {
      #if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      	Serial.print(" | Rotating Counter-clockwise");
      #endif
      if (rsValue < CURRENT_THRESHOLD) {
        analogWrite(chuteRPWM, 255);
        analogWrite(chuteLPWM, 0);
        chutePosition -= delta;
        if (chutePosition < -CHUTE_MAX_DURATION) chutePosition = -CHUTE_MAX_DURATION;
      } else {
       	#if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      		Serial.print(" - STALL");
      	#endif
        chutePositionRigged = true;
        chutePosition = -CHUTE_MAX_DURATION;
        analogWrite(chuteRPWM, 0);
        analogWrite(chuteLPWM, 0);
      }
    }
    // Stop
    else {
     	#if DEBUG_PRINT_CHUTE_ROTATION_INFO 
      	Serial.print(" | Stopped - we should never get here!!!");
     	#endif
      analogWrite(chuteRPWM, 0);
      analogWrite(chuteLPWM, 0);
    }
  } else {
    #if DEBUG_PRINT_CHUTE_ROTATION_INFO 
    Serial.print(" | Stopped - Knob has not yet moved.");
    	#endif
    analogWrite(chuteRPWM, 0);
    analogWrite(chuteLPWM, 0);
  }

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

/*
void stopChute() {
  digitalWrite(ENB, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}


void OLDcontrolChuteRotation(long ch8) {
  unsigned long now = millis();
  float delta = now - chuteLastUpdate;
  chuteLastUpdate = now;
  if (delta > 1000) delta = 1000;

  if (ch8 > 950 && ch8 < 2050) {
    lastCh8Pulse = now;
  } else if (now - lastCh8Pulse > 500) {
    ch8 = 1500;
  }

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

  if (knobMoved) {
    if (ch8 > 1600) {
      if (chutePosition < CHUTE_MAX_TIME) {
        digitalWrite(ENB, HIGH);
        digitalWrite(IN3, HIGH);
        digitalWrite(IN4, LOW);
        chutePosition += delta;
        if (chutePosition > CHUTE_MAX_TIME) chutePosition = CHUTE_MAX_TIME;
      } else {
        stopChute();
      }
    } else if (ch8 < 1400) {
      if (chutePosition > -CHUTE_MAX_TIME) {
        digitalWrite(ENB, HIGH);
        digitalWrite(IN3, LOW);
        digitalWrite(IN4, HIGH);
        chutePosition -= delta;
        if (chutePosition < -CHUTE_MAX_TIME) chutePosition = -CHUTE_MAX_TIME;
      } else {
        stopChute();
      }
    } else {
      stopChute();
    }
  } else {
    stopChute();
  }

  #if DEBUG_PRINT_CHUTE_ROTATION_INFO
    Serial.print("Chute Position: ");
    Serial.println(chutePosition);
  #endif
}
*/


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

  if (ch1 < 1460 && ch2 < 1460) {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 255);
    wasReverse = true;
    wasForward = false;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lifting Blower (triggered by reverse)");
    #endif
  } else if (wasReverse) {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 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(R_PWM, 255);
    analogWrite(L_PWM, 0);
    wasForward = true;
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (triggered by forward)");
    #endif
  } else if (wasForward) {
    analogWrite(R_PWM, 255);
    analogWrite(L_PWM, 0);
    #if DEBUG_PRINT_BLOWER_UP_DOWN_INFO
      Serial.println("Lowering Blower (persisting forward)");
    #endif
    if (ch1 < 1460 && ch2 < 1460) {
      wasForward = false;
    }
  } else {
    analogWrite(R_PWM, 0);
    analogWrite(L_PWM, 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
  }
}