From 260699ef4ac56d98eeda11621019ee3aef916017 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 16:03:16 -0800 Subject: [PATCH] Added method for setting initial Angle below 0 deg Added to robot.java in robot initial to make initial angle correct --- .../java/org/usfirst/frc4388/robot/Robot.java | 19 +++++++++---------- .../frc4388/robot/subsystems/Wrist.java | 11 +++++++++-- 2 files changed, 18 insertions(+), 12 deletions(-) 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 65c24e8..e75f4d9 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -49,8 +49,10 @@ public class Robot extends IterativeRobot private Command RRautonomousCommand; private Command RLautonomousCommand; private Command LRautonomousCommand; - private Command LLautonomousCommand; - + private Command LLautonomousCommand; + + public static final double OriginalDegreesWrist = arm.ENCODER_TICKS_TO_DEGREES; + public void robotInit() { //Printing game data to riolog @@ -70,16 +72,13 @@ public class Robot extends IterativeRobot operationModeChooser.addDefault("Test", OperationMode.TEST); operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - - - - + wrist.setInitAngle(); + //ledLights.setAllLightsOn(false); - } catch (Exception e) { + } + catch (Exception e) + { System.err.println("An error occurred in 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 5edbeb9..e54cb2f 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 @@ -60,8 +60,8 @@ public class Wrist extends Subsystem public static final double MIN_POSITION_INCHES = 0.0; public static final double MAX_POSITION_INCHES = 83.4; - public static final double MIN_ANGLE_DEGREES = 0.0; ////FIND ANGLE VALUES - public static final double MAX_ANGLE_DEGREES = 0.0; + public static final double MIN_ANGLE_DEGREES = -3; ////FIND ANGLE VALUES + public static final double MAX_ANGLE_DEGREES = 3; public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; @@ -89,6 +89,13 @@ public class Wrist extends Subsystem } } + //Set the degree to negative angle after initializing + public void setInitAngle() + { + double armAngleToHoriz = 70; + double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; + } + //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) {