RC Car with ESP8266 Interrupt Problem?

Hello

I'm working on making a RC car with ESP8266 esp-05 wifi module.

The problem I'm solving is, I'm not sure, Interrupt.

I put many functions in this project such as detecting obstacles by ultrasonic sensors, raising up the speed step by step, raising maximum speed at once, going forward & backward, and etc.

The front motor is in charge of changing direction and the back motor generates speed.

The problem is when I send a command to the back motor, it does work on the first command but not to next commands. For example, when I send "Go Forward", it goes forward. After it, I send "Stop", but it doesn't receive the last command so it doesn't stop going forward.

I didn't know about Interrupt well but apparently, Interrupt is used inside of SoftwareSerial.h. So I changed Rx and Tx pin to 2&3 which Arduino Uno provides Interrupt. But the problem is still not solved. Help me please I wanna go to bed... My code is quite long and messy... so if you don't mind to look into this.. http://blog.naver.com/b_ana/220412468748

#include <SoftwareSerial.h>

#define DEBUG true
#define BAUDRATE 9600

SoftwareSerial esp8266(2,3);

// Ultrasonic sensor
const int trigA = 11;
const int echoA = 12;
const int trigB = A5;
const int echoB = A4;
const int maximumRange = 200; // Maximum range
const int minimumRange = 0; // Minimum range
int distanceA; // Duration to calculate distance
long distanceB; // Duration to calculate distance
long distance, duration;

// Front(Direction) Motor
const int dir1PinA = 5;
const int dir2PinA = 6;
const int speedPinA = 10;

// Back(rear-wheel) Motor
const int dir1PinB = 7;
const int dir2PinB = 8;
const int speedPinB = 9;


const int speedAdjustment = 50; // speed gain per a step
const int FORWARD = 1;
const int BACKWARD = -1;
int currentSpeed = 0;
int movingDirection = BACKWARD;
bool isStopped = true;

/* 
 *  Function
 *  Control the Motor direction and speed
 */
void setMotor(int pin) {
  Serial.print("pin: ");
  Serial.println(pin);
  switch (pin)
  {
    case 1: // Left
    changeDirection(0, HIGH, LOW);
    analogWrite(speedPinA, 200);
    Serial.println("Turn Left");
    delay(20);
    break;
    
    case 2: // Center
    changeDirection(0, LOW, LOW);
    Serial.println("Put The Direction to the Center");
    delay(20);
    break;
    
    case 3: // Right
    changeDirection(0, LOW, HIGH);
    analogWrite(speedPinA, 200);
    Serial.println("Turn Right");
    delay(20);
    break;
    
    case 4: // Backward step by step
    Serial.println("Speed Down!");
    // if the CurrentSpeed is a negative integer or 0
    if (currentSpeed > 0 && currentSpeed <= 250) {
      currentSpeed += -speedAdjustment;
      analogWrite(speedPinB, currentSpeed);
    } else if (currentSpeed > -250 && currentSpeed < 0) {
      currentSpeed += -speedAdjustment;
      analogWrite(speedPinB, -currentSpeed);
    } else if (currentSpeed == 0) {
      currentSpeed += -speedAdjustment;
      analogWrite(speedPinB, -currentSpeed);
      changeDirection(1, LOW, HIGH);
      movingDirection = BACKWARD;
    } else if (currentSpeed == -250) { // Not to allow up to over 255 & -255
      Serial.println("Not Allowed to be over Max and Min Value");
    }
    
    isStopped = false;
    delay(20);
    break;
    
    // Control Motor B (Speed)
    case 6: // Forward step by step
    Serial.println("Speed Up!");
    // if the CurrentSpeed is a positive integer or 0
    if (currentSpeed > 0 && currentSpeed < 250) {
      currentSpeed += speedAdjustment;
      analogWrite(speedPinB, currentSpeed);
    } else if (currentSpeed >= -250 && currentSpeed < 0) {
      currentSpeed += speedAdjustment;
      analogWrite(speedPinB, -currentSpeed);
    } else if (currentSpeed == 0) {
      currentSpeed += speedAdjustment;
      changeDirection(1, HIGH, LOW);
      movingDirection = FORWARD;
      analogWrite(speedPinB, currentSpeed);
    } else if (currentSpeed == 250) { // Not to allow up to over 255 & -255
      Serial.println("Not Allowed to be over Max and Min Value");
    }
    isStopped = false;
    delay(20);
    break;
    
    case 7: // Backward at a max speed 
    Serial.println("GO BACKWARD AT A MAXIMUM SPEED");
    currentSpeed = -250;
    movingDirection = BACKWARD;
    analogWrite(speedPinB, -currentSpeed);
    changeDirection(1, LOW, HIGH);
    isStopped = false;
    delay(20);
    break;
    
    case 9: // Forward at a max speed
    Serial.println("GO FORWARD AT A MAXIMUM SPEED!");
    currentSpeed = 250;
    movingDirection = FORWARD;
    analogWrite(speedPinB, currentSpeed);
    changeDirection(1, HIGH, LOW);
    isStopped = false;
    delay(20);
    break;
    
    case 5:
    case 8:
    stopVehicle();
    break;
  } // Switch end
}

/*
 * It determines the propulsion way of motors whether going forward or backward, turning left or right
 */
void changeDirection(int motor, int a, int b) {
  switch(motor) {
    case 1: // Speed
    digitalWrite(dir1PinB, a);
    digitalWrite(dir2PinB, b);
    break;
    
    case 0: // Direction
    digitalWrite(dir1PinA, a);
    digitalWrite(dir2PinA, b);
    break;
  }
}

