From 5fbc5c78dfd150f425cea79fb8d92836533deaee Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 21 Jan 2019 14:48:55 -0800 Subject: [PATCH] Changed Arm to arm Note: Not done editing --- .../org/usfirst/frc4388/robot/Constants.java | 10 +-- .../frc4388/robot/commands/ElevatorBasic.java | 6 +- .../usfirst/frc4388/robot/subsystems/Arm.java | 81 +++++++------------ .../frc4388/robot/subsystems/Drive.java | 1 - 4 files changed, 35 insertions(+), 63 deletions(-) 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 96a6f89..f6a18a1 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 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; + 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; // CONTROL LOOP GAINS diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java index df2b718..7d8d0fc 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -19,16 +19,16 @@ public class ElevatorBasic extends Command { public static boolean isfinishedElevatorBasic; public ElevatorBasic(double targetHeightInchesAboveFloor) { - requires(Robot.elevator); + requires(Robot.arm); m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; } // Called just before this Command runs the first time protected void initialize() { - Robot.elevator.setControlMode(DriveControlMode.RAW); + Robot.arm.setControlMode(DriveControlMode.RAW); - double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); // 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")); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index dcea3cb..d70b56d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -34,7 +34,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; public class Arm extends Subsystem implements ControlLoopable { //PID encoder and motor - private CANTalonEncoder ArmMotor; + private CANTalonEncoder armMotor; //private WPI_TalonSRX ArmLeft; //PID controller Max Scale @@ -84,7 +84,7 @@ public class Arm extends Subsystem implements ControlLoopable public boolean pressed; SensorCollection isPressed; - public boolean ArmControlMode = false; + public boolean armControlMode = false; // Motor controllers //private ArrayList motorControllers = new ArrayList(); @@ -93,7 +93,7 @@ public class Arm extends Subsystem implements ControlLoopable try { //PID Arm encoder and talon - ArmMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); @@ -103,8 +103,8 @@ public class Arm extends Subsystem implements ControlLoopable //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); //ArmLeft.setInverted(false); //ArmLeft.setNeutralMode(NeutralMode.Brake); - ArmMotor.setNeutralMode(NeutralMode.Brake); - ArmMotor.setSensorPhase(true); + armMotor.setNeutralMode(NeutralMode.Brake); + armMotor.setSensorPhase(true); //Limit Switch Left //ArmLeft.overrideLimitSwitchesEnable(true); //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); @@ -208,59 +208,32 @@ public class Arm extends Subsystem implements ControlLoopable public void resetArmEncoder() { - ArmMotor.setSelectedSensorPosition(0, 0, 0); + armMotor.setSelectedSensorPosition(0, 0, 0); } public void moveArmXbox() { double moveArmInput; - double ArmSafeZone = 15; + double armSafeZone = 15; - double ArmPos = getEncoderArmPosition(); + double armPos = getEncoderArmPosition(); moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean ArmTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - if(ArmTuningPressed == true) + if(armTuningPressed == true) { - ArmMotor.set(moveArmInput * 0.5); + armMotor.set(moveArmInput * 0.5); } - else if(ArmTuningPressed == false) + else if(armTuningPressed == false) { - ArmMotor.set(moveArmInput); - } - - /* - if(ArmPos <= ArmSafeZone && ArmPos >= 0) - { - ArmMotor.set(moveArmInput); - } - else if(ArmPos > ArmSafeZone) - { - ArmMotor.set(moveArmInput * 0.65); - - - if(holdButtonPressed == true) - { - ArmMotor.set(-0.43 * (0.2)); - } - else if(holdButtonPressed == false) - { - ArmMotor.set(moveArmInput * 0.75); - } - - } - - else if(ArmPos < 0) - { - ArmMotor.set(moveArmInput * 0.75); - } - */ + armMotor.set(moveArmInput); } + } // System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range @@ -269,12 +242,12 @@ public class Arm extends Subsystem implements ControlLoopable //PID encoder position public double getEncoderArmPosition() { - return ArmMotor.getPositionWorld(); + return armMotor.getPositionWorld(); } public double getArmHeightInchesAboveFloor() { - return ArmMotor.getPositionWorld(); + return armMotor.getPositionWorld(); } public synchronized void setControlMode(DriveControlMode controlMode) @@ -317,28 +290,28 @@ public class Arm extends Subsystem implements ControlLoopable } */ public void rawSetOutput(double output){ - ArmMotor.set(/*ControlMode.PercentOutput,*/ output); + armMotor.set(/*ControlMode.PercentOutput,*/ output); } public void holdInPos() { - ArmMotor.set(-0.43 * 0.2); + armMotor.set(-0.43 * 0.2); } public void stopMotors() { - ArmMotor.set(0); + armMotor.set(0); } public void isSwitchPressed() { pressed = false; - isPressed = ArmMotor.getSensorCollection(); + isPressed = armMotor.getSensorCollection(); if(isPressed.isFwdLimitSwitchClosed() == true) { if (controlMode == DriveControlMode.JOYSTICK) { - Robot.Arm.setControlMode(DriveControlMode.STOP_MOTORS); + Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); } pressed = true; } @@ -346,7 +319,7 @@ public class Arm extends Subsystem implements ControlLoopable { if (controlMode == DriveControlMode.STOP_MOTORS){ { - Robot.Arm.setControlMode(DriveControlMode.JOYSTICK); + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); } pressed = false; @@ -411,10 +384,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, armMotor); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); this.periodMs = periodMs; } @@ -435,7 +408,7 @@ public class Arm extends Subsystem implements ControlLoopable { try { - SmartDashboard.putNumber("Arm Pos Ticks", ArmMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); //SmartDashboard.putData(pressed); } 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 b984af3..3d29c64 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,7 +210,6 @@ 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);