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