Revision 34a60a3b
Added RungeKutta to the helper classes and changed the odometry to use that instead. Also created PaintboardControl files, changed the Makefile to compile all control classes and deleted Cliffsensor control becase it did not work.
scout/libscout/CMakeLists.txt | ||
---|---|---|
36 | 36 |
FILE(GLOB BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/behaviors/*.cpp") |
37 | 37 |
FILE(GLOB TEST_BEHAVIOR_FILES "${PROJECT_SOURCE_DIR}/src/test_behaviors/*.cpp") |
38 | 38 |
FILE(GLOB HELPER_FILES "${PROJECT_SOURCE_DIR}/src/helper_classes/*.cpp") |
39 |
FILE(GLOB CONTROL_CLASSES "${PROJECT_SOURCE_DIR}/src/*Control.cpp") |
|
40 |
set(WIRELESS_CONTROL_CLASSES src/WirelessSender.cpp src/WirelessReceiver.cpp) |
|
39 | 41 |
|
40 |
set(CONTROL_CLASSES src/MotorControl.cpp src/SonarControl.cpp src/HeadlightControl.cpp src/ButtonControl.cpp src/WirelessSender.cpp src/WirelessReceiver.cpp src/EncodersControl.cpp src/LinesensorControl.cpp src/MetalDetectorControl.cpp) |
|
41 |
|
|
42 |
rosbuild_add_executable(libscout ${MAIN_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES} ${CONTROL_CLASSES} ${HELPER_FILES}) |
|
42 |
rosbuild_add_executable(libscout ${MAIN_FILES} ${CONTROL_CLASSES} ${WIRELESS_CONTROL_CLASSES} ${HELPER_FILES} ${BEHAVIOR_FILES} ${TEST_BEHAVIOR_FILES}) |
|
43 | 43 |
rosbuild_add_compile_flags(libscout -std=c++0x) |
scout/libscout/src/CliffsensorControl.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
**/ |
|
25 |
|
|
26 |
/** |
|
27 |
* @file CliffsensorControl.cpp |
|
28 |
* @brief Contains code to control the cliffsensor. |
|
29 |
* |
|
30 |
* Implementation of functions for cliffsensor use. |
|
31 |
* |
|
32 |
* @author Colony Project, CMU Robotics Club |
|
33 |
* @author Priyanka Deo |
|
34 |
* @author Leon Zhang |
|
35 |
**/ |
|
36 |
|
|
37 |
#include "ros/ros.h" |
|
38 |
#include "CliffsensorControl.h" |
|
39 |
#include <cstdlib> |
|
40 |
|
|
41 |
/** |
|
42 |
* @brief Initializes the Cliffsensor module in libscout. |
|
43 |
* |
|
44 |
* This is the main function for the cliffsensors node. It is run when the node |
|
45 |
* starts and initializes the cliffsensors. It then subscribes to the |
|
46 |
* cliff_status_changed topics |
|
47 |
**/ |
|
48 |
CliffsensorControl::CliffsensorControl(const ros::NodeHandle& libscout_node, |
|
49 |
std::string scoutname) : node(libscout_node) |
|
50 |
{ |
|
51 |
/* Subscribe to the cliff_status_changed topic */ |
|
52 |
cliff_status_changed_sub = node.subscribe("cliff_status_changed", |
|
53 |
QUEUE_SIZE, &CliffsensorControl::changed_cliff_status, this); |
|
54 |
|
|
55 |
ros::spin(); |
|
56 |
} |
|
57 |
|
|
58 |
/** |
|
59 |
* @brief Changes cliff sensor status |
|
60 |
* |
|
61 |
* Changes cliff sensor status based on subscription to topic cliff_status_changed |
|
62 |
* |
|
63 |
* @param msg The message from the cliff_status_changed topic, containing |
|
64 |
* status of all cliff sensors. |
|
65 |
* |
|
66 |
*/ |
|
67 |
void CliffsensorControl::changed_cliff_status(const ::messages::cliff_status_changed::ConstPtr& msg) |
|
68 |
{ |
|
69 |
front_raw = msg->front_raw; |
|
70 |
left_raw = msg->left_raw; |
|
71 |
right_raw = msg->right_raw; |
|
72 |
return; |
|
73 |
} |
|
74 |
|
|
75 |
/** |
|
76 |
* @brief get the current raw value of the front cliffsensor |
|
77 |
* @return the current raw value of the front cliffsensor |
|
78 |
*/ |
|
79 |
int CliffsensorControl::get_front_raw() |
|
80 |
{ |
|
81 |
return front_raw; |
|
82 |
} |
|
83 |
|
|
84 |
/** |
|
85 |
* @brief get the current raw value of the left cliffsensor |
|
86 |
* @return the current raw value of the left cliffsensor |
|
87 |
*/ |
|
88 |
int CliffsensorControl::get_left_raw() |
|
89 |
{ |
|
90 |
return left_raw; |
|
91 |
} |
|
92 |
|
|
93 |
/** |
|
94 |
* @brief get the current raw value of the right cliffsensor |
|
95 |
* @return the current raw value of the right cliffsensor |
|
96 |
*/ |
|
97 |
int CliffsensorControl::get_right_raw() |
|
98 |
{ |
|
99 |
return right_raw; |
|
100 |
} |
|
101 |
|
|
102 |
/** |
|
103 |
* @brief check if a cliff is being detected |
|
104 |
* @return true if there is a cliff, otherwise |
|
105 |
*/ |
|
106 |
bool CliffsensorControl::check_is_cliff() |
|
107 |
{ |
|
108 |
return (front_raw || left_raw || right_raw); |
|
109 |
} |
scout/libscout/src/CliffsensorControl.h | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
**/ |
|
25 |
|
|
26 |
/** |
|
27 |
* @file CliffsensorControl.h |
|
28 |
* @brief Contains cliffsensor declarations and functions. |
|
29 |
* |
|
30 |
* Contains functions and definitions for the use of |
|
31 |
* cliffsensors. |
|
32 |
* |
|
33 |
* @author Colony Project, CMU Robotics Club |
|
34 |
* @author Priyanka Deo |
|
35 |
* @author Leon Zhang |
|
36 |
**/ |
|
37 |
|
|
38 |
#ifndef _CLIFFSENSOR_H_ |
|
39 |
#define _CLIFFSENSOR_H_ |
|
40 |
|
|
41 |
#include <messages/query_cliff.h> |
|
42 |
#include <messages/cliff_status_changed.h> |
|
43 |
#include "constants.h" |
|
44 |
|
|
45 |
#define CLIFF_DETECTED 0x0 |
|
46 |
#define NO_CLIFF 0x1 |
|
47 |
|
|
48 |
class CliffsensorControl |
|
49 |
{ |
|
50 |
public: |
|
51 |
CliffsensorControl(const ros::NodeHandle& libscout_node, std::string scoutname); |
|
52 |
|
|
53 |
/** @brief Responds to topic to change cliff sensor status. **/ |
|
54 |
void changed_cliff_status( |
|
55 |
const messages::cliff_status_changed::ConstPtr& msg); |
|
56 |
|
|
57 |
int get_front_raw(); |
|
58 |
int get_left_raw(); |
|
59 |
int get_right_raw(); |
|
60 |
bool check_is_cliff(); |
|
61 |
|
|
62 |
private: |
|
63 |
/* Cliffsensor state variables |
|
64 |
*/ |
|
65 |
int front_raw; /**< The current raw value |
|
66 |
data of the front cliffsensor. */ |
|
67 |
int left_raw; /**< The current raw value data of |
|
68 |
the left cliffsensor. */ |
|
69 |
int right_raw; /**< The current raw value data of |
|
70 |
the right cliffsensor. */ |
|
71 |
|
|
72 |
ros::Subscriber cliff_status_changed_sub; |
|
73 |
ros::NodeHandle node; |
|
74 |
}; |
|
75 |
|
|
76 |
#endif |
scout/libscout/src/PaintBoardControl.cpp | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
/** |
|
27 |
* @file PaintBoardControl.cpp |
|
28 |
* @brief Contains paintboard declarations and functions |
|
29 |
* |
|
30 |
* Contains functions and definitions for the use of |
|
31 |
* paintboards |
|
32 |
* |
|
33 |
* @author Colony Project, CMU Robotics Club |
|
34 |
* @author Priyanka Deo |
|
35 |
**/ |
|
36 |
|
|
37 |
#include "PaintBoardControl.h" |
|
38 |
|
|
39 |
using namespace std; |
|
40 |
|
|
41 |
/** |
|
42 |
* @brief Initialize the paintboards module of libscout. |
|
43 |
* |
|
44 |
* Initialize the libscout node as a publisher of set_paintboards and a client of |
|
45 |
* query_paintboards. |
|
46 |
*/ |
|
47 |
PaintBoardControl::PaintBoardControl(const ros::NodeHandle& libscout_node, |
|
48 |
string scoutname) |
|
49 |
: node(libscout_node) |
|
50 |
{ |
|
51 |
set_paintboard_pub = |
|
52 |
node.advertise< ::messages::set_paintboard>(scoutname + "/set_paintboard", |
|
53 |
QUEUE_SIZE, true); |
|
54 |
query_paintboard_client = |
|
55 |
node.serviceClient< ::messages::query_paintboard>(scoutname + "/query_paintboard"); |
|
56 |
} |
|
57 |
|
|
58 |
PaintBoardControl::~PaintBoardControl() |
|
59 |
{ |
|
60 |
set(STOP_PAINT); |
|
61 |
} |
|
62 |
|
|
63 |
/** |
|
64 |
* @brief Sets all the speeds according to the specificiation of which paintboard |
|
65 |
* to set. |
|
66 |
* |
|
67 |
* @param which A bitmask of which paintboard need to be set. |
|
68 |
* @param speed The value to set the paintboard to. |
|
69 |
* @param units The units the speed is expressed in. |
|
70 |
* @return Function status |
|
71 |
*/ |
|
72 |
void PaintBoardControl::set(int paintboard_setting) |
|
73 |
{ |
|
74 |
::messages::set_paintboard msg; |
|
75 |
|
|
76 |
msg.setting = paintboard_setting; |
|
77 |
|
|
78 |
set_paintboard_pub.publish(msg); |
|
79 |
ros::spinOnce(); |
|
80 |
} |
|
81 |
|
|
82 |
/** |
|
83 |
* @brief Query the current speeds of the paintboard |
|
84 |
* |
|
85 |
* Sends a request to the query_paintboard service which will reply with the |
|
86 |
* current setting of the paintboard. |
|
87 |
* |
|
88 |
* @param which A bitmask that will specify which motor speed should be |
|
89 |
* returned |
|
90 |
* @return The speed of the selected motor |
|
91 |
*/ |
|
92 |
int PaintBoardControl::query() |
|
93 |
{ |
|
94 |
::messages::query_paintboard srv; |
|
95 |
if(query_paintboard_client.call(srv)) |
|
96 |
{ |
|
97 |
return srv.response.setting; |
|
98 |
} |
|
99 |
else |
|
100 |
{ |
|
101 |
ROS_FATAL("Failed to call service query_paintboard"); |
|
102 |
} |
|
103 |
|
|
104 |
return -1; |
|
105 |
} |
scout/libscout/src/PaintBoardControl.h | ||
---|---|---|
1 |
/** |
|
2 |
* Copyright (c) 2011 Colony Project |
|
3 |
* |
|
4 |
* Permission is hereby granted, free of charge, to any person |
|
5 |
* obtaining a copy of this software and associated documentation |
|
6 |
* files (the "Software"), to deal in the Software without |
|
7 |
* restriction, including without limitation the rights to use, |
|
8 |
* copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
9 |
* copies of the Software, and to permit persons to whom the |
|
10 |
* Software is furnished to do so, subject to the following |
|
11 |
* conditions: |
|
12 |
* |
|
13 |
* The above copyright notice and this permission notice shall be |
|
14 |
* included in all copies or substantial portions of the Software. |
|
15 |
* |
|
16 |
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
17 |
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
18 |
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
19 |
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
20 |
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
21 |
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
22 |
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
23 |
* OTHER DEALINGS IN THE SOFTWARE. |
|
24 |
*/ |
|
25 |
|
|
26 |
/** |
|
27 |
* @file PaintBoardControl.h |
|
28 |
* @brief Contains motor declarations and functions |
|
29 |
* |
|
30 |
* Contains functions and definitions for the use of |
|
31 |
* motors |
|
32 |
* |
|
33 |
* @author Colony Project, CMU Robotics Club |
|
34 |
* @author Priyanka Deo |
|
35 |
**/ |
|
36 |
|
|
37 |
#ifndef _PAINT_BOARD_CONTROL_H_ |
|
38 |
#define _PAINT_BOARD_CONTROL_H_ |
|
39 |
|
|
40 |
#include <ros/ros.h> |
|
41 |
#include "constants.h" |
|
42 |
#include <messages/set_paintboard.h> |
|
43 |
#include <messages/query_paintboard.h> |
|
44 |
|
|
45 |
#define STOP_PAINT 0 |
|
46 |
#define DISPENSE_PAINT 1 |
|
47 |
|
|
48 |
class PaintBoardControl |
|
49 |
{ |
|
50 |
public: |
|
51 |
/** Set up the paintboard node and prepare to communicate over ROS */ |
|
52 |
PaintBoardControl(const ros::NodeHandle& libscout_node, |
|
53 |
std::string scoutname); |
|
54 |
|
|
55 |
~PaintBoardControl(); |
|
56 |
|
|
57 |
/** Turns the paintboard on or off. */ |
|
58 |
void set(int paintboard_setting); |
|
59 |
|
|
60 |
/** Checks whether the paintboard is currently dispensing paint */ |
|
61 |
int query(); |
|
62 |
|
|
63 |
private: |
|
64 |
/** ROS publisher and client declaration */ |
|
65 |
ros::Publisher set_paintboard_pub; |
|
66 |
ros::ServiceClient query_paintboard_client; |
|
67 |
|
|
68 |
ros::NodeHandle node; |
|
69 |
}; |
|
70 |
|
|
71 |
#endif |
|
72 |
|
scout/libscout/src/Sensors.h | ||
---|---|---|
49 | 49 |
#include "LinesensorControl.h" |
50 | 50 |
#include "WirelessSender.h" |
51 | 51 |
#include "WirelessReceiver.h" |
52 |
#include "PaintBoardControl.h" |
|
52 | 53 |
|
53 | 54 |
class Sensors |
54 | 55 |
{ |
... | ... | |
71 | 72 |
LinesensorControl * linesensor; |
72 | 73 |
WirelessSender * wl_sender; |
73 | 74 |
WirelessReceiver * wl_receiver; |
74 |
|
|
75 |
PaintBoardControl * paintboard; |
|
75 | 76 |
}; |
76 | 77 |
|
77 | 78 |
|
scout/libscout/src/helper_classes/RungaKutta.cpp | ||
---|---|---|
1 |
#include "RungaKutta.h" |
|
2 |
#include <math.h> |
|
3 |
|
|
4 |
using namespace std; |
|
5 |
|
|
6 |
vector<double> RungaKutta::integrate(vector<double> a, vector<double> b, double t) |
|
7 |
{ |
|
8 |
//Copy over the a vector. |
|
9 |
vector<double> result(a); |
|
10 |
//Add half a timestep to the starting conditions. |
|
11 |
for(unsigned int i=0; i<a.size(); i++) |
|
12 |
{ |
|
13 |
result[i] += t*b[i]; |
|
14 |
} |
|
15 |
//Return the integrated result. |
|
16 |
return result; |
|
17 |
} |
|
18 |
|
|
19 |
vector<double> RungaKutta::diff_drive_robot(vector<double> x) |
|
20 |
{ |
|
21 |
vector<double> dx(5,0); |
|
22 |
dx[0] = x[3] * cos(x[2]); |
|
23 |
dx[1] = x[3] * sin(x[2]); |
|
24 |
dx[2] = x[4]; |
|
25 |
dx[3] = x[3]; |
|
26 |
dx[4] = x[4]; |
|
27 |
return dx; |
|
28 |
} |
|
29 |
|
|
30 |
vector<double> RungaKutta::rk4(vector<double> start, |
|
31 |
vector<double> (*func)(vector<double>), |
|
32 |
double loop_time) |
|
33 |
{ |
|
34 |
vector<double> k0 = func(start); |
|
35 |
vector<double> temp_k0 = integrate(start, k0, loop_time/2); |
|
36 |
vector<double> k1 = func(temp_k0); |
|
37 |
vector<double> temp_k1 = integrate(start, k1, loop_time/2); |
|
38 |
vector<double> k2 = func(temp_k1); |
|
39 |
vector<double> temp_k2 = integrate(start, k2, loop_time); |
|
40 |
vector<double> k3 = func(temp_k2); |
|
41 |
|
|
42 |
// Average out each loop's result. |
|
43 |
vector<double> result(start); |
|
44 |
for(unsigned int i=0; i<result.size(); i++) |
|
45 |
{ |
|
46 |
result[i] = loop_time/6*(k0[i]+2*k1[i]+2*k2[i]+k3[i]); |
|
47 |
} |
|
48 |
return result; |
|
49 |
} |
scout/libscout/src/helper_classes/RungaKutta.h | ||
---|---|---|
1 |
#ifndef _RUNGA_KUTTA_H_ |
|
2 |
#define _RUNGA_KUTTA_H_ |
|
3 |
|
|
4 |
#include <vector> |
|
5 |
|
|
6 |
class RungaKutta |
|
7 |
{ |
|
8 |
public: |
|
9 |
static std::vector<double> rk4( |
|
10 |
std::vector<double> start, |
|
11 |
std::vector<double> (*func)(std::vector<double>), |
|
12 |
double loop_time); |
|
13 |
static std::vector<double> diff_drive_robot(std::vector<double> x); |
|
14 |
private: |
|
15 |
static std::vector<double> integrate( |
|
16 |
std::vector<double> a, |
|
17 |
std::vector<double> b, |
|
18 |
double t); |
|
19 |
}; |
|
20 |
|
|
21 |
#endif |
scout/libscout/src/test_behaviors/Odometry_new.cpp | ||
---|---|---|
1 |
|
|
2 | 1 |
#include "Odometry_new.h" |
2 |
#include "../helper_classes/RungaKutta.h" |
|
3 | 3 |
|
4 | 4 |
using namespace std; |
5 | 5 |
|
... | ... | |
22 | 22 |
float avg_left_ticks, avg_right_ticks; |
23 | 23 |
float ang_vl, ang_vr, lin_vl, lin_vr; |
24 | 24 |
float robot_v, robot_w; |
25 |
float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32; |
|
25 |
|
|
26 | 26 |
// figure out the encoder ticks during the time |
27 | 27 |
encoder_readings scout_enc = encoders->query(); |
28 | 28 |
motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last; |
... | ... | |
30 | 30 |
motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last; |
31 | 31 |
motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last; |
32 | 32 |
|
33 |
// update the |
|
33 |
ROS_INFO("%d %d", motor_fl_ticks_iter, motor_fr_ticks_iter); |
|
34 |
|
|
35 |
// update the tick values |
|
34 | 36 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
35 | 37 |
motor_fr_ticks_last = scout_enc.fr_ticks; |
36 | 38 |
motor_bl_ticks_last = scout_enc.bl_ticks; |
... | ... | |
58 | 60 |
robot_w = (lin_vr - lin_vl)/WHEEL_B; |
59 | 61 |
|
60 | 62 |
// now use RK4 to integrade the velocities to get the the move in pos |
61 |
k00 = robot_v * cos(scout_pos->theta); |
|
62 |
k01 = robot_v * sin(scout_pos->theta); |
|
63 |
k02 = robot_w; |
|
64 |
|
|
65 |
k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02); |
|
66 |
k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02); |
|
67 |
k12 = robot_w; |
|
68 |
|
|
69 |
k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12); |
|
70 |
k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12); |
|
71 |
k22 = robot_w; |
|
72 |
|
|
73 |
k30 = robot_v * cos(scout_pos->theta + loop_time * k22); |
|
74 |
k31 = robot_v * sin(scout_pos->theta + loop_time * k22); |
|
75 |
k32 = robot_w; |
|
76 |
|
|
77 |
scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30); |
|
78 |
scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31); |
|
79 |
scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32); |
|
63 |
double state_temp[] = {scout_pos->x, scout_pos->y, scout_pos->theta, |
|
64 |
robot_v, robot_w}; |
|
65 |
vector<double> state (state_temp, state_temp+5); |
|
80 | 66 |
|
67 |
vector<double> new_state = RungaKutta::rk4(state, RungaKutta::diff_drive_robot, loop_time); |
|
81 | 68 |
|
69 |
scout_pos->x = new_state[0]; |
|
70 |
scout_pos->y = new_state[1]; |
|
71 |
scout_pos->theta = new_state[2]; |
|
82 | 72 |
} |
83 | 73 |
|
84 | 74 |
void Odometry_new::run() |
... | ... | |
87 | 77 |
double last_time = Time::now().toSec(); |
88 | 78 |
double loop_time, current_time; |
89 | 79 |
|
80 |
// Reset encoders. |
|
81 |
encoders->reset(); |
|
82 |
|
|
90 | 83 |
//Rate r(LOOP_RATE); |
91 | 84 |
Duration r = Duration(LOOP_TIME); |
92 | 85 |
|
scout/messages/msg/set_paintboard.msg | ||
---|---|---|
1 |
# The absolute motor speeds to set |
|
2 |
int8 setting |
|
3 |
|
scout/messages/srv/query_paintboard.srv | ||
---|---|---|
1 |
--- |
|
2 |
int8 setting |
Also available in: Unified diff