Project

General

Profile

Revision 393f899e

ID393f899ec5d0d9d61332ba9df2a0bb2ff83d4615

Added by Nicolas about 12 years ago

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.

View differences:

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