Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / CoordToPID.cpp @ 98711613

History | View | Annotate | Download (2.49 KB)

1
#include "nav_lib.h"
2
#include <geometry_msgs/Point.h>
3
#include <math.h>
4
#include "pid_control.h"
5
#include <time.h>
6

    
7
class CoordToPID
8
{
9
public:
10
  CoordToPID();
11
  void mainLoop();
12
  void pointCallback(const geometry_msgs::Point::ConstPtr& p);
13

    
14
private:
15
  MikrokopterControl control;
16
  PID_control pidanglecontrol;
17
  PID_control pidvelcontrol;
18
  ros::Publisher pub;
19
  ros::NodeHandle n;
20
  ros::Subscriber sub;
21
  int goaldistance;
22
};
23

    
24
CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1)
25
{
26
  //pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
27
  sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
28
      &CoordToPID::pointCallback, this);
29
 
30
  //Sets Goal values for the PID        
31
  //Goal angle is 0 degrees
32
  float p = 0.4;
33
  float d = 0.01;
34
  float i = 0.4;
35
  PID_control pidanglecontrol = PID_control(p,d,i,0);
36
  
37
  //pdi values for velocity        
38
  p = 0.4;
39
  d = 0.01;
40
  i = 0.4;
41
  //Target distance to stay from target
42
  goaldistance = 100; //What units this in?
43
  PID_control pidvelcontrol = PID_control(p,d,i,goaldistance);
44
}
45

    
46
void CoordToPID::mainLoop()
47
{
48
  /*ros::Rate loop_rate(25);
49
  while(ros::ok())
50
  {
51
    ros::spinOnce();
52
    control.publish_on(pub);
53
    loop_rate.sleep();
54
  }*/
55
  control.main_loop();
56
}
57

    
58
void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p)
59
{
60
          //call on Priya's PID
61
        /* Initializes the PID controller 
62
        PID_control(int p_term, int d_term, int i_term, float goal); 
63

64
        Initializes the PID controller without goal term 
65
        PID_control(int p_term, int d_term, int i_term); 
66

67
        Reset the I term error and change the goal value 
68
        void change_goal(float goal);
69

70
        Given an input, get the PID controlled output 
71
        float pid(float input)*/
72

    
73
        // angle going CCW from forward in horizontal plane
74
          float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct
75
          // should be from [-100, 100]
76
        float distance = sqrt(pow(p->x,2)+pow(p->y,2));
77
        //Updates pid with current value and gets pid output
78
        float pidangle = pidanglecontrol.pid(angle,(double)time(NULL));
79
        float pidvelocity = pidvelcontrol.pid(distance,(double)time(NULL));
80
    //ROS_INFO("angle %g, pidangle %g, distance %g, piddistance %g",angle,pidangle,distance,pidvelocity);        
81
        
82
    //sets pid output
83
        control.set_yaw(pidangle*100/M_PI);
84
        control.velocity_control(pidvelocity,0);
85
        //controls.set_thrust() //between [0,1]
86
}
87

    
88
int main(int argc, char **argv) {
89
  ros::init(argc, argv, "CoordToPID");
90
  CoordToPID ctp = CoordToPID();
91
  ctp.mainLoop();
92
  return 0;
93
}