Collision Avoidance System

Collision Avoidance System

Code

// Designed by RoboCircuits ... Its not complete yet... use at your own risk

#include<Servo.h>
#include<NewPing.h>

NewPing front(6,7,300); // first ultrasonic sensor pin - d6 and d7
NewPing right(3,2,300); // second us
NewPing back(14,15,300); // third us
NewPing left(5,4,300); // fourth us

Servo out1, out2;

unsigned long counter_3, counter_4, current_count;

//We create 4 variables to stopre the previous value of the input signal (if LOW or HIGH)
byte last_CH3_state, last_CH4_state;

//To store the 1000us to 2000us value we create variables and store each channel
int input_YAW;      //In my case channel 4 of the receiver and pin D12 of arduino
int input_PITCH;    //In my case channel 2 of the receiver and pin D10 of arduino


void setup() {
  out1.attach(8); // output to flight controller
  out2.attach(9);
  
  PCICR |= (1 << PCIE0);    //enable PCMSK0 scan                                                                                              
  PCMSK0 |= (1 << PCINT2);  //Set pin D10 trigger an interrupt on state change.                                               
  PCMSK0 |= (1 << PCINT4);  //Set pin D12 trigger an interrupt on state change.  
                                                 
                                          
  }

ISR(PCINT0_vect){
//First we take the current count value in micro seconds using the micros() function
  
  current_count = micros();

  ///////////////////////////////////////Channel 3
  if(PINB & B00000100 ){                             //pin D10 - B00000100                                         
    if(last_CH3_state == 0){                                             
      last_CH3_state = 1;                                                  
      counter_3 = current_count;                                               
    }
  }
  else if(last_CH3_state == 1){                                             
    last_CH3_state = 0;                                                    
    input_PITCH = current_count - counter_3;                            

  }
  
  ///////////////////////////////////////Channel 4
  if(PINB & B00010000 ){                             //pin D12  -- B00010000                      
    if(last_CH4_state == 0){                                               
      last_CH4_state = 1;                                                   
      counter_4 = current_count;                                              
    }
  }
  else if(last_CH4_state == 1){                                             
    last_CH4_state = 0;                                                  
    input_YAW = current_count - counter_4;                            
  }
}

void loop()
{
  
  if(front.ping_cm() < 50 && front.ping_cm() > 3)
  {
    digitalWrite(13,HIGH);
    out1.writeMicroseconds(input_PITCH);
    out2.writeMicroseconds(1650);
    delay(100);
    }
  else if(right.ping_cm() < 50 && right.ping_cm() > 3)
  {
    digitalWrite(13,HIGH);
    out1.writeMicroseconds(1350);
    out2.writeMicroseconds(input_YAW);
    delay(100);
  }
  else if(back.ping_cm() < 50 && back.ping_cm() > 3)
  {
    digitalWrite(13,HIGH);
    out1.writeMicroseconds(input_PITCH);
    out2.writeMicroseconds(1350);
    delay(100);
  }
  else if(left.ping_cm() < 50 && left.ping_cm() > 3)
  {
    digitalWrite(13,HIGH);
    out1.writeMicroseconds(1650);
    out2.writeMicroseconds(input_YAW);
    delay(100); 
  }
  else
  {
   out1.writeMicroseconds(input_PITCH);
    out2.writeMicroseconds(input_YAW);
    digitalWrite(13,LOW);
    delay(10);
  }
}

Leave a Reply