root / mikrokopter / mikrokopter / src / CoordToPID.cpp @ 98711613
History | View | Annotate | Download (2.49 KB)
1 | 393f899e | Nicolas | #include "nav_lib.h" |
---|---|---|---|
2 | #include <geometry_msgs/Point.h> |
||
3 | #include <math.h> |
||
4 | 98711613 | Tom Mullins | #include "pid_control.h" |
5 | 84a809d3 | Nicolas | #include <time.h> |
6 | 393f899e | Nicolas | |
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 | 84a809d3 | Nicolas | CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1) |
25 | 393f899e | Nicolas | { |
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 | 84a809d3 | Nicolas | 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 | 393f899e | Nicolas | 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 | } |