diff --git a/2019robot/build.gradle b/2019robot/build.gradle index 23028c6..f15e84c 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.1.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" @@ -50,6 +50,7 @@ dependencies { nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) testCompile 'junit:junit:4.12' + compile pathfinder() } // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java index 395d0be..986f17b 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -29,11 +29,11 @@ public class Constants { public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); // Elevator - public static int kArmEncoderTickesPerRev = 256; - public static double kArmInchesOfTravelPerRev = 3.75; - public static double kArmEncoderTicksPerInch = 126.36; - public static double kArmBasicPercentOutputUp = -0.85; - public static double kArmBasicPercentOutputDown =.7; + public static int kElevatorEncoderTickesPerRev = 256; + public static double kElevatorInchesOfTravelPerRev = 3.75; + public static double kElevatorEncoderTicksPerInch = 126.36; + public static double kElevatorBasicPercentOutputUp = -0.85; + public static double kElevatorBasicPercentOutputDown =.7; // CONTROL LOOP GAINS 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 5d66455..36fea3e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -32,7 +32,10 @@ public class Robot extends IterativeRobot - public static final Arm arm = new Arm(); + public static final Elevator elevator = new Elevator(); + + + public static final Climber climber = new Climber(); public static final Pnumatics pnumatics = new Pnumatics(); public static final long periodMS = 10; @@ -64,7 +67,7 @@ public class Robot extends IterativeRobot oi = OI.getInstance(); controlLoop.addLoopable(drive); - controlLoop.addLoopable(arm); + controlLoop.addLoopable(elevator); operationModeChooser = new SendableChooser(); @@ -146,15 +149,14 @@ public class Robot extends IterativeRobot LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); } - public Alliance getAlliance() - { + public Alliance getAlliance() { return m_ds.getAlliance(); } - public void updateStatus() - { + public void updateStatus() { drive.updateStatus(operationMode); - arm.updateStatus(operationMode); + elevator.updateStatus(operationMode); + } } 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 55e22a3..64e015f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -21,15 +21,10 @@ public class RobotMap { public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; - //Arm Motors - public static final int ARM_MOTOR1_ID = 6; - public static final int ARM_MOTOR2_ID = 7; - - //Climber Motors + //Elevator Motors + public static final int ELEVATOR_MOTOR1_ID = 6; + public static final int ELEVATOR_MOTOR2_ID = 7; public static final int CLIMBER_CAN_ID = 10; - public static final int CLIMBER_WHEEL1_ID = 9; - public static final int CLIMBER_WHEEL2_ID = 11; - public static final int CLIMBER_RATCHET_ID = 12; // Pneumatics diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java similarity index 83% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java index 8d724c4..df2b718 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class ArmBasic extends Command { +public class ElevatorBasic extends Command { private double m_targetHeightInchesAboveFloor; private double m_speed; private boolean m_goingUp; @@ -18,25 +18,25 @@ public class ArmBasic extends Command { private double m_lastCommandExecutePeriod = 0.0; public static boolean isfinishedElevatorBasic; - public ArmBasic(double targetHeightInchesAboveFloor) { - requires(Robot.arm); + public ElevatorBasic(double targetHeightInchesAboveFloor) { + requires(Robot.elevator); m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; } // Called just before this Command runs the first time protected void initialize() { - Robot.arm.setControlMode(DriveControlMode.RAW); + Robot.elevator.setControlMode(DriveControlMode.RAW); - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); // start out at half speed, as crude way to reduce slippage m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); if (m_goingUp) { - m_speed = Constants.kArmBasicPercentOutputUp; + m_speed = Constants.kElevatorBasicPercentOutputUp; } else { - m_speed = Constants.kArmBasicPercentOutputDown; + m_speed = Constants.kElevatorBasicPercentOutputDown; } m_commandInitTimestamp = Timer.getFPGATimestamp(); @@ -50,14 +50,14 @@ System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targe m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; } m_lastCommandExecuteTimestamp = currentTimestamp; - Robot.arm.rawSetOutput(m_speed); + Robot.elevator.rawSetOutput(m_speed); //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { boolean finished=false; - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); if (remaining < 0.0) { @@ -86,9 +86,9 @@ System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , ta isfinishedElevatorBasic = isFinished(); - Robot.arm.rawSetOutput(0.0); + Robot.elevator.rawSetOutput(0.0); - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); } // Called when another command which requires one or more of the same diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java similarity index 90% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java index 21e328a..7553d48 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java @@ -9,12 +9,12 @@ import edu.wpi.first.wpilibj.command.Command; /** * */ -public class ArmSetSpeed extends Command { +public class ElevatorSetSpeed extends Command { private double RaiseSpeed; // Constructor with speed - public ArmSetSpeed(double RaiseSpeed) { + public ElevatorSetSpeed(double RaiseSpeed) { this.RaiseSpeed = RaiseSpeed; // requires(Robot.elevatorAuton); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java index 021716c..3be9068 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java @@ -7,14 +7,14 @@ import edu.wpi.first.wpilibj.command.Command; public class InitiateClimber extends Command { - + public InitiateClimber() { requires(Robot.climber); } @Override protected void initialize() { - Robot.climber.setClimbSpeed(climb, speed); + Robot.climber.setClimbSpeed(kClimbLiftSpeed); } @Override @@ -29,9 +29,9 @@ public class InitiateClimber extends Command // Called once after isFinished returns true protected void end() { } - + @Override protected void interrupted() { - + } -} +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index 16392b3..e8046eb 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -1,80 +1,29 @@ package org.usfirst.frc4388.robot.subsystems; -import java.util.ArrayList; - -import org.usfirst.frc4388.controller.XboxController; -import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -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 com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -/** - * - */ public class Climber extends Subsystem{ + + private WPI_TalonSRX Climber; - private WPI_TalonSRX climberBack; - private WPI_TalonSRX climberFront; - - //Frequency Control - static float BACK_FREQ = 1; - static float FRONT_FREQ; - static float FREQ_RATIO = 0.2443744576F; - - - - //Time Control - static float TIME_TO_CLIMB = 1254; - static float TIME_TO_TILT = 1254; - static float TIME_TO_DRIVE = 1254; - static float TIME_TO_PULL = 1254; - - //Limit and Saftey vars - LimitSwitchSource limitSwitchSource; - SensorCollection isPressed; - boolean safteySwitch; - + public boolean on; + public Climber(){ + try{ - climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID); - climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID); - flipOutMotor = new WPI_TalonSRX(RobotMap.CLIMBER_RATCHET_ID); - - climber.set(ControlMode.Follower, carriageLeft.getDeviceID()); - - climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - } - catch (Exception e) { + + Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + + } catch (Exception e) { + System.err.println("An error occurred in the climbing constructor"); + } - FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed } @Override @@ -82,32 +31,22 @@ public class Climber extends Subsystem{ } + @Override public void periodic() { + // Put code here to be run every loop + } - - public void setClimbSpeed(boolean Climb, float speed) { - if (Climb && safteySwitch) { - if(isPressed.isFwdLimitSwitchClosed()) //If back at max height - { - climberBack.set(0); - climberFront.set(FRONT_FREQ * speed); - } - else if (isPressed.isFwdLimitSwitchClosed() == false) //If back not at max height - { - climberBack.set(BACK_FREQ * speed); - climberFront.set(FRONT_FREQ * speed); - } - - } - if (Climb == false) { - climberBack.set(0); - climberFront.set(0); - } - } + + public void setClimbSpeed(double Climb) { + Climber.set(Climb); } -/*TODO - * add command code for starting climb (including button press) - * add command code for flipping front out (including button press) - * add saftey switch on second controller - */ + // Put methods for controlling this subsystem + // here. Call these from Commands. + { + // TODO Auto-generated method stub + + } + +} + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 3d29c64..b984af3 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -210,6 +210,7 @@ public class Drive extends Subsystem implements ControlLoopable rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); // rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); //gyroPigeon = new PigeonImu(leftDrive2); gyroNavX = new AHRS(SPI.Port.kMXP); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java similarity index 57% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java index d70b56d..4a17e98 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -31,11 +31,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -public class Arm extends Subsystem implements ControlLoopable +public class Elevator extends Subsystem implements ControlLoopable { //PID encoder and motor - private CANTalonEncoder armMotor; - //private WPI_TalonSRX ArmLeft; + private CANTalonEncoder elevatorRight; + private WPI_TalonSRX elevatorLeft; //PID controller Max Scale private SoftwarePIDPositionController pidPositionControllerMaxScale; @@ -61,7 +61,7 @@ public class Arm extends Subsystem implements ControlLoopable private double targetPPosition; //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch; //control mode for joystick control private DriveControlMode controlMode = DriveControlMode.JOYSTICK; @@ -84,47 +84,46 @@ public class Arm extends Subsystem implements ControlLoopable public boolean pressed; SensorCollection isPressed; - public boolean armControlMode = false; + public boolean elevatorControlMode = false; // Motor controllers //private ArrayList motorControllers = new ArrayList(); - public Arm() + public Elevator() { try { - //PID Arm encoder and talon - armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - - //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); + //PID elevator encoder and talon + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); - //ArmMotor.setInverted(false); + elevatorRight.setInverted(false); - //Setting left Arm motor as follower - //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); - //ArmLeft.setInverted(false); - //ArmLeft.setNeutralMode(NeutralMode.Brake); - armMotor.setNeutralMode(NeutralMode.Brake); - armMotor.setSensorPhase(true); + //Setting left elevator motor as follower + elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); + elevatorLeft.setInverted(false); + elevatorLeft.setNeutralMode(NeutralMode.Brake); + elevatorRight.setNeutralMode(NeutralMode.Brake); + elevatorRight.setSensorPhase(true); //Limit Switch Left - //ArmLeft.overrideLimitSwitchesEnable(true); - //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorLeft.overrideLimitSwitchesEnable(true); + elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Limit Switch Right - //ArmMotor.overrideLimitSwitchesEnable(true); - //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorRight.overrideLimitSwitchesEnable(true); + //elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); //Change This boi - // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here - //ArmMotor.configReverseSoftLimitThreshold(5, 0); - //ArmMotor.configForwardSoftLimitEnable(true, 0); - //ArmMotor.configReverseSoftLimitEnable(true, 0); + // elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here + //elevatorRight.configReverseSoftLimitThreshold(5, 0); + //elevatorRight.configForwardSoftLimitEnable(true, 0); + //elevatorRight.configReverseSoftLimitEnable(true, 0); //sos - //ArmMotor.enableLimitSwitch(true, true); + //elevatorRight.enableLimitSwitch(true, true); @@ -133,7 +132,7 @@ public class Arm extends Subsystem implements ControlLoopable } catch(Exception e) { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Elevator Construtor"); } } @@ -201,53 +200,80 @@ public class Arm extends Subsystem implements ControlLoopable return move; } - public void setArmMode() + public void setElevatorMode() { setControlMode(DriveControlMode.JOYSTICK); } - public void resetArmEncoder() + public void resetElevatorEncoder() { - armMotor.setSelectedSensorPosition(0, 0, 0); + elevatorRight.setSelectedSensorPosition(0, 0, 0); } - public void moveArmXbox() + public void moveElevatorXbox() { - double moveArmInput; - double armSafeZone = 15; + double moveElevatorInput; + double elevatorSafeZone = 15; - double armPos = getEncoderArmPosition(); + double elevatorPos = getEncoderElevatorPosition(); - moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); + moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis(); - //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); + //double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY); boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - if(armTuningPressed == true) + if(elevatorTuningPressed == true) { - armMotor.set(moveArmInput * 0.5); + elevatorRight.set(moveElevatorInput * 0.5); } - else if(armTuningPressed == false) + else if(elevatorTuningPressed == false) { - armMotor.set(moveArmInput); + elevatorRight.set(moveElevatorInput); + } + + /* + if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) + { + elevatorRight.set(moveElevatorInput); + } + else if(elevatorPos > elevatorSafeZone) + { + elevatorRight.set(moveElevatorInput * 0.65); + + + if(holdButtonPressed == true) + { + elevatorRight.set(-0.43 * (0.2)); + } + else if(holdButtonPressed == false) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + + } + + else if(elevatorPos < 0) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + */ } - } -// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range +// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range //PID encoder position - public double getEncoderArmPosition() + public double getEncoderElevatorPosition() { - return armMotor.getPositionWorld(); + return elevatorRight.getPositionWorld(); } - public double getArmHeightInchesAboveFloor() + public double getElevatorHeightInchesAboveFloor() { - return armMotor.getPositionWorld(); + return elevatorRight.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -257,61 +283,61 @@ public class Arm extends Subsystem implements ControlLoopable isFinished = false; } /* - public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) + public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError) { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); } - public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) + public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError) { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); } - public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) + public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError) { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); } - public void setArmPIDLowest(double ArmPosition, double maxError, double minError) + public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError) { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); } */ public void rawSetOutput(double output){ - armMotor.set(/*ControlMode.PercentOutput,*/ output); + elevatorRight.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - armMotor.set(-0.43 * 0.2); + elevatorRight.set(-0.43 * 0.2); } public void stopMotors() { - armMotor.set(0); + elevatorRight.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = armMotor.getSensorCollection(); + isPressed = elevatorRight.getSensorCollection(); if(isPressed.isFwdLimitSwitchClosed() == true) { if (controlMode == DriveControlMode.JOYSTICK) { - Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); + Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); } pressed = true; } @@ -319,7 +345,7 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.STOP_MOTORS){ { - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); } pressed = false; @@ -339,31 +365,31 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { - moveArmXbox(); + moveElevatorXbox(); } else if (!isFinished) { //PID control mode if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) { - isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); + isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) { - isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); + isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) { - isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); + isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition()); } else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); } /* else if(controlMode == DriveControlMode.RAW) { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); } */ } @@ -384,10 +410,10 @@ public class Arm extends Subsystem implements ControlLoopable public void setPeriodMs(long periodMs) { //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight); this.periodMs = periodMs; } @@ -408,8 +434,8 @@ public class Arm extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); + SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } catch (Exception e)