001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.kinematics; 006 007/** Represents the wheel speeds for a differential drive drivetrain. */ 008@SuppressWarnings("MemberName") 009public class DifferentialDriveWheelSpeeds { 010 /** Speed of the left side of the robot. */ 011 public double leftMetersPerSecond; 012 013 /** Speed of the right side of the robot. */ 014 public double rightMetersPerSecond; 015 016 /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */ 017 public DifferentialDriveWheelSpeeds() {} 018 019 /** 020 * Constructs a DifferentialDriveWheelSpeeds. 021 * 022 * @param leftMetersPerSecond The left speed. 023 * @param rightMetersPerSecond The right speed. 024 */ 025 public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) { 026 this.leftMetersPerSecond = leftMetersPerSecond; 027 this.rightMetersPerSecond = rightMetersPerSecond; 028 } 029 030 /** 031 * Renormalizes the wheel speeds if any either side is above the specified maximum. 032 * 033 * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be 034 * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can 035 * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the 036 * absolute threshold, while maintaining the ratio of speeds between wheels. 037 * 038 * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. 039 */ 040 public void desaturate(double attainableMaxSpeedMetersPerSecond) { 041 double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond)); 042 043 if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { 044 leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 045 rightMetersPerSecond = 046 rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 047 } 048 } 049 050 @Override 051 public String toString() { 052 return String.format( 053 "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", 054 leftMetersPerSecond, rightMetersPerSecond); 055 } 056}