diff --git a/src/main/java/frc/robot/Commands/WristCommand.java b/src/main/java/frc/robot/Commands/WristCommand.java new file mode 100644 index 0000000..ab24cac --- /dev/null +++ b/src/main/java/frc/robot/Commands/WristCommand.java @@ -0,0 +1,60 @@ +// 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; + + +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.WristSubsystem; + +import java.util.function.DoubleSupplier; + + +import edu.wpi.first.wpilibj2.command.CommandBase; + + +/** An example command that uses an example subsystem. */ +public class WristCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final WristSubsystem wristSubsystem; +private DoubleSupplier speedSupplier; + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public WristCommand(WristSubsystem wristSubsystem, DoubleSupplier speedSupplier) { + this.wristSubsystem = wristSubsystem; + this.speedSupplier = speedSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(wristSubsystem); + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + wristSubsystem.setTargetAngle(50); //update the targetAngle here + + + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80a3c9d..c884900 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,4 +28,11 @@ public static class Elevator{ public static final double GEAR_CIRCUMFERENCE = 1.432*Math.PI; } +public static class Wrist{ + /**the gear ratio of the motor to the final gear revolutions */ + public static final double GEAR_RATIO = 0.02585856914; + /**The number of ticks per motor revolution */ + public static final double TICKS_PER_REVOLUTION = 2048; + /**The gear circumferance for distance */ + public static final double GEAR_CIRCUMFERENCE = 1.432*Math.PI; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java index 5b3fb64..a3ded73 100644 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -4,8 +4,10 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -13,35 +15,47 @@ public class WristSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - private TalonFX left_motor; + private TalonFX motor; /** leader */ - private TalonFX right_motor; private PIDController pidController; - private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); + private final ShuffleboardTab WristTab = Shuffleboard.getTab("Elevator"); - private double targetHeight; + private double targetAngle; private double motorPower; + + //creates the WristSubsystem public WristSubsystem() { + motor = new TalonFX(16); + motorPower = 0; + targetAngle = 0; //sets the "targetAngle" variable to 0 by default + //creates the new PID :) WOOHOOOO + pidController = new PIDController(0, 0, 0); + // + WristTab.addNumber("Current Motor Power", () -> motorPower); + WristTab.addNumber("Target Angle", () -> this.targetAngle); + } + + public static double inchesToTicks(double height) { + return height * ((Wrist.GEAR_RATIO * Wrist.TICKS_PER_REVOLUTION) / (Wrist.GEAR_CIRCUMFERENCE)); + } + public static double ticksToInches(double ticks) { + return (ticks * Wrist.GEAR_CIRCUMFERENCE) / (Wrist.TICKS_PER_REVOLUTION * Wrist.GEAR_RATIO); + } +// updates the "targetAngle" variable if you change the value in WristCommand in public void execute +public void setTargetAngle(double targetAngle) { + this.targetAngle = targetAngle; +} - - - } - @Override public void periodic() { + motor.set(TalonFXControlMode.PercentOutput, motorPower); } - - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } - } \ No newline at end of file +} \ No newline at end of file