2WD Dagu Magician Robot

2WD Robot

// Bluetooth module is an HC-06
// default pw = 1234
// Vcc=5v, Gnd=0, Rx > Tx1, Tx > Rx1
// Flashing red LED on BT module = not connected

// Using ArduinoRC for control.
// AduinoRC is currently set for WSAD to control Fwd, Bk, L and R

#include <AFMotor.h>
#include <NewPing.h>

#define TRIGGER_PIN  23  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     22  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 100 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define STOP_DISTANCE 30 // if object is 10cm in front, STOP

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

AF_DCMotor mLeft(3);
AF_DCMotor mRight(4);

int BluetoothData; // the data given from Adroid

void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);
}

void loop()
{
  int uS = sonar.ping_cm();  // uS will be the ping distance in CM
  Serial.println(uS);
  
    if (Serial1.available()){
      BluetoothData=Serial1.read();

      if(BluetoothData=='w' && uS > STOP_DISTANCE){   // if fwd pressed & no obstruction
        forward(200);  // go forward at speed 200 (max is 255)
        delay(500);   // keep moving for 0.5 sec (= 500 milliseconds)
        halt();
      }

      if (BluetoothData=='z'){// if back pressed ....
          reverse(200);  // now reverse
          delay(500);
          halt();
      }
    
      if (BluetoothData=='a'){// if left pressed ....
          spinLeft(200);  // now spin left
          delay(500);
          halt();
      }
    
      if (BluetoothData=='s'){// if right pressed ....
          spinRight(200);  // now spin right
          delay(500);
          halt();
      }

    }
  
}

// ============================================

void forward (int spd)
{
  Serial.println("Forward");
  mLeft.run(FORWARD);
  mRight.run(FORWARD);
  mLeft.setSpeed(spd);  
  mRight.setSpeed(spd);  
}
 
void reverse(int spd)
{
  Serial.println("Reverse");
  mLeft.run(BACKWARD);
  mRight.run(BACKWARD);
  mLeft.setSpeed(spd);  
  mRight.setSpeed(spd);  
 }
 
void spinLeft(int spd)
{
  Serial.println("Spin Left");
  mLeft.run(FORWARD);
  mRight.run(BACKWARD);
  mLeft.setSpeed(spd);  
  mRight.setSpeed(spd);  
 }
 
void spinRight(int spd)
{
  Serial.println("Spin Right");
  mLeft.run(BACKWARD);
  mRight.run(FORWARD);
  mLeft.setSpeed(spd);  
  mRight.setSpeed(spd);  
}
 
void halt()
{
  Serial.println("Stop");
  mLeft.run(RELEASE);
  mRight.run(RELEASE);
  delay(10);
}