/*
 * To stop, The back motor will work against the car's direction for a few microsecs.
 */
void stopVehicle() {
  Serial.println("STOP!");
  
  switch (movingDirection) {
    case FORWARD:
    changeDirection(1, LOW, HIGH);
    for (int i = currentSpeed; i > currentSpeed/2; i--) {
      analogWrite(speedPinB, i);
      delay(20);
      i-=2;
    } // case 1 end
    break;
    
    case BACKWARD:
    changeDirection(1, HIGH, LOW);
    for (int i = currentSpeed; i < currentSpeed/2; i++) {
      analogWrite(speedPinB, -i);
      delay(20);
      i+=2;
    } // case 0 end
    break;
  } // movingDirection end
  
  currentSpeed = 0;
  analogWrite(speedPinB, currentSpeed);
  isStopped = true;
  Serial.println("Stopped!");
}

/*
 * Ultrasonic sensor
 */
int operateUltrasonic(int trig, int echo) {
  int distances[6] = {0,0,0,0,0,0};
  
  for (int i=0; i < 5; i++) {
    // Calculating distances by Ultrasonic Sensor
    digitalWrite(trig, LOW);
    delayMicroseconds(2);
  
    digitalWrite(trig, HIGH);
    delayMicroseconds(10);
  
    digitalWrite(trig, LOW);
    duration = pulseIn(echo, HIGH);
  
    //Calculate the distance (in cm) based on the speed of sound.
    distances[i] = duration/58.2;
    distances[5] += distances[i];
  }
  
  return distances[5] / 5;
}

/*
 * send data to the esp8266 Serial
 */
String sendData(String command, const int timeout, boolean debug)
{
  String response = "";
  esp8266.print(command); // send the read character to the esp8266
  long int time = millis();
  while( (time+timeout) > millis())
  {
    while(esp8266.available())
    {
      // The esp has data so display its output to the serial window 
      char c = esp8266.read(); // read the next character.
      response+=c;
      }  
    }
    
    if(debug)
    {
      Serial.print(response);
    }
    
    return response;
}

/*
 * Initial Setting for the esp8266 module.
 */
void setupWifi() {
  sendData("AT+RST\r\n",3000,DEBUG); // reset module
  //sendData("AT+CIOBAUD=9600\r\n",3000,DEBUG); 
  sendData("AT+CWLAP\r\n", 5000, DEBUG);
  sendData("AT+CWJAP=\"test\",\"1111\"\r\n", 5000, DEBUG);
  sendData("AT+CWMODE=3\r\n",3000,DEBUG); // configure as access point
  sendData("AT+CIFSR\r\n",3000,DEBUG); // get ip address
  sendData("AT+CIPMUX=1\r\n",3000,DEBUG); // configure for multiple connections
  sendData("AT+CIPSERVER=1,80\r\n",3000,DEBUG); // turn on server on port 80
}

void setup() {
  Serial.begin(BAUDRATE);
  esp8266.begin(BAUDRATE); // your esp's baud rate might be different
  setupWifi();
  
  // The Front Motor
  pinMode(dir1PinA, OUTPUT);
  pinMode(dir2PinA, OUTPUT);
  pinMode(speedPinA, OUTPUT);
  
  // The Back Motor
  pinMode(dir1PinB, OUTPUT);
  pinMode(dir2PinB, OUTPUT);
  pinMode(speedPinB, OUTPUT);
  
  // Front Ultrasonic
  pinMode(trigA, OUTPUT);
  pinMode(echoA, INPUT);
  
  pinMode(trigB, OUTPUT);
  pinMode(echoB, INPUT);
}

void closeCommand(int connectionId) {
  String closeCommand ="AT+CIPCLOSE=";
  closeCommand += connectionId;
  closeCommand += "\r\n";
  sendData(closeCommand,500,DEBUG);
  Serial.println();
}

void loop() {
  // Calculate the distance by Ultrasonic Sensor
  if (!isStopped) {
    distanceA = operateUltrasonic(trigA, echoA);
    distanceB = operateUltrasonic(trigB, echoB);
    Serial.print("distanceA: ");
    Serial.print(distanceA);
    Serial.print(", distanceB: ");
    Serial.println(distanceB);
  
    // If any obstacle is close to the veichle back and forth
    switch(movingDirection) {
    case FORWARD:
      if (distanceA < ((currentSpeed>125)?currentSpeed*0.3:currentSpeed*0.1) + 15) {
        stopVehicle();
      }
      break;
    case BACKWARD:
      if (distanceB < -((-currentSpeed>125)?currentSpeed*0.3:currentSpeed*0.1) + 15) {
        stopVehicle();
      }
      break;
    }
  }
  // Get http data
  if(esp8266.available()) // check if the esp is sending a message 
  {
    if(esp8266.find("+IPD,"))
    {
      Serial.println("esp found");
      delay(1000);
      int connectionId = esp8266.read()-48; // subtract 48 because the read() function returns 
                                           // the ASCII decimal value and 0 (the first decimal number) starts at 48
      
      esp8266.find("pin="); // advance cursor to "pin="
      int pin = (esp8266.read() - 48);
      //print pinNumber
      Serial.print("command number: ");
      Serial.println(pin);
      setMotor(pin);
      
      closeCommand(connectionId);
    } // ESP.find end
  } // ESP.available end
} // loop end