Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Finished Drive Straight Velocity PID and started Position PID

Browse Source
This commit is contained in:
aarav18
2020-02-06 19:43:37 -07:00
parent 987a3a2647
commit 92c4a18c6e
5 changed files with 32 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java
+10 -15
View File
@@ -14,38 +14,33 @@ import frc4388.robot.subsystems.Drive;
public class DriveStraightToPositionPID extends CommandBase {
Drive m_drive;
double m_distance;
double m_leftTarget;
double m_rightTarget;
double m_targetPos;
double m_targetGyro;
/**
* Creates a new DriveToDistancePID.
* @param subsystem drive subsystem
* @param distance distance to travel in inches
* @param targetPos distance to travel in inches
*/
public DriveStraightToPositionPID(Drive subsystem, double distance) {
public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_distance = distance * DriveConstants.TICKS_PER_INCH;
m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH;
addRequirements(m_drive);
SmartDashboard.putNumber("Distance Target Inches", distance);
SmartDashboard.putNumber("Distance Target Inches", targetPos);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
SmartDashboard.putNumber("Distance Target Ticks", m_distance);
SmartDashboard.putNumber("Left Target Ticks", m_leftTarget);
SmartDashboard.putNumber("Right Target Ticks", m_rightTarget);
m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
//m_drive.runPositionPID(m_drive.m_leftFrontMotor, m_leftTarget);
//m_drive.runPositionPID(m_drive.m_rightFrontMotor, m_rightTarget);
System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
m_drive.runDriveStraightPositionPID(m_targetPos, m_targetGyro);
}
// Called once the command ends or is interrupted.
@@ -56,7 +51,7 @@ public class DriveStraightToPositionPID extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_targetPos) < 500){
return true;
} else {
return false;