diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index e75f4d9..b4543fa 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -50,8 +50,6 @@ public class Robot extends IterativeRobot private Command RLautonomousCommand; private Command LRautonomousCommand; private Command LLautonomousCommand; - - public static final double OriginalDegreesWrist = arm.ENCODER_TICKS_TO_DEGREES; public void robotInit() { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index e54cb2f..33e2628 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -42,6 +42,8 @@ public class Wrist extends Subsystem //Encoder ticks to inches for encoders public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI; + + public double WRIST_ANGLE_DEGREES = 0; // PID controller and params private MPTalonPIDController mpController; @@ -94,6 +96,8 @@ public class Wrist extends Subsystem { double armAngleToHoriz = 70; double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; + + WRIST_ANGLE_DEGREES = initAngle; } //Method for setting the control mode for the wrist