Examples with DifferentialDriveWheelSpeeds edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds used on opensource projects

Example 1 with DifferentialDriveWheelSpeeds

use of edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds in project RapidReact2022 by pittsfordrobotics.

the class Drive method periodic.

@Override
public void periodic() {
    pose = odometry.update(Rotation2d.fromDegrees(getAngle()), leftEncoder.getPosition(), rightEncoder.getPosition());
    wheelSpeeds = new DifferentialDriveWheelSpeeds(getLeftVelocity(), getRightVelocity());
}

Example 2 with DifferentialDriveWheelSpeeds

use of edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds in project RobotCode2022 by Mechanical-Advantage.

the class MotionProfileCommand method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    State setpoint = trajectory.sample(timer.get());
    Logger.getInstance().recordOutput("Odometry/ProfileSetpoint", new double[] { setpoint.poseMeters.getX(), setpoint.poseMeters.getY(), setpoint.poseMeters.getRotation().getRadians() });
    ChassisSpeeds chassisSpeeds = controller.calculate(drive.getPose(), setpoint);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
    drive.driveVelocity(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
}

Example 3 with DifferentialDriveWheelSpeeds

use of edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds in project RapidReact2022 by team223.

the class AutoDrive1 method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    RamseteController controller = new RamseteController();
    double currentTime = (double) (System.currentTimeMillis() - startTimeMillis) / 1000;
    Trajectory.State goal = trajectory.sample(currentTime);
    ChassisSpeeds updatedSpeeds = controller.calculate(RobotContainer.driveSubsystem.getPosition(), goal);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(updatedSpeeds);
    double left = wheelSpeeds.leftMetersPerSecond;
    SmartDashboard.putNumber("Target X", goal.poseMeters.getX());
    SmartDashboard.putNumber("Target Y", goal.poseMeters.getY());
    SmartDashboard.putNumber("Actual X", RobotContainer.driveSubsystem.getPosition().getX());
    SmartDashboard.putNumber("Actual Y", RobotContainer.driveSubsystem.getPosition().getY());
    double right = wheelSpeeds.rightMetersPerSecond;
    double leftSpeed = RobotContainer.driveSubsystem.getLeftEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    double rightSpeed = RobotContainer.driveSubsystem.getRightEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    System.out.println(leftSpeed + "vs" + left);
    RobotContainer.driveSubsystem.setMotors(drivePid.calculate(leftSpeed, left), drivePid.calculate(rightSpeed, right));
}

Example 4 with DifferentialDriveWheelSpeeds

use of edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds in project FRC2022 by Team2338.

the class LimelightAutoAim method execute.

