content: /**
   * 
   * 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;    
  }        
  