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 bbb6698..82531b5 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -98,7 +98,8 @@ public class Robot extends TimedRobot drive.endGyroCalibration(); drive.resetEncoders(); drive.resetGyro(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); + drive.setIsRed(getAlliance().equals(Alliance.Red)); + arm.resetencoder(); } @@ -147,7 +148,8 @@ public class Robot extends TimedRobot public void updateStatus() { arm.updateStatus(operationMode); - drive.updateStatus(operationMode); + drive.updateStatus(operationMode); + wrist.updateStatus(operationMode); } 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 28341e2..5e99543 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(1110)); + addSequential(new ArmSetPositionPID(1400)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionPID.java new file mode 100644 index 0000000..79d9be2 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionPID.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Wrist; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class WristSetPositionPID extends Command { + + private double targetPositionInches; + + public WristSetPositionPID(double targetPositionInches) { + this.targetPositionInches = targetPositionInches; + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.wrist.setPositionPID(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 Math.abs(Robot.wrist.getPositionInches() - this.targetPositionInches) < Wrist.PID_ERROR_INCHES; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} 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 fe0dc45..83c3d05 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 @@ -44,8 +44,8 @@ public class Arm extends Subsystem implements ControlLoopable public static final double TEST_SPEED_UP = 0.3; public static final double TEST_SPEED_DOWN = -0.3; public static final double AUTO_ZERO_SPEED = -0.3; - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; - public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; + 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; @@ -84,13 +84,14 @@ 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.06; - public static final double KF_DOWN = 0.01; - public static final double P_Value = 0.6; - public static final double I_Value = 0.0005; - public static final double D_Value = 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 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 = 1.0; + public static final double PID_ERROR_INCHES = 5.0; LimitSwitchSource limitSwitchSource; // Pneumatics private Solenoid speedShift; @@ -117,11 +118,12 @@ public class Arm extends Subsystem implements ControlLoopable // 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); SmartDashboard.putNumber("arm P value", 0); @@ -138,6 +140,9 @@ public class Arm extends Subsystem implements ControlLoopable public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); } + public void resetencoder(){ + motor1.setPosition(0); + } private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; @@ -168,12 +173,16 @@ public class Arm extends Subsystem implements ControlLoopable public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); double startPositionInches = motor1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - //motor1.set(ControlMode.Position, targetPositionInches); - //motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); - //motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); - //motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); - System.err.println(motor1.getControlMode()); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + motor1.set(ControlMode.Position, targetPositionInches); + motor1.configClosedloopRamp(.02); + //motor1.configPeakCurrentLimit(5); + motor1.configContinuousCurrentLimit(2); + motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kD(0, D_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); + //System.err.println(motor1.getControlMode()); //System.err.print(motor1.getClosedLoopError()); } 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 8295059..5c6ae97 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 @@ -83,9 +83,14 @@ public class Wrist 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.005; - public static final double KF_DOWN = 0.0; - public static final double PID_ERROR_INCHES = 1.0; + + public static final double WristKF_UP = 0.01; + public static final double WristKF_DOWN = 0.0; + public static final double WristP_Value = 2; + public static final double WristI_Value = 0.00300; + public static final double WristD_Value = 200; + public static final double WristRampRate = 0.0; + public static final double PID_ERROR_INCHES = 10; LimitSwitchSource limitSwitchSource; // Pneumatics private Solenoid speedShift; @@ -156,9 +161,18 @@ public class Wrist extends Subsystem implements ControlLoopable } public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); + targetPositionInchesPID = limitPosition(targetPositionInches); double startPositionInches = wristmotor1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + wristmotor1.set(ControlMode.Position, targetPositionInches); + wristmotor1.configClosedloopRamp(WristRampRate); + //motor1.configPeakCurrentLimit(5); + wristmotor1.configClosedloopRamp(1); + wristmotor1.configContinuousCurrentLimit(2); + wristmotor1.config_kP(0, WristP_Value, TalonSRXEncoder.TIMEOUT_MS); + wristmotor1.config_kI(0, WristI_Value, TalonSRXEncoder.TIMEOUT_MS); + wristmotor1.config_kD(0, WristD_Value, TalonSRXEncoder.TIMEOUT_MS); + wristmotor1.config_kF(0, targetPositionInchesPID > startPositionInches ? WristKF_UP : WristKF_DOWN, TalonSRXEncoder.TIMEOUT_MS); } public void setPositionMP(double targetPositionInches) {