Revision 84a809d3
| ID | 84a809d3edf1be1ee41414e9dd6c9850d8758e62 |
Modified nav_lib to cap input for set_velocity and set_yaw between -1 and 1. Also modified CoordToPID to pass timestamp to PID.
| mikrokopter/src/CoordToPID.cpp | ||
|---|---|---|
| 2 | 2 |
#include <geometry_msgs/Point.h> |
| 3 | 3 |
#include <math.h> |
| 4 | 4 |
#include "../../control/pid_control.h" |
| 5 |
#include <time.h> |
|
| 5 | 6 |
|
| 6 | 7 |
class CoordToPID |
| 7 | 8 |
{
|
| ... | ... | |
| 20 | 21 |
int goaldistance; |
| 21 | 22 |
}; |
| 22 | 23 |
|
| 23 |
CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,100)
|
|
| 24 |
CoordToPID::CoordToPID():pidanglecontrol(0.4,0.01,0.4,0),pidvelcontrol(0.4,0.01,0.4,1) |
|
| 24 | 25 |
{
|
| 25 | 26 |
//pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
|
| 26 | 27 |
sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
|
| ... | ... | |
| 73 | 74 |
float angle = atan2(-p->x, p->z); // TODO not sure if these axes are correct |
| 74 | 75 |
// should be from [-100, 100] |
| 75 | 76 |
float distance = sqrt(pow(p->x,2)+pow(p->y,2)); |
| 76 |
|
|
| 77 | 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 |
|
| 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 |
|
| 82 | 83 |
control.set_yaw(pidangle*100/M_PI); |
| 83 | 84 |
control.velocity_control(pidvelocity,0); |
| 84 | 85 |
//controls.set_thrust() //between [0,1] |
Also available in: Unified diff