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 91c431f..4fce867 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.*; import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; -import org.usfirst.frc4388.robot.subsystems.Drive; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.buttons.Button; @@ -94,6 +93,7 @@ public class OI SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); + SmartDashboard.putData("wrist test", new wristTest()); JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); safteySwitch.whenPressed(new setClimberSafety(true)); 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 5e99543..1da3bed 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(1400)); + addSequential(new ArmSetPositionPID(600)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java index 10f1f3b..6168205 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java @@ -8,6 +8,8 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -17,6 +19,7 @@ public class SetManual extends CommandGroup { */ public SetManual() { addSequential(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL)); + addParallel(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); // e.g. addSequential(new Command1()); // addSequential(new Command2()); // these will run in order. diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java new file mode 100644 index 0000000..e0951ec --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java @@ -0,0 +1,50 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class WristSetMode extends Command { + + private WristControlMode controlMode; + + public WristSetMode(WristControlMode controlMode) { + this.controlMode = controlMode; + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (controlMode == WristControlMode.JOYSTICK_PID) { + Robot.wrist.setPositionPID(Robot.wrist.getPositionInches()); + } + else if (controlMode == WristControlMode.JOYSTICK_MANUAL) { + Robot.wrist.setSpeedJoystick(0); + } + else { + Robot.wrist.setSpeed(0.0); + } + } + + // 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 true; + } + + // 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/commands/wristTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java new file mode 100644 index 0000000..ee1fb58 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 wristTest extends CommandGroup { + /** + * Add your docs here. + */ + public wristTest() { + addSequential(new WristSetPositionPID(500)); + // Add Commands here: + // 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 83c3d05..3c82ae8 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 @@ -125,7 +125,7 @@ public class Arm extends Subsystem implements ControlLoopable motor2.setNeutralMode(NeutralMode.Brake); motor1.enableCurrentLimit(true); motorControllers.add(motor1); - SmartDashboard.putNumber("arm P value", 0); + } catch (Exception e) { @@ -281,7 +281,7 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.JOYSTICK_PID){ - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); controlPidWithJoystick(); @@ -368,7 +368,6 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println("the encoder is right after this"); try { - p = SmartDashboard.getNumber("arm P value", 0); SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); 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 5c6ae97..af02701 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 @@ -33,7 +33,7 @@ public class Wrist extends Subsystem implements ControlLoopable public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; // 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 = ((360/4096)/(3))-60; + public static final double ENCODER_TICKS_TO_INCHES = (1); private double periodMs; @@ -44,15 +44,15 @@ public class Wrist 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; 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 = 83.4; + public static final double MAX_POSITION_INCHES = 4096; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -73,7 +73,7 @@ public class Wrist extends Subsystem implements ControlLoopable // Motor controllers private ArrayList motorControllers = new ArrayList(); - private TalonSRXEncoder wristmotor1; + private TalonSRXEncoder wristMotor1; // PID controller and params private MPTalonPIDController mpController; @@ -83,19 +83,18 @@ 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 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; + 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 wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later + public static final double PID_ERROR_INCHES = 5.0; LimitSwitchSource limitSwitchSource; // Pneumatics private Solenoid speedShift; - private WristControlMode controlMode = WristControlMode.JOYSTICK_MANUAL; // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; @@ -103,22 +102,25 @@ public class Wrist extends Subsystem implements ControlLoopable private double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; + private double p = 0; + public Wrist() { try { - wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); - System.err.println("the talon should be made in wrist"); - - wristmotor1.setInverted(true); - -// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { + wristMotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); + wristMotor1.setInverted(false); + + +// if (wristMotor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } - wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - wristmotor1.setNeutralMode(NeutralMode.Brake); - motorControllers.add(wristmotor1); + wristMotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristMotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristMotor1.setNeutralMode(NeutralMode.Brake); + wristMotor1.enableCurrentLimit(true); + //wristMotor1.setSensorPhase(true); + motorControllers.add(wristMotor1); } @@ -134,6 +136,9 @@ public class Wrist extends Subsystem implements ControlLoopable public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); } + public void resetencoder(){ + wristMotor1.setPosition(0); + } private synchronized void setWristControlMode(WristControlMode controlMode) { this.wristControlMode = controlMode; @@ -144,39 +149,41 @@ public class Wrist extends Subsystem implements ControlLoopable } public void setSpeed(double speed) { - wristmotor1.set(ControlMode.PercentOutput, speed); + wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.MANUAL); } public void setSpeedJoystick(double speed) { - wristmotor1.set(ControlMode.PercentOutput, speed*.7); + wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } public void setPositionPID(double targetPositionInches) { - mpController.setPIDSlot(PID_SLOT); + wristMotor1.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); setWristControlMode(WristControlMode.JOYSTICK_PID); setFinished(false); } public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = wristmotor1.getPositionWorld(); + targetPositionInchesPID = limitPosition(targetPositionInches); + double startPositionInches = wristMotor1.getPositionWorld(); //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); + wristMotor1.set(ControlMode.Position, targetPositionInches); + wristMotor1.configClosedloopRamp(.02); + //wristMotor1.configPeakCurrentLimit(5); + wristMotor1.configContinuousCurrentLimit(2); + wristMotor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kD(0, D_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); + //System.err.println(wristMotor1.getControlMode()); + //System.err.print(wristMotor1.getClosedLoopError()); } public void setPositionMP(double targetPositionInches) { - double startPositionInches = wristmotor1.getPositionWorld(); + double startPositionInches = wristMotor1.getPositionWorld(); mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); setFinished(false); firstMpPoint = true; @@ -197,9 +204,7 @@ public class Wrist extends Subsystem implements ControlLoopable public void setPeriodMs(long periodMs) { 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.setPID(wristPIDParams, PID_SLOT); mpController.setPIDSlot(PID_SLOT); this.periodMs = periodMs; } @@ -262,18 +267,26 @@ public class Wrist extends Subsystem implements ControlLoopable lastControlLoopUpdateTimestamp = currentTimestamp; // Do the update - if (controlMode == WristControlMode.JOYSTICK_MANUAL) { + if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); + } else if (!isFinished) { - if (controlMode == WristControlMode.MOTION_PROFILE) { + if (wristControlMode == WristControlMode.MOTION_PROFILE) { isFinished = mpController.controlLoopUpdate(getPositionInches()); + + } + if (wristControlMode == WristControlMode.JOYSTICK_PID){ + System.err.println(wristMotor1.getControlMode()); + controlPidWithJoystick(); + + } - /*else if (controlMode == WristControlMode.MP_PATH_VELOCITY) { + /*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) { isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } - else if (controlMode == WristControlMode.ADAPTIVE_PURSUIT) { + else if (wristControlMode == WristControlMode.ADAPTIVE_PURSUIT) { updatePose(); isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); }*/ @@ -291,17 +304,19 @@ public class Wrist extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); + + } private void ControlWithJoystickhold(){ - double holdPosition = wristmotor1.getPositionWorld(); + double holdPosition = wristMotor1.getPositionWorld(); updatePositionPID(holdPosition); } private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); - setSpeedJoystick(joyStickSpeed*.50); + setSpeedJoystick(joyStickSpeed); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -322,16 +337,14 @@ public class Wrist extends Subsystem implements ControlLoopable } */ public double getPositionInches() { - return wristmotor1.getPositionWorld(); + return wristMotor1.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 (wristmotor1.getOutputCurrent()); - } + public synchronized boolean isFinished() { return isFinished; @@ -346,19 +359,23 @@ public class Wrist extends Subsystem implements ControlLoopable } public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { + //System.err.println("the encoder is right after this"); try { - SmartDashboard.putNumber("Elevator Position Inches", wristmotor1.getPositionWorld()); - SmartDashboard.putNumber("Elevator Motor 1 Amps", wristmotor1.getOutputCurrent()); - SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); + + SmartDashboard.putNumber("Wrist Position Ticks", wristMotor1.getPositionWorld()); + SmartDashboard.putNumber("Wrist Motor 1 Amps", wristMotor1.getOutputCurrent()); + SmartDashboard.putNumber("wrist pid error", wristMotor1.getClosedLoopError()); + SmartDashboard.putNumber("wrist motor output", wristMotor1.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("Elevator Target PID Position", targetPositionInchesPID); + SmartDashboard.putNumber("Wrist Target PID Position", targetPositionInchesPID); } catch (Exception e) { + System.err.println("Drivetrain update status error" +e.getMessage()); } - } + } public static Wrist getInstance() { @@ -367,4 +384,4 @@ public class Wrist extends Subsystem implements ControlLoopable } return instance; } -} \ No newline at end of file +}