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 82531b5..340d148 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -99,7 +99,7 @@ public class Robot extends TimedRobot drive.resetEncoders(); drive.resetGyro(); drive.setIsRed(getAlliance().equals(Alliance.Red)); - arm.resetencoder(); + arm.resetEncoder(); } 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 cd69583..f0d86c8 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,9 +34,9 @@ public class Arm extends Subsystem implements ControlLoopable 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,13 +48,13 @@ 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; public static final double NEAR_ZERO_POSITION_INCHES = 0.0; public static final double MIN_POSITION_INCHES = 0.0; - public static final double MAX_POSITION_INCHES = 4096; + public static final double MAX_POSITION_INCHES = 3900; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double HATCH_LOW_POSITION_WORLD = 1000; @@ -65,7 +65,7 @@ public class Arm extends Subsystem implements ControlLoopable 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; @@ -77,36 +77,34 @@ public class Arm extends Subsystem implements ControlLoopable 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 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); + 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.00300; + 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.0; + public static final double PID_ERROR_INCHES = 5; LimitSwitchSource limitSwitchSource; - DigitalInput forwardLimitSwitch = new DigitalInput(1); - DigitalInput reverseLimitSwitch = new DigitalInput(2); - + // Pneumatics private Solenoid speedShift; @@ -118,7 +116,7 @@ public class Arm extends Subsystem implements ControlLoopable 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; @@ -127,28 +125,28 @@ public class Arm extends Subsystem implements ControlLoopable 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.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.setNeutralMode(NeutralMode.Brake); motor2.setNeutralMode(NeutralMode.Brake); motor1.enableCurrentLimit(true); motorControllers.add(motor1); - - + + } catch (Exception e) { System.err.println("An error occurred in the Arm constructor"); @@ -158,18 +156,18 @@ public class Arm extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } - public void resetencoder(){ + } + public void resetEncoder(){ motor1.setPosition(0); } - + private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; } - + private synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } @@ -177,7 +175,7 @@ public class Arm extends Subsystem implements ControlLoopable public synchronized void setArmPositionMode(ArmPositionMode controlMode) { this.armPositionMode = controlMode; } - + private synchronized ArmPositionMode getArmPositionMode() { return this.armPositionMode; } @@ -186,26 +184,29 @@ public class Arm extends Subsystem implements ControlLoopable motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } - + 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); + 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(.02); + motor1.configClosedloopRamp(0); //motor1.configPeakCurrentLimit(5); motor1.configContinuousCurrentLimit(2); motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); @@ -215,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; @@ -231,7 +232,7 @@ public class Arm extends Subsystem implements ControlLoopable else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -255,26 +256,26 @@ public class Arm extends Subsystem implements ControlLoopable @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: + case JOYSTICK_PID: controlPidWithJoystick(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); break; - case MOTION_PROFILE: + case MOTION_PROFILE: if (!isFinished()) { if (firstMpPoint) { mpController.setPIDSlot(MP_SLOT); firstMpPoint = false; } - setFinished(mpController.controlLoopUpdate()); + setFinished(mpController.controlLoopUpdate()); if (isFinished()) { mpController.setPIDSlot(PID_SLOT); } @@ -299,11 +300,9 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - - if (reverseLimitSwitch.get()){ - motor1.setPosition((int) MIN_POSITION_INCHES); - } else if (forwardLimitSwitch.get()){ - motor1.setPosition((int) MAX_POSITION_INCHES); + + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ + resetEncoder(); } // Do the update @@ -312,8 +311,8 @@ 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){ //System.err.println(motor1.getControlMode()); @@ -324,13 +323,13 @@ public class Arm extends Subsystem implements ControlLoopable 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); }*/ } } @@ -358,14 +357,12 @@ public class Arm extends Subsystem implements ControlLoopable } } } - + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); - - } private void ControlWithJoystickhold(){ @@ -373,7 +370,7 @@ public class Arm extends Subsystem implements ControlLoopable updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick((joyStickSpeed*.30)+.1); @@ -391,7 +388,7 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -399,38 +396,38 @@ 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", 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("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)); @@ -439,9 +436,9 @@ public class Arm extends Subsystem implements ControlLoopable catch (Exception e) { 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/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index af02701..e405ce0 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 @@ -85,7 +85,7 @@ public class Wrist extends Subsystem implements ControlLoopable 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 P_Value = .5; public static final double I_Value = 0.00300; public static final double D_Value = 200; public static final double RampRate = 0.0;