Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add holonomic follower examples (#2052)

Browse Source
This commit is contained in:
CTT
2019-11-21 19:52:56 -08:00
committed by Peter Johnson
parent 9a8067465c
commit a58dbec8aa
51 changed files with 4793 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java Normal file
View File

@@ -0,0 +1,58 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
/**
* Represents the motor voltages for a mecanum drive drivetrain.
*/
@SuppressWarnings("MemberName")
public class MecanumDriveMotorVoltages {
/**
* Voltage of the front left motor.
*/
public double frontLeftVoltage;
/**
* Voltage of the front right motor.
*/
public double frontRightVoltage;
/**
* Voltage of the rear left motor.
*/
public double rearLeftVoltage;
/**
* Voltage of the rear right motor.
*/
public double rearRightVoltage;
/**
* Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
*/
public MecanumDriveMotorVoltages() {
}
/**
* Constructs a MecanumDriveMotorVoltages.
*
* @param frontLeftVoltage Voltage of the front left motor.
* @param frontRightVoltage Voltage of the front right motor.
* @param rearLeftVoltage Voltage of the rear left motor.
* @param rearRightVoltage Voltage of the rear right motor.
*/
public MecanumDriveMotorVoltages(double frontLeftVoltage,
double frontRightVoltage,
double rearLeftVoltage,
double rearRightVoltage) {
this.frontLeftVoltage = frontLeftVoltage;
this.frontRightVoltage = frontRightVoltage;
this.rearLeftVoltage = rearLeftVoltage;
this.rearRightVoltage = rearRightVoltage;
}
}

36
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
View File

@@ -11,7 +11,11 @@ import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
/**
@@ -77,10 +81,34 @@ public class TrajectoryConfig {
}
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
* Adds a mecanum drive kinematics constraint to ensure that
* no wheel velocity of a mecanum drive goes above the max velocity.
*
* @param kinematics The mecanum drive kinematics.
* @return Instance of the current config object.
*/
public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
return this;
}
/**
* Adds a swerve drive kinematics constraint to ensure that
* no wheel velocity of a swerve drive goes above the max velocity.
*
* @param kinematics The swerve drive kinematics.
* @return Instance of the current config object.
*/
public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
return this;
}
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
*/
public double getStartVelocity() {
return m_startVelocity;
}

84
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java Normal file
View File

@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
/**
* A class that enforces constraints on the mecanum drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for all 4 wheels of the drivetrain stay below a certain
* limit.
*/
public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final MecanumDriveKinematics m_kinematics;
/**
* Constructs a mecanum drive dynamics constraint.
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity,
ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
return new MinMax();
}
}

84
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java Normal file
View File

@@ -0,0 +1,84 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory.constraint;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
/**
* A class that enforces constraints on the swerve drive kinematics.
* This can be used to ensure that the trajectory is constructed so that the
* commanded velocities for all 4 wheels of the drivetrain stay below a certain
* limit.
*/
public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final SwerveDriveKinematics m_kinematics;
/**
* Constructs a mecanum drive dynamics constraint.
*
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
*/
public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
double velocityMetersPerSecond) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds = new ChassisSpeeds(xdVelocity,
ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory
* given pose, curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
double curvatureRadPerMeter,
double velocityMetersPerSecond) {
return new MinMax();
}
}