Project

General

Profile

Revision 0121ead7

ID0121ead71d532ad27980027f0bda30ea591d8e27
Parent c406f16b
Child b64c27b7

Added by Ben Wasserman about 8 years ago

Added the ROS stack with the libscout, motors, and sonar packages to the reposititory, and associated files. There are probably still things missing. Libscout won't run properly, but it and motors will compile. Sonar is probably broken, but nothing depends on it yet, so this shouldn't be an issue.

View differences:

scout/libscout/src/constants.h
45 45
/* Libscout Defines */
46 46

  
47 47
/* Init defines */
48
const int LIB_ALL = ~0;
49
const int LIB_MOTORS = 0x1;
50
const int LIB_SONAR = 0x2;
51
const int LIB_CLIFFSENSORS = 0x4;
52
const int LIB_HEADLIGHTS = 0x8;
48
#define LIB_ALL -1
49
#define LIB_MOTORS 0x1
50
#define LIB_SONAR 0x2
51
#define LIB_CLIFFSENSORS 0x4
52
#define LIB_HEADLIGHTS 0x8
53 53

  
54 54
/* Status defines */
55
const int LIB_ERROR = ~0;
56
const int LIB_OK = 0;
55
//TODO MAKE ENUMS
56
#define LIB_ERROR -1
57
#define LIB_OK 0
57 58

  
58
/* Global objects */
59
ros::NodeHandle libscout_node;
60 59

  
61 60
#endif
scout/libscout/src/libmotors.cpp
1 1
/**
2
 * Copyright (c) 2007 Colony Project
2
 * Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
39 39

  
40 40
#include "libmotors.h"
41 41

  
42
/* ROS node created in libscout.cpp */
43
extern ros::NodeHandle libscout_node;
44

  
45
/** ROS publisher and client declaration */
46
ros::Publisher set_motors_publisher;
47
ros::ServiceClient query_motors_client;
48

  
49
/*!
50
 * \brief Initialize the motors module of libscout.
51
 *
52
 * Initialize the libscout node as a publisher of set_motors and a client of
53
 * query_motors.
54
 */
