Project

General

Profile

Revision 393f899e

ID393f899ec5d0d9d61332ba9df2a0bb2ff83d4615

Added by Nicolas over 7 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