Skip to content

Commit

Permalink
PID sample command
Browse files Browse the repository at this point in the history
  • Loading branch information
ctychen committed Nov 13, 2019
1 parent 2fec07b commit 81065ca
Show file tree
Hide file tree
Showing 6 changed files with 67 additions and 97 deletions.
41 changes: 0 additions & 41 deletions MiniLib/src/frc/team670/pi/PIDMotor.java

This file was deleted.

2 changes: 1 addition & 1 deletion MiniLib/src/frc/team670/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ public void robotInit() {
Logger.consoleLog();

// The command we want to test goes here
Scheduler.getInstance().add(new PIDDistanceTest(50));
Scheduler.getInstance().add(new PIDDistanceDrive(10));
// Scheduler.getInstance().add(new DistanceDrive(10, 1.0));
}

Expand Down
4 changes: 2 additions & 2 deletions MiniLib/src/frc/team670/robot/RobotConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@

public class RobotConstants {

/** Drive Base Wheel Diameter in Centimeters */
public static final double DRIVE_BASE_WHEEL_DIAMETER = 6.343;
/** Drive Base Wheel Diameter in Inches */
public static final double DRIVE_BASE_WHEEL_DIAMETER = 2.497;
public static final int ENCODER_TICKS_PER_ROTATION = 800;


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,11 @@
import frc.team670.robot.Robot;
import frc.team670.robot.utils.Logger;

public class DistanceD extends Command {
public class DistanceDrive extends Command {

private double speedL, speedR, dist;

public DistanceD(double distance_cm, double lspeed, double rspeed) {
public DistanceDrive(double distance_cm, double lspeed, double rspeed) {
this.speedL = lspeed;
this.speedR = rspeed;
//this.seconds = seconds;
Expand Down
62 changes: 62 additions & 0 deletions MiniLib/src/frc/team670/robot/commands/drive/PIDDistanceDrive.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
package frc.team670.robot.commands.drive;

import edu.wpi.first.wpilibj.command.*;
import edu.wpi.first.wpilibj.controller.PIDController;
import frc.team670.robot.Robot;
import frc.team670.robot.utils.Logger;
import frc.team670.robot.utils.MathUtils;

public class PIDDistanceDrive extends Command {

PIDController m_leftController, m_rightController;

double m_targetInches;

// TODO: Find values
// proportional speed constant
double kP = 0.0;

// integral speed constant
double kI = 0.0;

// derivative speed constant
double kD = 0.0;

// margin of error
double TOLERANCE_INCHES = 0.5;

/**
*
* @param target Target distance in inches
*/
public PIDDistanceDrive(double targetInches) {
m_targetInches = targetInches;
m_leftController = new PIDController(kP, kI, kD);
m_leftController.setTolerance(MathUtils.convertInchesToEncoderTicks(TOLERANCE_INCHES));
m_rightController = new PIDController(kP, kI, kD);
m_rightController.setTolerance(MathUtils.convertInchesToEncoderTicks(TOLERANCE_INCHES));
requires(Robot.driveBase);
}

public void initialize() {
m_leftController.setSetpoint(MathUtils.convertInchesToEncoderTicks(m_targetInches));
m_rightController.setSetpoint(MathUtils.convertInchesToEncoderTicks(m_targetInches));
Logger.consoleLog("Target ticks: %s TicksL: %s TicksR: %s", m_leftController.getSetpoint(),
Robot.driveBase.getLeftEncoder().getTicks(), Robot.driveBase.getRightEncoder().getTicks());
}

public void execute() {
Logger.consoleLog("TicksL: %s TicksR: %s", Robot.driveBase.getLeftEncoder().getTicks(),
Robot.driveBase.getRightEncoder().getTicks());
Robot.driveBase.tankDrive(m_leftController.calculate(Robot.driveBase.getLeftEncoder().getTicks()),
m_rightController.calculate(Robot.driveBase.getRightEncoder().getTicks()));
}

public boolean isFinished() {
return m_leftController.atSetpoint() || m_rightController.atSetpoint();
}

public void end() {
Robot.driveBase.stop();
}
}
51 changes: 0 additions & 51 deletions MiniLib/src/frc/team670/robot/commands/drive/PIDDistanceTest.java

This file was deleted.

0 comments on commit 81065ca

Please sign in to comment.