gunvarrel 0.0.5

gunvarrel 0.0.5.c

/*
 * Gunvarrel v 0.0.5
 * Purpose: Sonar Robot with H-Bridge for smoother rotation
 * Description: The robot will move to avoid obstacles
 * Author: Go Frendi Gunawan
 */
 
 #include<NewPing.h>

int left_front          = 6;                // The ports
int left_back           = 5;
int right_front         = 9;
int right_back          = 10;
int sonar_trig          = 12;   
int sonar_echo          = 13;   

int min_turn_distance   = 10;               // minimum distance to turn
int max_turn_distance   = 20;               // maximum distance to turn
int velocity_voltage    = 255;              // Signal needed to make the motors spin
int ninety_degree_time  = 1000;             // How long should we wait to make the robot spin 90 degree

float distance          = 0;

NewPing sonar(sonar_trig, sonar_echo, 300);

void setup(){
  Serial.begin(9600);
  pinMode(left_front,   OUTPUT);
  pinMode(right_front,  OUTPUT);
  pinMode(left_back,    OUTPUT);
  pinMode(right_back,   OUTPUT);
  pinMode(sonar_trig,   OUTPUT);
  pinMode(sonar_echo,   INPUT );
  stop_movement();
}

void loop(){   
  delay(100);                                           // wait for previous ping to be cleared
  distance = sonar.ping_cm();
  
  Serial.println(distance);  
  
  if(distance > max_turn_distance || distance == 0){    // no obstacle, go on
    go_straight();
  }else if(distance < 10){                              // there is obstacle, but it is too late to turn, stop
    stop_movement();
  }else{                                                // there is obstacle, the robot should turn
    turn_left();                                        // turn left and wait for while
    delay(ninety_degree_time);
    stop_movement();
    delay(100);                                         // clear the previous ping, and measure distance
    distance = sonar.ping_cm();
    if(distance >= min_turn_distance && distance <= max_turn_distance){ // there is also obstacle on the left
      turn_right();                                     // turn right and wait fo a while
      delay(2 * ninety_degree_time);
      stop_movement();
      delay(100);                                       // clear the previous ping, and measure distance
      distance = sonar.ping_cm();
      if(distance >= min_turn_distance && distance <= max_turn_distance){ // there is also obstacle on the right
        turn_right();                                   // turn around
        delay(ninety_degree_time);
        stop_movement();                                // loop again :)
      }
    }
  } 
  
}

void go_straight(){
  stop_movement();
  analogWrite(left_front,  velocity_voltage);
  analogWrite(right_front, velocity_voltage);
}

void turn_right(){
  stop_movement();
  analogWrite(left_front,  velocity_voltage);
  analogWrite(right_back,  velocity_voltage);
}

void turn_left(){
  stop_movement();
  analogWrite(left_back,   velocity_voltage);
  analogWrite(right_front, velocity_voltage);
}

void stop_movement(){
  analogWrite(left_front,  LOW);
  analogWrite(right_front, LOW);
  analogWrite(left_back,   LOW);
  analogWrite(right_back,  LOW);
}