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] |
mikrokopter/src/nav_lib.cpp | ||
---|---|---|
17 | 17 |
|
18 | 18 |
void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed) |
19 | 19 |
{ |
20 |
if(forward_speed > 1) forward_speed = 1; |
|
21 |
if(forward_speed < -1) forward_speed = -1; |
|
22 |
if(lateral_speed > 1) lateral_speed = 1; |
|
23 |
if(lateral_speed < -1) lateral_speed = -1; |
|
20 | 24 |
int k = 50; // TODO set scaling constant and check signs |
21 | 25 |
control.roll = k*(forward_speed - lateral_speed); |
22 | 26 |
control.pitch = k*(forward_speed + lateral_speed); |
... | ... | |
60 | 64 |
|
61 | 65 |
void MikrokopterControl::set_yaw(float yaw) |
62 | 66 |
{ |
63 |
int k = 50; |
|
67 |
if(yaw>1) yaw = 1; |
|
68 |
if(yaw<-1) yaw = -1; |
|
69 |
int k = 50;; |
|
64 | 70 |
control.yaw = k*yaw; |
65 | 71 |
} |
66 | 72 |
|
Also available in: Unified diff