131 lines
5.0 KiB
Java
131 lines
5.0 KiB
Java
/**
|
|
*
|
|
* BreezySLAM: Simple, efficient SLAM in Java
|
|
*
|
|
* WheeledRobot.java - Java class for wheeled robots
|
|
*
|
|
* Copyright (C) 2014 Simon D. Levy
|
|
*
|
|
* This code is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License as
|
|
* published by the Free Software Foundation, either version 3 of the
|
|
* License, or (at your option) any later version.
|
|
*
|
|
* This code is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public License
|
|
* along with this code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
|
|
package edu.wlu.cs.levy.breezyslam.robots;
|
|
|
|
import edu.wlu.cs.levy.breezyslam.components.Velocities;
|
|
|
|
|
|
/**
|
|
* An abstract class for wheeled robots. Supports computation of forward and angular
|
|
* velocities based on odometry. Your subclass should should implement the
|
|
* extractOdometry method.
|
|
*/
|
|
public abstract class WheeledRobot
|
|
{
|
|
|
|
/**
|
|
* Builds a WheeledRobot object. Parameters should be based on the specifications for
|
|
* your robot.
|
|
* @param wheel_radius_mm radius of each odometry wheel, in meters
|
|
* @param half_axle_length_mm half the length of the axle between the odometry wheels, in meters
|
|
* @return a new WheeledRobot object
|
|
*/
|
|
protected WheeledRobot(double wheel_radius_mm, double half_axle_length_mm)
|
|
{
|
|
this.wheel_radius_mm = wheel_radius_mm;
|
|
this.half_axle_length_mm = half_axle_length_mm;
|
|
|
|
this.timestamp_seconds_prev = 0;
|
|
this.left_wheel_degrees_prev = 0;
|
|
this.right_wheel_degrees_prev = 0;
|
|
}
|
|
|
|
public String toString()
|
|
{
|
|
|
|
return String.format("<Wheel radius=%f m Half axle Length=%f m | %s>",
|
|
this.wheel_radius_mm, this.half_axle_length_mm, this.descriptorString());
|
|
}
|
|
|
|
/**
|
|
* Computes forward and angular velocities based on odometry.
|
|
* @param timestamp time stamp, in whatever units your robot uses
|
|
* @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
|
|
* @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
|
|
* @return velocities object representing velocities for these odometry values
|
|
*/
|
|
|
|
public Velocities computeVelocities( double timestamp, double left_wheel_odometry, double right_wheel_odometry)
|
|
{
|
|
WheelOdometry odometry = this.extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry);
|
|
|
|
double dxy_mm = 0;
|
|
double dtheta_degrees = 0;
|
|
double dt_seconds = 0;
|
|
|
|
if (this.timestamp_seconds_prev > 0)
|
|
{
|
|
double left_diff_degrees = odometry.left_wheel_degrees - this.left_wheel_degrees_prev;
|
|
double right_diff_degrees = odometry.right_wheel_degrees - this.right_wheel_degrees_prev;
|
|
|
|
dxy_mm = this.wheel_radius_mm * (java.lang.Math.toRadians(left_diff_degrees) + java.lang.Math.toRadians(right_diff_degrees));
|
|
|
|
dtheta_degrees = this.wheel_radius_mm / this.half_axle_length_mm * (right_diff_degrees - left_diff_degrees);
|
|
|
|
dt_seconds = odometry.timestamp_seconds - this.timestamp_seconds_prev;
|
|
}
|
|
|
|
// Store current odometry for next time
|
|
this.timestamp_seconds_prev = odometry.timestamp_seconds;
|
|
this.left_wheel_degrees_prev = odometry.left_wheel_degrees;
|
|
this.right_wheel_degrees_prev = odometry.right_wheel_degrees;
|
|
|
|
return new Velocities(dxy_mm, dtheta_degrees, dt_seconds);
|
|
}
|
|
|
|
/**
|
|
* Extracts usable odometry values from your robot's odometry.
|
|
* @param timestamp time stamp, in whatever units your robot uses
|
|
* @param left_wheel_odometry odometry for left wheel, in whatever units your robot uses
|
|
* @param right_wheel_odometry odometry for right wheel, in whatever units your robot uses
|
|
* @return WheelOdometry object containing timestamp in seconds, left wheel degrees, right wheel degrees
|
|
*/
|
|
protected abstract WheelOdometry extractOdometry(double timestamp, double left_wheel_odometry, double right_wheel_odometry);
|
|
|
|
protected abstract String descriptorString();
|
|
|
|
protected class WheelOdometry
|
|
{
|
|
public WheelOdometry(double timestamp_seconds, double left_wheel_degrees, double right_wheel_degrees)
|
|
{
|
|
this.timestamp_seconds = timestamp_seconds;
|
|
this.left_wheel_degrees = left_wheel_degrees;
|
|
this.right_wheel_degrees = right_wheel_degrees;
|
|
}
|
|
|
|
public double timestamp_seconds;
|
|
public double left_wheel_degrees;
|
|
public double right_wheel_degrees;
|
|
|
|
}
|
|
|
|
|
|
private double wheel_radius_mm;
|
|
private double half_axle_length_mm;
|
|
|
|
private double timestamp_seconds_prev;
|
|
private double left_wheel_degrees_prev;
|
|
private double right_wheel_degrees_prev;
|
|
}
|