42 55
void libmotors_init(){
43 56
  set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
44 57
  query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
45 58
}
46 59

  
47
int motors_set(int speed, int which){
60
/*!
61
 * \brief Set motor speeds
62
 * Sets the speeds of the motors as a percentage of top speed. Can selectively
63
 * select which motors to set, and which to remain at previous speed.
64
 * \param speed The speed the motors should be set to
65
 * \param which A bitmask of which motors should be set
66
 * \return Function status
67
 */
68
int motors_set(int speed, int which, char units=MOTOR_PERCENT){
48 69
  /** \todo Set fields of the message based on params */
49 70
  
50 71
  if(!ros::ok()){
51 72
    return LIB_ERROR;
52 73
  }
53
  
74
 
75
  /* Set the speed for each motor according to the which bitmask*/
54 76
  motors::set_motors msg;
55
  msg.fl_speed = speed;
56
  msg.fr_speed = speed;
57
  msg.bl_speed = speed;
58
  msg.br_speed = speed;
77
  if(which & MOTOR_FL_REV){
78
    msg.fl_speed = -1 * speed;
79
  }
80
  if(which & MOTOR_FR_REV){
81
    msg.fr_speed = -1 * speed;
82
  }
83
  if(which & MOTOR_BL_REV){
84
    msg.bl_speed = -1 * speed;
85
  }
86
  if(which & MOTOR_BR_REV){
87
    msg.br_speed = -1 * speed;
88
  }if(which & MOTOR_FL){
89
    msg.fl_speed = speed;
90
  }
91
  if(which & MOTOR_FR){
92
    msg.fr_speed = speed;
93
  }
94
  if(which & MOTOR_BL){
95
    msg.bl_speed = speed;
96
  }
97
  if(which & MOTOR_BR){
98
    msg.br_speed = speed;
99
  }
100

  
101
  /* Specify which units the speeds are given in */
102
  msg.units = units;
59 103

  
60
//  set_motors_publisher.publish(msg);
104
  /* Publishes message to set_motors topic */
105
  set_motors_publisher.publish(msg);
61 106
  ros::spinOnce();
62 107

  
63 108
  return LIB_OK;
64 109
}
65 110

  
66
int motors_speed_set(int speed, int which){
67
  return LIB_OK;
68
}
111
/*!
112
 * \brief Query the current speeds of the motors
113
 *
114
 * Sends a request to the query_motors service which will reply with the
115
 *  current speed of each motor.
116
 *
117
 * \param which A bitmask that will specify which motor speed should be
118
 *  returned
119
 * \return The speed of the selected motor, or LIB_ERR if no motor selected
120
 */
69 121

  
70 122
int motors_query(int which){
71 123
  motors::query_motors srv;
......
79 131
        return srv.response.bl_speed;
80 132
      case MOTOR_BR:
81 133
        return srv.response.br_speed;
82
      default:{
134
      default:
83 135
        ROS_WARN("Bad WHICH in motors_query.");
84 136
        return LIB_ERROR;
85
      }
86 137
    }
87 138
  }else{
88 139
    ROS_ERROR("Failed to call service query_motors");
scout/libscout/src/libmotors.h
1 1
/**
2
 * Copyright (c) 2007 Colony Project
2
 * Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
42 42
#include "motors/query_motors.h"
43 43
#include "motors/set_motors.h"
44 44

  
45

  
46 45
#ifndef _LIBMOTORS_H_
47 46
#define _LIBMOTORS_H_
48 47

  
49 48
/* Defines */
50
const int MOTOR_FL = 0x8;
51
const int MOTOR_FR = 0x4;
52
const int MOTOR_BL = 0x2;
53
const int MOTOR_BR = 0x1;
54

  
55

  
56
ros::Publisher set_motors_publisher;
57
ros::ServiceClient query_motors_client;
49
#define MOTOR_ALL 0xF
50
#define MOTOR_ALL_REV 0xF0
51
#define MOTOR_NONE 0x0
52
#define MOTOR_FL 0x8
53
#define MOTOR_FR 0x4
54
#define MOTOR_BL 0x2
55
#define MOTOR_BR 0x1
56
#define MOTOR_FL_REV 0x80
57
#define MOTOR_FR_REV 0x40
58
#define MOTOR_BL_REV 0x20
59
#define MOTOR_BR_REV 0x10
60
#define MOTOR_FRONT MOTOR_FL | MOTOR_FR
61
#define MOTOR_BACK MOTOR_BR | MOTOR_BR
62
#define MOTOR_LEFT MOTOR_FL | MOTOR_BL
63
#define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
64
#define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
65
#define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
66
#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
67
#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
68
#define MOTOR_PERCENT 'p'
69
#define MOTOR_MMS 'm'
70
#define MOTOR_CMS 'c'
58 71

  
59 72
void libmotors_init();
60 73
int motors_set(int speed, int which);
61
int motors_speed_set(int speed, int which);
62 74
int motors_query(int which);
63 75

  
64 76
#endif
77

  
scout/libscout/src/libscout.cpp
1 1
/**
2
 * Copyright (c) 2007 Colony Project
2
 * Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
39 39

  
40 40
#include "libscout.h"
41 41

  
42
/* Global objects */
43
ros::NodeHandle libscout_node;
44

  
42 45
/** \todo Decide how to call user behaviors
43 46
  * I'm thinking that there be a function call in main that calls the "main"
44 47
  fn in the user behavior. However, their "main" function will not be called 
......
52 55
  user behavior, or any other package that works below the surface.
53 56
**/
54 57

  
55

  
58
/*!
59
 * \brief LibScout. Primary library for interfacing behaviors with scout hw.
60
 * 
61
 * This is the main function for libscout. It is run when the node starts and
62
 * starts the library. It calls init() which initializes the clients and
63
 * publishers/subscribers for the other parts of the library.
64
 *
65
 * \param argc The number of command line arguments (should be 1)
66
 * \param argv The array of command line arguments
67
 **/
56 68
int main(int argc, char **argv){
57 69
  ros::init(argc, argv, "libscout");
58 70

  
59 71
  init(LIB_ALL);
60 72
  /** \todo remove this test code **/
61
//  motors_set(100, 0);
62
//  ROS_INFO("%d", motors_query(0));
73
//  motors_set(100, MOTOR_ALL);
74
  ROS_INFO("%d", motors_query(MOTOR_FL));
63 75

  
64 76
  return 0;
65 77
}
66 78

  
79
/*!
80
 * \brief Initializes modules in the libscout.
81
 * 
82
 * Calls init functions for each module in libscout.
83
 * \param modules A bitmask of the modules that will be initialized.
84
 **/
67 85
int init(int modules){
86
  /** \todo Copy this if for each module that gets added to the library */
68 87
  if(modules & LIB_MOTORS){
69 88
    libmotors_init();
70 89
  }
71 90
  /** \todo Add other lib inits **/
72 91
  return 0;
73 92
}
93

  
scout/libscout/src/libscout.h
1 1
/**
2
 * Copyright (c) 2007 Colony Project
2
 * Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
44 44
#include "constants.h"
45 45
#include "libmotors.h"
46 46

  
47

  
48 47
/* Libscout functions */
49 48
int main(int argc, char **argv);
50 49
int init(int modules);
51 50

  
52 51
#endif
52

  
scout/messages/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
# Set the build type.  Options are:
5
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
6
#  Debug          : w/ debug symbols, w/o optimization
7
#  Release        : w/o debug symbols, w/ optimization
8
#  RelWithDebInfo : w/ debug symbols, w/ optimization
9
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
10
#set(ROS_BUILD_TYPE RelWithDebInfo)
11

  
12
rosbuild_init()
13

  
14
#set the default path for built executables to the "bin" directory
15
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16
#set the default path for built libraries to the "lib" directory
17
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18

  
19
#uncomment if you have defined messages
20
#rosbuild_genmsg()
21
#uncomment if you have defined services
22
rosbuild_gensrv()
23

  
24
#common commands for building c++ executables and libraries
25
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
26
#target_link_libraries(${PROJECT_NAME} another_library)
27
#rosbuild_add_boost_directories()
28
#rosbuild_link_boost(${PROJECT_NAME} thread)
29
#rosbuild_add_executable(example examples/example.cpp)
30
#target_link_libraries(example ${PROJECT_NAME})
scout/messages/Makefile
1
include $(shell rospack find mk)/cmake.mk
scout/messages/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b messages is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
scout/messages/manifest.xml
1
<package>
2
  <description brief="messages">
3

  
4
     messages
5

  
6
  </description>
7
  <author>Ben</author>
8
  <license>BSD</license>
9
  <review status="unreviewed" notes=""/>
10
  <url>http://ros.org/wiki/messages</url>
11

  
12
</package>
13

  
14

  
scout/messages/srv/query_sonar.srv
1
int8 direction
2
---
3
#front sonar
4
int16 distance0
5
#back sonar
6
int16 distance1
scout/motors/msg/set_motors.msg
3 3
int8 fr_speed
4 4
int8 bl_speed
5 5
int8 br_speed
6
int8 units
scout/motors/src/motors.cpp
1 1
/**
2
 * Copyright (c) 2007 Colony Project
2
 * Copyright (c) 2011 Colony Project
3 3
 * 
4 4
 * Permission is hereby granted, free of charge, to any person
5 5
 * obtaining a copy of this software and associated documentation
......
36 36

  
37 37
#include "ros/ros.h"
38 38
#include "motors.h"
39
//#include "libscout/src/constants.h"
39 40
#include <cstdlib>
40 41

  
41 42
/**
42 43
 * @defgroup motors Motors
43 44
 * @brief Functions for using the motors
44 45
 *
45
 * @{
46 46
 **/
47 47

  
48
/* Motor state variables */
49
/** \todo Fix types: static */
50
int motor_fl_speed; /**< The current speed of the front left motor. */
51
int motor_fr_speed; /**< The current speed of the front right motor. */
52
int motor_bl_speed; /**< The current speed of the back left motor. */
53
int motor_br_speed; /**< The current speed of the back right motor. */
54

  
48 55
/*!
49 56
 * \brief Motors driver. This is a ROS node that controls motor speeds.
50 57
 *
......
108 115
  res.bl_speed = motor_bl_speed;
109 116
  res.br_speed = motor_br_speed;
110 117

  
111
  ROS_INFO("Motor speeds queried");
118
  ROS_DEBUG("Motor speeds queried");
112 119
  return true;
113 120
}
114 121

  
115

  
116
/**
117
 * }
118
 **/
scout/motors/src/motors.h
43 43
#include "motors/query_motors.h"
44 44
#include "motors/set_motors.h"
45 45

  
46
/* Motor state variables */
47
int motor_fl_speed; /**< The current speed of the front left motor. */
48
int motor_fr_speed; /**< The current speed of the front right motor. */
49
int motor_bl_speed; /**< The current speed of the back left motor. */
50
int motor_br_speed; /**< The current speed of the back right motor. */
51 46

  
52 47
/** @brief Initialize the motors module and driver **/
53 48
int main(int argc, char **argv);
scout/sonar/CMakeLists.txt
1
cmake_minimum_required(VERSION 2.4.6)
2
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3

  
4
# Set the build type.  Options are:
5
#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
6
#  Debug          : w/ debug symbols, w/o optimization
7
#  Release        : w/o debug symbols, w/ optimization
8
#  RelWithDebInfo : w/ debug symbols, w/ optimization
9
#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
10
#set(ROS_BUILD_TYPE RelWithDebInfo)
11

  
12
rosbuild_init()
13

  
14
#set the default path for built executables to the "bin" directory
15
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16
#set the default path for built libraries to the "lib" directory
17
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
18

  
19
#uncomment if you have defined messages
20
#rosbuild_genmsg()
21
#uncomment if you have defined services
22
rosbuild_gensrv()
23

  
24
#common commands for building c++ executables and libraries
25
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
26
#target_link_libraries(${PROJECT_NAME} another_library)
27
#rosbuild_add_boost_directories()
28
#rosbuild_link_boost(${PROJECT_NAME} thread)
29
#rosbuild_add_executable(example examples/example.cpp)
30
#target_link_libraries(example ${PROJECT_NAME})
scout/sonar/Makefile
1
include $(shell rospack find mk)/cmake.mk
scout/sonar/mainpage.dox
1
/**
2
\mainpage
3
\htmlinclude manifest.html
4

  
5
\b sonar is ... 
6

  
7
<!-- 
8
Provide an overview of your package.
9
-->
10

  
11

  
12
\section codeapi Code API
13

  
14
<!--
15
Provide links to specific auto-generated API documentation within your
16
package that is of particular interest to a reader. Doxygen will
17
document pretty much every part of your code, so do your best here to
18
point the reader to the actual API.
19

  
20
If your codebase is fairly large or has different sets of APIs, you
21
should use the doxygen 'group' tag to keep these APIs together. For
22
example, the roscpp documentation has 'libros' group.
23
-->
24

  
25

  
26
*/
scout/sonar/manifest.xml
1
<package>
2
  <description brief="sonar">
3

  
4
     sonar
5

  
6
  </description>
7
  <author>Ben</author>
8
  <license>BSD</license>
9
  <review status="unreviewed" notes=""/>
10
  <url>http://ros.org/wiki/sonar</url>
11
  <depend package="roscpp"/>
12
  <depend package="std_msgs"/>
13

  
14
</package>
15

  
16

  
scout/sonar/src/sonar.cpp
1
/**
2
 * Copyright (c) 2007 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
/**
28
 * @file sonar.cpp
29
 * @brief Sonar
30
 *
31
 * Implementation of functions for sonar use.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * @author Benjamin Wasserman
35
 **/
36

  
37
#include "scout/sonar.h"
38
#include "ros/ros.h"
39
#include <cstdlib>
40

  
41
/**
42
 * @defgroup sonar Sonar
43
 * @brief Functions for using the sonar
44
 *
45
 * @{
46
 **/
47

  
48
int8_t sonar_stepper_pos; //This should only contain values in [0,47]
49
/**
50
 * Sonar driver/service. This is a ROS node that communicates directly with the sonar
51
 hardware. It serves the query_sonar service, and takes a direction, and responds with a distance. It is registered by sonar_driver_init.
52
 *
53
 **/
54
bool sonar_get_distance(sonar::query_sonar::Request   &req,
55
                  sonar::query_sonar::Response  &res){
56
  /* Set the stepper position (position from 0-47 for direction)*/
57
  //stepper_set(req.direction)
58
  /* Delay if necessary for stepper to get in position */
59
  /* Get value from sonar */
60
  //res.distance0 = sonar_sensor(0);
61
  //res.distance1 = sonar_sensor(1);
62
  ROS_INFO("request: sonar direction #%d", req.direction);
63
  ROS_INFO("response: sonar distances %d %d", res.distance0, res.distance1);
64
  return true;
65
}
66

  
67
/**
68
 * Initialize the sonar driver. This must be called before any sonar
69
 * requests are made. It will then run in the background until the sonar node is
70
 * shut down.
71
 *
72
 * @see sonar_get_distance
73
 **/
74
void main(int argc, char **argv){
75
  /* Initialize in ROS the sonar driver node */
76
  ros::init(argc, argv, "sonar_driver");
77

  
78
  /* Advertise that this serves the query_sonar service */
79
  ros::NodeHandle n;
80
  ros:ServiceServer service = n.advertiseService("query_sonar", sonar_get_distance);
81
  
82
  /* Initialize hardware for sonar */
83
  //Hardware init functions here
84
    
85
  ROS_INFO("Ready to read sonar.");
86
  ros::spin();
87

  
88
  return 0;
89
}
90

  
91
/**
92
 * Initialize the sonar system. This must be called before sonar_read.
93
 * 
94
 * @see sonar_read
95
 * @see sonar_read_raw
96
 **/
97
void sonar_init(void){
98
  sonar_stepper_pos = 0;
99
}
100

  
101
/**
102
 * Read raw sonar data from the sonar module via the query_sonar service. 
103
 *
104
 * @param direction The direction to set the stepper motor to. (0-47)
105
 * @param sensor The sonar sensor that should have its value returned. (0-1)
106
 * @return distance The distance in the specified direction. Or -1 if an
107
 * illegal parameter or error.
108
 **/
109
int sonar_read_raw(int direction, int sensor){
110
  /* Check for illegal arguments */
111
  if(direction < 0 || direction > 47 || sensor < 0 || sensor > 1){
112
    ROS_WARNING("Illegal argument");
113
    return -1;
114
  }
115
  /* Register client to query_sonar service. */
116
  ros::NodeHandle n;
117
  ros::ServiceClient client = n.serviceClient<messages::query_sonar>("query_sonar");
118
  /* Create and populate query */
119
  messages::query_sonar srv;
120
  srv.request.direction = direction;
121
  /* Send query */
122
  if(client.call(srv)){
123
    /* Return value of correct sensor */
124
    if(sensor){
125
      ROS_INFO("Sonar 1 at %d reads %d", direction, srv.response.distance1);
126
      return srv.response.distance1;
127
    }else{
128
      ROS_INFO("Sonar 0 at %d reads %d", direction, srv.response.distance0);
129
      return srv.response.distance0;
130
    }
131
  }else{
132
    ROS_ERROR("Failed to call service query_sonar");
133
    return -1;
134
  }
135
}
136

  
137
/**
138
 * Read sonar values in human readable fashion. Wrapper for
139
 * sonar_read_raw.
140
 *
141
 * @param direction Direction to get sonar value at. Will be rounded to
142
 *  closest possible position. In degrees. Can be negative or > 360.
143
 * @return distance Distance that sonar reads in the given direction.
144
 * @see sonar_read_raw
145
 **/
146
int sonar_read(int direction){
147
  int dir1; //The position of the second sonar head;
148
  int distance;
149
  /* Get angle in 0<=dir<359 */
150
  direction %= 360;
151
  /* Get stepper positions */
152
  direction /= 48;
153
  /* Move stepper to closest position to get reading */
154
  if(direction == sonar_stepper_pos){
155
    /* If direction is the same */
156
    distance = sonar_read_raw(direction, 0);
157
  }else if(direction == sonar_stepper_pos + 24 || direction + 24 == sonar_stepper_pos){
158
    /* If direction is opposite last direction */
159
    distance = sonar_read_raw(sonar_stepper_pos, 1);
160
  }else{
161
    /*TODO: Replace this next line with code that will move the stepper the
162
    ** minimum necessary distance to get one of the heads in the desired
163
    ** position. Update sonar_stepper_pos to this new position, and call
164
    ** sonar_read_raw with the stepper position (not necessarily the
165
    ** direction of the reading) and which sonar sensor should be
166
    ** returned (0 or 1). 
167
    */
168
    distance = sonar_read_raw(direction,0);
169
  }
170
  /*TODO: Make a function to convert raw sonar distances to cm for
171
  ** usability. Replace next line with line that calls function.
172
  */
173
  //return convert_to_cm(distance);
174
  return distance;
175
}
scout/sonar/src/sonar.h
1
/**
2
 * Copyright (c) 2007 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
/**
28
 * @file sonar.h
29
 * @brief Contains sonar declarations and functions
30
 * 
31
 * Contains functions and definitions for the use of
32
 * sonar
33
 *
34
 * @author Colony Project, CMU Robotics Club
35
 **/
36

  
37
/* Author: Ben Wasserman
38
*/
39

  
40
#ifndef _SONAR_H_
41
#define _SONAR_H_
42

  
43
/** @brief Initialize the sonar in the library module **/
44
void sonar_init(void);
45
/** @brief Initialize the sonar module and drivers **/
46
void sonar_module_init(void);
47
/** @brief Read raw sonar data from the sonar module. Takes stepper
48
** position and sensor to read **/
49
int sonar_read_raw(int direction, int sensor);
50
/** @brief Wrapper for reading sonar data (angle in degrees and distance in
51
** cm) **/
52
int sonar_read(int direction);
53
/** @brief Subscribe function to sonar topic **/
54
void sonar_subscribe(int *function());
55

  
56
#endif
scout/sonar/srv/query_sonar.srv
1
int8 direction
2
---
3
#front sonar
4
int16 distance0
5
#back sonar
6
int16 distance1

Also available in: Unified diff