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

Example 16 with Robot

use of me.wobblyyyy.pathfinder2.robot.Robot 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 17 with Robot

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

the class ZoneProcessor method update.

/**
 * Based on the provided point, call the correct methods of each of
 * the zones.
 *
 * <ul>
 *     <li>
 *         If the robot has ENTERED the zone (previously, it was not in
 *         the zone, but now it is), the zone's {@link Zone#onEnter(Pathfinder)}
 *         method will be called.
 *     </li>
 *     <li>
 *         If the robot has EXITED the zone (previously, it was in the zone,
 *         but now it is not), the zone's {@link Zone#onExit(Pathfinder)}
 *         method will be called.
 *     </li>
 *     <li>
 *         If the robot is INSIDE the zone (this will be activated every
 *         time the {@code onEnter} method is called, as well as whenever
 *         the robot is inside the zone), the zone's
 *         {@link Zone#whileInside(Pathfinder)} method will be called.
 *     </li>
 * </ul>
 *
 * @param pathfinder the instance of Pathfinder.
 */
public void update(Pathfinder pathfinder) {
    if (zones.size() == 0)
        return;
    PathfinderPluginManager manager = pathfinder.getPluginManager();
    List<Zone> lastZones = currentZones;
    currentZones = getContainingZones(pathfinder.getPosition());
    List<Zone> enteredZones = getEnteredZones(lastZones, currentZones);
    List<Zone> exitedZones = getExitedZones(lastZones, currentZones);
    for (Zone zone : enteredZones) {
        zone.onEnter(pathfinder);
        manager.onEnterZone(pathfinder, zone);
    }
    for (Zone zone : currentZones) {
        zone.whileInside(pathfinder);
        manager.whileInsideZone(pathfinder, zone);
    }
    for (Zone zone : exitedZones) {
        zone.onExit(pathfinder);
        manager.onExitZone(pathfinder, zone);
    }
}

Example 18 with Robot

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

the class TestExecutorManager method testExecutorManager2.

@Test
public void testExecutorManager2() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory1 = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Trajectory trajectory2 = new LinearTrajectory(new PointXYZ(20, 20, 45), 1.0, 0.1, Angle.fromDeg(3));
    Trajectory trajectory3 = new LinearTrajectory(new PointXYZ(30, 30, 90), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower1 = generator.generate(robot, trajectory1);
    Follower follower2 = generator.generate(robot, trajectory2);
    Follower follower3 = generator.generate(robot, trajectory3);
    List<Follower> followers = new ArrayList<Follower>() {

        {
            add(follower1);
            add(follower2);
            add(follower3);
        }
    };
    ExecutorManager manager = new ExecutorManager(robot);
    manager.addExecutor(followers);
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(20, 20, 30));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(20, 20, 45));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(30, 30, 45));
    manager.tick();
    Assertions.assertTrue(manager.isActive());
    odometry.setRawPosition(new PointXYZ(30, 30, 90));
    manager.tick();
    Assertions.assertFalse(manager.isActive());
}

Example 19 with Robot

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

the class TestExecutorManager method testExecutorManager.

@Test
public void testExecutorManager() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    List<Follower> list = new ArrayList<Follower>() {

        {
            add(follower);
        }
    };
    FollowerExecutor executor = new FollowerExecutor(odometry, drive, list);
    Assertions.assertFalse(executor.tick());
    ExecutorManager manager = new ExecutorManager(robot);
    Assertions.assertFalse(manager.isActive());
    manager.addExecutor(list);
    Assertions.assertTrue(manager.isActive());
    Assertions.assertFalse(manager.isInactive());
    Assertions.assertFalse(manager.tick());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertEquals(1, manager.howManyExecutors());
    Assertions.assertTrue(manager.tick());
    Assertions.assertFalse(manager.isActive());
    Assertions.assertTrue(manager.isInactive());
}

Example 20 with Robot

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

the class TestFollowerExecutor method testSingleFollowerExecution.

@Test
public void testSingleFollowerExecution() {
    SimulatedOdometry odometry = new SimulatedOdometry();
    SimulatedDrive drive = new SimulatedDrive();
    Robot robot = new Robot(drive, odometry);
    Trajectory trajectory = new LinearTrajectory(new PointXYZ(10, 10, 0), 1.0, 0.1, Angle.fromDeg(3));
    Controller controller = new GenericTurnController(0.1);
    GenericFollowerGenerator generator = new GenericFollowerGenerator(controller);
    Follower follower = generator.generate(robot, trajectory);
    List<Follower> list = new ArrayList<Follower>() {

        {
            add(follower);
        }
    };
    FollowerExecutor executor = new FollowerExecutor(odometry, drive, list);
    Assertions.assertFalse(executor.tick());
    odometry.setRawPosition(new PointXYZ(10, 10, 0));
    Assertions.assertTrue(executor.tick());
}