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

Example 1 with MecanumState

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

the class MecanumKinematics method toTranslation.

public Translation toTranslation(MecanumState state) {
    SimpleMatrix wheelSpeedsMatrix = new SimpleMatrix(4, 1);
    wheelSpeedsMatrix.setColumn(0, 0, state.fl(), state.fr(), state.bl(), state.br());
    SimpleMatrix speedVector = forwardsKinematics.mult(wheelSpeedsMatrix);
    return new Translation(speedVector.get(0, 0), speedVector.get(1, 0), speedVector.get(2, 0));
}

Example 2 with MecanumState

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

the class MecanumDrive method setTranslation.

/**
 * Set a translation to the drivetrain. This translation should
 * always be relative to the robot, not relative to the field,
 * yourself, or any other stationary object.
 *
 * @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.
 */
@Override
public void setTranslation(Translation translation) {
    this.translation = getDriveModifier().apply(translation);
    MecanumState state = kinematics.calculate(this.translation);
    fr.setPower(state.fr());
    fl.setPower(state.fl());
    br.setPower(state.br());
    bl.setPower(state.bl());
}

Example 3 with MecanumState

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

the class MecanumKinematics method calculate.

public MecanumState calculate(Translation translation, PointXY centerOfRotation) {
    if (!centerOfRotation.equals(lastCenterOfRotation)) {
        PointXY fl = frontLeftPosition.subtract(centerOfRotation);
        PointXY fr = frontRightPosition.subtract(centerOfRotation);
        PointXY bl = backLeftPosition.subtract(centerOfRotation);
        PointXY br = backRightPosition.subtract(centerOfRotation);
        setInverseKinematics(fl, fr, bl, br);
        lastCenterOfRotation = centerOfRotation;
    }
    SimpleMatrix speedVector = new SimpleMatrix(3, 1);
    speedVector.setColumn(0, 0, translation.vx(), translation.vy(), translation.vz());
    SimpleMatrix wheelMatrix = inverseKinematics.mult(speedVector);
    double fl = wheelMatrix.get(0, 0);
    double fr = wheelMatrix.get(1, 0);
    double bl = wheelMatrix.get(2, 0);
    double br = wheelMatrix.get(3, 0);
    return new MecanumState(fr, fl, br, bl);
}

Example 4 with MecanumState

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

the class RelativeMecanumKinematics method calculate.

@Override
public MecanumState calculate(Translation translation) {
    double xyMagnitude = MinMax.clip(Math.hypot(translation.vx(), translation.vy()), minMagnitude, maxMagnitude);
    Angle angle = Angle.atan2(translation.vy(), translation.vx()).add(angleOffset);
    double turn = translation.vz() * turnMagnitude;
    double fl = (calculatePower(angle, WHEEL_ANGLES[0]) * xyMagnitude) + turn;
    double fr = (calculatePower(angle, WHEEL_ANGLES[1]) * xyMagnitude) - turn;
    double bl = (calculatePower(angle, WHEEL_ANGLES[2]) * xyMagnitude) + turn;
    double br = (calculatePower(angle, WHEEL_ANGLES[3]) * xyMagnitude) - turn;
    return new MecanumState(fl, fr, bl, br).normalizeFromMaxUnderOne();
}