@Override
public void execute() {
    // we don't need this if limelight can stay on all the time
    // if (++delayCounter < 12) return; // Give limelight enough time to turn on LEDs before taking snapshot
    // we want the shooter to start revving up so the robot can shoot as soon as it settles
    // distance zones //more accurate than rohan (TM)
    double distanceFromHub = Math.abs((Constants.Shooter.UPPER_HUB_HEIGHT - Constants.Shooter.LIMELIGHT_HEIGHT) / Math.tan(Math.toRadians(Constants.Shooter.LIMELIGHT_ANGLE + Robot.limelight.getYOffset())));
    if (distanceFromHub >= 200) {
        // Far Shot
        Robot.hood.setHoodUp();
        Robot.shooter.setSpeedPID(Constants.Shooter.RPM_FAR_COURT);
        System.out.println("Distance - Far: " + distanceFromHub);
    } else if (distanceFromHub >= 130) {
        // LaunchPad shot
        Robot.hood.setHoodUp();
        Robot.shooter.setSpeedPID(Constants.Shooter.RPM_LAUNCHPAD);
        System.out.println("Distance - Launch: " + distanceFromHub);
    } else if (distanceFromHub >= 50) {
        // Ring shot
        Robot.hood.setHoodUp();
        Robot.shooter.setSpeedPID(Constants.Shooter.RPM_RING_UPPER_HUB);
        System.out.println("Distance - Ring: " + distanceFromHub);
    } else {
        // Fender shot
        Robot.hood.setHoodDown();
        Robot.shooter.setSpeedPID(Constants.Shooter.RPM_FENDER_UPPER_HUB);
        System.out.println("Distance - Fender: " + distanceFromHub);
    }
    // bot must not be moving anymore
    if (!robotHasSettled) {
        DifferentialDriveWheelSpeeds wheelSpeeds = Robot.drivetrain.getWheelSpeeds();
        // Make sure both wheels are within tolerance of not moving
        if (Math.abs(wheelSpeeds.leftMetersPerSecond) < velocityCap && Math.abs(wheelSpeeds.rightMetersPerSecond) < velocityCap) {
            robotHasSettled = true;
            System.out.println("AutoFire: Robot has settled");
        }
    }
    if (robotHasSettled) {
        // Note: can't combine this using else because robotHasSettled can be set to true in the above section
        double offset = Robot.limelight.getXOffset();
        if (targetLocked) {
            // we need to check again to make sure the robot hasn't overshot the target
            if (offset > -1.0 && offset < 1.0) {
                if (Robot.shooter.isInTolerance()) {
                    // fire away!
                    System.out.println("Shooting - I hope it went in");
                    Robot.indexer.setBeltMotorSpeedPercent(1.0);
                    Robot.indexer.setMidMotorSpeed(1.0);
                } else {
                    // shooter is still spinning up or just can't get to the desired speed
                    System.out.println("Robot is settled and locked. Flywheel not in tolerance.");
                }
            } else {
                System.out.println("Offset Adjusting at: " + offset);
                // need to relock
                targetLocked = false;
                robotHasSettled = false;
            }
        } else {
            if (offset > -1.0 && offset < 1.0) {
                // target is locked
                Robot.drivetrain.tankDriveVolts(0, 0);
                targetLocked = true;
            } else if (offset < 0) {
                // still not in tolerance, need to rotate
                Robot.drivetrain.tankDriveVolts(-Constants.Shooter.MAX_PIVOT_VOLTS, Constants.Shooter.MAX_PIVOT_VOLTS);
                targetLocked = false;
            } else {
                // still not in tolerance, need to rotate
                Robot.drivetrain.tankDriveVolts(Constants.Shooter.MAX_PIVOT_VOLTS, -Constants.Shooter.MAX_PIVOT_VOLTS);
                targetLocked = false;
            }
        }
    }
}

Example 5 with DifferentialDriveWheelSpeeds

use of edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds in project RapidReact2022 by team223.

the class DrivePath method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    RamseteController controller = new RamseteController();
    double currentTime = (double) (System.currentTimeMillis() - startTimeMillis) / 1000;
    Trajectory.State goal = trajectory.sample(currentTime);
    ChassisSpeeds updatedSpeeds = controller.calculate(RobotContainer.driveSubsystem.getPosition(), goal);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(updatedSpeeds);
    double left = wheelSpeeds.leftMetersPerSecond;
    double right = wheelSpeeds.rightMetersPerSecond;
    double leftSpeed = RobotContainer.driveSubsystem.getLeftEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    double rightSpeed = RobotContainer.driveSubsystem.getRightEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
    System.out.println(RobotContainer.driveSubsystem.getPosition().getRotation().getDegrees() + "vs " + goal.poseMeters.getRotation().getDegrees());
    System.out.println("target: " + goal.poseMeters.getX() + ", " + goal.poseMeters.getY());
    System.out.println("actual: " + RobotContainer.driveSubsystem.getPosition().getX() + ", " + RobotContainer.driveSubsystem.getPosition().getY());
    RobotContainer.driveSubsystem.setMotors(drivePid.calculate(leftSpeed, left), drivePid.calculate(rightSpeed, -right));
}