Revision 393f899e
ID | 393f899ec5d0d9d61332ba9df2a0bb2ff83d4615 |
Created file CoordToPID. It takes the simulates coordinates from Tom's simulator and plugs them into Priya's pid to come up with mikrokopter controls.
mikrokopter/CMakeLists.txt | ||
---|---|---|
29 | 29 |
rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp) |
30 | 30 |
target_link_libraries(keyboard_control ncurses) |
31 | 31 |
rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp) |
32 |
rosbuild_add_executable(turn_to_target src/turn_to_target.cpp src/nav_lib.cpp)
|
|
32 |
rosbuild_add_executable(CoordToPID src/CoordToPID.cpp src/nav_lib.cpp ../control/pid_control.cpp)
|
|
33 | 33 |
rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp) |
mikrokopter/src/CoordToPID.cpp | ||
---|---|---|
1 |
#include "nav_lib.h" |
|
2 |
#include <geometry_msgs/Point.h> |
|
3 |
#include <math.h> |
|
4 |
#include "../../control/pid_control.h" |
|
5 |
|
|
6 |
class CoordToPID |
|
7 |
{ |
|
8 |
public: |
|
9 |
CoordToPID(); |
|
10 |
void mainLoop(); |
|
11 |
void pointCallback(const geometry_msgs::Point::ConstPtr& p); |
|
12 |
|
|
13 |
private: |
|
14 |
MikrokopterControl control; |
|
15 |
PID_control pidanglecontrol; |
|
16 |
PID_control pidvelcontrol; |
|
17 |
ros::Publisher pub; |
|
18 |
ros::NodeHandle n; |
|
19 |
ros::Subscriber sub; |
|
20 |
int goaldistance; |
|
21 |
}; |
|
22 |
|
|
23 |
CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,100) |
|
24 |
{ |
|
25 |
//pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100); |
|
26 |
sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100, |
|
27 |
&CoordToPID::pointCallback, this); |
|
28 |
|
|
29 |
//Sets Goal values for the PID |
|
30 |
//Goal angle is 0 degrees |
|
31 |
float p = 0.4; |
|
32 |
float d = 0.01; |
|
33 |
float i = 0.4; |
|
34 |
PID_control pidanglecontrol = PID_control(p,d,i,0); |
|
35 |
|
|
36 |
//pdi values for velocity |
|
37 |
p = 0.4; |
|
38 |
d = 0.01; |
|
39 |
i = 0.4; |
|
40 |
//Target distance to stay from target |
|
41 |
goaldistance = 100; //What units this in? |
|
42 |
PID_control pidvelcontrol = PID_control(p,d,i,goaldistance); |
|
43 |
} |
|
44 |
|
|
45 |
void CoordToPID::mainLoop() |
|
46 |
{ |
|
47 |
/*ros::Rate loop_rate(25); |
|
48 |
while(ros::ok()) |
|
49 |
{ |
|
50 |
ros::spinOnce(); |
|
51 |
control.publish_on(pub); |
|
52 |
loop_rate.sleep(); |
|
53 |
}*/ |
|
54 |
control.main_loop(); |
|
55 |
} |
|
56 |
|
|
57 |
void CoordToPID::pointCallback(const geometry_msgs::Point::ConstPtr& p) |
|
58 |
{ |
|
59 |
//call on Priya's PID |
|
60 |
/* Initializes the PID controller |
|
61 |
PID_control(int p_term, int d_term, int i_term, float goal); |
|
62 |
|
|
63 |
Initializes the PID controller without goal term |
|
64 |
PID_control(int p_term, int d_term, int i_term); |
|
65 |
|
|
66 |
Reset the I term error and change the goal value |
|
67 |
void change_goal(float goal); |
|
68 |
|
|
69 |
Given an input, get the PID controlled output |
|
70 |
float pid(float input)*/ |
|
71 |
|
|
72 |
// angle going CCW from forward in horizontal plane |
|
73 |
float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct |
|
74 |
// should be from [-100, 100] |
|
75 |
float distance = sqrt(pow(p->x,2)+pow(p->y,2)); |
|
76 |
|
|
77 |
//Updates pid with current value and gets pid output |
|
78 |
float pidangle = pidanglecontrol.pid(angle); |
|
79 |
float pidvelocity = pidvelcontrol.pid(distance); |
|
80 |
|
|
81 |
//sets pid output |
|
82 |
control.set_yaw(pidangle*100/M_PI); |
|
83 |
control.velocity_control(pidvelocity,0); |
|
84 |
//controls.set_thrust() //between [0,1] |
|
85 |
} |
|
86 |
|
|
87 |
int main(int argc, char **argv) { |
|
88 |
ros::init(argc, argv, "CoordToPID"); |
|
89 |
CoordToPID ctp = CoordToPID(); |
|
90 |
ctp.mainLoop(); |
|
91 |
return 0; |
|
92 |
} |
Also available in: Unified diff