From 9fbe77138be8169c4812df94aff6cb5b3aff97cb Mon Sep 17 00:00:00 2001 From: HFocus Date: Sun, 8 Sep 2019 18:58:16 -0600 Subject: [PATCH] Set up Smart Motion Control - All thats left is implementing Smart Motion with Rev Robotics libraries --- .../usfirst/frc4388/robot/subsystems/Arm.java | 31 ++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) 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 95900b9..1f2ecce 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 @@ -116,9 +116,6 @@ public class Arm extends Subsystem implements ControlLoopable public static double kMinOutput = -1; public static double kMaxOutput = 1; - - - private PIDParams armPIDParams = new PIDParams(kP, kI, kD, kFF_Down); // KF gets updated later public static final double PID_ERROR_INCHES = 50; LimitSwitchSource limitSwitchSource; @@ -226,7 +223,7 @@ public class Arm extends Subsystem implements ControlLoopable // System.err.println(motor1.getControlMode()); } else if (armControlMode == ArmControlMode.SMART_MOTION){ - + controlSMWithJoystick(); } /* if (armControlMode == ArmControlMode.MOTION_PROFILE) { //isFinished = mpController.controlLoopUpdate(getPositionInches()); @@ -342,6 +339,31 @@ public class Arm extends Subsystem implements ControlLoopable // System.err.print(motor1.getClosedLoopError()); } + private void controlSMWithJoystick() { + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesSM = targetPositionInchesSM + deltaPosition; + updatePositionSM(targetPositionInchesSM); + } + + public void setPositionSM(double targetPositionInches) { + // motorController.setReference(targetPositionInches, ControlType.kSmartMotion); + updatePositionSM(targetPositionInches); + setArmControlMode(ArmControlMode.SMART_MOTION); + setFinished(false); + } + + public void updatePositionSM(double targetPositionInches) { + targetPositionInchesSM = limitPosition(targetPositionInches); + if (limitPosition(motorEncoder.getPosition()) == MIN_POSITION_INCHES){ + resetEncoder(); + } + + double startPositionInches = motorEncoder.getPosition(); + motorController.setReference(targetPositionInches, ControlType.kSmartMotion); + motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down); + } + private double limitPosition(double targetPosition) { if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; @@ -458,6 +480,7 @@ public class Arm extends Subsystem implements ControlLoopable //SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError()); SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent()); SmartDashboard.putNumber("Arm Target SM", targetPositionInchesSM); + SmartDashboard.putString("Arm Control Mode", armControlMode.toString()); //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));