Examples with DrivetrainSubsystem org.snobotv2.examples.base.subsystems.DrivetrainSubsystem used on opensource projects

Example 1 with DrivetrainSubsystem

use of org.snobotv2.examples.base.subsystems.DrivetrainSubsystem in project SnobotSimV2 by snobotsim.

the class DriveTrajectoryCommand method createWithVoltage.

public static Command createWithVoltage(DrivetrainSubsystem drivetrain, Trajectory trajectory, boolean resetOnStart) {
    DrivetrainSubsystem.DrivetrainConstants drivetrainConstants = drivetrain.getConstants();
    RamseteCommand ramseteCommand = new RamseteCommand(trajectory, drivetrain::getPose, new RamseteController(TrajectoryFactory.AutoConstants.kRamseteB, TrajectoryFactory.AutoConstants.kRamseteZeta), new SimpleMotorFeedforward(drivetrainConstants.getKsVolts(), drivetrainConstants.getKvVoltSecondsPerMeter(), drivetrainConstants.getKaVoltSecondsSquaredPerMeter()), drivetrainConstants.getKinematics(), drivetrain::getWheelSpeeds, new PIDController(TrajectoryFactory.DriveConstants.kPDriveVel, 0, 0), new PIDController(TrajectoryFactory.DriveConstants.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    drivetrain::tankDriveVolts, drivetrain);
    Command runThenStop = ramseteCommand.andThen(() -> drivetrain.stop());
    if (resetOnStart) {
        Command resetPose = new InstantCommand(() -> drivetrain.resetOdometry(trajectory.getInitialPose()));
        return resetPose.andThen(runThenStop);
    }
    return runThenStop;
}

Example 2 with DrivetrainSubsystem

use of org.snobotv2.examples.base.subsystems.DrivetrainSubsystem in project SnobotSimV2 by snobotsim.

the class DriveTrajectoryCommand method createWithVelocity.

public static Command createWithVelocity(DrivetrainSubsystem drivetrain, Trajectory trajectory, boolean resetOnStart) {
    DrivetrainSubsystem.DrivetrainConstants drivetrainConstants = drivetrain.getConstants();
    RamseteCommand ramseteCommand = new RamseteCommand(trajectory, drivetrain::getPose, new RamseteController(TrajectoryFactory.AutoConstants.kRamseteB, TrajectoryFactory.AutoConstants.kRamseteZeta), drivetrainConstants.getKinematics(), drivetrain::smartVelocityControlMetersPerSec, drivetrain);
    Command runThenStop = ramseteCommand.andThen(() -> drivetrain.stop());
    if (resetOnStart) {
        Command resetPose = new InstantCommand(() -> drivetrain.resetOdometry(trajectory.getInitialPose()));
        return resetPose.andThen(runThenStop);
    }
    return runThenStop;
}