Arduino.ino

#include <Arduino.h>
#include <Servo.h>
#include <AccelStepper.h>

const int PIN_ANALOG = A2;   // Pin analógico a leer (A2)

/* ===================== Pines ===================== */
// Hall
const uint8_t PIN_HALL_R = A0;   // Hall derecho
const uint8_t PIN_HALL_L = A1;   // Hall izquierdo
// Servos
const uint8_t PIN_SERVO_FRONT = 32;    // Servo dirección delantero
const uint8_t PIN_SERVO_REAR  = 33;    // Servo dirección trasero
// Stepper 1 (eje trasero)
const uint8_t M1_IN1 = 41;
const uint8_t M1_IN2 = 39;
const uint8_t M1_IN3 = 37;
const uint8_t M1_IN4 = 35;
// Stepper 2 (eje trasero)
const uint8_t M2_IN1 = 43;
const uint8_t M2_IN2 = 45;
const uint8_t M2_IN3 = 47;
const uint8_t M2_IN4 = 49;

/* ==================== Servos ===================== */
Servo servoFront, servoRear;
int SERVO_FRONT_CENTER_US = 1000; // 1000
int SERVO_REAR_CENTER_US  = 2000; //2000
const int MAX_DELTA_US = 500;

/* =============== Sentido y ganancias ============= */
const int   E_SIGN   = +1;
float Kp_us   = 2000.0f;
float Kd_us   = 350.0f;
float deadband = 0.02f;
float alpha    = 0.2f;

/* ============== Rangos fijos Hall ================ */
float L_min = 2040;
float L_max = 2060;
float R_min = 2080;
float R_max = 2115;

/* ================ Trim del servo ================= */
float SERVO_FRONT_TRIM_US = 0.0f;
const float NO_FIELD_COUNTS = 20.0f;
const float TRIM_KI_US_PER_S = 400.0f;
const float TRIM_MAX_ABS_US  = 200.0f;

/* ======== Servo trasero relación/sentido ========= */
const bool  REAR_INVERT = true;
const float REAR_RATIO  = 1.0f;

/* ============== Slew-rate (suavizado) ============ */
float SLEW_US_PER_SEC_FRONT = 2000.0f;
float SLEW_US_PER_SEC_REAR  = 2000.0f;
int servo_front_us_prev = SERVO_FRONT_CENTER_US;
int servo_rear_us_prev  = SERVO_REAR_CENTER_US;

/* ================== Steppers ===================== */
AccelStepper stepper1(AccelStepper::HALF4WIRE, M1_IN1, M1_IN3, M1_IN2, M1_IN4);
AccelStepper stepper2(AccelStepper::HALF4WIRE, M2_IN1, M2_IN3, M2_IN2, M2_IN4);

const long  STEPS_PER_REV = 4096;
float wheel_rps = 2.30f;
float steps_per_second = wheel_rps * STEPS_PER_REV;
float MAX_SPS = 1500.0f;
bool invert_stepper1 = true;
bool invert_stepper2 = true;

/* ================== Estado ======================= */
float e_filt = 0.0f, e_prev = 0.0f;
unsigned long t_prev = 0;
const unsigned long CTRL_MS  = 2;
const unsigned long PRINT_MS = 50;
unsigned long t_last_ctrl  = 0;
unsigned long t_last_print = 0;
// ======= ADDED: Hall ignore/read state (must be global) =======
bool hallL_enabled = true;   // true = read left sensor, false = ignore left
bool hallR_enabled = true;   // true = read right sensor, false = ignore right
bool go_straight   = false;  // true = force straight (center servos)
// =============================================================

/* ================= Utilidades ==================== */
int clampMicroseconds(int u) {
  if (u < -MAX_DELTA_US) return -MAX_DELTA_US;
  if (u >  MAX_DELTA_US) return  MAX_DELTA_US;
  return u;
}
int readStableAnalog(uint8_t pin) {
  long acc = 0; const int N = 6;
  for (int i = 0; i < N; ++i) acc += analogRead(pin);
  return (int)(acc / N);
}
float ema(float x, float y_prev, float a) { return a * x + (1.0f - a) * y_prev; }

/* ================= Steppers ====================== */
void setBothStepperSpeeds(float sps) {
  if (sps >  MAX_SPS) sps =  MAX_SPS;
  if (sps < -MAX_SPS) sps = -MAX_SPS;
  steps_per_second = sps;
  float s1 = invert_stepper1 ? -sps : sps;
  float s2 = invert_stepper2 ? -sps : sps;
  stepper1.setSpeed(s1);
  stepper2.setSpeed(s2);
}
// ======= zmineno!!! =======
// ======= !!! sensor ignorieren =======
void handleESP32Command() {

  if (Serial1.available()) {

    String cmd = Serial1.readStringUntil('\n');
    cmd.trim();

    if (cmd == "sensor left on") {
      // читати тільки ЛІВИЙ
      hallL_enabled = true;
      hallR_enabled = false;
      go_straight   = false;
    }
    else if (cmd == "sensor right on") {
      // читати тільки ПРАВИЙ
      hallL_enabled = false;
      hallR_enabled = true;
      go_straight   = false;
    }
    else if (cmd == "sensor both off") {
      // ігнорувати обидва датчики
      hallL_enabled = false;
      hallR_enabled = false;
      // їхати прямо
      go_straight = true;
    }
    else if (cmd == "sensor both on") {
      // читати обидва датчики
      hallL_enabled = true;
      hallR_enabled = true;
      // включаємо нормальний режим PD
      go_straight = false;
    }

  }
}
// ======= zmineno!!! =======

