bubble_buddy.cpp

// Include the V5 Library
#include "vex.h"
  
// Allows for easier use of the VEX Library
using namespace vex;

enum Mode {
  SEARCH,
  FOLLOW_FAST,
  FOLLOW_SLOW,
  HOLD,
  BACK_UP,
  PANIC_BACK,
};

Mode mode = SEARCH; // init mode is SEARCH

// --- Tunable constants (mm) ---
const int NEAR = 250;
const int FAR = 350;
const int SUPER_FAR = 600; // optional: faster catch-up
const int PANIC = 140; // very close → back up hard

// --- Tunable speeds (%) ---
const int FAST_SPEED = 60;
const int SLOW_SPEED = 25;
const int BACK_SPEED = 30;
const int PANIC_SPEED = 70;
const int TURN_SEARCH_SPEED = 15;

int main() {
  vexcodeInit();

  Drivetrain.setDriveVelocity(FAST_SPEED, percent);
  Drivetrain.setTurnVelocity(TURN_SEARCH_SPEED, percent);

  while (true) {
    // 1) SENSE
    if (!DistanceSensor.isObjectDetected()) {
      // If not seen → SEARCH mode
      mode = SEARCH;
    } else {
      double d = DistanceSensor.objectDistance(mm);   // distance in mm
      double v = DistanceSensor.objectVelocity();     // approach speed (m/s)

      // 2) THINK (choose state)
      if (d < PANIC || (d < NEAR && fabs(v) > 0.6)) {
        mode = PANIC_BACK;
      } else if (d < NEAR) {
        mode = BACK_UP;
      } else if (d > SUPER_FAR) {
        mode = FOLLOW_FAST;
      } else if (d > FAR) {
        mode = FOLLOW_SLOW;
      } else {
        mode = HOLD;
      }
    }

    // 3) ACT (do behavior)
    if (mode == SEARCH) {
      Drivetrain.stop();
      Drivetrain.turn(right);   // slow spin to reacquire target
    } else if (mode == FOLLOW_FAST) {
      Drivetrain.setDriveVelocity(FAST_SPEED, percent);
      Drivetrain.drive(forward);
    } else if (mode == FOLLOW_SLOW) {
      Drivetrain.setDriveVelocity(SLOW_SPEED, percent);
      Drivetrain.drive(forward);
    } else if (mode == HOLD) {
      Drivetrain.stop();  // inside the bubble zone
    } else if (mode == BACK_UP) {
      Drivetrain.setDriveVelocity(BACK_SPEED, percent);
      Drivetrain.drive(reverse);
    } else if (mode == PANIC_BACK) {
      Drivetrain.setDriveVelocity(PANIC_SPEED, percent);
      Drivetrain.drive(reverse);
    }

    // Debug output (optional but helpful)
    Brain.Screen.clearScreen();
    Brain.Screen.setCursor(1, 1);
    Brain.Screen.print("Mode: %d", (int)mode);
    Brain.Screen.newLine();
    Brain.Screen.print("Seen: %d", (int)DistanceSensor.isObjectDetected());
    Brain.Screen.newLine();
    Brain.Screen.print("Dist(mm): %.1f", DistanceSensor.objectDistance(mm));
    Brain.Screen.newLine();
    Brain.Screen.print("Vel(m/s): %.2f", DistanceSensor.objectVelocity());

    wait(20, msec);  // friendly loop timing
  }
}