From 2cd409e80797c8939ecb6f16bdbe349bd58aad67 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 17:58:02 -0800 Subject: [PATCH] Added arm angle to determine wrist angle --- .../java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 d168cad..9d977c5 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 @@ -15,6 +15,7 @@ import org.usfirst.frc4388.utility.CANTalonEncoder; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.robot.subsystems.Arm; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; @@ -66,12 +67,14 @@ public class Wrist extends Subsystem 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; public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; + + public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; private boolean isFinished; private double targetPositionInchesPID = 0; - private double targetAngleDegreesPID = 0; + private double targetAngleDegreesPID = -(180 - armAngleDegrees); private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;