Rush.ino

#define DEBUG 0  // Set to 1 to enable debug output

#if DEBUG
#define DEBUG_PRINT(x) Serial.print(x)
#define DEBUG_PRINTLN(x) Serial.println(x)
#else
#define DEBUG_PRINT(x)
#define DEBUG_PRINTLN(x)
#endif


// Tuning HERE!
// these are sensor's reading.
int slowing_threshold = 6;
int stop_threshold = 3;
int motorACoeff = 1.0;
int motorBCoeff = 1.0;

// │
// │
// │
// ┼────────────────────────────────x  <--slowing_threshold
// │                                 x
// │                                  x
// │                                   x
// │                                    x
// │                                     x
// │                                      x
// │                                       x
// │                                        x
// │                                         x
// │                                          x
// │                                           x
// │                                            x
// │                         stop_threshold -->  x
// │                                             │
// │                                             │
// │                                             │
// │                                             │
// └─────────────────────────────────────────────┴────


// Include the (new) library
#include <L298NX2.h>
#include <HCSR04.h>
#include <BlockNot.h>

BlockNot printInfoTimer(1000);  //In Milliseconds

const unsigned int EN_A = 9;
const unsigned int IN1_A = 5;
const unsigned int IN2_A = 4;

const unsigned int IN1_B = 3;
const unsigned int IN2_B = 2;
const unsigned int EN_B = 10;

// Ultrasonic Sensor Pins
const unsigned int trigPin = 6;
const unsigned int echoPin = 7;
HCSR04 hc(echoPin, trigPin);  //initialisation class HCSR04 (trig pin , echo pin)

int IRSensor = 12; // connect IR sensor module to Arduino pin D9




//Initialize both motors
L298NX2 motors(EN_A, IN1_A, IN2_A, EN_B, IN1_B, IN2_B);

void setup() {
  // Used to display information
  Serial.begin(9600);

  pinMode(IRSensor, INPUT); // IR Sensor pin INPUT

  // ultrasonic.setTimeout(40000UL);

  motors.setSpeedA(255*motorACoeff);
  motors.setSpeedB(255*motorBCoeff);

  //Print initial information
  //printInfo();
}


unsigned long delayA = 10;
int directionA = L298N::BACKWARD;

unsigned long delayB = 10;
int directionB = L298N::BACKWARD;

int state = 0;  // 0 => stage 0 (start cooldown)
                // 1 => stage 1
                // 2 => stage 2
                // 3 => stage 3 (finish)

void loop() {
  stage0(); 
  stage1();
  directionA = L298N::FORWARD;
  directionB = L298N::FORWARD;
  delay(100);
  stage2_timer();
  // stage3();
  delay(3000);
  motors.stop();
  exit(0);
}

void stage0() {
  delay(3000); // sleep for 3 seconds to allow some time for calibration.
}

void stage1() {
  // doubleCheck is to make sure that an errounus reading doesn't fuck up and move us to next stage.
  int doubleCheck = 0;
  // yes, I "double"Check 10 times! I couldn't find a better variable name :(
  while (doubleCheck < 10) {
    long distance = hc.dist();
    DEBUG_PRINT("Distance: ");
    DEBUG_PRINT(distance);

    long speed = getSpeed(distance);
    DEBUG_PRINT("\tSpeed: ");
    DEBUG_PRINTLN(speed);

    motors.setSpeedA(speed*motorACoeff);
    motors.setSpeedB(speed*motorBCoeff);



    if (distance < 20) {
      motors.stop();
      doubleCheck++;
      // printInfo();
      DEBUG_PRINTLN("=============== Stopping! ======================");
    } else {
      doubleCheck=0; // reset errors so they don't accumlate
      // Drive both motors without blocking code execution
      motors.runA(directionA);
      motors.runB(directionB);
    }

    // broke man's RTOS
    //; if (printInfoTimer.TRIGGERED)
    //   printInfo();
  }
}

void stage2_timer(){
  motors.setSpeedA(255*motorACoeff); // left
  motors.setSpeedB(255*motorBCoeff);

  motors.runA(directionA);
  motors.runB(directionB);

}

void stage2() {
  // doubleCheck is to make sure that an errounus reading doesn't fuck up and move us to next stage.
  int doubleCheck = 0;
  // yes, I "double"Check 10 times! I couldn't find a better variable name :(
  while (doubleCheck < 10) {
  int sensorStatus = digitalRead(IRSensor); // Set the GPIO as Input
    DEBUG_PRINT("SensorStatus: ");
    DEBUG_PRINTLN(sensorStatus);

    motors.setSpeedA(255*motorACoeff);
    motors.setSpeedB(255*motorBCoeff);



    if (0) {
      motors.stop();
      doubleCheck++;
      // printInfo();
      DEBUG_PRINTLN("=============== Stopping! ======================");
    } else {
      doubleCheck=0; // reset errors so they don't accumlate
      // Drive both motors without blocking code execution
      motors.runA(directionA);
      motors.runB(directionB);
    }

    // broke man's RTOS
    // if (printInfoTimer.TRIGGERED)
    //   printInfo();
  }
}



long getSpeed(long distance) {
  // TODO: create a sophsticated function to calculate speed :)

  if (distance > slowing_threshold) {
    return 255;
  }
  int stopDistance = stop_threshold;    // Distance at which the motors should completely stop
  int maxDistance = slowing_threshold;  // Maximum distance for full speed

  // Map the distance to motor speed (0-255)
  int motorSpeed = map(distance, stopDistance, maxDistance, 0, 255);

  // Ensure the speed stays within valid PWM range
  motorSpeed = constrain(motorSpeed, 0, 255);

  return motorSpeed;
}
/*
Based on the number of current step, perform some changes for motor A.
*/
void callbackA() {
  motors.resetA();
}

/*
Each time is called, invert direction for motor B.
*/
void callbackB() {
  // directionB = !directionB;
  motors.resetB();
}


//Print info in Serial Monitor
void printInfo() {
  DEBUG_PRINT("Motor A | Speed = ");
  DEBUG_PRINT(motors.getSpeedA());
  DEBUG_PRINT(" | Direction = ");
  DEBUG_PRINT(motors.getDirectionA());


  DEBUG_PRINT("Motor B | Speed = ");
  DEBUG_PRINT(motors.getSpeedB());
  DEBUG_PRINT(" | Direction = ");
  DEBUG_PRINTLN(motors.getDirectionB());
}