root / mikrokopter / mikrokopter / src / nav_lib.cpp @ 98711613
History | View | Annotate | Download (1.95 KB)
| 1 | da889457 | Tom Mullins | #include "nav_lib.h" |
|---|---|---|---|
| 2 | |||
| 3 | dd56aeef | Tom Mullins | MikrokopterControl::MikrokopterControl() |
| 4 | 493cca7c | Chris Burchhardt | {
|
| 5 | dd56aeef | Tom Mullins | control.digital[0] = 0; |
| 6 | control.digital[1] = 0; |
||
| 7 | control.remoteKey = 0;
|
||
| 8 | control.pitch = 0;
|
||
| 9 | control.roll = 0;
|
||
| 10 | control.yaw = 0;
|
||
| 11 | control.thrust = 40;
|
||
| 12 | control.height = 0;
|
||
| 13 | control.free = 0;
|
||
| 14 | control.frame = 7;
|
||
| 15 | control.config = 1;
|
||
| 16 | da889457 | Tom Mullins | } |
| 17 | |||
| 18 | fd29b28e | Priya | void MikrokopterControl::velocity_control(float forward_speed, float lateral_speed) |
| 19 | bc4b408e | Tom Mullins | {
|
| 20 | 84a809d3 | Nicolas | 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; |
||
| 24 | fd29b28e | Priya | int k = 50; // TODO set scaling constant and check signs |
| 25 | bc4b408e | Tom Mullins | control.roll = k*(forward_speed - lateral_speed); |
| 26 | control.pitch = k*(forward_speed + lateral_speed); |
||
| 27 | } |
||
| 28 | |||
| 29 | dd56aeef | Tom Mullins | void MikrokopterControl::forward()
|
| 30 | da889457 | Tom Mullins | {
|
| 31 | d58012b5 | Chris Burchhardt | control.roll = 15;
|
| 32 | control.pitch = 15;
|
||
| 33 | } |
||
| 34 | |||
| 35 | void MikrokopterControl::backward()
|
||
| 36 | {
|
||
| 37 | dd56aeef | Tom Mullins | control.roll = -15;
|
| 38 | control.pitch = -15;
|
||
| 39 | da889457 | Tom Mullins | } |
| 40 | |||
| 41 | d58012b5 | Chris Burchhardt | void MikrokopterControl::left()
|
| 42 | da889457 | Tom Mullins | {
|
| 43 | dd56aeef | Tom Mullins | control.roll = 15;
|
| 44 | d58012b5 | Chris Burchhardt | control.pitch = -15;
|
| 45 | da889457 | Tom Mullins | } |
| 46 | |||
| 47 | d58012b5 | Chris Burchhardt | void MikrokopterControl::right()
|
| 48 | da889457 | Tom Mullins | {
|
| 49 | dd56aeef | Tom Mullins | control.roll = -15;
|
| 50 | control.pitch = 15;
|
||
| 51 | da889457 | Tom Mullins | } |
| 52 | |||
| 53 | d58012b5 | Chris Burchhardt | void MikrokopterControl::level()
|
| 54 | da889457 | Tom Mullins | {
|
| 55 | d58012b5 | Chris Burchhardt | control.roll = 0;
|
| 56 | control.pitch = 0;
|
||
| 57 | da889457 | Tom Mullins | } |
| 58 | |||
| 59 | fd29b28e | Priya | void MikrokopterControl::set_thrust(float thrust) |
| 60 | da889457 | Tom Mullins | {
|
| 61 | 737bd480 | Tom Mullins | if (thrust < 0) thrust = 0; |
| 62 | if (thrust > 1) thrust = 1; |
||
| 63 | 08b4f0df | Tom Mullins | int k = 255; |
| 64 | fd29b28e | Priya | control.thrust = k*thrust; |
| 65 | } |
||
| 66 | |||
| 67 | void MikrokopterControl::set_yaw(float yaw) |
||
| 68 | {
|
||
| 69 | 84a809d3 | Nicolas | if(yaw>1) yaw = 1; |
| 70 | if(yaw<-1) yaw = -1; |
||
| 71 | int k = 50;; |
||
| 72 | fd29b28e | Priya | control.yaw = k*yaw; |
| 73 | dd56aeef | Tom Mullins | } |
| 74 | |||
| 75 | void MikrokopterControl::publish_on(ros::Publisher& pub)
|
||
| 76 | {
|
||
| 77 | pub.publish(control); |
||
| 78 | da889457 | Tom Mullins | } |
| 79 | 493cca7c | Chris Burchhardt | |
| 80 | 7a6ed02d | Tom Mullins | void MikrokopterControl::main_loop()
|
| 81 | {
|
||
| 82 | ros::Publisher pub = nh.advertise< ::mikrokopter::Control>( |
||
| 83 | "/mk_wrapper/control", 200); |
||
| 84 | |||
| 85 | int publish_rate;
|
||
| 86 | bc5901d3 | Tom Mullins | ros::param::param<int>("~publish_rate", publish_rate, 20); |
| 87 | 7a6ed02d | Tom Mullins | ros::Rate loop_rate(publish_rate); |
| 88 | |||
| 89 | while (ros::ok())
|
||
| 90 | {
|
||
| 91 | ros::spinOnce(); |
||
| 92 | pub.publish(control); |
||
| 93 | loop_rate.sleep(); |
||
| 94 | } |
||
| 95 | |||
| 96 | } |