diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java new file mode 100644 index 0000000..b1c0654 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java @@ -0,0 +1,56 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ArmSetPositionMM extends Command { + + private double targetPositionInches; + private boolean isAtTarget; + private static final double MIN_DELTA_TARGET = 3; + + public ArmSetPositionMM(double targetPositionInches) { + this.targetPositionInches = targetPositionInches; + requires(Robot.arm); + } + + // Called just before this Command runs the first time + protected void initialize() { + + if (Math.abs(targetPositionInches - Robot.arm.getPositionInches()) < MIN_DELTA_TARGET) { + isAtTarget = true; + } + else { + isAtTarget = false; + Robot.arm.setPositionMM(targetPositionInches); + } +// System.out.println("Arm set MP initialized, target = " + targetPositionInches); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return isAtTarget || Robot.arm.isFinished(); + } + + // Called once after isFinished returns true + protected void end() { + Robot.arm.setPositionMM(Robot.arm.getPositionInches()); +// System.out.println("Arm set MP end"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { +// System.out.println("ArmSetPositionMP interrupted"); + Robot.arm.setFinished(true); + Robot.arm.setPositionMM(Robot.arm.getPositionInches()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java index 1da3bed..0fa8b18 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java @@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup { * Add your docs here. */ public ArmTest() { - addSequential(new ArmSetPositionPID(600)); + addSequential(new ArmSetPositionMM(600)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); 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 a2db85c..8523ed8 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 @@ -30,7 +30,7 @@ public class Arm extends Subsystem implements ControlLoopable { private static Arm instance; - public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; // 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); @@ -84,12 +84,18 @@ public class Arm extends Subsystem implements ControlLoopable 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; + public static final double KF_UP = 0;//0.01; + public static final double KF_DOWN = 0;// 0.0; + public static final double P_Value = 0;// 2; + public static final double I_Value = 0;// 0.00030; + public static final double D_Value = 0;// 200; + public static final double RampRate = 0;// 0.0; + public static final int A_value = 25; + public static final int CV_value = 25; + + + + 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; @@ -101,6 +107,7 @@ public class Arm extends Subsystem implements ControlLoopable private boolean isFinished; private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; private double targetPositionInchesPID = 0; + private double targetPositionInchesMM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -125,6 +132,7 @@ public class Arm extends Subsystem implements ControlLoopable motor2.setNeutralMode(NeutralMode.Brake); motor1.enableCurrentLimit(true); motorControllers.add(motor1); + //motor1.setSelectedSensorPosition(0, , ); } @@ -161,6 +169,55 @@ public class Arm extends Subsystem implements ControlLoopable motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } + public void setPositionMM(double targetPositionInches){ + motor1.set(ControlMode.MotionMagic, targetPositionInches); + System.err.println(motor1.getControlMode()); + setArmControlMode(ArmControlMode.MOTION_MAGIC); + updatePositionMM(targetPositionInches); + setFinished(false); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + System.err.println("should get here"); + + + } + public void updatePositionMM(double targetPositionInches){ + targetPositionInchesMM = limitPosition(targetPositionInches); + double startPositionInches = motor1.getPositionWorld(); + motor1.set(ControlMode.MotionMagic, targetPositionInches); + 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(1, 0); + motor1.config_kF(1, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kP(1, P_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kI(1, I_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kD(1, D_Value, TalonSRXEncoder.TIMEOUT_MS); + + + motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); + motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); + + + } public void setPositionPID(double targetPositionInches) { motor1.set(ControlMode.Position, targetPositionInches); @@ -212,51 +269,6 @@ 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; - } - } - } - -*/ @@ -280,12 +292,12 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.JOYSTICK_PID){ - //System.err.println(motor1.getControlMode()); - if (Robot.oi.getOperatorController().getDpadAngle() == -1){ controlPidWithJoystick(); - } else { - - } + System.err.println(motor1.getControlMode()); + } + if (armControlMode == ArmControlMode.MOTION_MAGIC){ + controlMMWithJoystick(); + System.err.println(motor1.getControlMode()); } /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { @@ -311,6 +323,14 @@ public class Arm extends Subsystem implements ControlLoopable updatePositionPID(targetPositionInchesPID); + } + private void controlMMWithJoystick() { + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesMM = targetPositionInchesMM + deltaPosition; + updatePositionMM(targetPositionInchesMM); + + } private void ControlWithJoystickhold(){ @@ -370,19 +390,19 @@ public class Arm extends Subsystem implements ControlLoopable 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("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("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("Drivetrain update status error" +e.getMessage()); + System.err.println("Arm update status error" +e.getMessage()); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java index c7ef11a..23b19e6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java @@ -81,4 +81,7 @@ public class TalonSRXEncoder extends WPI_TalonSRX public double getVelocityWorld() { return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1); } + + public void configNominalOutputVoltage(float f, float g) { + } } \ No newline at end of file