root / quad2 / mikrokopter / src / CoordToPID.cpp @ 98e35b23
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 
} 