diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 855a5cd..f9a75b0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -2,18 +2,16 @@ package org.usfirst.frc4388.robot; -public class RobotMap { +public class RobotMap +{ // USB Port IDs public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; - // MOTORS - public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; - public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; @@ -29,8 +27,6 @@ public class RobotMap { public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; - - // Pneumatics public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; @@ -40,7 +36,4 @@ public class RobotMap { public static final int LOWER_HATCH_INTAKE_PCM_ID = 5; public static final int END_EFECTOR_EXPAND_PCM_ID = 6; public static final int END_EFECTOR_CONTRACT_PCM_ID = 7; - - - } 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 a8e3ff7..a84e0bf 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 @@ -79,6 +79,8 @@ public class Wrist extends Subsystem public static final double jumpBarAngle = 50; //hard limit switch? public static final double armAngleForPIDSwitch = -45; ///Change values + + public static final boolean ballIntakeOut = true; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -185,14 +187,21 @@ public class Wrist extends Subsystem synchronized (Wrist.this) { switch(getWristControlMode() ) { case PID: + if(armAngleDegrees > armAngleForPIDSwitch) { controlPID(); } else if(armAngleDegrees <= armAngleForPIDSwitch) { + if(ballIntakeOut) + { + } + else + { + } } - controlPID(); + break; case JOYSTICK_MANUAL: controlManualWithJoystick(); @@ -240,11 +249,25 @@ public class Wrist extends Subsystem public void controlPIDBallIn() { updatePositionPID(targetAngleDegreesBallIn); + //Needs limit to lift up (so it can get over bar) in command group } public void controlPIDBallOut() { + updatePositionPID(targetAngleDegreesBallOut); + //Needs to be tuned a lot + } + public void controlPIDHatchIn() + { + updatePositionPID(targetAngleDegreesHatchIn); + //Needs limit to lift up (so it can get over bar) in command group + } + + public void controlPIDHatchOut() + { + updatePositionPID(targetAngleDegreesHatchOut); + //Needs to be tuned a lot } public synchronized boolean isFinished()