APM2.5 + ADK + Ultrasonic Sensor Autonomous x Quadcopter

Projects Discussion and Showcase Robotics
#1

Hi,
im on quest to build an autonomous quadcopter . I have an APM2.5 and a separate Arduino mega .I have connected the input pins to my arduino and few ultrasonic sensors on the sides of my quadcopter body and i was wondering whether this code will do what it supposed to do or is the any bug which will cause my quadcopter to crash :fearful: the main aim is Obstacle avoidance . i have connected the throttle and aux of my receiver directly to my APM due to safety reasons
and here is my code
(still unfinished i have one it only for roll)

//*********** INCLUDES ************//
#include <Wire.h>
#include<Servo.h>
#include <PID_v1.h>
#include <NewPing.h>
//*********** DEFINITIONS ************//
#define Kp 2
#define Ki 0.6
#define Kd 0.4
//*********** VARIABLES *************//

Servo throttle,aleron,elevator,rudder,aux;
double Setpoint, Input, Output, Input1, Output1;
double Input2, Output2, Input3, Output3;
PID leftPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT);
PID rightPID(&Input1, &Output1, &Setpoint,Kp,Ki,Kd, DIRECT);
PID upPID(&Input2, &Output2, &Setpoint,Kp,Ki,Kd, DIRECT);
PID downPID(&Input3, &Output3, &Setpoint,Kp,Ki,Kd, DIRECT);
int ch1,ch2,ch3,ch4,ch5;
int left,right,alt,front,back;
NewPing sonarLeft(14, 15, 200);
NewPing sonarRight(16, 17, 200);
NewPing sonarFront(18, 19, 400);
NewPing sonarBack(24, 25, 400);
NewPing sonarAlt(22, 23, 400);

//*************** USER DEFINED FUNCTIONS***************//
void althold()
{  
}
void rollLeft()
{

}
void rollRight()
{

}
void pitchUp(int c)
{
}
void pitchDown(int d)
{
}
void yawLeft(int e)
{
}
void yawRight(int f)
{
}
void Equalise()
{
}
int pingLeft()
{
  delay(50);
  unsigned int us=sonarLeft.ping();
  left=us/US_ROUNDTRIP_CM;
  return left;
 }
int pingRight()
{
  delay(50);
  unsigned int us=sonarLeft.ping();
  right=us/US_ROUNDTRIP_CM;
  return right;
}
int pingFront()
{
  delay(50);
  unsigned int us=sonarLeft.ping();
  front=us/US_ROUNDTRIP_CM;
  return front;
}
int pingBack()
{
  delay(50);
  unsigned int us=sonarLeft.ping();
  back=us/US_ROUNDTRIP_CM;
  return back;
}
int pingAlt()
{
  delay(50);
  unsigned int us=sonarLeft.ping();
  alt=us/US_ROUNDTRIP_CM;
  return alt;
}

//********** MAIN LOOPS **************//
void setup()
{
  aleron.attach(9);
  elevator.attach(10);
  throttle.attach(0);
  rudder.attach(12);
  aux.attach(13);
  Serial.begin(9600);
  Wire.begin(4);
  pingLeft();
  pingRight();
  Input=left;
  Input1=right;
  Setpoint=30;
  leftPID.SetOutputLimits(0, 300);
  leftPID.SetMode(AUTOMATIC);
  rightPID.SetOutputLimits(0, 300);
  rightPID.SetMode(AUTOMATIC);
}

void loop()
{
  ch1=pulseIn(2,HIGH,25000);
  ch2=pulseIn(3,HIGH,25000);
  ch3=pulseIn(4,HIGH,25000);
  ch4=pulseIn(5,HIGH,25000);
  ch5=pulseIn(6,HIGH,25000);
  ch5=map(ch5,1000,2031,0,147);
  if(ch5>=140) // this is for calibrating the radio on apm2.5.
  {
    aleron.writeMicroseconds(1000);
    elevator.writeMicroseconds(1000);
    rudder.writeMicroseconds(1000);
    delay(2000);
    aleron.writeMicroseconds(2000);
    elevator.writeMicroseconds(2000);
    rudder.writeMicroseconds(2000);
    delay(2000);
    aleron.writeMicroseconds(1500);
    elevator.writeMicroseconds(1500);
    rudder.writeMicroseconds(1500);
    delay(10000);
  }
  left=pingLeft();
  Serial.println(left);
  right=pingRight();
  if((left<=Setpoint)&&(left!=0))
  {
     int op2;
  Input=left;
 leftPID.Compute();
 op2=1500+Output;
 aleron.writeMicroseconds(op2);
  }
  if((right<=Setpoint)&&(right!=0))
  {
 int op1;
 Input1=right;
 rightPID.Compute();
 op1=1500-Output1;
 aleron.writeMicroseconds(op1);
 Serial.println(op1);
  } 
 else
{ 
  aleron.writeMicroseconds(1500);
  elevator.writeMicroseconds(1500);
  throttle.writeMicroseconds(ch3);
  rudder.writeMicroseconds(1500);
  aux.writeMicroseconds(ch5);
}
 }

Thanks.

#2