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 a23da6c..cea2dc6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.*; import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; +import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; @@ -78,6 +79,10 @@ public class OI JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS); JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS); + JoystickButton resetArmEncoder = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.START_BUTTON); + resetArmEncoder.whenPressed(new ResetArmEncoder()); + resetArmEncoder.whenPressed(new ResetWristEncoder()); + /** DEPRICATED, TRIGGERS ON THE DRIVER JOYSTICK COVER FOR THIS BUTTON */ //JoystickButton ratchetFlip = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.Y_BUTTON); //ratchetFlip.toggleWhenPressed(new ratchetFlip(0.5)); @@ -107,17 +112,19 @@ public class OI JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON); - lowHeight.whenPressed(new GrabFromLoadingSatation()); + lowHeight.whenPressed(new SetPositionArmWrist(550, 450)); - JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - stow.whenPressed(new StowArm()); - stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); + JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO)); + cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH)); + //stow.whenPressed(new StowArm()); + //stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); SmartDashboard.putData("switch to manuel", new SetManual()); // SmartDashboard.putData("run turn test", new TestTurn()); SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); SmartDashboard.putData("wrist test", new wristTest()); - + //SmartDashboard.putData("PRE GAME!!!!", new PreGame()); } catch (Exception e) { 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 864d3ce..896a60f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -118,6 +118,8 @@ public class Robot extends TimedRobot //System.err.println("TeleopInit after resetEncoders"); drive.endGyroCalibration(); System.err.println("TeleopInit after endGyroCalibrations"); + arm.targetPositionInchesMM = arm.motor1.getPositionWorld(); + wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld(); updateStatus(); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java new file mode 100644 index 0000000..dcdb5b9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetArmEncoder.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ResetArmEncoder extends Command { + public ResetArmEncoder() { + requires(Robot.arm); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.arm.resetEncoder(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java new file mode 100644 index 0000000..8cdf431 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ResetWristEncoder.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class ResetWristEncoder extends Command { + public ResetWristEncoder() { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.wrist.resetencoder(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java new file mode 100644 index 0000000..6eecaab --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetPositionArmWrist.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* 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 SetPositionArmWrist extends CommandGroup { + /** + * Add your docs here. + */ + public SetPositionArmWrist(double arm, double wrist) { + + addParallel(new WristSetPositionPID(wrist)); + addSequential(new ArmSetPositionMM(arm)); + + // 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/commands/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index 1f2c239..526bc8c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -17,8 +17,8 @@ public class StowArm extends CommandGroup { public StowArm() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addParallel(new WristSetPositionPID(200), 2); - addSequential(new ArmSetPositionMM(-10), 4); + addParallel(new WristSetPositionPID(110), 2); + addSequential(new ArmSetPositionMM(10)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java new file mode 100644 index 0000000..d02370d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SwitchArmPlaceMode.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode; + +import edu.wpi.first.wpilibj.command.Command; + + + +public class SwitchArmPlaceMode extends Command { + + public PlaceMode placeMode; + + public SwitchArmPlaceMode(PlaceMode placeMode) { + requires(Robot.arm); + this.placeMode = placeMode; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.arm.placeMode = placeMode; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + 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 d550938..941092f 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 @@ -4,6 +4,9 @@ import java.util.ArrayList; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.SetPositionArmWrist; +import org.usfirst.frc4388.robot.commands.StowArm; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPTalonPIDController; @@ -33,10 +36,11 @@ public class Arm extends Subsystem implements ControlLoopable private static Arm instance; public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; + public static enum PlaceMode { HATCH, CARGO }; + + // 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 +52,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 = 35; public static final double JOYSTICK_INCHES_PER_MS_LO = 35; - + // 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 = -10; - public static final double MAX_POSITION_INCHES = 3900; + public static final double MIN_POSITION_INCHES = -25; + public static final double MAX_POSITION_INCHES = 4400; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -68,16 +72,16 @@ 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 = 300; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 - - // Motor controllers - private ArrayList motorControllers = new ArrayList(); - private TalonSRXEncoder motor1; + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + + public TalonSRXEncoder motor1; private TalonSRX motor2; - + // PID controller and params private MPTalonPIDController mpController; @@ -85,8 +89,8 @@ public class Arm extends Subsystem implements ControlLoopable public static int MM_SLOT = 1; public static int MP_SLOT = 2; - 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 = 1;//0.01; public static final double KF_DOWN = 0;// 0.0; public static final double P_Value = 0.5;// 2; @@ -104,34 +108,35 @@ public class Arm extends Subsystem implements ControlLoopable 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; - + // Pneumatics private Solenoid speedShift; // 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.MOTION_MAGIC; - private double targetPositionInchesPID = 0; - private double targetPositionInchesMM = 0; + public PlaceMode placeMode = PlaceMode.HATCH; + public double targetPositionInchesPID = 0; + public double targetPositionInchesMM = 0; private boolean firstMpPoint; 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.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); @@ -150,8 +155,8 @@ public class Arm extends Subsystem implements ControlLoopable motor1.enableCurrentLimit(true); motorControllers.add(motor1); //motor1.setSelectedSensorPosition(0, , ); - - + + } catch (Exception e) { System.err.println("An error occurred in the Arm constructor"); @@ -161,19 +166,22 @@ public class Arm extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } + } + public void resetEncoder(){ motor1.setPosition(0); + targetPositionInchesMM = 0; + targetPositionInchesPID = 0; } - + private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; } - - public synchronized ArmControlMode getArmControlMode() { + + private synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } @@ -181,7 +189,7 @@ 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); @@ -193,9 +201,6 @@ public class Arm extends Subsystem implements ControlLoopable setArmControlMode(ArmControlMode.MOTION_MAGIC); updatePositionMM(targetPositionInches); setFinished(false); - - - } public double calcGravityCompensationAtCurrentPosition() { @@ -215,25 +220,25 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println(motor1.getControlMode()); motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); - + } - + 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); 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(0); //motor1.configPeakCurrentLimit(5); @@ -245,23 +250,23 @@ 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) { + /*if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; - } - else if (targetPosition > MAX_POSITION_INCHES) { + }*/ + if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -273,9 +278,38 @@ public class Arm extends Subsystem implements ControlLoopable this.periodMs = periodMs; } + private int lastDPadAngle = -1; + public void dPadButtons(){ + int dPadAngle = Robot.oi.getOperatorController().getDpadAngle(); + if (placeMode == PlaceMode.HATCH){ + if (dPadAngle == 0 && lastDPadAngle == -1){ + new SetPositionArmWrist(3605, 1144).start(); + } + if (dPadAngle == 90 && lastDPadAngle == -1){ + new SetPositionArmWrist(2000, 750).start(); + } + if (dPadAngle == 180 && lastDPadAngle == -1){ + new SetPositionArmWrist(590, 450).start(); + } + } + if (placeMode == PlaceMode.CARGO) { + if (dPadAngle == 0 && lastDPadAngle == -1){ + new SetPositionArmWrist(4298, 3243).start(); + } + if (dPadAngle == 90 && lastDPadAngle == -1){ + new SetPositionArmWrist(2830, 2830).start(); + } + if (dPadAngle == 180 && lastDPadAngle == -1){ + new SetPositionArmWrist(1388, 2500).start(); + } + } - - + if (dPadAngle == 270 && lastDPadAngle == -1){ + new StowArm().start(); + } + SmartDashboard.putNumber("DPad Angle", dPadAngle); + lastDPadAngle = dPadAngle; + } public synchronized void controlLoopUpdate() { // Measure *actual* update period @@ -284,7 +318,9 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - + + dPadButtons(); + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ resetEncoder(); } @@ -296,8 +332,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){ controlPidWithJoystick(); @@ -307,13 +343,13 @@ public class Arm extends Subsystem implements ControlLoopable controlMMWithJoystick(); System.err.println(motor1.getControlMode()); } - + /*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); }*/ } } @@ -323,7 +359,7 @@ public class Arm extends Subsystem implements ControlLoopable - + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; @@ -335,8 +371,10 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesMM = targetPositionInchesMM + deltaPosition; updatePositionMM(targetPositionInchesMM); - - + //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); + + } private void ControlWithJoystickhold(){ @@ -344,7 +382,7 @@ public class Arm extends Subsystem implements ControlLoopable updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick((joyStickSpeed*.3)+.08); @@ -362,7 +400,7 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -370,31 +408,31 @@ 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 Ticks", motor1.getPositionWorld()); //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); @@ -412,9 +450,9 @@ public class Arm extends Subsystem implements ControlLoopable catch (Exception e) { System.err.println("Arm 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 d5353c0..b28e064 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 @@ -32,9 +32,9 @@ 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 = (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; @@ -46,7 +46,7 @@ public class Wrist 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 = 27; - + // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; public static final double ZERO_POSITION_INCHES = -0.25; @@ -66,23 +66,23 @@ public class Wrist 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 TalonSRXEncoder wristMotor1; - + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + + public TalonSRXEncoder wristMotor1; + // 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 = 1; @@ -96,33 +96,33 @@ public class Wrist extends Subsystem implements ControlLoopable private Solenoid speedShift; // 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 WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; - private double targetPositionInchesPID = 0; + public 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); + 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); wristMotor1.enableCurrentLimit(true); //wristMotor1.setSensorPhase(true); motorControllers.add(wristMotor1); - - + + } catch (Exception e) { System.err.println("An error occurred in the Wrist constructor"); @@ -132,19 +132,20 @@ public class Wrist extends Subsystem implements ControlLoopable @Override public void initDefaultCommand() { } - + public void resetZeroPosition(double position) { mpController.resetZeroPosition(position); - } + } public void resetencoder(){ wristMotor1.setPosition(0); + targetPositionInchesPID = 0; } - - + + private synchronized void setWristControlMode(WristControlMode controlMode) { this.wristControlMode = controlMode; } - + private synchronized WristControlMode getWristControlMode() { return this.wristControlMode; } @@ -153,24 +154,24 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { wristMotor1.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } - + public void setPositionPID(double targetPositionInches) { 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); + setWristControlMode(WristControlMode.JOYSTICK_PID); setFinished(false); } - + public void updatePositionPID(double 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(.02); //wristMotor1.configPeakCurrentLimit(5); @@ -182,23 +183,24 @@ public class Wrist extends Subsystem implements ControlLoopable //System.err.println(wristMotor1.getControlMode()); //System.err.print(wristMotor1.getClosedLoopError()); } - + public void setPositionMP(double targetPositionInches) { double startPositionInches = wristMotor1.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; setWristControlMode(WristControlMode.MOTION_PROFILE); } - + private double limitPosition(double targetPosition) { + if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; } else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -222,26 +224,26 @@ public class Wrist extends Subsystem implements ControlLoopable @Override public void onStop(double timestamp) { // TODO Auto-generated method stub - + } @Override public void onLoop(double timestamp) { synchronized (Wrist.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); } @@ -266,30 +268,30 @@ public class Wrist extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - + // Do the update if (wristControlMode == WristControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); - + } else if (!isFinished) { if (wristControlMode == WristControlMode.MOTION_PROFILE) { - isFinished = mpController.controlLoopUpdate(getPositionInches()); - + isFinished = mpController.controlLoopUpdate(getPositionInches()); + } if (wristControlMode == WristControlMode.JOYSTICK_PID){ //System.err.println(wristMotor1.getControlMode()); controlPidWithJoystick(); - - + + } - + /*else if (wristControlMode == WristControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } else if (wristControlMode == WristControlMode.ADAPTIVE_PURSUIT) { updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); }*/ } } @@ -299,14 +301,14 @@ public class Wrist extends Subsystem implements ControlLoopable - + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); - - + + } private void ControlWithJoystickhold(){ @@ -314,7 +316,7 @@ public class Wrist extends Subsystem implements ControlLoopable updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); setSpeedJoystick(joyStickSpeed*.5); @@ -332,7 +334,7 @@ public class Wrist extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -340,34 +342,34 @@ public class Wrist extends Subsystem implements ControlLoopable public double getPositionInches() { 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 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("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)); @@ -376,9 +378,9 @@ public class Wrist extends Subsystem implements ControlLoopable catch (Exception e) { System.err.println("Drivetrain update status error" +e.getMessage()); } - - } - + + } + public static Wrist getInstance() { if(instance == null) { instance = new Wrist();