FSM_ultrasound4WD.ino

/*
  Finite state machine with
  Ultrasound HC-SR04 with Cherokey 4WD.

  Programmed by Rudi - NYU Shanghai
  Date: 28 Sept 2022


  This example code is in the public domain.

  It uses the following sources:
  NewPing
  https://forum.arduino.cc/t/newping-library-hc-sr04-srf05-srf06-dyp-me007-parallax-ping-v1-7/103737
  4WD Cherokey Wiki Page
  https://wiki.dfrobot.com/Basic_Kit_for_Cherokey_4WD_SKU_ROB0117

*/


#include <NewPing.h>


#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define FULL_SPEED 255 // Maximum speed for the motors
#define OBSTACLE_UNDETECTED 1
#define OBSTACLE_AHEAD 2
#define OBSTACLE_NEAR 3

int TRIGGER_PIN = 12;  // Arduino pin tied to trigger pin on the ultrasonic sensor.
int ECHO_PIN  =   11;  // Arduino pin tied to echo pin on the ultrasonic sensor.

int speedPin_M1 = 5;       //M1 Speed Control
int speedPin_M2 = 6;      //M2 Speed Control
int directionPin_M1 = 4;  //M1 Direction Control
int directionPin_M2 = 7;  //M1 Direction Control


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

// Global variable declaration
int distance;

int state = 0;

// the setup routine runs once when you press reset:
void setup() {
  // initialize the outputs that the motor drivers need
  for (int i = 4; i <= 7; i++)
    pinMode(i, OUTPUT);
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
}

// the loop routine runs over and over again forever:
void loop() {
  // read all the sensors here
  readSensors();

  // here I will check all the sensors
  evaluateSensors();

  Serial.println("the current state is " + state);
  // decide the behavior according to the state machine
  switch (state) {
    case OBSTACLE_UNDETECTED:
      //here the robot will perform actions when no obstacle
      carAdvance(FULL_SPEED, FULL_SPEED);
      break;
    case OBSTSACLE_AHEAD:
      //here the robot will perform actions when an obstacle is close by
      //in our case, we will slow down
      break;

    case OBSTACLE_NEAR:
      //in our case, when in the closest we can get to the obstacle,
      //we will blink an LED or signal (use millis!!!!, check the example BlinkWithoutDelay)
      carStop();
      
      break;

    default:
      break;
  }
  // decide how to control the actions here
  if ( distance >= 10 ) {

    carAdvance(FULL_SPEED, FULL_SPEED);
    //delay(500);  //do not use long delays within loop, unless necessary
  } else {
    carStop();
  }

  Serial.print("Ping: ");
  Serial.print(distance); // Send ping, get distance in cm and print result (0 = outside set distance range)
  Serial.println("cm");

  delay(1); //adding up to 10ms in the loop is harmless to our moving robot
}


// Auxiliary function for the Finite State Machine
void readSensors() {
  //obsatacle = digitalRead(pin);
  distance = sonar.ping_cm();
}

void evaluateSensors() {
  if (distance > 100) {
    state = OBSTACLE_UNDETECTED;
  } else if ((distance <= 100) && (distance > 50)) {
    state = OBSTACLE_AHEAD;
  } else (distance <= 50) {
    state = OBSTACLE_NEAR;
  }
}

// Auxiliary functions to move the robot

void carStop() {  //  Motor Stop
  digitalWrite(speedPin_M2, 0);
  digitalWrite(directionPin_M1, LOW);
  digitalWrite(speedPin_M1, 0);
  digitalWrite(directionPin_M2, LOW);
}

void carBack(int leftSpeed, int rightSpeed) {  //Move backward
  analogWrite(speedPin_M2, leftSpeed);         //PWM Speed Control
  digitalWrite(directionPin_M1, HIGH);
  analogWrite(speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}

void carAdvance(int leftSpeed, int rightSpeed) {  //Move forward
  analogWrite(speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, LOW);
  analogWrite(speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, LOW);
}

void carTurnLeft(int leftSpeed, int rightSpeed) {  //Turn Left
  analogWrite(speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, LOW);
  analogWrite(speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, HIGH);
}
void carTurnRight(int leftSpeed, int rightSpeed) {  //Turn Right
  analogWrite(speedPin_M2, leftSpeed);
  digitalWrite(directionPin_M1, HIGH);
  analogWrite(speedPin_M1, rightSpeed);
  digitalWrite(directionPin_M2, LOW);
}