Commit b4b28c05 authored by José Luis Rivero Partida's avatar José Luis Rivero Partida
Browse files

segwayRMP200 is now returning values in international units.

Don't need to convert to them anymore.
parent 0f3b1227
......@@ -198,7 +198,7 @@ CSegwayRMP400::get_yaw_displacement()
if (use_correction_wheel_factor_)
fix_with_wheel_factor(yaw_displacement_tmp);
yaw_displacement_ = yaw_displacement_tmp * 2 * PI; // convert to radians
yaw_displacement_ = yaw_displacement_tmp;
return yaw_displacement_;
}
......@@ -207,7 +207,7 @@ CSegwayRMP400::get_yaw_rate()
{
need_to_be_connected();
yaw_rate_ = get_yaw_rate() * (PI / 180);
yaw_rate_ = get_yaw_rate();
return yaw_rate_;
}
......@@ -252,7 +252,7 @@ CSegwayRMP400::move(float vT, float vR)
{
need_to_be_connected();
float vRradsec = vR * 2.0 * PI; // revs per sec to rads per sec
float vRradsec = vR;
if (fabs(vT) < fabs(RMP400_b * vRradsec / 2)) {
// minimum translational velocity needed to enforce rotation about a ICR outside of the robot base
......
......@@ -3,13 +3,11 @@
#include <segway_rmp200.h>
#include <ftdiserver.h>
#include <math.h>
#include <vector>
#include <string>
#include <iostream>
#define RMP400_b 0.54 // distance between weels for one rmp unit in meters
#define PI 3.141592
#define RMP200_DEFAULT_WHEEL_RADIUS 0.24
const int NUM_SEGWAY_200 = 2;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment