Project

General

Profile

Revision da889457

IDda889457e79e3e7e2d37b071dffdef00b81595f3

Added by Thomas Mullins over 12 years ago

Created nav_lib.cpp/h, with functions forward, backward, left, right,
and stop to set the mikrokopter's movement direction.
Replaced code in wasd_nav.cpp's main with forward(pub) and stop(pub)
from nav_lib.cpp.
Added nav_lib.cpp to CMakeLists.txt

View differences:

mikrokopter/CMakeLists.txt
26 26
target_link_libraries(${PROJECT_NAME} serial)
27 27

  
28 28
rosbuild_add_executable(externctrl_client src/externctrl_client.cpp)
29
rosbuild_add_executable(wasd_nav src/wasd_nav.cpp)
29
rosbuild_add_executable(wasd_nav src/wasd_nav.cpp src/nav_lib.cpp)
mikrokopter/src/nav_lib.cpp
1
#include "nav_lib.h"
2

  
3
void set_roll_pitch(ros::Publisher& pub, int roll, int pitch) {
4
    mikrokopter::Control req;
5
    req.digital[0] = 0;
6
    req.digital[1] = 0;
7
    req.remoteKey = 0;
8
    req.pitch = roll;
9
    req.roll = pitch;
10
    req.yaw = 0;
11
    req.thrust = 40;
12
    req.height = 0;
13
    req.free = 0;
14
    req.frame = 7;
15
    req.config = 1;
16
    pub.publish(req);
17
    ros::spinOnce();
18
}
19

  
20

  
21
void forward(ros::Publisher& pub)
22
{
23
    set_roll_pitch(pub, -15, -15);
24
}
25

  
26
void backward(ros::Publisher& pub)
27
{
28
    set_roll_pitch(pub, 15, 15);
29
}
30

  
31
void left(ros::Publisher& pub)
32
{
33
    set_roll_pitch(pub, -15, 15);
34
}
35

  
36
void right(ros::Publisher& pub)
37
{
38
    set_roll_pitch(pub, 15, -15);
39
}
40

  
41
void stop(ros::Publisher& pub)
42
{
43
    set_roll_pitch(pub, 0, 0);
44
}
mikrokopter/src/nav_lib.h
1
#include "ros/ros.h"
2
#include "mikrokopter/Control.h"
3

  
4
void forward(ros::Publisher& pub);
5
void backward(ros::Publisher& pub);
6
void left(ros::Publisher& pub);
7
void right(ros::Publisher& pub);
8
void stop(ros::Publisher& pub);
mikrokopter/src/wasd_nav.cpp
1 1
#include "ros/ros.h"
2 2
#include "mikrokopter/Control.h"
3
#include "nav_lib.h"
3 4
#include <cstdlib>
4 5

  
5 6

  
......
9 10

  
10 11
    ros::NodeHandle n;
11 12
    ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
12
    mikrokopter::Control req;
13
    req.digital[0] = 0;
14
    req.digital[1] = 0;
15
    req.remoteKey = 0;
16
    req.pitch = 0;
17
    req.roll = 0;
18
    req.yaw = 0;
19
    req.thrust = 40;
20
    req.height = 0;
21
    req.free = 0;
22
    req.frame = 7;
23
    req.config = 1;
24 13

  
25 14
    ros::Rate loop_rate(25);
26 15
    
27 16
    printf("moving\n");
28

  
29
    req.roll = -15;
30
    req.pitch = -15; /* nick is pitch */
31
    pub.publish(req);
32
    ros::spinOnce();
17
    forward(pub);
33 18
    
34 19
    sleep(5);
35 20
    printf("evening out\n");
36
    req.roll = 0;
37
    req.pitch = 0;
38
    pub.publish(req);
39
    ros::spinOnce();
40

  
41
}
42
void forward()
43
{
44
	
45
	
21
    stop(pub);
22
    
23
    return 0;
46 24
}
47

  

Also available in: Unified diff