Revision cc9e9213
ID | cc9e921357336fb8e4bb5975a87e1e55f9d696a7 |
Changed motors node to write to pwm device files
scout/motors/src/motors.cpp | ||
---|---|---|
35 | 35 |
|
36 | 36 |
#include <ros/ros.h> |
37 | 37 |
#include <cstdlib> |
38 |
#include <fstream> |
|
38 | 39 |
#include "motors.h" |
39 | 40 |
|
41 |
using namespace std; |
|
42 |
|
|
40 | 43 |
/** |
41 | 44 |
* @defgroup motors Motors |
42 | 45 |
* @brief Functions for using the motors |
... | ... | |
53 | 56 |
static int motor_bl_speed; /**< The current speed of the back left motor. */ |
54 | 57 |
static int motor_br_speed; /**< The current speed of the back right motor. */ |
55 | 58 |
|
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. */ |
|
63 |
|
|
56 | 64 |
/** |
57 | 65 |
* @brief Sets motor speed |
58 | 66 |
* |
... | ... | |
66 | 74 |
if(msg->fl_set) |
67 | 75 |
{ |
68 | 76 |
motor_fl_speed = msg->fl_speed; |
77 |
motor_fl_file << motor_fl_speed; |
|
69 | 78 |
} |
70 | 79 |
if(msg->fr_set) |
71 | 80 |
{ |
72 | 81 |
motor_fr_speed = msg->fr_speed; |
82 |
motor_fr_file << motor_fr_speed; |
|
73 | 83 |
} |
74 | 84 |
if(msg->bl_set) |
75 | 85 |
{ |
76 | 86 |
motor_bl_speed = msg->bl_speed; |
87 |
motor_bl_file << motor_bl_speed; |
|
77 | 88 |
} |
78 | 89 |
if(msg->br_set) |
79 | 90 |
{ |
80 | 91 |
motor_br_speed = msg->br_speed; |
92 |
motor_br_file << motor_br_speed; |
|
81 | 93 |
} |
82 | 94 |
|
83 |
/** @todo Add code to write speeds to hardware */ |
|
84 |
|
|
85 | 95 |
ROS_DEBUG("Motor speeds set"); |
86 | 96 |
} |
87 | 97 |
|
... | ... | |
97 | 107 |
motors::query_motors::Response &res) |
98 | 108 |
{ |
99 | 109 |
/** @todo Are we checking hardware or just using the saved values? |
100 |
* Saved values sound fine to me (Alex). Pleas confirm. */ |
|
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). */ |
|
101 | 113 |
res.fl_speed = motor_fl_speed; |
102 | 114 |
res.fr_speed = motor_fr_speed; |
103 | 115 |
res.bl_speed = motor_bl_speed; |
... | ... | |
132 | 144 |
/* Subscribe to the set_motors topic */ |
133 | 145 |
ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set); |
134 | 146 |
|
135 |
/* Initialize hardware for motors */ |
|
136 |
// Hardware init functions here |
|
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; |
|
137 | 159 |
|
138 | 160 |
ROS_INFO("Motors node ready."); |
139 | 161 |
ros::spin(); |
Also available in: Unified diff