Examples with Drive me.wobblyyyy.pathfinder2.robot.Drive used on opensource projects

Example 1 with Drive

use of me.wobblyyyy.pathfinder2.robot.Drive 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);
}

Example 2 with Drive

use of me.wobblyyyy.pathfinder2.robot.Drive in project Pathfinder2 by Wobblyyyy.

the class SwerveDriveKinematics method calculate.

/**
 * Calculate a {@link SwerveState}, given a translation and a center
 * of rotation.
 *
 * @param translation      the translation to calculate a swerve state
 *                         based on.
 * @param centerOfRotation the center of rotation. To be entirely honest,
 *                         I have absolutely no idea what this means.
 * @return create a new swerve drive.
 */
public SwerveState calculate(Translation translation, PointXY centerOfRotation) {
    if (!centerOfRotation.equals(previousCenterOfRotation)) {
        for (int i = 0; i < moduleCount; i++) {
            PointXY modulePosition = modulePositions[i];
            double x = modulePosition.x() - centerOfRotation.x();
            double y = -modulePosition.y() + centerOfRotation.y();
            inverseKinematics.setRow(i * 2 + 0, 0, 1, 0, y);
            inverseKinematics.setRow(i * 2 + 1, 0, 0, 1, x);
        }
        previousCenterOfRotation = centerOfRotation;
    }
    SimpleMatrix speedVector = new SimpleMatrix(3, 1);
    speedVector.setColumn(0, 0, translation.vx(), translation.vy(), translation.vz());
    SimpleMatrix moduleStateMatrix = inverseKinematics.mult(speedVector);
    SwerveModuleState[] states = new SwerveModuleState[moduleCount];
    for (int i = 0; i < moduleCount; i++) {
        double x = moduleStateMatrix.get(i * 2 + 0, 0);
        double y = moduleStateMatrix.get(i * 2 + 1, 0);
        double speed = Math.hypot(x, y);
        Angle angle = Angle.atan2(y, x);
        states[i] = new SwerveModuleState(speed, angle);
    }
    return new SwerveState(states[0], states[1], states[2], states[3]);
}

Example 3 with Drive

use of me.wobblyyyy.pathfinder2.robot.Drive in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method newEmptyPathfinder.

/**
 * Create a new, "empty" instance of Pathfinder.
 *
 * <p>
 * This is pretty much only useful for debugging or testing purposes.
 * </p>
 *
 * @param coefficient the coefficient to use for the turn controller.
 * @return a new instance of Pathfinder that makes use of both the
 * {@link EmptyDrive} and {@link EmptyOdometry} classes.
 */
public static Pathfinder newEmptyPathfinder(double coefficient) {
    Drive drive = new EmptyDrive();
    Odometry odometry = new EmptyOdometry();
    Robot robot = new Robot(drive, odometry);
    return new Pathfinder(robot, coefficient);
}

Example 4 with Drive

use of me.wobblyyyy.pathfinder2.robot.Drive in project Pathfinder2 by Wobblyyyy.

the class TestSimulatedDrive method testTranslation.

@Test
public void testTranslation() {
    Translation translation = new Translation(0, 0, 0);
    SimulatedDrive drive = new SimulatedDrive();
    drive.setTranslation(translation);
    Assertions.assertEquals(translation, drive.getTranslation());
}

Example 5 with Drive

use of me.wobblyyyy.pathfinder2.robot.Drive in project Pathfinder2 by Wobblyyyy.

the class ExampleTimedRobot method robotInit.

@Override
public void robotInit() {
    // initialize everything. if this was a real implementation, you would
    // not want to use SimulatedDrive or SimulatedOdometry
    drive = new SimulatedDrive();
    odometry = new SimulatedOdometry();
    robot = new Robot(drive, odometry);
    pathfinder = new Pathfinder(robot, controller).setSpeed(0.5).setTolerance(2).setAngleTolerance(Angle.fromDeg(5));
}