Examples with RelativeSwerveState me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveState used on opensource projects

Example 1 with RelativeSwerveState

use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveState in project Pathfinder2 by Wobblyyyy.

the class SwerveDrive method setTranslation.

/**
 * Set a translation to the robot. In the case of this swerve chassis,
 * this does a couple of things. Firstly, it applies the modifier to
 * the inputted translation. Secondly, it calculates a swerve chassis
 * state based on the translation. Thirdly, it determines the swerve
 * module states of each individual module. And finally, it sets the
 * state to each of these modules, making the robot move.
 *
 * @param translation a translation the robot should act upon. This
 *                    translation should always be <em>relative</em>,
 *                    meaning whatever the translation says should make
 *                    the robot act accordingly according to the robot's
 *                    position and the robot's current heading. I'm
 *                    currently exhausted and just about entirely unable
 *                    to type, so this isn't coherent, but guess what -
 */
@Override
public void setTranslation(Translation translation) {
    translation = modifier.apply(translation);
    this.translation = translation;
    RelativeSwerveState state = kinematics.calculate(translation);
    RelativeSwerveModuleState frState = state.fr();
    RelativeSwerveModuleState flState = state.fl();
    RelativeSwerveModuleState brState = state.br();
    RelativeSwerveModuleState blState = state.bl();
    this.frontRightModule.set(frState);
    this.frontLeftModule.set(flState);
    this.backRightModule.set(brState);
    this.backLeftModule.set(blState);
}

Example 2 with RelativeSwerveState

use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveState in project Pathfinder2 by Wobblyyyy.

the class RelativeSwerveDriveKinematics method calculateOptimized.

/**
 * Calculate an optimized swerve state based on a translation.
 *
 * <p>
 * My left hand feels like it's about to fall off from typing this much,
 * so I'm just gonna ask you to go read the documentation in the
 * swerve module state class about optimization to figure it out.
 * Good luck!
 * </p>
 *
 * @param translation the translation the robot should follow.
 * @return an optimized swerve state according to that translation.
 */
@SuppressWarnings("DuplicatedCode")
public RelativeSwerveState calculateOptimized(Translation translation) {
    Angle angle = translation.angle();
    double vz = translation.vz() * turnMultiplier;
    double frontRightTurn = frontRightKinematics.calculate(frontRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double frontLeftTurn = frontLeftKinematics.calculate(frontLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double backRightTurn = backRightKinematics.calculate(backRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double backLeftTurn = backLeftKinematics.calculate(backLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double frontRightDrive = translation.magnitude();
    double frontLeftDrive = translation.magnitude();
    double backRightDrive = translation.magnitude();
    double backLeftDrive = translation.magnitude();
    RelativeSwerveModuleState frontRightState = RelativeSwerveModuleState.optimized(angle, frontRightDrive, frontRightModuleAngle.get(), frontRightKinematics);
    RelativeSwerveModuleState frontLeftState = RelativeSwerveModuleState.optimized(angle, frontLeftDrive, frontLeftModuleAngle.get(), frontLeftKinematics);
    RelativeSwerveModuleState backRightState = RelativeSwerveModuleState.optimized(angle, backRightDrive, backRightModuleAngle.get(), backRightKinematics);
    RelativeSwerveModuleState backLeftState = RelativeSwerveModuleState.optimized(angle, backLeftDrive, backLeftModuleAngle.get(), backLeftKinematics);
    return new RelativeSwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
}

Example 3 with RelativeSwerveState

use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveState in project Pathfinder2 by Wobblyyyy.

the class RelativeSwerveDriveKinematics method calculate.

/**
 * Create a swerve drive chassis state based on a translation.
 *
 * @param translation the translation to create a state for.
 * @return a created relative swerve drive state based
 * on the provided translation.
 */
@SuppressWarnings("DuplicatedCode")
@Override
public RelativeSwerveState calculate(Translation translation) {
    Angle angle = translation.angle();
    double vz = translation.vz() * turnMultiplier;
    double frontRightTurn = frontRightKinematics.calculate(frontRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double frontLeftTurn = frontLeftKinematics.calculate(frontLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double backRightTurn = backRightKinematics.calculate(backRightModuleAngle.get(), angle.add(Angle.fromDeg(vz)));
    double backLeftTurn = backLeftKinematics.calculate(backLeftModuleAngle.get(), angle.add(Angle.fromDeg(-vz)));
    double frontRightDrive = translation.magnitude();
    double frontLeftDrive = translation.magnitude();
    double backRightDrive = translation.magnitude();
    double backLeftDrive = translation.magnitude();
    RelativeSwerveModuleState frontRightState = new RelativeSwerveModuleState(frontRightTurn, frontRightDrive);
    RelativeSwerveModuleState frontLeftState = new RelativeSwerveModuleState(frontLeftTurn, frontLeftDrive);
    RelativeSwerveModuleState backRightState = new RelativeSwerveModuleState(backRightTurn, backRightDrive);
    RelativeSwerveModuleState backLeftState = new RelativeSwerveModuleState(backLeftTurn, backLeftDrive);
    return new RelativeSwerveState(frontRightState, frontLeftState, backRightState, backLeftState);
}