-
-
Save 22raor/ba301b715badd19cfec38341e1929b64 to your computer and use it in GitHub Desktop.
Basic drive distance with PID
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// Copyright (c) FIRST and other WPILib contributors. | |
// Open Source Software; you can modify and/or share it under the terms of | |
// the WPILib BSD license file in the root directory of this project. | |
package frc.robot.commands; | |
import edu.wpi.first.math.controller.PIDController; | |
import edu.wpi.first.wpilibj2.command.CommandBase; | |
import frc.robot.RobotContainer.Subsystems; | |
public class SimpleDriveDistance extends CommandBase { | |
/** Creates a new SimpleDriveDistance. */ | |
double distance; | |
public SimpleDriveDistance(double distance) { | |
// Use addRequirements() here to declare subsystem dependencies. | |
this.distance = distanceToRevolutions(distance); | |
} | |
public double distanceToRevolutions(double distance) { | |
return distance; //here u should multiply by wheel circumference and gear ratio | |
} | |
// Called when the command is initially scheduled. | |
public PIDController pidControl; | |
@Override | |
public void initialize() { | |
pidControl = new PIDController(0.05, 0, 0); | |
} | |
// Called every time the scheduler runs while the command is scheduled. | |
@Override | |
public void execute() { | |
double measurement = Subsystems.driveSubsystem.getAverageEncoderDistance(); | |
double minEffort = 0.2; //minimum effort for the robot to move | |
double pidEffort = pidControl.calculate(measurement, distance ); | |
double direction = Math.signum(pidControl.getPositionError()); //get the sign (0, 1, or -1) of the error | |
Subsystems.driveSubsystem.arcadeDrive(pidEffort + minEffort * direction, 0); | |
} | |
// Called once the command ends or is interrupted. | |
@Override | |
public void end(boolean interrupted) { | |
Subsystems.driveSubsystem.arcadeDrive(0, 0); | |
} | |
// Returns true when the command should end. | |
@Override | |
public boolean isFinished() { | |
return false; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment