diff --git a/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java b/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java index 6b00bbe..205a3d0 100644 --- a/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/ElevatorDrive.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import static com.stuypulse.robot.constants.Settings.Elevator.*; + public class ElevatorDrive extends CommandBase { private final Elevator elevator; private IStream velocity; @@ -22,8 +24,8 @@ public ElevatorDrive(Gamepad gamepad) { @Override public void execute() { - elevator.setTargetHeight(velocity.get()); - SmartDashboard.putNumber("Gamepad velocity", velocity.getAsDouble()); + elevator.setTargetHeight(velocity.get() * MAX_HEIGHT); + SmartDashboard.putNumber("Gamepad Velocity", velocity.getAsDouble()); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 808d89d..8ccef33 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -16,12 +16,12 @@ public interface Settings { public interface Elevator { double DT = 0.3; // time between each simulation update - double GEARING = 1; - double CARRIAGE_MASS = 15; // kg + double GEARING = 9; // ~9:1 + double CARRIAGE_MASS = 10; // kg double DRUM_RADIUS = 0.025; // meters double MIN_HEIGHT = 0; - double MAX_HEIGHT = 1.1; + double MAX_HEIGHT = 1.8475325; // meters double MAX_HEIGHT_ERROR = 0.03; diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator.java index 94830b1..4e6782c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator.java @@ -12,6 +12,7 @@ import com.stuypulse.stuylib.streams.filters.MotionProfile; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Elevator extends SubsystemBase { diff --git a/src/main/java/com/stuypulse/robot/subsystems/SimElevator.java b/src/main/java/com/stuypulse/robot/subsystems/SimElevator.java index 9f0b85c..cfe3130 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SimElevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/SimElevator.java @@ -3,6 +3,7 @@ import static com.stuypulse.robot.constants.Settings.Elevator.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SimElevator extends Elevator { private ElevatorSim sim; @@ -32,5 +33,7 @@ public void simulationPeriodic() { height = sim.getPositionMeters(); velocity = sim.getVelocityMetersPerSecond(); + + SmartDashboard.putNumber("Height", getHeight()); } } diff --git a/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java index e9b4ab0..f365966 100644 --- a/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/ElevatorVisualizer.java @@ -135,14 +135,5 @@ public void setHeight(double newHeight) { secondLeftBottomRoot.setPosition(leftRootX + 2 * STAGE_SPACING, secondStageBottomY); secondRightBottomRoot.setPosition(rightRootX - 2 * STAGE_SPACING, secondStageBottomY); secondTopRoot.setPosition(leftRootX + 2 * STAGE_SPACING, INNER_STAGE_HEIGHT + secondStageBottomY); - - /* - THE FACTS: - - carriage moves at the same speed RELATIVE to teh inner stage - - carriage moves at twice the speed of the inner stage releative to teh observer - (use twice the speed for observer since we have them as separate from each other) - - - carriage not connected to inner stage - - carria*/ } } \ No newline at end of file