root / mikrokopter / src / CoordToPID.cpp @ 84a809d3
History | View | Annotate | Download (2.5 KB)
1 |
#include "nav_lib.h" |
---|---|
2 |
#include <geometry_msgs/Point.h> |
3 |
#include <math.h> |
4 |
#include "../../control/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 |
} |