Revision 6257c97d
Continuing work on standardization of units.
scout/scoutsim/src/scout.cpp | ||
---|---|---|
113 | 113 |
sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0); |
114 | 114 |
} |
115 | 115 |
|
116 |
float Scout::absolute_to_mps(int absolute_speed) |
|
117 |
{ |
|
118 |
return ((float) absolute_speed) * MAX_SPEED_MPS / MAX_ABSOLUTE_SPEED; |
|
119 |
} |
|
120 |
|
|
116 | 121 |
/** |
117 | 122 |
* A callback function that sets velocity based on a set_motors |
118 | 123 |
* request. |
... | ... | |
124 | 129 |
|
125 | 130 |
if(msg->fl_set) |
126 | 131 |
{ |
127 |
motor_fl_speed = msg->fl_speed;
|
|
132 |
motor_fl_speed = absolute_to_mps(msg->fl_speed);
|
|
128 | 133 |
} |
129 | 134 |
if(msg->fr_set) |
130 | 135 |
{ |
131 |
motor_fr_speed = msg->fr_speed;
|
|
136 |
motor_fr_speed = absolute_to_mps(msg->fr_speed);
|
|
132 | 137 |
} |
133 | 138 |
if(msg->bl_set) |
134 | 139 |
{ |
135 |
motor_bl_speed = msg->bl_speed;
|
|
140 |
motor_bl_speed = absolute_to_mps(msg->bl_speed);
|
|
136 | 141 |
} |
137 | 142 |
if(msg->br_set) |
138 | 143 |
{ |
139 |
motor_br_speed = msg->br_speed;
|
|
144 |
motor_br_speed = absolute_to_mps(msg->br_speed);
|
|
140 | 145 |
} |
141 | 146 |
} |
142 | 147 |
|
Also available in: Unified diff