diff --git a/2019robot/build.gradle b/2019robot/build.gradle index 01830a7..d4fbe1c 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" + id "edu.wpi.first.GradleRIO" version "2019.2.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java index 21b73c4..a933550 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -30,12 +30,17 @@ public class Constants { public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); // Arm - public static int kArmEncoderTickesPerRev = 256; - public static double kArmInchesOfTravelPerRev = 3.75; - public static double kArmEncoderTicksPerInch = 126.36; + public static int kArmEncoderTickesPerRev = 4096; + public static double kArmDegreesOfTravelPerRev = 360; + public static double kArmEncoderTicksPerDegree = 11.38; public static double kArmBasicPercentOutputUp = -0.85; public static double kArmBasicPercentOutputDown =.7; - + + + // Wrist + public static int kWristEncoderTicksPerRev = 4096; + public static double kWristDegreesOfTravelPerRev = 360; + public static double kWristEncoderTicksPerDegree = 11.38; // CONTROL LOOP GAINS // PID gains for drive velocity loop (LOW GEAR) 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 02fda7b..a12f61d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -65,7 +65,12 @@ public class OI //Wrist JoystickButton wristManualMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); wristManualMode.whenPressed(new WristSetMode(WristControlMode.JOYSTICK_MANUAL)); - + + +// uncoment the line above + + + //Operator Xbox /* 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 03d8165..a1f5c15 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -65,7 +65,7 @@ public class Robot extends IterativeRobot oi = OI.getInstance(); controlLoop.addLoopable(drive); - controlLoop.addLoopable(arm); + //controlLoop.addLoopable(arm); operationModeChooser = new SendableChooser(); 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 d70b56d..a145532 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 @@ -1,11 +1,15 @@ + package org.usfirst.frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.command.Subsystem; + import java.util.ArrayList; import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.utility.MPTalonPIDController; import org.usfirst.frc4388.robot.commands.*; import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; import org.usfirst.frc4388.utility.CANTalonEncoder; @@ -13,16 +17,6 @@ import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; @@ -31,394 +25,185 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -public class Arm extends Subsystem implements ControlLoopable +import com.ctre.phoenix.motorcontrol.ControlMode; + +/** + * Add your docs here. + */ +public class Arm extends Subsystem { - //PID encoder and motor - private CANTalonEncoder armMotor; - //private WPI_TalonSRX ArmLeft; + //Control Mode Array + public static enum ArmControlMode {PID, JOYSTICK_MANUAL}; - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerMaxScale; - //private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPMaxScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowScale; - //private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerSwitch; - //private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPSwitch; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowest; - //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowest; + //Motor Controllers + private ArrayList motorControllers = new ArrayList(); - //PID target - private double targetPPosition; - - //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; - - //control mode for joystick control - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; - - private double periodMs; - - //Non Linear Joystick - public static final double STICK_DEADBAND = 0.02; - public static final double MOVE_NON_LINEARITY = 1.0; - private int moveNonLinear = 0; - private double moveScale = 1.0; - private double moveTrim = 0.0; - - private boolean isFinished; - private DifferentialDrive m_drive; - - //private LimitSwitchSource limitSwitch = new DigitalInput(1); - LimitSwitchSource limitSwitchSource; - - public boolean pressed; - SensorCollection isPressed; - - public boolean armControlMode = false; - // Motor controllers - //private ArrayList motorControllers = new ArrayList(); - - public Arm() - { - try - { - //PID Arm encoder and talon - armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - - //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - - //ArmMotor.setInverted(false); + private CANTalonEncoder arm1; - //Setting left Arm motor as follower - //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); - //ArmLeft.setInverted(false); - //ArmLeft.setNeutralMode(NeutralMode.Brake); - armMotor.setNeutralMode(NeutralMode.Brake); - armMotor.setSensorPhase(true); - //Limit Switch Left - //ArmLeft.overrideLimitSwitchesEnable(true); - //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - //Limit Switch Right - //ArmMotor.overrideLimitSwitchesEnable(true); - //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - - //Change This boi - - // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here - //ArmMotor.configReverseSoftLimitThreshold(5, 0); - //ArmMotor.configForwardSoftLimitEnable(true, 0); - //ArmMotor.configReverseSoftLimitEnable(true, 0); - - //sos - //ArmMotor.enableLimitSwitch(true, true); + //Encoder ticks to inches for encoders + public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; + + // PID controller and params + private MPTalonPIDController mpController; - - - - - } - catch(Exception e) - { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); - } + 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 pidPIDParamsLoGear = new PIDParams(0.45, 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; + + // Defined positions + public static final double MIN_POSITION_INCHES = 0.0; + public static final double MAX_POSITION_INCHES = 83.4; + 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; + + //Misc + private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; + private boolean isFinished; + private double targetPositionInchesPID = 0; + + private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; + + public Arm() + { + try + { + //PID arm encoder and talon + arm1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); } - - private double nonlinearStickCalcPositive(double move, double moveNonLinearity) + catch(Exception e) { - return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity); + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); + } + } + + //Method for setting the control mode for the arm + private synchronized void setArmControlMode(ArmControlMode controlMode) + { + this.armControlMode = controlMode; + } + + //Getting the control mode for the arm + private synchronized ArmControlMode getArmControlMode() + { + return this.armControlMode; + } + + //Setting the speed for the motor on the arm along with setting the control mode to manual + public void setSpeed(double speed) + { + arm1.set(ControlMode.PercentOutput, speed); + setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } - - private double nonlinearStickCalcNegative(double move, double moveNonLinearity) - { - return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity); - } - - private boolean inDeadZone(double input) - { - boolean inDeadZone; - if (Math.abs(input) < STICK_DEADBAND) - { - inDeadZone = true; - } - else - { - inDeadZone = false; + + //Setting the target position for the PID loop and set the control mode to PID + public void setPositionPID(double targetPositionInches) + { + mpController.setPIDSlot(PID_SLOT); + updatePositionPID(targetPositionInches); + setArmControlMode(ArmControlMode.PID); + setFinished(false); + } + + //Setting range for target position + 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 inDeadZone; + return targetPosition; } + + //Method for updating the PID target position + public void updatePositionPID(double targetPositionInches) + { + targetPositionInchesPID = limitPosition(targetPositionInches); + double startPositionInches = arm1.getPositionWorld(); + mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + } + + //Getting the current encoder position + public double getPositionInches() + { + return arm1.getPositionWorld(); + } + + //Setting the speed for the motors in manual mode + public void setSpeedJoystick(double speed) + { + arm1.set(ControlMode.PercentOutput, speed); + setArmControlMode(armControlMode.JOYSTICK_MANUAL); + } + + //@Override + public void onLoop(double timestamp) + { + synchronized (Arm.this) { + switch(getArmControlMode() ) { + case PID: + controlPID(); + break; + case JOYSTICK_MANUAL: + controlManualWithJoystick(); + break; + default: + break; + } + } + } + + private void controlPID() + { + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + + targetPositionInchesPID = targetPositionInchesPID + deltaPosition; + updatePositionPID(targetPositionInchesPID); + } + + //Method for controlling the motor with the joystick + private void controlManualWithJoystick() + { + double joystickSpeed; - private double limitValue(double value) - { - if (value > 1.0) - { - value = 1.0; - } - else if (value < -1.0) - { - value = -1.0; - } - return value; - } - - public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) - { - if (inDeadZone(move)) - { - return 0; - } - - move += trim; - move *= scale; - move = limitValue(move); - - int iterations = Math.abs(nonLinearFactor); - for (int i = 0; i < iterations; i++) - { - if (nonLinearFactor > 0) - { - move = nonlinearStickCalcPositive(move, wheelNonLinearity); - } - else - { - move = nonlinearStickCalcNegative(move, wheelNonLinearity); - } - } - return move; - } - - public void setArmMode() - { - setControlMode(DriveControlMode.JOYSTICK); - } - - public void resetArmEncoder() - { - armMotor.setSelectedSensorPosition(0, 0, 0); - } - - public void moveArmXbox() - { - double moveArmInput; - double armSafeZone = 15; - - double armPos = getEncoderArmPosition(); - - moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); - - //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); - - boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - - if(armTuningPressed == true) - { - armMotor.set(moveArmInput * 0.5); - } - else if(armTuningPressed == false) - { - armMotor.set(moveArmInput); - } - } - - -// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range - - - //PID encoder position - public double getEncoderArmPosition() - { - return armMotor.getPositionWorld(); - } - - public double getArmHeightInchesAboveFloor() - { - return armMotor.getPositionWorld(); + joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + setSpeedJoystick(joystickSpeed); } - public synchronized void setControlMode(DriveControlMode controlMode) - { - this.controlMode = controlMode; - - isFinished = false; - } - /* - public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); - } - - public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); - } - - public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); - } - - public void setArmPIDLowest(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); - } - */ - public void rawSetOutput(double output){ - armMotor.set(/*ControlMode.PercentOutput,*/ output); - } - - public void holdInPos() - { - armMotor.set(-0.43 * 0.2); - } - - public void stopMotors() - { - armMotor.set(0); - } - - public void isSwitchPressed() - { - pressed = false; - isPressed = armMotor.getSensorCollection(); - - if(isPressed.isFwdLimitSwitchClosed() == true) - { - if (controlMode == DriveControlMode.JOYSTICK) { - Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); - } - pressed = true; - } - else - { - if (controlMode == DriveControlMode.STOP_MOTORS){ - { - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - pressed = false; - } - } - - } - - //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; - - - - - - @Override - public void controlLoopUpdate() - { - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) - { - moveArmXbox(); - } - else if (!isFinished) - { - //PID control mode - if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) - { - isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) - { - isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) - { - isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - /* - else if(controlMode == DriveControlMode.RAW) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - */ - } - } - - @Override - public void initDefaultCommand() - { - } - - @Override - public void periodic() - { - // isSwitchPressed(); - } - - @Override - public void setPeriodMs(long periodMs) - { - //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); - - this.periodMs = periodMs; - } - - public synchronized boolean isFinished() - { + public synchronized boolean isFinished() + { return isFinished; } - - public double getPeriodMs() - { - return periodMs; + + public synchronized void setFinished(boolean isFinished) + { + this.isFinished = isFinished; } - - public void updateStatus(Robot.OperationMode operationMode) + + @Override + public void initDefaultCommand() + { + } + + public void updateStatus(Robot.OperationMode operationMode) { if (operationMode == Robot.OperationMode.TEST) { try { - SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); - //SmartDashboard.putData(pressed); } - catch (Exception e) + catch (Exception e) { - System.err.println("Drivetrain update status error"); + System.err.println("Arm update status error"); } } } - - } - 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 4e2a196..248276a 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 @@ -40,7 +40,7 @@ public class Wrist extends Subsystem private CANTalonEncoder wristRight; //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; // PID controller and params private MPTalonPIDController mpController;