Membuat Robot Rover Self avoiding

Tutorial kali ini akan dijelaskan tentang bagaimana cara membuat robot jenis rover yang dapat menghindari penghalang secara otomatis.
Bahan-bahan:

• baterai x8
• motor gearbox + roda X4
• 4xAA baterai holder x2
• saklar
• kabel
• arduino uno
• dual l293d motor shield
• microservo
• hc-sr04 ultrasonic sensor

cara merakit:

• hubungkan servo dengan konektor servo di serv0.
• rakit 2 motor secara pararel kemudian hubungkan masing_masing pasangan motor dengan M1 dan M3.
• hubungkan baterai holder dengan saklar, kemudian hubungkan dengan konektor eksternal power pada motor shield. 
• hubungkan ultrasonic sensor pada motor shield (trig ke a5, echo ke a4, vcc ke 5v, grnd ke ground).
• download program robot pada arduino uno.
• satukan motor shield dengan arduino.

kode program:
#include <AFMotor.h>
#include <Servo.h>
#include <Ultrasonic.h>
typedef enum {
  RADAR_LEFT = 0, RADAR_CENTER = 60, RADAR_RIGHT = 120 } RadarDirection;
Servo radarServo;  // create servo object to control a servo
int RADAR_INPUT_PIN = 9;
int RADAR_MOVEMENT_DELAY = 250;
int RADAR_DISTANCE_MINIMAL_IN_CM = 80; // centimeter
int RADAR_DISTANCE_UNKNOWN_IN_CM = 0; // centimeter
int RADAR_DISTANCE_UNDEFINED = -1;
Ultrasonic radarUltrasonic(A5,A4);
AF_DCMotor motorLeft(3, MOTOR12_64KHZ); // create motor #2, 64KHz pwm
AF_DCMotor motorRight(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm
int MOTOR_SPEED = 200;// 150;
int MOTOR_TURN_SPEED = 200; //250;
int MOTOR_DELAY = 150;
int MOTOR_TURN_DELAY = 500;
int MOTOR_BACKWARD_DELAY = 1000;
struct RobotState {
  int leftMotorDirection;
  int rightMotorDirection;
  int leftMotorSpeed;
  int rightMotorSpeed;
  int motorDelay;
  boolean isBackwardState;
};
struct RadarDistances {
  int left;
  int center;
  int right;
};
RobotState forwardState = {
  FORWARD,FORWARD,MOTOR_SPEED,MOTOR_SPEED,MOTOR_DELAY,false};
RobotState backwardState = {
  BACKWARD,BACKWARD,MOTOR_SPEED,MOTOR_SPEED,MOTOR_BACKWARD_DELAY,true};
RobotState backwardUturnState = {
  FORWARD,BACKWARD,MOTOR_SPEED,MOTOR_SPEED,MOTOR_TURN_DELAY,true};
RobotState turnLeftState = {
  RELEASE,FORWARD,MOTOR_SPEED,MOTOR_TURN_SPEED,MOTOR_TURN_DELAY,false};
RobotState turnRightState = {
  FORWARD,RELEASE,MOTOR_TURN_SPEED,MOTOR_SPEED,MOTOR_TURN_DELAY,false};
RobotState stopState = {
  RELEASE,RELEASE,MOTOR_SPEED,MOTOR_SPEED,MOTOR_BACKWARD_DELAY,false};
RobotState currentState = forwardState;
RadarDistances currentDistances = {
  RADAR_DISTANCE_UNDEFINED,RADAR_DISTANCE_UNDEFINED,RADAR_DISTANCE_UNDEFINED};
RadarDistances lastDistances = {
  RADAR_DISTANCE_UNDEFINED,RADAR_DISTANCE_UNDEFINED,RADAR_DISTANCE_UNDEFINED};
void setup() {
  initTrace();
  initRadar();
  initMotors();
}
void initTrace(){
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("My First Robot");
}
void initRadar(){
  radarServo.attach(RADAR_INPUT_PIN);  // attaches the servo on pin RADAR_INPUT_PIN to the servo object
  radarServo.write(RADAR_CENTER);
}
void initMotors(){
  motorLeft.setSpeed(MOTOR_SPEED);     // set the speed to 200/255
  motorRight.setSpeed(MOTOR_SPEED);     // set the speed to 200/255
}
void loop() {
  calculateRobotState();
  makeDistancesSnapshot();
  go();
}
void calculateRobotState(){
  measureDistanceAhead();
  if(
    isCenterDistanceUndefinded() ||
    isNoObstacleInPathAhead())
  {
//    if(isInCorner()){
//      currentState = backwardState; 
//      return;
//    }
    currentState = forwardState; 
    return;
  }
  measureDistanceLeft();
  measureDistanceRight();
  if(greater(currentDistances.left,currentDistances.right)){
    currentState = turnLeftState; 
    return;
  }
  if(greater(currentDistances.right,currentDistances.left)){
    currentState = turnRightState; 
    return;
  }
  if(eq(currentDistances.left,currentDistances.right)){
    currentState = backwardState; 
    return;
  }
  currentState = stopState;
}
void go(){
  Serial.print("Go : { ");
  Serial.print(currentState.leftMotorDirection);
  Serial.print(" , ");
  Serial.print(currentState.rightMotorDirection);
  Serial.println(" }");
  Serial.print("with : { left:");
  Serial.print(currentDistances.left);
  Serial.print(" , center:");
  Serial.print(currentDistances.center);
  Serial.print(" , right:");
  Serial.print(currentDistances.right);
  Serial.println(" }");
  motor();
}
void motor(){
  motorLeft.setSpeed(currentState.leftMotorSpeed);
  motorRight.setSpeed(currentState.rightMotorSpeed);
  motorLeft.run(currentState.leftMotorDirection);
  motorRight.run(currentState.rightMotorDirection);
  if(currentState.isBackwardState){
    delay(currentState.motorDelay);
    motorLeft.setSpeed(backwardUturnState.leftMotorSpeed);
    motorRight.setSpeed(backwardUturnState.rightMotorSpeed);
    motorLeft.run(backwardUturnState.leftMotorDirection);
    motorRight.run(backwardUturnState.rightMotorDirection);
    delay(backwardUturnState.motorDelay);
  }
}
void measureDistanceLeft(){
  moveRadarFastTo(RADAR_LEFT);
  currentDistances.left = internalMeasure();
}
void measureDistanceRight(){
  moveRadarFastTo(RADAR_RIGHT);
  currentDistances.right = internalMeasure();
}
void measureDistanceAhead(){
  moveRadarFastTo(RADAR_CENTER);
  currentDistances.center = internalMeasure();
}
int internalMeasure(){
  long microsec = radarUltrasonic.timing();
  return radarUltrasonic.convert(microsec, Ultrasonic::CM);
}
void moveRadarFastTo(int radarServoPosition){
  radarServo.write(radarServoPosition);              // tell servo to go to position in variable 'pos'
  delay(RADAR_MOVEMENT_DELAY);
}
boolean isCenterDistanceUndefinded(){
  return currentDistances.center == RADAR_DISTANCE_UNKNOWN_IN_CM;
}
boolean isNoObstacleInPathAhead(){
  return currentDistances.center >= RADAR_DISTANCE_MINIMAL_IN_CM;
}
boolean isInCorner(){
  boolean left = eq(currentDistances.left,lastDistances.left);
  boolean center = eq(currentDistances.center,lastDistances.center);
  boolean right = eq(currentDistances.right,lastDistances.right);
  return left && center && right;
}
boolean eq(int a, int b){
  int epsilon = 5;
  int diff = a - b;
  diff = diff < 0 ? (-1)*diff : diff;
  return diff <= epsilon ? true : false;
}
boolean greater(int a, int b){
  int epsilon = 5;
  int diff = a - b;
  if(diff > 0){
    diff = diff - epsilon;
  }
  return diff > 0;
}
void makeDistancesSnapshot(){
  lastDistances.left = currentDistances.left;
  lastDistances.center = currentDistances.center;
  lastDistances.right = currentDistances.right;
}
 
video robot rover 4 roda:

video robot 2 roda: