AtmoCar 3.0

gistfile1.cpp

//input sensor reading
//make decision based on sensor reading

//do one of below actions:

//to drive straight both wheels move forward at same speed

//to drive reverse both wheels move back at same speed

//to turn left the left wheel moves in reverse and the right wheel moves forward

//to turn right the right wheel moves in reverse and the left wheel moves forward
#include <AFMotor.h>

AF_DCMotor right_motor(3);
AF_DCMotor left_motor(4);
const int pingPin = 2;
const int right_speed_modifier = 0;
const int left_speed_modifier = 0;
int buffer = 60;
int forward_speed = 200;
int backward_speed = 150;

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");

  // turn on motor
  right_motor.setSpeed(200);
 
  right_motor.run(RELEASE);
  
  

}

void loop() {
  
 int cm = get_distance();
 long randNumber = random(2);
 
 //get distance from the ping sensor and compare to the buffer space
 if (cm < buffer) {
   
   
     //if the distance is less then the buffer, turn left
     rotate_left(forward_speed);
     delay(500);
     motor_stop();
     //get distance from the ping sensor and compare to the buffer space again
     if (cm < buffer) {
        //rotate past the orignal angle 
        rotate_right(forward_speed);
        delay(1000);
        motor_stop();
        if (cm < buffer) {
          
          rotate_left(forward_speed);
          delay(500);
          motor_stop();
          
          backward(backward_speed);
          delay(1000);
          motor_stop();
          
          rotate_right(forward_speed);
          delay(500);
          motor_stop();
          
        }
        
     }
     
   
   
 }else{
   
   forward(forward_speed);
   delay(500);
   motor_stop();
 }
  
}

void rotate_right(int motor_speed) {
 int right_speed = motor_speed + right_speed_modifier;
 int left_speed = motor_speed + left_speed_modifier;
 motor_backward(left_motor,left_speed);
 motor_forward(right_motor,right_speed);
  
}

void rotate_left(int motor_speed) {
 int right_speed = motor_speed + right_speed_modifier;
 int left_speed = motor_speed + left_speed_modifier;
 motor_backward(right_motor,right_speed);
 motor_forward(left_motor,left_speed);
  
}

void motor_stop() {
 
 motor_release(right_motor);
 motor_release(left_motor);
 
}

void forward(int motor_speed) {
   int right_speed = motor_speed + right_speed_modifier;
   int left_speed = motor_speed + left_speed_modifier;
   motor_forward(right_motor,right_speed);
   motor_forward(left_motor,left_speed);
   
}

void backward(int motor_speed) {
   int right_speed = motor_speed + right_speed_modifier;
   int left_speed = motor_speed + left_speed_modifier;
   motor_backward(right_motor,right_speed);
   motor_backward(left_motor,left_speed);
  
}

void motor_forward(AF_DCMotor motor,int motor_speed) {
  
  if (motor_speed == -1){
  }else{
   motor.setSpeed(motor_speed); 
  }
  motor.run(FORWARD);

}

void motor_backward(AF_DCMotor motor,int motor_speed) {
  
  if (motor_speed == -1){
  }else{
   motor.setSpeed(motor_speed); 
  }
  
  motor.run(BACKWARD);
  
  

} 

void motor_release(AF_DCMotor motor) {
  
  motor.run(RELEASE);

}

int get_distance() {
 // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
  long duration, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration); 
  return cm;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}