From: Cindy Zhang Date: Sat, 28 Jan 2017 04:20:14 +0000 (-0800) Subject: finish implement driveDistance and TurnForAngle using PID algorithm X-Git-Url: http://challenge-bot.com/repos/?p=3501%2F2017steamworks;a=commitdiff_plain;h=f0a71840f17c1039ce4be1f66cf324cc979a9966 finish implement driveDistance and TurnForAngle using PID algorithm --- diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index a9dd0ff..5f4715c 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team3501.robot; -import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; import org.usfirst.frc.team3501.robot.subsystems.Intake; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -40,7 +39,6 @@ public class Robot extends IterativeRobot { @Override public void autonomousInit() { - Scheduler.getInstance().add(new DriveDistance(25, 10)); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java index 5c0c20b..b569958 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -70,14 +70,12 @@ public class DriveDistance extends Command { xVal = -this.gyroController .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle()); } - // System.out.println("turn: " + xVal); double leftDrive = yVal - xVal; double rightDrive = yVal + xVal; this.driveTrain.setMotorValues(leftDrive, rightDrive); driveTrain.printEncoderOutput(); - // System.out.println("motorval: " + yVal); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java index a56b1f3..d1e0f9c 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -7,8 +7,8 @@ import edu.wpi.first.wpilibj.command.Command; /** * This command will run throughout teleop and listens for joystick inputs to - * drive the driveTrain. This never finishes until teleop ends. - * - works in conjunction with OI.java + * drive the driveTrain. This never finishes until teleop ends. - works in + * conjunction with OI.java */ public class JoystickDrive extends Command { diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java index 334fa0b..86176f1 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java @@ -1,7 +1,10 @@ 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; @@ -9,41 +12,75 @@ 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(); } } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 44744c4..c445f63 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.command.Subsystem; 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; @@ -77,20 +77,12 @@ public class DriveTrain extends Subsystem { 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 @@ -104,8 +96,9 @@ public class DriveTrain extends Subsystem { } 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() {