use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveModuleState 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);
}
use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveModuleState 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);
}
use of me.wobblyyyy.pathfinder2.kinematics.RelativeSwerveModuleState 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);
}
Aggregations