Quadcopter with IR sensors (Obstacles avoidance)

[chong012]

Hi all,

I'm currently working on a project on quadcopter with obstacles avoidance capability, I am using a KK2.0 flight controller, Arduino Uno board and sharp infrared sensor (Kingduino GP2D12).

The IR sensor measures the distance between the sensor itself and the object and sends it to the arduino in the form of analog signal (value range:  0 to 1023). We should make a setpoint say 30cm from the IR sensor so if the setpoint reaches below 30cm the PID algorithm in the arduino will start its work by sending out the correction value to the stabilization board (kk2.0).

I came across a similar project done by cyril.anthony777 online but have some queries on the codes he shared. The following is the code,

#include<Servo.h>
Servo aileron,elevator;
int setpoint=60,Kp=3;

void setup()
{
  Serial.begin(9600);
  aileron.attach(9);
  elevator.attach(10);
}
void loop()
{
  setpoint=map(setpoint,20,150,0,1023);
  int left,right,front,back;
  int d1,d2,d3,d4;
  int temp,temp1,temp2,temp3;
  int e1,e2,e3,e4;
  int op1,op2,op3,op4,opx,opy;
  temp=0;
  temp1=0;
  temp2=0;
  temp3=0;
 
  for(int i=0;i<10;i++)
  {
    left = analogRead(A0);
    delay(1);
    temp=temp+left;
  }
  d1=temp/10;
 
  for(int i=0;i<10;i++)
  {
    right = analogRead(A1);
    delay(1);
    temp1=temp1+right;
  }
  d2=temp1/10;
 
  for(int i=0;i<10;i++)
  {
    front = analogRead(A2);
    delay(1);
    temp2=temp2+front;
  }
  d3=temp2/10;
 
  for(int i=0;i<10;i++)
  {
    back = analogRead(A3);
    delay(1);
    temp3=temp3+back;
  }
  d4=temp3/10;
 
  e1=d1-setpoint;
  e2=d2-setpoint;
  e3=d3-setpoint;
  e4=d4-setpoint;
 
  op1=e1*Kp;
  op2=e2*Kp;
  op3=e3*Kp;
  op4=e4*Kp;
 
  opx=1500-op1+op2;
  opy=1500-op3+op4;
 
  aileron.writeMicroseconds(opx);
  elevator.writeMicroseconds(opy);
 
}
 
Question1: How is the Setpoint value of 60 and its Min of 20 and Max of 150 obtained?
Question2: Is Kp refering to the present error (Tuning Parameter) and how is it derived?
Question3: What is this op1=e1*Kp? why is the e1 value (difference between setpoint and actual distance) multiplied by Kp?
Question4: And lastly opx=1500-op1+op2, what is is about? I understand that the aileron (Roll) control is affected by the left and right pairs of motors and elevator (pitch) control is affected by the front and rear pairs of motors. Is the 1500 refering to the 1500us which is the center or neutral value for roll pitch and yaw?

It would be great if any guru out there can help me with this, thanks in advance! Peace

[PaulS]

Question1: How is the Setpoint value of 60 and its Min of 20 and Max of 150 obtained?

int setpoint=60;

The min and max are hardcoded in the map statement.

If your question is why were those values picked out of thin air, you'd have to address it to the code author. I presume that the author felt that they were reasonable values.

Question2: Is Kp refering to the present error (Tuning Parameter) and how is it derived?

Kp is a constant. I don't understand the question.

Question3: What is this op1=e1*Kp? why is the e1 value (difference between setpoint and actual distance) multiplied by Kp?

That is a line of code. The program is scaling the error by a constant.

Question4: And lastly opx=1500-op1+op2, what is is about?

Presumably, the author tested his/her servos, and determined that 1500 was the value that made the servo spin at maximum speed in one direction. The actual value to apply to the servo is based on how far away from the desired speed the servo is currently running. The desired speed is based on where the obstacle is. The desire is to turn, so one side needs to speed up while the other slows down.

[chong012]

After abit more of research....

As for question 1 - I'm thinking that according to his project, his uses a GP2Y0A21YK IR Sensors. Based on his mapping code, I'm assuming the minimum detection range of the sensor is 20cm and maximum range is 150cm. The setpoint is declared as 60 (cm) because he intend to make a setpoint of 40cm from the IR sensor so if the setpoint reaches below 40cm the PID algorithm in the arduino will start its work by sending out the correction value to the stabilization board (kk2.0 flight controller).

Question 2 - The Kp should be the P which is in the PID equation and accounts for present values of the error. PID equation: http://brettbeauregard.com/blog/wp-content/uploads/2011/03/pidalgorithm.png

Question 3 - Something to do with the PID equation as shown above. Not sure why only Kp is accounted for while Ki and Kd are not.

Question 4 - Found that in RC universe, normal range is 1000us to 2000us and 1500 us is the center or neutral value for roll pitch and yaw.