Anonymous Anonymous - 2 months ago
507 0

No description

Other

arduino project

/* Rosie Hyde Coursework*/

const int lSensor = 2; //left ir sensor input
const int rSensor = 5; //right ir sensor input

const int TrigPin = 3; //connects sensor to Arduino digital pin #2
const int EchoPin = 4; //connects sensor to Arduino digital pin #3

const int lMotPos = 9;  //left motor positive
const int lMotNeg = 8;  //left motor negative

const int rMotNeg = 7;  //right motor positive
const int rMotPos = 6; //right motor negative

const int lMotPWM = 10;  //left motor PWM
const int rMotPWM = 11; //right motor PWM

const int paceOfSound = 29.15;  //pace of sound = 1/speed of sound 
                                //              = 1/0.03435 cm/ss
                                //                    n = 29.15 us/cm

int stoppingPoint = 13;  //turn when an obstacle is within 20cm (approx 8in)

int lMotSpeed = 160;  
int rMotSpeed = 125; 

//wait 100msec until next sensor check for object detection .
int sensDelay = 100; 

//wait 50msec until next sensor check for ir.
int sensDelay2 = 50; 

//used only in testing phase
boolean DEBUG = false;    //change to true when debuging. See serial monitor for log


void setup() {
  
  pinMode (lSensor,INPUT);
  pinMode (rSensor,INPUT);

  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  
  pinMode (lMotNeg, OUTPUT);
  pinMode (lMotPos, OUTPUT);
  pinMode (lMotPWM, OUTPUT);
  pinMode (rMotNeg, OUTPUT);
  pinMode (rMotPos, OUTPUT);
  pinMode (lMotPWM, OUTPUT);
  
  if (DEBUG) Serial.begin (115200); 
  
  delay (1000);

}

void loop() {
  // put your main code here, to run repeatedly:

objectDetect();


}

void detectLines() {

  int lSensStat = digitalRead(lSensor);  //LOW = signal detected (white surface): HIGH = black line
  int rSensStat = digitalRead(rSensor);  //LOW = signal detected (white surface): HIGH = black line
  
  if (DEBUG) if (!lSensStat) { Serial.print (digitalRead(4));} else { Serial.print (digitalRead(4));};
  if (DEBUG) if (!rSensStat) { Serial.println (digitalRead(5));} else { Serial.println (digitalRead(5));};
  
  //4 possibilities - go forward, turn right, turn left, or stop)
  if (!lSensStat && !rSensStat) { 
    goForward(); //both sensors detected white floor. go straight
    
   } else if (!lSensStat && rSensStat) {   
      turnLeft(); //detected black floor on left (off), so turn left 
 
   } else if (lSensStat && !rSensStat) { 
      turnRight(); //detected black floor on right (off), so turn right
      
   } else if (lSensStat && rSensStat) { 
        stop(); //detected black floor on both sensor, so stop!
   
   } 

  delay(sensDelay2);               // wait 

}

void objectDetect(){

    int distance;

    distance = getDistance();
  
  if (distance >= stoppingPoint || distance <= 0) {
      detectLines();
  } else {
      if (DEBUG) {
        Serial.println("Alert: Turn left");
      }

        stop();
        delay(1000);
        turnRight();
        delay(2000);
        objectDetect();
  }
  
  delay(sensDelay);
}

int getDistance() {
  
  long duration, distance;
  
  digitalWrite(TrigPin, LOW); 
  delayMicroseconds(2); 

  //send 10 micro second pulse to trigger
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigPin, LOW);

  duration = pulseIn(EchoPin, HIGH);   //wait for HIGH 
                                         
  //calculate the distance in cm
  distance = (duration/2) / paceOfSound; 

  if (distance >= 20 || distance <= -1){
      if (DEBUG) Serial.println("getDistance: Out of range");
      return -1;
  } else {
      if (DEBUG) {
        Serial.print("getDistance: ");
        Serial.print(distance);
        Serial.println(" cm");
      }
      return distance;
  }
}

void goForward ()
{
  //set left motor control parameter
  digitalWrite (lMotNeg, HIGH);
  digitalWrite (lMotPos, LOW);   
  analogWrite(lMotPWM,lMotSpeed);
   
  //set right motor contro parameters
  digitalWrite (rMotNeg, LOW);    
  digitalWrite (rMotPos, HIGH);
  analogWrite(rMotPWM, rMotSpeed);
}

void turnLeft ()
{
 digitalWrite (lMotNeg, LOW);  //left motor reverse
  digitalWrite (lMotPos, HIGH);
  analogWrite(lMotPWM, lMotSpeed);
  
  digitalWrite (rMotNeg, LOW);    //right motor forward
  digitalWrite (rMotPos, HIGH);
  analogWrite(rMotPWM, rMotSpeed);
}

void turnRight ()
{
  digitalWrite (lMotNeg, HIGH);    //left motor forward
  digitalWrite (lMotPos, LOW);
  analogWrite(lMotPWM, lMotSpeed);
  
  digitalWrite (rMotNeg, HIGH);    //right motor reverse
  digitalWrite (rMotPos, LOW);
  analogWrite(rMotPWM, rMotSpeed);
}

void goBackward ()
{
  //set left motor control parameter
  digitalWrite (lMotNeg, LOW);
  digitalWrite (lMotPos, HIGH);   
  analogWrite(lMotPWM,lMotSpeed);
   
  //set right motor contro parameters
  digitalWrite (rMotNeg, HIGH);    
  digitalWrite (rMotPos, LOW);
  analogWrite(rMotPWM, rMotSpeed);
}

void stop() {
  digitalWrite (lMotNeg, LOW);
  digitalWrite (lMotPos, LOW);
  digitalWrite (rMotNeg, LOW);
  digitalWrite (rMotPos, LOW);
}