package org.usfirst.frc.team3501.robot.commands.driving;
+import org.usfirst.frc.team3501.robot.Constants;
import org.usfirst.frc.team3501.robot.Constants.Direction;
import org.usfirst.frc.team3501.robot.Robot;
+import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.utils.PIDController;
import edu.wpi.first.wpilibj.command.Command;
* This command turns the robot for a specified angle in specified direction -
* either left or right
*
- * parameters:
- * angle: the robot will turn - in degrees
- * direction: the robot will turn - either right or left
- * motorVal: the motor input to set the motors to
+ * parameters: angle: the robot will turn - in degrees direction: the robot will
+ * turn - either right or left maxTimeOut: the max time this command will be
+ * allowed to run on for
*/
public class TurnForAngle extends Command {
+ private DriveTrain driveTrain = Robot.getDriveTrain();
- public TurnForAngle(double angle, Direction direction, double motorVal) {
+ Direction direction;
+ private double maxTimeOut;
+ private PIDController gyroController;
+
+ private double target;
+ private double gyroP;
+ private double gyroI;
+ private double gyroD;
+
+ public TurnForAngle(double angle, Direction direction, double maxTimeOut) {
requires(Robot.getDriveTrain());
+ this.direction = direction;
+ this.maxTimeOut = maxTimeOut;
+ this.target = angle;
+
+ this.gyroP = driveTrain.defaultGyroP;
+ this.gyroI = driveTrain.defaultGyroI;
+ this.gyroD = driveTrain.defaultGyroD;
+
+ this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD);
+ this.gyroController.setDoneRange(1);
+ this.gyroController.setMinDoneCycles(5);
}
- // Called just before this Command runs the first time
@Override
protected void initialize() {
+ this.driveTrain.resetEncoders();
+ this.driveTrain.resetGyro();
+ this.gyroController.setSetPoint(this.target);
}
- // Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
+ double xVal = 0;
+
+ xVal = this.gyroController
+ .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
+
+ double leftDrive;
+ double rightDrive;
+ if (direction == Constants.Direction.RIGHT) {
+ leftDrive = xVal;
+ rightDrive = -xVal;
+ } else {
+ leftDrive = -xVal;
+ rightDrive = xVal;
+ }
+
+ this.driveTrain.setMotorValues(leftDrive, rightDrive);
}
- // Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
- return false;
+ return timeSinceInitialized() >= maxTimeOut || gyroController.isDone();
}
- // Called once after isFinished returns true
@Override
protected void end() {
}
- // Called when another command which requires one or more of the same
- // subsystems is scheduled to run
@Override
protected void interrupted() {
+ end();
}
}
public class DriveTrain extends Subsystem {
public static double driveP = 0.008, driveI = 0.001, driveD = -0.002;
- public static double defaultGyroP = 0.009, defaultGyroI = 0.00000,
+ public static double defaultGyroP = 0.006, defaultGyroI = 0.00000,
defaultGyroD = -0.000;
private double gyroZero = 0;
setMotorValues(0, 0);
}
- public double getFrontLeftMotorVal() {
- return frontLeft.get();
+ public double getLeftMotorVal() {
+ return (frontLeft.get() + rearLeft.get()) / 2;
}
- public double getFrontRightMotorVal() {
- return frontRight.get();
- }
-
- public double getRearLeftMotorVal() {
- return frontLeft.get();
- }
-
- public double getRearRightMotorVal() {
- return frontLeft.get();
+ public double getRightMotorVal() {
+ return (frontRight.get() + rearRight.get()) / 2;
}
// ENCODER METHODS
}
public void printEncoderOutput() {
- System.out.println("left: " + getLeftEncoderDistance());
- System.out.println("right: " + getRightEncoderDistance());
+ // System.out.println("left: " + getLeftEncoderDistance());
+ // System.out.println("right: " + getRightEncoderDistance());
+ System.out.println(getAvgEncoderDistance());
}
public double getAvgEncoderDistance() {