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