Project

General

Profile

Revision 0121ead7

ID0121ead71d532ad27980027f0bda30ea591d8e27
Parent c406f16b
Child b64c27b7

Added by Ben Wasserman about 7 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/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");

Also available in: Unified diff