Revision 560d2317

View differences:

scout/libscout/src/MotorControl.h
65 65
#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
66 66
#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
67 67

  
68
#define MAXSPEED 255
68
#define MAXSPEED 100
69 69
#define MOTOR_PERCENT 'p'
70 70
#define MOTOR_MMS 'm'
71 71
#define MOTOR_CMS 'c'
scout/motors/src/motors.cpp
31 31
 *
32 32
 * @author Colony Project, CMU Robotics Club
33 33
 * @author Ben Wasserman
34
 * @author Tom Mullins
34 35
 */
35 36

  
36 37
#include <ros/ros.h>
37 38
#include <cstdlib>
38
#include <fstream>
39 39
#include "motors.h"
40 40

  
41 41
using namespace std;
......
47 47
 * @{
48 48
 */
49 49

  
50
/* Motor state variables
51
 * Speeds expressed as absolute speed out of max speed (0 - +-MAXSPEED)
52
 * Absolute speed is the speed written to the hardware to move the motors
50
/**
51
 * @brief Motor constructor
52
 *
53
 * Opens device file and reads initial PWM value
54
 *
55
 * @param filename The name of the PWM device file
53 56
 */
54
static int motor_fl_speed; /**< The current speed of the front left motor. */
55
static int motor_fr_speed; /**< The current speed of the front right motor. */
56
static int motor_bl_speed; /**< The current speed of the back left motor. */
57
static int motor_br_speed; /**< The current speed of the back right motor. */
57
Motor::Motor(const char* pwmfile, const char* in1file, const char* in2file)
58
  : fpwm(pwmfile), fin1(in1file), fin2(in2file)
59
{
60
  int pwm, in1, in2;
61

  
62
  // read current values from device files
63
  fpwm >> pwm;
64
  fin1 >> in1;
65
  fin2 >> in2;
66

  
67
  // convert to our speed units
68
  if (in1)
69
  {
70
    if (in2)
71
    {
72
      // short brake
73
      speed = 0;
74
    }
75
    else
76
    {
77
      // CW
78
      speed = pwm;
79
    }
80
  }
81
  else
82
  {
83
    if (in2)
84
    {
85
      // CCW
86
      speed = -pwm;
87
    }
88
    else
89
    {
90
      // off
91
      speed = 0;
92
    }
93
  }
58 94

  
59
static fstream motor_fl_file; /**< The device file for the front left motor. */
60
static fstream motor_fr_file; /**< The device file for the front right motor. */
61
static fstream motor_bl_file; /**< The device file for the back left motor. */
62
static fstream motor_br_file; /**< The device file for the back right motor. */
95
  // ensure we are in a consistent state by writing back to hardware
96
  set_speed(speed);
97
}
98

  
99
/**
100
 * @brief Returns current motor speed
101
 *
102
 * Note that this doesn't read from the hardware
103
 */
104
int Motor::get_speed()
105
{
106
  return speed;
107
}
108

  
109
/**
110
 * @brief Sets motor speed
111
 *
112
 * This will set the member this->speed and write the new speed to the hardware
113
 *
114
 * @param new_speed The speed to set, between +/- MAXSPEED inclusive
115
 */
116
void Motor::set_speed(int new_speed)
117
{
118
  int pwm, in1, in2;
119

  
120
  speed = new_speed;
121

  
122
  // convert to hardware units
123
  if (speed == 0)
124
  {
125
    /// @todo should this be off (00) or short brake (11)?
126
    pwm = 0;
127
    in1 = 0;
128
    in2 = 0;
129
  }
130
  else if (speed > 0)
131
  {
132
    // CW
133
    pwm = speed;
134
    in1 = 1;
135
    in2 = 0;
136
  }
137
  else
138
  {
139
    // CCW
140
    pwm = -speed;
141
    in1 = 0;
142
    in2 = 1;
143
  }
144

  
145
  // write to hardware
146
  fpwm << pwm;
147
  fin1 << in1;
148
  fin2 << in2;
149
}
150

  
151
/// @todo change these to the correct files
152
/// @todo also, a problem: gpio can only be done as root!
153
// Motor state variables
154
static Motor motor_fl("/dev/pwm8", "/dev/null", "/dev/null");
155
static Motor motor_fr("/dev/pwm9", "/dev/null", "/dev/null");
156
static Motor motor_bl("/dev/pwm10", "/dev/null", "/dev/null");
157
static Motor motor_br("/dev/pwm11", "/dev/null", "/dev/null");
63 158

  
64 159
/**
65 160
 * @brief Sets motor speed
......
73 168
{
74 169
    if(msg->fl_set)
75 170
    {
76
      motor_fl_speed = msg->fl_speed;
77
      motor_fl_file << motor_fl_speed;
171
      motor_fl.set_speed(msg->fl_speed);
78 172
    }
79 173
    if(msg->fr_set)
80 174
    {
81
      motor_fr_speed = msg->fr_speed;
82
      motor_fr_file << motor_fr_speed;
175
      motor_fr.set_speed(msg->fr_speed);
83 176
    }
84 177
    if(msg->bl_set)
85 178
    {
86
      motor_bl_speed = msg->bl_speed;
87
      motor_bl_file << motor_bl_speed;
179
      motor_bl.set_speed(msg->bl_speed);
88 180
    }
89 181
    if(msg->br_set)
90 182
    {
91
      motor_br_speed = msg->br_speed;
92
      motor_br_file << motor_br_speed;
183
      motor_br.set_speed(msg->br_speed);
93 184
    }
94 185

  
95 186
    ROS_DEBUG("Motor speeds set");
......
106 197
bool motors_query(motors::query_motors::Request &req,
107 198
                  motors::query_motors::Response &res)
108 199
{
109
    /** @todo Are we checking hardware or just using the saved values?
110
     *  Saved values sound fine to me (Alex). Pleas confirm.
111
     *  I think saved values are ok; they're read in from hardware at the
112
     *  beginning (Tom). */
113
    res.fl_speed = motor_fl_speed;
114
    res.fr_speed = motor_fr_speed;
115
    res.bl_speed = motor_bl_speed;
116
    res.br_speed = motor_br_speed;
200
    res.fl_speed = motor_fl.get_speed();
201
    res.fr_speed = motor_fr.get_speed();
202
    res.bl_speed = motor_bl.get_speed();
203
    res.br_speed = motor_br.get_speed();
117 204

  
118 205
    ROS_DEBUG("Motor speeds queried");
119 206

  
......
144 231
    /* Subscribe to the set_motors topic */
145 232
    ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set);
