Revision cc9e9213
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