Timed event for navigation


I am working on a school project involving the installation of a “sense and avoid” system in a UAV.

So far i have managed to make my plane steer right when it detects an obstacle in front of it.

What i need help with:

1 . I need the plane to keep the ailerons and rudder in a right turn position for 0.3 seconds when an obstacle is detected.

  1. After the 0.3 s have passed i need the plane to fly straight for another 3 seconds.

  2. After the 3 seconds have passed i need the plane to steer left for 0,3 seconds so the flight path is parallel to the first heading.

  3. After the 0,3 seconds of left steering have passed i need the plane to fly straigt for 6 seconds.

  4. After the 6 seconds of parallel to first heading flight i need to make the plane steer left for 0,3 seconds.

6 and fly like that for another 3 seconds.

  1. After the 3 seconds have passed i need the plane to steer right 0,3 seconds so the first heading and last heading coincide,

  2. and fly like that until another obstacle appears.

In the attachment is a graphic explanation.

The code i am using is this:

#include <Servo.h> 
#define GreenLED 2
#define YellowLED 3
#define RedLED 4
#define Buzzer1 5
#define Buzzer2 6

int SafetyDistanceLI = 100;  // Li = inferior limit 
int UncertaintyDistanceLS = 100; // Ls = superior limit
int ActionDistanceLS = 70;
int sound = 1500;
int AileronIdlePositioin = 60 ;
int RudderIdlePosition = 50 ;
int AileronSteeringUp = 20;
int RudderSteeringRight = 40;
int AileronSteeringDown = - 20;  // i need to insert this based on time needs 
int RudderSteeringLeft = - 40;    // same as the above

Servo servoAileron; 
Servo servoRudder;

void setup() {
 pinMode(2, OUTPUT);
 pinMode(3, OUTPUT);
 pinMode(4, OUTPUT);
 pinMode(5, OUTPUT);
 pinMode(6, OUTPUT);

void loop() {
  int measuredDistance = readDistanceMediata(10, 0); // distance measured by IR sensor
  if (measuredDistance > SafetyDistanceLI)
        digitalWrite(GreenLED, HIGH); // The green led lights up if the distance is greater than the minimum safety distance
        digitalWrite(GreenLED, LOW); // the green led will be off only when the distance is lower than the safety distance

  if ((measuredDistance <= UncertaintyDistanceLS) && (measuredDistance > ActionDistanceLS))
        digitalWrite(YellowLED, HIGH); // the yellow led will be on if the measured distance is in the uncertainty distance interval
        tone(Buzzer2, sound-800); // the buzzer starts to buzz
        digitalWrite(YellowLED, LOW); 
        noTone(Buzzer2); // buzzer is of 

    if (measuredDistance <= ActionDistanceLS)  //[b] i need to make this right turn last for 0,3 seconds, then the aircraft to fly with new heading for 3 seconds, then to turn left for 0,3 seconds, to fly another 6 seconds straight, then to turn left for 0,3 seconds and fly like that for 3 seconds, then to turn right for 0,3 seconds and keed flying until measuredDistance <= ActionDistanceLS[/b]
        digitalWrite(RedLED, HIGH); // ledul Rosu se aprinde daca distanta scade sub limita superioara a distantei de actiune
        tone(Buzzer1, sound); // difuzorul incepe sa atentioneze faptul ca distanta a scazut sub limita superioara a distantei de actiune
        servoAileron.write(AileronIdlePosition + AileronPositionUp); // the ailron servo is turning with the AileronPositionUp value
        servoRudder.write(RudderIdlePosition + RudderPositionRight); // the rudder servo is turning with the RudderPositionRight value
        digitalWrite(RedLED, LOW);
        servoAileron.write(AileronIdlePosition); // aileron servo comes back to idle position
        servoRudder.write(RudderIdlePosition); // rudder servo comes back to idle position 

int readDistanceMediata(int count, int pin) 
  int sum = 0;
  for (int i = 0; i<count; i++) 
    float volts = analogRead(pin) * ((float) 5 / 1024);
    float distance = 65 * pow(volts, -1.10); 
    sum = sum + distance;
  return (int) (sum/count);


The code i am using is this:

What does it do that you don't want?
What does it not do that you do want?

You absolutely CANNOT use delay() in that sort of program. Use millis() to manage your timing as illustrated in several things at a time.

What you are trying to do is reasonably complex so I strongly recommend that you break your code into functions as illustrated in planning and implementing a program. Otherwise you will have code that is tangled like a bird's nest in which you cannot distinguish the overall logic from the details of implementation of different actions.

The other important advantage of functions is that they allow you to test the code in small pieces.


What does it not do that you do want?

It does not do the points from 1 to 8.

Thank you both for your help! Thanks Robin2 for the links.