Collision Avoidance System
Code
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 |
// 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); } } |