From 850c2222a63e407d36047fbfbd80c297ca7cf30e Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Thu, 14 Mar 2019 20:10:47 -0600 Subject: [PATCH] customization for josh and max --- .../java/org/usfirst/frc4388/robot/OI.java | 8 +- .../frc4388/robot/commands/ArmStow.java | 38 +++ .../usfirst/frc4388/robot/subsystems/Arm.java | 303 ++++++++++-------- .../usfirst/frc4388/robot/subsystems/LED.java | 2 +- 4 files changed, 209 insertions(+), 142 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index ebe8368..163683a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -96,8 +96,12 @@ public class OI JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ - JoystickButton lowheight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - lowheight.whenPressed(new GrabFromLoadingSatation()); + + JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + stow.whenPressed(new ArmStow()); + + JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); + lowHeight.whenPressed(new GrabFromLoadingSatation()); SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java new file mode 100644 index 0000000..e07d9a6 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class ArmStow extends CommandGroup { + /** + * Add your docs here. + */ + public ArmStow() { + addSequential(new WristSetPositionPID(0), 1); + addParallel(new HatchFlip(false)); + addParallel(new WristPlacement(true)); + addSequential(new ArmSetPositionMM(10)); + + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} 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 00b9501..912b234 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 @@ -12,7 +12,6 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; @@ -32,11 +31,12 @@ public class Arm extends Subsystem implements ControlLoopable { private static Arm instance; - public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; + public static enum ArmPositionMode { CARGO, HATCH }; + + // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks + public static final double ENCODER_TICKS_TO_INCHES = (1); - // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks - public static final double ENCODER_TICKS_TO_INCHES = (1); - private double periodMs; private double lastControlLoopUpdatePeriod = 0.0; @@ -48,7 +48,7 @@ public class Arm extends Subsystem implements ControlLoopable public static final double AUTO_ZERO_SPEED = -0.3; public static final double JOYSTICK_INCHES_PER_MS_HI = 20; public static final double JOYSTICK_INCHES_PER_MS_LO = 20; - + // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; public static final double ZERO_POSITION_INCHES = -0.25; @@ -57,7 +57,16 @@ public class Arm extends Subsystem implements ControlLoopable public static final double MAX_POSITION_INCHES = 3900; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - public static final double SWITCH_POSITION_INCHES = 24.0; + public static final double HATCH_LOW_POSITION_WORLD = 135; + public static final double HATCH_MID_POSITION_WORLD = 2400; + public static final double HATCH_HIGH_POSITION_WORLD = 3800; + public static final double CARGO_LOW_POSITION_WORLD = 1500; + public static final double CARGO_MID_POSITION_WORLD = 2900; + public static final double CARGO_HIGH_POSITION_WORLD = 4300; + public static final double CARGO_PICKUP_POSITION_WORLD = 0; + public static final double HATCH_PICKUP_POSITION_WORLD = 0; + + /*public static final double SWITCH_POSITION_INCHES = 24.0; public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR public static final double SCALE_LOW_POSITION_INCHES = 56.0; public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0 @@ -65,83 +74,70 @@ public class Arm extends Subsystem implements ControlLoopable public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES; public static final double CLIMB_BAR_POSITION_INCHES = 70.0; public static final double CLIMB_HIGH_POSITION_INCHES = 10.0; - public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0; + public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;*/ // Motion profile max velocities and accel times - public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; + public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; public static final double MP_T1 = 400; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 - + // Motor controllers - private ArrayList motorControllers = new ArrayList(); + private ArrayList motorControllers = new ArrayList(); private TalonSRXEncoder motor1; private TalonSRX motor2; - + // PID controller and params private MPTalonPIDController mpController; public static int PID_SLOT = 0; - public static int MM_SLOT = 1; - public static int MP_SLOT = 2; - - private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); - private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); - public static final double KF_UP = 1;//0.01; - public static final double KF_DOWN = 0;// 0.0; - public static final double P_Value = 0.5;// 2; - public static final double I_Value = 0.0005;// 0.00030; - public static final double D_Value = 100;// 200; - public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms) - public static final double maxGravityComp = 0.08; - public static final double RampRate = 0;// 0.0; - public static final int A_value = 450; - public static final int CV_value = 740; - - - + public static int MP_SLOT = 1; + private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); + public static final double KF_UP = 0.01; + public static final double KF_DOWN = 0.0; + public static final double P_Value = 2; + public static final double I_Value = 0.00030; + public static final double D_Value = 200; + public static final double RampRate = 0.0; private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 5; LimitSwitchSource limitSwitchSource; - + // Pneumatics private Solenoid speedShift; + //DPAD + public static final double DPAD_UP = 0; + public static final double DPAD_RIGHT = 90; + public static final double DPAD_DOWN = 180; + public static final double DPAD_LEFT = 270; + public static final double DPAD_RELEASED = -1; + // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; + public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; + public ArmPositionMode armPositionMode = ArmPositionMode.HATCH; private double targetPositionInchesPID = 0; - private double targetPositionInchesMM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; - + public Arm() { try { motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); - - + + motor1.setInverted(true); motor2.setInverted(true); - + // if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } - - motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); - motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); - motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); - motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); - - motor1.selectProfileSlot(MM_SLOT, 0); - motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); - motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); @@ -149,9 +145,8 @@ public class Arm extends Subsystem implements ControlLoopable motor2.setNeutralMode(NeutralMode.Brake); motor1.enableCurrentLimit(true); motorControllers.add(motor1); - //motor1.setSelectedSensorPosition(0, , ); - - + + } catch (Exception e) { System.err.println("An error occurred in the Arm constructor"); @@ -161,79 +156,55 @@ public class Arm extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } + } public void resetEncoder(){ motor1.setPosition(0); } - + private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; } - + private synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } + public synchronized void setArmPositionMode(ArmPositionMode controlMode) { + this.armPositionMode = controlMode; + } + + private synchronized ArmPositionMode getArmPositionMode() { + return this.armPositionMode; + } + public void setSpeed(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } - public void setPositionMM(double targetPositionInches){ - motor1.set(ControlMode.MotionMagic, targetPositionInches); - System.err.println(motor1.getControlMode()); - motor1.selectProfileSlot(MM_SLOT, 0); - setArmControlMode(ArmControlMode.MOTION_MAGIC); - updatePositionMM(targetPositionInches); - setFinished(false); - - - } - - public double calcGravityCompensationAtCurrentPosition() { - int ticks = motor1.getSelectedSensorPosition(); - double degreesFromDown = (ticks+920)*(360.0/(4096*3)); - double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); - System.err.println("comp(" + degreesFromDown + "^) = " + compensation); - return compensation; - } - public void updatePositionMM(double targetPositionInches){ - targetPositionInchesMM = limitPosition(targetPositionInches); - //double startPositionInches = motor1.getPositionWorld(); - double compensation = calcGravityCompensationAtCurrentPosition(); - //System.err.println("compensation = " + compensation); - // motor1.set(ControlMode.MotionMagic, targetPositionInches); - motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation); - //System.err.println(motor1.getControlMode()); - motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); - motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); - - - } - public void setPositionPID(double targetPositionInches) { motor1.set(ControlMode.Position, targetPositionInches); mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set updatePositionPID(targetPositionInches); - setArmControlMode(ArmControlMode.JOYSTICK_PID); + setArmControlMode(ArmControlMode.JOYSTICK_PID); setFinished(false); } - + public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){ resetEncoder(); } double startPositionInches = motor1.getPositionWorld(); - //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); motor1.set(ControlMode.Position, targetPositionInches); motor1.configClosedloopRamp(0); //motor1.configPeakCurrentLimit(5); @@ -245,15 +216,15 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println(motor1.getControlMode()); //System.err.print(motor1.getClosedLoopError()); } - + public void setPositionMP(double targetPositionInches) { double startPositionInches = motor1.getPositionWorld(); - mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); setFinished(false); firstMpPoint = true; setArmControlMode(ArmControlMode.MOTION_PROFILE); } - + private double limitPosition(double targetPosition) { if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; @@ -261,7 +232,7 @@ public class Arm extends Subsystem implements ControlLoopable else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -272,6 +243,51 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPIDSlot(PID_SLOT); this.periodMs = periodMs; } + /*@Override + public void onStart(double timestamp) { + mpController = new MPTalonPIDController(periodMs, motorControllers); + mpController.setPID(mpPIDParams, MP_SLOT); + mpController.setPID(pidPIDParamsHiGear, PID_SLOT); + mpController.setPIDSlot(PID_SLOT); + mpController.setPID(pidPIDParamsHiGear, PID_SLOT); + mpController.setPIDSlot(PID_SLOT); + } + + @Override + public void onStop(double timestamp) { + // TODO Auto-generated method stub + + } + + @Override + public void onLoop(double timestamp) { + synchronized (Arm.this) { + switch( getElevatorControlMode() ) { + case JOYSTICK_PID: + controlPidWithJoystick(); + break; + case JOYSTICK_MANUAL: + controlManualWithJoystick(); + break; + case MOTION_PROFILE: + if (!isFinished()) { + if (firstMpPoint) { + mpController.setPIDSlot(MP_SLOT); + firstMpPoint = false; + } + setFinished(mpController.controlLoopUpdate()); + if (isFinished()) { + mpController.setPIDSlot(PID_SLOT); + } + } + break; + default: + break; + } + } + } + +*/ @@ -284,7 +300,7 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ resetEncoder(); } @@ -295,58 +311,69 @@ public class Arm extends Subsystem implements ControlLoopable } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { - isFinished = mpController.controlLoopUpdate(getPositionInches()); - + isFinished = mpController.controlLoopUpdate(getPositionInches()); + } if (armControlMode == ArmControlMode.JOYSTICK_PID){ - controlPidWithJoystick(); - System.err.println(motor1.getControlMode()); - } - if (armControlMode == ArmControlMode.MOTION_MAGIC){ - controlMMWithJoystick(); //System.err.println(motor1.getControlMode()); + int dPadAngle = Robot.oi.getOperatorController().getDpadAngle(); + if (dPadAngle == DPAD_RELEASED){ + controlPidWithJoystick(); + } else { + controlPidWithDPad(dPadAngle); + } } - + /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } else if (armControlMode == ArmControlMode.ADAPTIVE_PURSUIT) { updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); }*/ } } + private void controlPidWithDPad(int dPadAngle){ + if (armPositionMode == ArmPositionMode.HATCH){ + if (dPadAngle == DPAD_UP){ + updatePositionPID(HATCH_HIGH_POSITION_WORLD); + } else if (dPadAngle == DPAD_RIGHT){ + updatePositionPID(HATCH_MID_POSITION_WORLD); + } else if (dPadAngle == DPAD_DOWN){ + setPositionPID(HATCH_LOW_POSITION_WORLD); + } else if (dPadAngle == DPAD_LEFT){ + //updatePositionPID(HATCH_PICKUP_POSITION_WORLD); + } + } else if (armPositionMode == ArmPositionMode.CARGO){ + if (dPadAngle == DPAD_UP){ + updatePositionPID(CARGO_HIGH_POSITION_WORLD); + } else if (dPadAngle == DPAD_RIGHT){ + updatePositionPID(CARGO_MID_POSITION_WORLD); + } else if (dPadAngle == DPAD_DOWN){ + updatePositionPID(CARGO_LOW_POSITION_WORLD); + } else if (dPadAngle == DPAD_LEFT){ + //updatePositionPID(CARGO_PICKUP_POSITION_WORLD); + } + } + } - - - - - private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); } - private void controlMMWithJoystick() { - double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; - targetPositionInchesMM = targetPositionInchesMM + deltaPosition; - updatePositionMM(targetPositionInchesMM); - - - } private void ControlWithJoystickhold(){ double holdPosition = motor1.getPositionWorld(); updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick((joyStickSpeed*.3)+.08); + setSpeedJoystick((joyStickSpeed*.30)+.1); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -361,7 +388,7 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -369,51 +396,49 @@ public class Arm extends Subsystem implements ControlLoopable public double getPositionInches() { return motor1.getPositionWorld(); } - + // public double getAverageMotorCurrent() { // return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3; // } - + public double getAverageMotorCurrent() { return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; } - + public synchronized boolean isFinished() { return isFinished; } - + public synchronized void setFinished(boolean isFinished) { this.isFinished = isFinished; } - + public double getPeriodMs() { return periodMs; } - + public void updateStatus(Robot.OperationMode operationMode) { //System.err.println("the encoder is right after this"); try { - - SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); - //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); - //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); - SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity()); + + SmartDashboard.putNumber("Arm Position", motor1.getPositionWorld()); + SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); + SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError()); SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent()); - SmartDashboard.putNumber("Arm Target MM Position", targetPositionInchesMM); - //SmartDashboard.putNumber("arm output", motor1.getMotorOutputPercent()); + // SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID); } catch (Exception e) { - System.err.println("Arm update status error" +e.getMessage()); + System.err.println("Drivetrain update status error" +e.getMessage()); } - - } - + + } + public static Arm getInstance() { if(instance == null) { instance = new Arm(); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java index 37462b4..e97f1a4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java @@ -9,6 +9,7 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.HashMap; +import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.constants.LEDPatterns; import edu.wpi.first.wpilibj.Spark; @@ -25,7 +26,6 @@ import edu.wpi.first.wpilibj.command.Subsystem; public LED(){ setPattern(LEDPatterns.C1_HEARTBEAT_FAST); - LEDController.set(currentLED); } public void periodic() {