diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e431cf0..7cc1206 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -45,6 +45,7 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + robotContainer.periodic(); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 964c827..6faddb0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,8 +21,6 @@ import static frc.robot.settings.Constants.Ps4.*; -import org.opencv.core.Point; - import frc.robot.commands.PointAtCargo; import frc.robot.commands.climber.ArmPneumaticTipping; import frc.robot.commands.climber.AutomatedClimb; @@ -32,6 +30,7 @@ // import frc.robot.subsystems.LightsHardware; import frc.robot.subsystems.Vision; import frc.robot.subsystems.Climber.RungLockState; +import frc.robot.subsystems.HallEffectSensorSelection; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pixy2SubSystem; import frc.robot.commands.climber.ClimberClimb; @@ -45,6 +44,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.JoystickButton; + /** * This class is where the bulk of the robot should be declared. Since * Command-based is a @@ -123,19 +123,33 @@ public RobotContainer() { private void configureSmartDashboard() { SmartDashboard.putData("Test Vision", new PointAtCargo(drivetrain, vision)); SmartDashboard.putData("drivetrain", drivetrain); - SmartDashboard.putData("Burn In", new BurnIn(drivetrain)); - SmartDashboard.putData("forwardOneSecond", new ForwardDistance(drivetrain, 1, .25)); + //SmartDashboard.putData("Burn In", new BurnIn(drivetrain)); + SmartDashboard.putData("forwardOneSecond", new ForwardDistance(drivetrain, .5, .2)); + //SmartDashboard.putData("forwardOneSecond2", new ForwardDistance(drivetrain, .5, .2)); SmartDashboard.putData("climbUp", new ClimberClimb(climber, ArmExtendState.OUT)); SmartDashboard.putData("climbDown", new ClimberClimb(climber, ArmExtendState.IN)); //SmartDashboard.putData("armLock", new WedgePneumatic(climber, RungLockState.Locked)); SmartDashboard.putData("tiltDown", new ArmPneumaticTipping(climber, ArmTipState.DOWN)); SmartDashboard.putData("tiltUp", new ArmPneumaticTipping(climber, ArmTipState.UP)); + + SmartDashboard.putData("ArmDown", new MoveArm(intake, IntakeArmState.armDown)); + SmartDashboard.putData("ArmUp", new MoveArm(intake, IntakeArmState.armUp)); + + SmartDashboard.putData("ClimbTestingUp", new ClimberClimb(climber, ArmExtendState.OUT, true)); + SmartDashboard.putData("ClimbTestingDown", new ClimberClimb(climber, ArmExtendState.IN,true)); + } public void initTelemetry() { } + void periodic() { + SmartDashboard.putBoolean("TopRight", climber.isHallEffectSensorClosed(HallEffectSensorSelection.TopRight)); + SmartDashboard.putBoolean("TopLeft", climber.isHallEffectSensorClosed(HallEffectSensorSelection.TopLeft)); + SmartDashboard.putBoolean("BottomRight", climber.isHallEffectSensorClosed(HallEffectSensorSelection.BottomRight)); + SmartDashboard.putBoolean("BottomLeft", climber.isHallEffectSensorClosed(HallEffectSensorSelection.BottomLeft)); + } /** * Use this method to define your button->command mappings. Buttons can be * created by diff --git a/src/main/java/frc/robot/commands/climber/ClimberClimb.java b/src/main/java/frc/robot/commands/climber/ClimberClimb.java index 83314ab..66e957c 100644 --- a/src/main/java/frc/robot/commands/climber/ClimberClimb.java +++ b/src/main/java/frc/robot/commands/climber/ClimberClimb.java @@ -3,11 +3,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Climber; import static frc.robot.settings.Constants.Climber.CLIMBER_SPEED; +import static frc.robot.settings.Constants.Climber.TESTING_CLIMBER_SPEED; public class ClimberClimb extends CommandBase { Climber climber; ArmExtendState state; + double currentSpeed; public enum ArmExtendState { IN, @@ -26,6 +28,25 @@ public ClimberClimb(Climber climber, ArmExtendState armState) { addRequirements(climber); state = armState; + currentSpeed = CLIMBER_SPEED; + } + + public ClimberClimb(Climber climber, ArmExtendState armState, boolean testingSpeed) + { + //new ClimberClimb(climber, armState); + this.climber = climber; + addRequirements(climber); + + state = armState; + + if (testingSpeed == true) + { + currentSpeed = TESTING_CLIMBER_SPEED; + } + else + { + currentSpeed = CLIMBER_SPEED; + } } @@ -35,10 +56,10 @@ public void execute() { switch (state) { case OUT: - climber.climberOut(CLIMBER_SPEED); + climber.climberOut(currentSpeed); break; case IN: - climber.climberIn(CLIMBER_SPEED); + climber.climberIn(currentSpeed); break; } } diff --git a/src/main/java/frc/robot/commands/climber/HallEfectArmEncoderReset.java b/src/main/java/frc/robot/commands/climber/HallEfectArmEncoderReset.java new file mode 100644 index 0000000..74897a0 --- /dev/null +++ b/src/main/java/frc/robot/commands/climber/HallEfectArmEncoderReset.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.climber; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Climber; + +public class HallEfectArmEncoderReset extends CommandBase { + /** Creates a new HallEfectArmEncoderReset. */ + Climber climber; + public HallEfectArmEncoderReset(Climber climber) { + this.climber = climber; + } + + @Override + public void execute() { + climber.climberIn(.1); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + climber.stop(); + climber.resetEncoders(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + + + + + return climber.isClimberFullyIn(); + } +} diff --git a/src/main/java/frc/robot/commands/drivetrain/GoForwardInInches.java b/src/main/java/frc/robot/commands/drivetrain/GoForwardInInches.java new file mode 100644 index 0000000..071412c --- /dev/null +++ b/src/main/java/frc/robot/commands/drivetrain/GoForwardInInches.java @@ -0,0 +1,35 @@ +package frc.robot.commands.drivetrain; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + + +public class GoForwardInInches extends CommandBase{ + private Drivetrain drivetrain; + private double power; + private double ticksNeeded; + private double ticksTraveled; + public GoForwardInInches(Drivetrain drivetrain, double power, double inches){ + addRequirements(drivetrain); + this.drivetrain = drivetrain; + this.power = power; + ticksNeeded = drivetrain.convertInchesToTicks(inches); + } + + @Override + public void initialize() { + ticksTraveled = drivetrain.getLeftEncoderValue(); + drivetrain.setDrive(ControlMode.PercentOutput, power, power); + } + @Override + public void end(boolean interrupted) { + drivetrain.setDrive(ControlMode.PercentOutput, 0, 0); + } + @Override + public boolean isFinished() { + return drivetrain.getLeftEncoderValue() + ticksNeeded == ticksTraveled; + } +} diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index daf4cc1..6c128fb 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -37,6 +37,13 @@ private Climber() { public static final double TIME_FOR_ARM_TO_GO_UP = 2; public static final double CLIMBER_SPEED = .2; + public static final double TESTING_CLIMBER_SPEED = .1; + + public static final double ENCODER_TICKS_TO_ARMS_LENGTH = + (1/2048.0)*(1/23.73)*(1.25*Math.PI)*(1/24.0); + //nu> motor > spool > inches > arm lengths + + public static final double ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE = ENCODER_TICKS_TO_ARMS_LENGTH / 1; } public final class Ps4 { @@ -69,6 +76,10 @@ private Drivetrain() { public static final int RIGHT_LEAD_ID = 2; public static final int RIGHT_FOLLOW_ID = 4; + public static final double ENCODER_TICKS_TO_DISTANCE = + (1/2048)*(8/1)*(2*Math.PI); + //Units, ChainRatio, WheelSize + //1/2048, 8:1, 2pi } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ba888ca..8da8d18 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -9,12 +9,16 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import org.opencv.objdetect.CascadeClassifier; + import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.robot.settings.Constants.Climber.*; +import javax.xml.crypto.dsig.keyinfo.RetrievalMethod; + public class Climber extends SubsystemBase { public enum RungLockState { @@ -55,6 +59,9 @@ public Climber() { rightWinchMotor.setNeutralMode(NeutralMode.Brake); leftWinchMotor.setNeutralMode(NeutralMode.Brake); + + rightWinchMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE); + leftWinchMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_ARMS_LENGTH_DIVIDED_BY_ONE); //negative percent output values bring climber in, positive bring it out. toggleLock(); @@ -151,6 +158,31 @@ public boolean isClimberFullyOut() // if not open(closed) return leftWinchMotor.isFwdLimitSwitchClosed() != 0 && rightWinchMotor.isFwdLimitSwitchClosed() != 0; } + + public boolean isHallEffectSensorClosed(HallEffectSensorSelection sensor) + { + switch (sensor) { + case TopRight: + return rightWinchMotor.isFwdLimitSwitchClosed() != 0; + case BottomRight: + return rightWinchMotor.isRevLimitSwitchClosed() != 0; + case TopLeft: + return leftWinchMotor.isFwdLimitSwitchClosed() != 0; + case BottomLeft: + return leftWinchMotor.isRevLimitSwitchClosed() != 0; + } + return true; + } + + public double getLeftArmPos() + { + return leftWinchMotor.getSelectedSensorPosition() * ENCODER_TICKS_TO_ARMS_LENGTH; + } +//1.65 1.25 - 26.5 in + public double getRightArmPos() + { + return rightWinchMotor.getSelectedSensorPosition() * ENCODER_TICKS_TO_ARMS_LENGTH; + } /** * use motors to move the climber into extended position * @@ -170,9 +202,15 @@ public void climberIn(double speed) { setMotorSpeed(leftSpeed, rightSpeed); } -public boolean isClimberFullyIn(){ + public boolean isClimberFullyIn(){ return leftWinchMotor.isRevLimitSwitchClosed() != 0 && rightWinchMotor.isRevLimitSwitchClosed() != 0; -} + } + + public void resetEncoders(){ + leftWinchMotor.setSelectedSensorPosition(0); + rightWinchMotor.setSelectedSensorPosition(0); + + } public void stop() { setMotorSpeed(0, 0); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 36a115f..818e934 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -27,6 +27,11 @@ public Drivetrain() { leftMotors = new MotorControllerGroup(leftLeadMotor, leftFollowMotor); rightMotors = new MotorControllerGroup(rightLeadMotor, rightFollowMotor); + leftLeadMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE); + rightLeadMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE); + leftFollowMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE); + leftFollowMotor.configForwardSoftLimitThreshold(ENCODER_TICKS_TO_DISTANCE); + leftFollowMotor.follow(leftLeadMotor); rightFollowMotor.follow(rightLeadMotor); @@ -68,6 +73,7 @@ public void setDrive(double leftSpeed, double rightSpeed) { * Controls motors with control mode. (Left, Right) * Also, never gonna give you up, never gonna let you down, never going to annoy * Liam by typing this comment. :) + * Ha! I put more stuff here --Ben */ public void setDrive(ControlMode mode, double leftSpeed, double rightSpeed) { setDriveLeft(mode, leftSpeed); @@ -96,4 +102,23 @@ public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInP public void periodic() { bbDriveSystem.feed(); } + + public void resetEncoders() { + leftLeadMotor.setSelectedSensorPosition(0); + rightLeadMotor.setSelectedSensorPosition(0); + leftFollowMotor.setSelectedSensorPosition(0); + rightFollowMotor.setSelectedSensorPosition(0); + } + + public double getLeftEncoderValue(){ + return leftLeadMotor.getSelectedSensorPosition(); + } + + public double getRightEncoderValue(){ + return rightLeadMotor.getSelectedSensorPosition(); + } + + public double convertInchesToTicks(double inches){ + return inches * ENCODER_TICKS_TO_DISTANCE; + } } diff --git a/src/main/java/frc/robot/subsystems/HallEffectSensorSelection.java b/src/main/java/frc/robot/subsystems/HallEffectSensorSelection.java new file mode 100644 index 0000000..4ddb381 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HallEffectSensorSelection.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems; + + +public enum HallEffectSensorSelection +{ + TopRight, + TopLeft, + BottomRight, + BottomLeft +}