/* =================== Setup ======================= */
void setup() {
  analogReadResolution(12);
  pinMode(PIN_ANALOG, INPUT);
  Serial.begin(115200);
  Serial1.begin(115200);
  unsigned long t_start = millis();
  while (!Serial && (millis() - t_start < 2000)) {}

  pinMode(PIN_HALL_R, INPUT);
  pinMode(PIN_HALL_L, INPUT);

  servoFront.attach(PIN_SERVO_FRONT);
  servoRear.attach(PIN_SERVO_REAR);
  servoFront.writeMicroseconds(SERVO_FRONT_CENTER_US);
  servoRear.writeMicroseconds(SERVO_REAR_CENTER_US);
  delay(200);

  stepper1.setMaxSpeed(MAX_SPS);
  stepper2.setMaxSpeed(MAX_SPS);
  setBothStepperSpeeds(wheel_rps * STEPS_PER_REV);

  t_prev = millis();
  t_last_ctrl = t_prev;
  t_last_print = t_prev;

  Serial.println(F("# v2.3 (valores fijos Lmin/Lmax/Rmin/Rmax)"));
  Serial.print(F("L_min=")); Serial.println(L_min);
  Serial.print(F("L_max=")); Serial.println(L_max);
  Serial.print(F("R_min=")); Serial.println(R_min);
  Serial.print(F("R_max=")); Serial.println(R_max);
}

/* ==================== Loop ======================= */
void loop() {
  handleESP32Command();
  int value = analogRead(PIN_ANALOG);
  float voltage = value * (3.3 / 4095);

  if (voltage >= 2.0) {
    stepper1.runSpeed();
    stepper2.runSpeed();
  }

  unsigned long now = millis();
  if (now - t_last_ctrl >= CTRL_MS) {
    float dt = (now - t_last_ctrl) / 1000.0f;
    if (dt < 0.0005f) dt = 0.0005f;
    t_last_ctrl = now;

    if (go_straight) {
    servoFront.writeMicroseconds(SERVO_FRONT_CENTER_US + (int)SERVO_FRONT_TRIM_US);
    servoRear.writeMicroseconds(SERVO_REAR_CENTER_US);

  servo_front_us_prev = SERVO_FRONT_CENTER_US + (int)SERVO_FRONT_TRIM_US;
  servo_rear_us_prev  = SERVO_REAR_CENTER_US;

  e_filt = 0.0f;
  e_prev = 0.0f;
  return; // вихід з loop(), щоб PD не рахувався
}

    int R_raw = hallR_enabled ? readStableAnalog(PIN_HALL_R) : (int)R_min;
    int L_raw = hallL_enabled ? readStableAnalog(PIN_HALL_L) : (int)L_min;

    float rel_R = R_min - R_raw;
    float rel_L = L_min - L_raw;

    float e = 0.0f;
    bool inNeutral_R = (R_raw >= R_min && R_raw <= R_max);
    bool inNeutral_L = (L_raw >= L_min && L_raw <= L_max);

    if (!(inNeutral_R && inNeutral_L)) {
      const float eps = 1.0f;
      float denom = fabsf(R_min - R_raw) + fabsf(L_min - L_raw) + eps;
      e = ((R_min - R_raw) - (L_min - L_raw)) / denom;
      e *= (float)E_SIGN;
    } else {
      e = 0.0f;
    }

    e_filt = ema(e, e_filt, alpha);
    float e_db = (fabs(e_filt) < deadband) ? 0.0f : e_filt;
    float de = (e_filt - e_prev) / dt;

    int u_us = (int)(Kp_us * e_db + Kd_us * de);
    int u_us_clamped = clampMicroseconds(u_us);

    int front_center_eff = SERVO_FRONT_CENTER_US + (int)SERVO_FRONT_TRIM_US;
    int rear_center_eff  = SERVO_REAR_CENTER_US;

    int desired_front_us = front_center_eff + u_us_clamped;
    int rear_delta = (int)(REAR_RATIO * u_us_clamped);
    if (REAR_INVERT) rear_delta = -rear_delta;
    int desired_rear_us  = rear_center_eff + rear_delta;

    float max_step_f = SLEW_US_PER_SEC_FRONT * dt;
    float delta_f = (float)desired_front_us - (float)servo_front_us_prev;
    delta_f = constrain(delta_f, -max_step_f, max_step_f);
    int servo_front_us = servo_front_us_prev + (int)delta_f;

    float max_step_r = SLEW_US_PER_SEC_REAR * dt;
    float delta_r = (float)desired_rear_us - (float)servo_rear_us_prev;
    delta_r = constrain(delta_r, -max_step_r, max_step_r);
    int servo_rear_us = servo_rear_us_prev + (int)delta_r;

    servoFront.writeMicroseconds(servo_front_us);
    servoRear.writeMicroseconds(servo_rear_us);

    servo_front_us_prev = servo_front_us;
    servo_rear_us_prev  = servo_rear_us;
    e_prev = e_filt;
  }

    if (Serial && (millis() - t_last_print >= PRINT_MS)) {
      t_last_print = millis();
      Serial.print(F("L_raw:")); Serial.print(analogRead(PIN_HALL_L)); Serial.print('\t');
      Serial.print(F("R_raw:")); Serial.print(analogRead(PIN_HALL_R)); Serial.print('\t');
      Serial.print(F("Lmin:"));  Serial.print(L_min); Serial.print('\t');
      Serial.print(F("Lmax:"));  Serial.print(L_max); Serial.print('\t');
      Serial.print(F("Rmin:"));  Serial.print(R_min); Serial.print('\t');
      Serial.print(F("Rmax:"));  Serial.print(R_max); Serial.print('\t');
      Serial.print(F("servo_front:")); Serial.print(servo_front_us_prev); Serial.print('\t');
      Serial.print(F("servo_rear:")); Serial.print(servo_rear_us_prev); Serial.print('\n');
    }
}