Revision fd29b28e
| ID | fd29b28eb2f84596cc133172863b245968088898 |
Tested velocity control and added yaw and throttle control to joystick.
| mikrokopter/src/joystick_control.cpp | ||
|---|---|---|
| 46 | 46 |
|
| 47 | 47 |
void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) |
| 48 | 48 |
{
|
| 49 |
// TODO call control.velocity_control(...) based on Joy message |
|
| 49 |
control.velocity_control(joy->axes[1], -joy->axes[0]); |
|
| 50 |
control.set_yaw(-joy->axes[2]); |
|
| 51 |
control.set_thrust((joy->axes[3]+1)/2); ///Adjust thrust to be between [0, 1] instead of [-1, 1] |
|
| 52 |
return; |
|
| 50 | 53 |
} |
| 51 | 54 |
|
| 52 | 55 |
int main(int argc, char** argv) |
| mikrokopter/src/nav_lib.cpp | ||
|---|---|---|
| 16 | 16 |
control.config = 1; |
| 17 | 17 |
} |
| 18 | 18 |
|
| 19 |
void MikrokopterControl::velocity_control(int forward_speed, int lateral_speed)
|
|
| 19 |
void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed)
|
|
| 20 | 20 |
{
|
| 21 |
int k = 1; // TODO set scaling constant and check signs
|
|
| 21 |
int k = 50; // TODO set scaling constant and check signs
|
|
| 22 | 22 |
control.roll = k*(forward_speed - lateral_speed); |
| 23 | 23 |
control.pitch = k*(forward_speed + lateral_speed); |
| 24 | 24 |
} |
| ... | ... | |
| 53 | 53 |
control.pitch = 0; |
| 54 | 54 |
} |
| 55 | 55 |
|
| 56 |
void MikrokopterControl::set_thrust(int thrust)
|
|
| 56 |
void MikrokopterControl::set_thrust(float thrust)
|
|
| 57 | 57 |
{
|
| 58 |
control.thrust = thrust; |
|
| 58 |
int k = 100; |
|
| 59 |
control.thrust = k*thrust; |
|
| 60 |
} |
|
| 61 |
|
|
| 62 |
void MikrokopterControl::set_yaw(float yaw) |
|
| 63 |
{
|
|
| 64 |
int k = 50; |
|
| 65 |
control.yaw = k*yaw; |
|
| 59 | 66 |
} |
| 60 | 67 |
|
| 61 | 68 |
void MikrokopterControl::publish_on(ros::Publisher& pub) |
| mikrokopter/src/nav_lib.h | ||
|---|---|---|
| 4 | 4 |
class MikrokopterControl {
|
| 5 | 5 |
public: |
| 6 | 6 |
MikrokopterControl(); |
| 7 |
void velocity_control(int forward_speed, int lateral_speed);
|
|
| 7 |
void velocity_control(float forward_speed, float lateral_speed);
|
|
| 8 | 8 |
void forward(); |
| 9 | 9 |
void backward(); |
| 10 | 10 |
void left(); |
| 11 | 11 |
void right(); |
| 12 | 12 |
void level(); |
| 13 |
void set_thrust(int thrust); |
|
| 13 |
void set_thrust(float thrust); |
|
| 14 |
void set_yaw(float yaw); |
|
| 14 | 15 |
void publish_on(ros::Publisher& pub); |
| 15 | 16 |
private: |
| 16 | 17 |
mikrokopter::Control control; |
Also available in: Unified diff