diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 9bc33e1..d0cdc9f 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -7,6 +7,7 @@ public class Robot { public static Intake intake; public static Wrist wrist; public static Elevator elevator; + public static Shoulder shoulder; public static Drive drive; public static Gyro gyro; public static Lights lights; @@ -16,6 +17,7 @@ public static void robotInit() { intake = new Intake(); wrist = new Wrist(); elevator = new Elevator(); + shoulder = new Shoulder(); drive = new Drive(); gyro = new Gyro(); lights = new Lights(); diff --git a/src/main/java/com/team766/robot/mechanisms/Elevator.java b/src/main/java/com/team766/robot/mechanisms/Elevator.java index 0e047d4..6aebb00 100644 --- a/src/main/java/com/team766/robot/mechanisms/Elevator.java +++ b/src/main/java/com/team766/robot/mechanisms/Elevator.java @@ -1,4 +1,6 @@ package com.team766.robot.mechanisms; + +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; @@ -25,15 +27,15 @@ public enum Position { /** Elevator is fully retracted. Starting position. */ RETRACTED(0), /** Elevator is the appropriate height to place game pieces at the low node. */ - LOW(5), + LOW(0), /** Elevator is the appropriate height to place game pieces at the mid node. */ MID(18), /** Elevator is at appropriate height to place game pieces at the high node. */ HIGH(40), /** Elevator is at appropriate height to grab cubes from the human player. */ - HUMAN_CUBES(40.5), + HUMAN_CUBES(39), /** Elevator is at appropriate height to grab cones from the human player. */ - HUMAN_CONES(40.5), + HUMAN_CONES(40), /** Elevator is fully extended. */ EXTENDED(40); @@ -48,7 +50,7 @@ private double getHeight() { } } - private static final double NUDGE_INCREMENT = 5.0; + private static final double NUDGE_INCREMENT = 2.0; private static final double NUDGE_DAMPENER = 0.25; private static final double NEAR_THRESHOLD = 2.0; @@ -78,6 +80,9 @@ public Elevator() { throw new IllegalStateException("Motor are not CANSparkMaxes!"); } + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + leftMotor = (CANSparkMax) halLeftMotor; rightMotor = (CANSparkMax) halRightMotor; diff --git a/src/main/java/com/team766/robot/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/mechanisms/Shoulder.java index 6378390..d87627f 100644 --- a/src/main/java/com/team766/robot/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/mechanisms/Shoulder.java @@ -1,5 +1,6 @@ package com.team766.robot.mechanisms; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.SparkMaxPIDController; @@ -78,6 +79,9 @@ public Shoulder() { throw new IllegalStateException("Motor are not CANSparkMaxes!"); } + halLeftMotor.setNeutralMode(NeutralMode.Brake); + halRightMotor.setNeutralMode(NeutralMode.Brake); + leftMotor = (CANSparkMax) halLeftMotor; rightMotor = (CANSparkMax) halRightMotor; diff --git a/src/main/java/com/team766/robot/mechanisms/Wrist.java b/src/main/java/com/team766/robot/mechanisms/Wrist.java index ac6a258..d3f4f5e 100644 --- a/src/main/java/com/team766/robot/mechanisms/Wrist.java +++ b/src/main/java/com/team766/robot/mechanisms/Wrist.java @@ -33,11 +33,11 @@ public enum Position { /** Wrist is in the position for moving around the field. */ RETRACTED(-120.0), /** Wrist is level with ground. */ - LEVEL(0.0), - HIGH_NODE(-30), + LEVEL(-65), + HIGH_NODE(-20), MID_NODE(-25.5), - HUMAN_CONES(-17.5), - HUMAN_CUBES(-17.5), + HUMAN_CONES(-4), + HUMAN_CUBES(-8), /** Wrist is fully down. **/ BOTTOM(60);