Playful Robots (Part 1)

This week, we upgraded our bug-like robot to a forklift-like one. Now it’s able to detect whether it is within the black circle by utilizing the IR sensor. Whenever it touches the border, it will turn around in a randomized degree between 160 and 220 so that it can move toward the inside. Meanwhile, we calculate the shortest distance between the robot and the three card boxes by using the ultrasonic sensor, which is placed in the front, right, and left. And this will help to determine the robot’s moving track. Last but not least, we modify the exterior structure of it so that it can push the box easily and protect the sensors at the same time.

Here is the demo video showing how it successfully pushes three boxes out of the circle.

Here is the Arduino Code.

#include <NewPing.h>

#define TRIGGER_PIN_H  3  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_H     4  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define TRIGGER_PIN_L  10  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_L     9
#define TRIGGER_PIN_R  7  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN_R     8

#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
#define Head 0
#define Left 1
#define Right 2
#define Stop 3
#define Far 4


NewPing sonar_H(TRIGGER_PIN_H, ECHO_PIN_H, MAX_DISTANCE); // HeadPing setup of pins and maximum distance.
NewPing sonar_L(TRIGGER_PIN_L, ECHO_PIN_L, MAX_DISTANCE); // LeftPing setup of pins and maximum distance.
NewPing sonar_R(TRIGGER_PIN_R, ECHO_PIN_R, MAX_DISTANCE); // RightPing setup of pins and maximum distance.

//IR sensor
int IRSensor = 2; // connect ir sensor to arduino pin 2
int LED = 13; // conect Led to arduino pin 13

//Standard PWM DC control
int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;    //M1 Direction Control
int M2 = 7;    //M1 Direction Control
int distance_H;
int distance_L;
int distance_R;
int statusSensor;
int state;

long ranNum;

void stop(void)                    //Stop
{
  digitalWrite(E1, LOW);
  digitalWrite(E2, LOW);
}
void advance(char a, char b)         //Move forward
{
  analogWrite (E1, a);     //PWM Speed Control
  digitalWrite(M1, HIGH);
  analogWrite (E2, b);
  digitalWrite(M2, HIGH);
}
void back_off (char a, char b)         //Move backward
{
  analogWrite (E1, a);
  digitalWrite(M1, LOW);
  analogWrite (E2, b);
  digitalWrite(M2, LOW);
}
void turn_L (char a, char b)            //Turn Left
{
  analogWrite (E1, a);
  digitalWrite(M1, LOW);
  analogWrite (E2, b);
  digitalWrite(M2, HIGH);
}
void turn_R (char a, char b)            //Turn Right
{
  analogWrite (E1, a);
  digitalWrite(M1, HIGH);
  analogWrite (E2, b);
  digitalWrite(M2, LOW);
}

void readSensors() {

  distance_H = sonar_H.ping_cm();
  distance_L = sonar_L.ping_cm();
  distance_R = sonar_R.ping_cm();

  if (distance_H == 0) {
    distance_H = 200;
  }

  if (distance_L == 0) {
    distance_L = 200;
  }

  if (distance_R == 0) {
    distance_R = 200;
  }


}

void evaluateSensors() {

  if ( min(distance_H, distance_L) == distance_H && min(distance_H, distance_R) == distance_H) {
    state = Head;
    //left最近
  } else if (min(distance_H, distance_L) == distance_L && min(distance_L, distance_R) == distance_L) {
    state = Left;
    //right最近
  } else if (min(distance_H, distance_R) == distance_R && min(distance_L, distance_R) == distance_R) {
    state = Right;
  } else {
    state = Stop;
  }
}

void setup(void)
{
  int i;
  for (i = 4; i <= 7; i++)
    pinMode(i, OUTPUT);
  Serial.begin(9600);      //Set Baud Rate

  //IR_sensor
  pinMode (IRSensor, INPUT); // sensor pin INPUT
  pinMode (LED, OUTPUT); // Led pin OUTPUT

}


void loop(void)
{

  int statusSensor = digitalRead (IRSensor);
  readSensors();
  evaluateSensors();

  if (statusSensor == 1) {
    digitalWrite(LED, LOW);

    ranNum = random(0, 1);
    turn_L(255, 255);
    delay(1100);
    if (ranNum == 1) {
      turn_L(255, 255);
      delay(1700);
    } else {
      turn_R(255, 255);
      delay(1700);
    }
  } else {
    //    digitalWrite(LED, HIGH);
    //    Serial.print(distance_H);
    //    Serial.print(" ");
    //    Serial.print(distance_L);
    //    Serial.print(" ");
    //    Serial.print(distance_R);
    //    Serial.println();
    switch (state) {

      case Head:
        Serial.print("Head");
        Serial.println();
        advance(255, 255);
        break;
      case Left:
        Serial.print("Left");
        Serial.println();
        //        turn_L(255, 255);
        //        delay(500);
        advance(255, 255);
        break;
      case Right:
        Serial.print("Right");
        Serial.println();

        //        turn_R(255, 255);
        //        delay(500);
        advance(255, 255);
        break;
    }
  }
}