146 233

  
147
    /* Open device files for PWM values */
148
    /** @todo These are probably not the correct order */
149
    motor_fl_file.open("/dev/pwm8");
150
    motor_fr_file.open("/dev/pwm9");
151
    motor_bl_file.open("/dev/pwm10");
152
    motor_br_file.open("/dev/pwm11");
153

  
154
    /* Get initial values, for motor queries */
155
    motor_fl_file >> motor_fl_speed;
156
    motor_fr_file >> motor_fr_speed;
157
    motor_bl_file >> motor_bl_speed;
158
    motor_br_file >> motor_br_speed;
159

  
160 234
    ROS_INFO("Motors node ready.");
161 235
    ros::spin();
162 236

  
scout/motors/src/motors.h
33 33
 * @author Colony Project, CMU Robotics Club
34 34
 * @author Benjamin Wasserman
35 35
 * @author Alex Zirbel
36
 * @author Tom Mullins
36 37
 **/
37 38

  
38 39
#ifndef _MOTORS_H_
39 40
#define _MOTORS_H_
40 41

  
42
#include <fstream>
41 43
#include <motors/set_motors.h>
42 44
#include <motors/query_motors.h>
43 45

  
......
53 55
bool motors_query(motors::query_motors::Request &req,
54 56
                  motors::query_motors::Response &res);
55 57

  
56
/** @brief Converts speeds from speeds in units to absolute speeds. **/
57
int motors_rel_to_abs(int rel_speed, int units);
58

  
59
/** @brief Converts speeds from absolute speeds to speeds in units. **/
60
int motors_abs_to_rel(int rel_speed, int units);
58
class Motor
59
{
60
  public:
61
    Motor(const char* pwmfile, const char* in1file, const char* in2file);
62
    int get_speed();
63
    void set_speed(int speed);
64
  private:
65
    int speed; /**< The current speed of the motor */
66
    std::fstream fpwm; /**< The device file controlling PWM */
67
    std::fstream fin1; /**< The device file controlling IN1 */
68
    std::fstream fin2; /**< The device file controlling IN2 */
69
};
61 70

  
62 71
#endif

Also available in: Unified diff