Skip to content

Faster EncoderMapper.java #130

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
themehdev wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Faster EncoderMapper.java #130

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 50 additions & 57 deletions src/main/java/org/xerosw/util/EncoderMapper.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,98 +19,91 @@
/// Encoders
/// </pre>
public class EncoderMapper {
private double kEncoder2Robot_ ;
private double rmax_ ;
private double rmin_ ;
private double rc_ ;
private double emax_ ;
private double emin_ ;
private double ec_ ;
private double kEncoder2Robot_;
private double rmax_;
private double rmin_;
private double rc_;
private double emax_;
private double emin_;
private double ec_;

/// \brief create a new encoder mapper objects
/// \param rmax the maximum range of the robot physical characteristic
/// \param rmin the minimum range of the robot physical characteristic
/// \param emax the maximum range of the electrical value returned from the encoder
/// \param emin the minimum range of the electrical value returned from the encoder
/// \param emax the maximum range of the electrical value returned from the
/// encoder
/// \param emin the minimum range of the electrical value returned from the
/// encoder
public EncoderMapper(double rmax, double rmin, double emax, double emin) {
rmax_ = rmax ;
rmin_ = rmin ;
emax_ = emax ;
emin_ = emin ;
kEncoder2Robot_ = (rmax - rmin) / (emax - emin) ;
rmax_ = rmax;
rmin_ = rmin;
emax_ = emax;
emin_ = emin;
kEncoder2Robot_ = (rmax - rmin) / (emax - emin);
}

/// \brief calibrate the encoder mapper.
/// This establishes the wrap point of the encoder versus the physical system being measured.
/// This establishes the wrap point of the encoder versus the physical system
/// being measured.
/// \param robot a reading from the robot
/// \param encoder a reading from the encoder
public void calibrate(double robot, double encoder) {
ec_ = encoder ;
rc_ = robot ;
ec_ = encoder;
rc_ = robot;
}

/// \brief convert an encoder value to a physical robot value
/// \param encoder the encoder value
/// \returns the robot value
public double toRobot(double encoder) {
double ret ;
double offset;

encoder = clamp(encoder, emax_, emin_) ;
offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_) ;
ret = normalize((encoder - offset) * kEncoder2Robot_ + rmin_, rmax_, rmin_) ;
return ret ;
double ret;
double offset;

encoder = clamp(encoder, emax_, emin_);
offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_);
ret = normalize((encoder - offset) * kEncoder2Robot_ + rmin_, rmax_, rmin_);

return ret;
}

/// \brief convert an robot value to a encoder value
/// \param robot the robot value
/// \returns the encoder value
public double toEncoder(double robot) {
double ret ;
double offset ;
double ret;
double offset;

robot = clamp(robot, rmax_, rmin_) ;
offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_) ;
ret = normalize(offset + (robot - rmin_) / kEncoder2Robot_, emax_, emin_) ;

return ret ;
}
robot = clamp(robot, rmax_, rmin_);
offset = normalize(ec_ - (rc_ - rmin_) / kEncoder2Robot_, emax_, emin_);
ret = normalize(offset + (robot - rmin_) / kEncoder2Robot_, emax_, emin_);

private double normalize(double value, double vmax, double vmin) {
if (vmax < vmin)
{
double temp = vmax ;
vmax = vmin ;
vmin = temp ;
}
return ret;
}

while (value < vmin)
{
value += (vmax - vmin) ;
private double normalize(double value, double vmax, double vmin) {
if (vmax < vmin) {
double temp = vmax;
vmax = vmin;
vmin = temp;
}

while (value > vmax)
{
value -= (vmax - vmin) ;
}
value = value + (vmax - vmin) * -Math.floor(((value - vmin) / (vmax - vmin)));

return value ;
return value;
}

private double clamp(double value, double vmax, double vmin) {
if (vmax < vmin)
{
double temp = vmax ;
vmax = vmin ;
vmin = temp ;
if (vmax < vmin) {
double temp = vmax;
vmax = vmin;
vmin = temp;
}

if (value > vmax)
value = vmax ;
value = vmax;
else if (value < vmin)
value = vmin ;
value = vmin;

return value ;
}
return value;
}
}
Loading