Quadrotor

#include "nav_lib.h"

#include <geometry_msgs/Point.h>

#include <math.h>

#include "pid_control.h"

#include <time.h>

class CoordToPID

{

public:

  CoordToPID();

  void mainLoop();

  void pointCallback(const geometry_msgs::Point::ConstPtr& p);

private:

  MikrokopterControl control;

  PID_control pidanglecontrol;

  PID_control pidvelcontrol;

  ros::Publisher pub;

  ros::NodeHandle n;

  ros::Subscriber sub;

  int goaldistance;

};

CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)

{

  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);

  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,

      &CoordToPID::pointCallback, this);

  //Sets Goal values for the PID        

  //Goal angle is 0 degrees

  float p = 0.4;

  float d = 0.01;

  float i = 0.4;

  PID_control pidanglecontrol = PID_control(p,d,i,0);

  //pdi values for velocity        

  p = 0.4;

  d = 0.01;

  i = 0.4;

  //Target distance to stay from target

  goaldistance = 100; //What units this in?

  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);

}

void CoordToPID::mainLoop()

{

  /*ros::Rate loop_rate(25);

  while(ros::ok())

  {

    ros::spinOnce();

    control.publish_on(pub);

    loop_rate.sleep();

  }*/

  control.main_loop();

}

void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)

{

          //call on Priya's PID

        /* Initializes the PID controller 

        PID_control(int p_term, int d_term, int i_term, float goal); 

        Initializes the PID controller without goal term 

        PID_control(int p_term, int d_term, int i_term); 

        Reset the I term error and change the goal value 

        void change_goal(float goal);

        Given an input, get the PID controlled output 

        float pid(float input)*/

        // angle going CCW from forward in horizontal plane

          float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct

          // should be from [-100, 100]

        float distance = sqrt(pow(p->x,2)+pow(p->y,2));

        //Updates pid with current value and gets pid output

        float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));

        float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));

    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);        

    //sets pid output

        control.set_yaw(pidangle*100/M_PI);

        control.velocity_control(pidvelocity,0);

        //controls.set_thrust() //between [0,1]

}

int main(int argc, char **argv) {

  ros::init(argc, argv, "CoordToPID");

  CoordToPID ctp = CoordToPID();

  ctp.mainLoop();

  return 0;

}