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

Velocity Control PID

Browse Source 
Does not work in testing
This commit is contained in:
aarav18
2020-01-24 20:27:42 -07:00
parent 0da2e35400
commit dec773cdcd
3 changed files with 48 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java
+11 -5
View File
@@ -9,6 +9,7 @@ package frc4388.robot.commands;
import com.ctre.phoenix.motorcontrol.FollowerType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.subsystems.Drive;
@@ -18,6 +19,7 @@ public class DriveAtVelocityPID extends CommandBase {
double m_targetVel;
double m_leftTarget;
double m_rightTarget;
double m_copiedTargetVel;
/**
* Creates a new DriveAtVelocityPID.
* @param subsystem drive subsystem
@@ -26,23 +28,27 @@ public class DriveAtVelocityPID extends CommandBase {
public DriveAtVelocityPID(Drive subsystem, double targetVel) {
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_targetVel = targetVel /* DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;*/;
m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;
m_copiedTargetVel = targetVel;
addRequirements(m_drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_leftTarget = -m_targetVel;
m_leftTarget = m_targetVel;
m_rightTarget = m_targetVel;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.runVelocityPID(m_rightTarget);
m_drive.runVelocityPID(m_leftTarget);
m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
m_drive.runVelocityPID(m_targetVel);
SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel);
SmartDashboard.putNumber("Output Target Velocity", m_targetVel);
//m_drive.runVelocityPID(m_leftTarget);
//m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput);
}
// Called once the command ends or is interrupted.