From 33141cddfe931d276b0068687c9bac25f29f8ba5 Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Sun, 14 Feb 2016 15:47:22 -0800 Subject: [PATCH] Add pid subsystem --- .../usfirst/frc/team3501/robot/Constants.java | 27 +-- .../usfirst/frc/team3501/robot/MathLib.java | 4 +- .../robot/commands/driving/DriveDistance.java | 60 +++++ .../robot/commands/driving/JoystickDrive.java | 42 ++++ .../team3501/robot/commands/driving/Stop.java | 40 +-- .../intakearm/MoveIntakeArmUp.java~HEAD | 30 +++ ...ctor commands and change drivetrain to pid | 30 +++ .../team3501/robot/subsystems/DriveTrain.java | 227 ++++++++++++++---- .../team3501/robot/subsystems/IntakeArm.java | 2 +- 9 files changed, 385 insertions(+), 77 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java create mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD create mode 100644 src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index e0921698..8f121992 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -23,24 +23,26 @@ public class Constants { public static class DriveTrain { // Drivetrain Motor Related Ports - public static final int FRONT_LEFT = 0; - public static final int FRONT_RIGHT = 0; - public static final int REAR_LEFT = 0; - public static final int REAR_RIGHT = 0; + public static final int FRONT_LEFT = 1; + public static final int FRONT_RIGHT = 4; + public static final int REAR_LEFT = 2; + public static final int REAR_RIGHT = 3; // Encoder related ports - public final static int ENCODER_LEFT_A = 3; - public final static int ENCODER_LEFT_B = 4; - public final static int ENCODER_RIGHT_A = 2; - public final static int ENCODER_RIGHT_B = 1; + public final static int ENCODER_LEFT_A = 0; + public final static int ENCODER_LEFT_B = 1; + public final static int ENCODER_RIGHT_A = 9; + public final static int ENCODER_RIGHT_B = 8; private final static double WHEEL_DIAMETER = 6.0; // in inches private final static double PULSES_PER_ROTATION = 256; // in pulses private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches - public final static double INCHES_PER_PULSE = (((Math.PI) - * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; + public static final double INCHES_PER_PULSE = + ((3.66 / 5.14) * 6 * Math.PI) / 256; + + public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; + public static double time = 0; } public static class Scaler { @@ -77,9 +79,6 @@ public class Constants { public static final int POT_CHANNEL = 0; public static final double INTAKE_SPEED = 0.5; public static final double OUTPUT_SPEED = -0.5; - public static final double UP_SPEED = 0.3; - public static final double DOWN_SPEED = -0.3; - public static final double STOP_SPEED = 0.0; public final static double FULL_RANGE = 270.0; // in degrees public final static double OFFSET = -135.0; // in degrees public static final double ZERO_ANGLE = 0; diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java index a57b5ce7..b7eaeb7c 100644 --- a/src/org/usfirst/frc/team3501/robot/MathLib.java +++ b/src/org/usfirst/frc/team3501/robot/MathLib.java @@ -57,7 +57,7 @@ public class MathLib { * the largest acceptable value. * @return returns value restricted to be within the range [low, high]. */ - public static double restrictToRange(double value, double low, double high) { + public static double constrain(double value, double low, double high) { value = Math.max(value, low); value = Math.min(value, high); return value; @@ -65,7 +65,7 @@ public class MathLib { /*** * Returns true if val is in the range [low, high] - * + * * @param val * value to test * @param low diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java new file mode 100644 index 00000000..1eb707c0 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -0,0 +1,60 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/*** + * This command will move the robot at the specified speed for the specified + * distance. + * + * post-condition: has driven for the distance and speed specified + * + * @author Meryem and Avi + * + */ +public class DriveDistance extends Command { + private double maxTimeOut; + double distance; + int count = 0; + + public DriveDistance(double distance, double maxTimeOut) { + requires(Robot.driveTrain); + this.maxTimeOut = maxTimeOut; + this.distance = distance; + } + + @Override + protected void initialize() { + Robot.driveTrain.resetEncoders(); + } + + @Override + protected void execute() { + Robot.driveTrain.driveDistance(distance, maxTimeOut); + + } + + @Override + protected boolean isFinished() { + if (timeSinceInitialized() >= maxTimeOut + || Robot.driveTrain + .reachedTarget() || Robot.driveTrain.getError() < 1) { + System.out.println("time: " + timeSinceInitialized()); + Constants.DriveTrain.time = timeSinceInitialized(); + return true; + } + return false; + } + + @Override + protected void end() { + Robot.driveTrain.disable(); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java new file mode 100644 index 00000000..41e44d31 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -0,0 +1,42 @@ +package org.usfirst.frc.team3501.robot.commands.driving; + +import org.usfirst.frc.team3501.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class JoystickDrive extends Command { + + public JoystickDrive() { + requires(Robot.driveTrain); + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + // IDK why but the joystick gives positive values for pulling backwards + double left = -Robot.oi.leftJoystick.getY(); + double right = -Robot.oi.rightJoystick.getY(); + Robot.driveTrain.drive(left, right); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + Robot.driveTrain.stop(); + } + + @Override + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java b/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java index dc134394..675f9a90 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/Stop.java @@ -1,30 +1,34 @@ package org.usfirst.frc.team3501.robot.commands.driving; +import org.usfirst.frc.team3501.robot.Robot; + import edu.wpi.first.wpilibj.command.Command; public class Stop extends Command { - public Stop() { - } + public Stop() { + } - @Override - protected void initialize() { - } + @Override + protected void initialize() { + Robot.driveTrain.stop(); + } - @Override - protected void execute() { - } + @Override + protected void execute() { + } - @Override - protected boolean isFinished() { - return false; - } + @Override + protected boolean isFinished() { + return true; + } - @Override - protected void end() { - } + @Override + protected void end() { + } - @Override - protected void interrupted() { - } + @Override + protected void interrupted() { + end(); + } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD new file mode 100644 index 00000000..c22bf82d --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~HEAD @@ -0,0 +1,30 @@ +package org.usfirst.frc.team3501.robot.commands.intakearm; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveIntakeArmUp extends Command { + + public MoveIntakeArmUp() { + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid new file mode 100644 index 00000000..bc7b824f --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmUp.java~Refactor commands and change drivetrain to pid @@ -0,0 +1,30 @@ +package org.usfirst.frc.team3501.robot.commands.intakearm; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveIntakeArmUp extends Command { + public MoveIntakeArmUp() { + } + + @Override + protected void initialize() { + } + + @Override + protected void execute() { + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + } + + @Override + protected void interrupted() { + } + +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 935b4302..81f80b92 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,59 +1,82 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; -import org.usfirst.frc.team3501.robot.Lidar; +import org.usfirst.frc.team3501.robot.GyroLib; +import org.usfirst.frc.team3501.robot.MathLib; +import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; -import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.RobotDrive; +import edu.wpi.first.wpilibj.command.PIDSubsystem; + +public class DriveTrain extends PIDSubsystem { + private static double pidOutput = 0; + private static double encoderTolerance = 8.0, gyroTolerance = 5.0; + private int DRIVE_MODE = 1; + + private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; -public class DriveTrain extends Subsystem { - // Drivetrain related objects private Encoder leftEncoder, rightEncoder; - public static Lidar lidar; private CANTalon frontLeft, frontRight, rearLeft, rearRight; - private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC; - // Drivetrain specific constants that relate to the inches per pulse value for - // the encoders - private final static double WHEEL_DIAMETER = 6.0; // in inches - private final static double PULSES_PER_ROTATION = 256; // in pulses - private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches - private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches - public final static double INCHES_PER_PULSE = (((Math.PI) - * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; - - // Drivetrain specific constants that relate to the PID controllers - private final static double Kp = 1.0, Ki = 0.0, - Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; - - public AnalogInput channel; + private RobotDrive robotDrive; + + private GyroLib gyro; public DriveTrain() { + super(kp, ki, kd); + frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT); rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT); - lidar = new Lidar(I2C.Port.kOnboard); + robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X); leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); - leftEncoder.setDistancePerPulse(INCHES_PER_PULSE); - rightEncoder.setDistancePerPulse(INCHES_PER_PULSE); + + leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); + rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE); + + gyro = new GyroLib(I2C.Port.kOnboard, false); + + DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; + setEncoderPID(); + this.disable(); + gyro.start(); } @Override protected void initDefaultCommand() { + setDefaultCommand(new JoystickDrive()); + } + + public void printOutput() { + System.out.println("PIDOutput: " + pidOutput); + } + + private double getAvgEncoderDistance() { + return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2; + } + + public boolean reachedTarget() { + if (this.onTarget()) { + this.disable(); + return true; + } else { + return false; + } + } + + public void stop() { + drive(0, 0); } public void resetEncoders() { @@ -61,10 +84,6 @@ public class DriveTrain extends Subsystem { rightEncoder.reset(); } - public double getLidarDistance() { - return lidar.pidGet(); - } - public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -85,20 +104,144 @@ public class DriveTrain extends Subsystem { return leftEncoder.getDistance(); // in inches } - public double getDistance() { - return (getRightDistance() + getLeftDistance()) / 2.0; // in inches + public double getError() { + if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) + return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); + else + return Math.abs(this.getSetpoint() + getGyroAngle()); } - public void stop() { - setMotorSpeeds(0, 0); + public double getGyroAngle() { + return gyro.getRotationZ().getAngle(); + } + + public void resetGyro() { + gyro.reset(); + } + + public void printEncoder(int i, int n) { + if (i % n == 0) { + System.out.println("Left: " + this.getLeftDistance()); + System.out.println("Right: " + this.getRightDistance()); + + } + } + + public void printGyroOutput() { + System.out.println("Gyro Angle" + -this.getGyroAngle()); + } + + public double getOutput() { + return pidOutput; + } + + public void updatePID() { + if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) + this.getPIDController().setPID(kp, ki, kd); + else + this.getPIDController().setPID(gp, gd, gi); + } + + public CANTalon getFrontLeft() { + return frontLeft; + } + + public CANTalon getFrontRight() { + return frontRight; + } + + public CANTalon getRearLeft() { + return rearLeft; + } + + public CANTalon getRearRight() { + return rearRight; + } + + public int getMode() { + return DRIVE_MODE; + } + + @Override + protected void usePIDOutput(double output) { + double left = 0; + double right = 0; + if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) { + double drift = this.getLeftDistance() - this.getRightDistance(); + if (Math.abs(output) > 0 && Math.abs(output) < 0.3) + output = Math.signum(output) * 0.3; + left = output; + right = output + drift * kp / 10; + } + else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { + left = output; + right = -output; + } + drive(left, right); + pidOutput = output; } - public void setMotorSpeeds(double leftSpeed, double rightSpeed) { - // speed passed to right motor is negative because right motor rotates in - // opposite direction - this.frontLeft.set(leftSpeed); - this.frontRight.set(-rightSpeed); - this.rearLeft.set(leftSpeed); - this.rearRight.set(-rightSpeed); + @Override + protected double returnPIDInput() { + return sensorFeedback(); } + + private double sensorFeedback() { + if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) + return getAvgEncoderDistance(); + else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) + return -this.getGyroAngle(); + // counterclockwise is positive on joystick but we want it to be negative + else + return 0; + } + + public void drive(double left, double right) { + robotDrive.tankDrive(-left, -right); + // dunno why but inverted drive (- values is forward) + } + + public void driveDistance(double dist, double maxTimeOut) { + dist = MathLib.constrain(dist, -100, 100); + setEncoderPID(); + setSetpoint(dist); + } + + public void setEncoderPID() { + DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; + this.updatePID(); + this.setAbsoluteTolerance(encoderTolerance); + this.setOutputRange(-1.0, 1.0); + this.setInputRange(-200.0, 200.0); + this.enable(); + } + + private void setGyroPID() { + DRIVE_MODE = Constants.DriveTrain.GYRO_MODE; + this.updatePID(); + this.getPIDController().setPID(gp, gi, gd); + + this.setAbsoluteTolerance(gyroTolerance); + this.setOutputRange(-1.0, 1.0); + this.setInputRange(-360.0, 360.0); + this.enable(); + } + + public void turnAngle(double angle) { + angle = MathLib.constrain(angle, -360, 360); + setGyroPID(); + setSetpoint(angle); + } + + public void setMotorSpeeds(double left, double right) { + // positive setpoint to left side makes it go backwards + // positive setpoint to right side makes it go forwards. + frontLeft.set(-left); + rearLeft.set(-left); + frontRight.set(right); + rearRight.set(right); + } + + private static double kp = 0.013, ki = 0.000015, kd = -0.002; + private static double gp = 0.018, gi = 0.000015, gd = 0; } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java index a707777e..a9e9ecd5 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/IntakeArm.java @@ -49,7 +49,7 @@ public class IntakeArm extends Subsystem { } public void stopRollers() { - intakeRoller.set(Constants.IntakeArm.STOP_SPEED); + intakeRoller.set(0); } /*** -- 2.30.2