From d7ce32e5c8858092ca8a5609c053f760fc101fe2 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 14 Feb 2019 15:55:12 -0800 Subject: [PATCH] Slight edit --- .../usfirst/frc4388/robot/subsystems/Arm.java | 543 ++++++++++++------ .../frc4388/robot/subsystems/Wrist.java | 2 - 2 files changed, 363 insertions(+), 182 deletions(-) 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 a8bedac..0278e79 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,240 +1,423 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; -import java.lang.Math; +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.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.Loop; -import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.TalonSRXEncoder; +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.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Arm extends Subsystem implements Loop +public class Arm extends Subsystem implements ControlLoopable { - private static Arm instance; + //PID encoder and motor + private CANTalonEncoder armMotor; + //private WPI_TalonSRX ArmLeft; - public static enum ArmControlMode {PID, JOYSTICK_MANUAL }; - public static final double DEGREE_START_OFFSET = 70; - - public static final double RADIUS_OF_ARM = 43.3; - public static final double OFF_SET_BELOW = 49.3; - - // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks - public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET); - public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW); - public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES)); + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerMaxScale; + //private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPMaxScale; - public double ARM_ANGLE_DEGREES = 0; - - // Defined speeds - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerLowScale; + //private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPLowScale; - // Defined positions - public static final double MIN_POSITION_INCHES = 0.0; - public static final double MAX_POSITION_INCHES = 83.4; - public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - - - LimitSwitchSource limitSwitchSource; - // Motion profile max velocities and accel times - public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; - public static final double MP_T2 = 150; // Fast = 150 + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerSwitch; + //private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPSwitch; - // Motor controllers - private ArrayList motorControllers = new ArrayList(); + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerLowest; + //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPLowest; - private CANTalonEncoder motor1; - private TalonSRX motor2; + //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; - // 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); - 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; - private long periodMs = (long)(Constants.kLooperDt * 1000.0); - - - // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; - private double targetPositionInchesPID = 0; - private boolean firstMpPoint; + private DifferentialDrive m_drive; + //private LimitSwitchSource limitSwitch = new DigitalInput(1); + LimitSwitchSource limitSwitchSource; - public Arm() { - try { - motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); - //motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID); + 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); - motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - } - catch (Exception e) { - System.err.println("An error occurred in the DriveTrain constructor"); - } - } - @Override - public void initDefaultCommand() { - } - - public void resetZeroPosition(double position) { - mpController.resetZeroPosition(position); - } - - private synchronized void setArmControlMode(ArmControlMode controlMode) { - this.armControlMode = controlMode; - } - - private synchronized ArmControlMode getArmControlMode() { - return this.armControlMode; - } + //ArmMotor.setInverted(false); + //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); - - - public void setSpeedJoystick(double speed) { - motor1.set(ControlMode.PercentOutput, speed); - setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + + + + + } + catch(Exception e) + { + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); + } + } + + private double nonlinearStickCalcPositive(double move, double moveNonLinearity) + { + return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity); } - - public void setPositionPID(double targetPositionInches) { - mpController.setPIDSlot(PID_SLOT); - updatePositionPID(targetPositionInches); - setArmControlMode(ArmControlMode.PID); - setFinished(false); + + private double nonlinearStickCalcNegative(double move, double moveNonLinearity) + { + return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity); } - - public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld()*Y_POSITION_MATH; - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - } - - - private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { - return MIN_POSITION_INCHES; - } - else if (targetPosition > MAX_POSITION_INCHES) { - return MAX_POSITION_INCHES; + + private boolean inDeadZone(double input) + { + boolean inDeadZone; + if (Math.abs(input) < STICK_DEADBAND) + { + inDeadZone = true; + } + else + { + inDeadZone = false; } - return targetPosition; + return inDeadZone; } - - @Override - public void onStart(double timestamp) { - mpController.setPIDSlot(PID_SLOT); - mpController.setPIDSlot(PID_SLOT); + + 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; + } - @Override - public void onStop(double timestamp) { - // TODO Auto-generated method stub - - } + move += trim; + move *= scale; + move = limitValue(move); - @Override - public void onLoop(double timestamp) { - synchronized (Arm.this) { - switch( getArmControlMode() ) { - case PID: - controlPidWithJoystick(); - break; - case JOYSTICK_MANUAL: - controlManualWithJoystick(); - break; - default: - break; - - + 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(); + } + + 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; + + - - private void controlPidWithJoystick() { - //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - //double deltaPosition = joystickPosition *.5; - targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition; - updatePositionPID(targetPositionInchesPID); + @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()); + } + */ + } } - private void controlManualWithJoystick() { - double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick(joyStickSpeed); + @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 double getYPositionInches() { - return motor1.getPositionWorld()*Y_POSITION_MATH; - - } - public double getXPositionInches() { - return motor1.getPositionWorld()*X_POSITION_MATH; - } - -// 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() { + public synchronized boolean isFinished() + { return isFinished; } - public synchronized void setFinished(boolean isFinished) { - this.isFinished = isFinished; - } - - public double getPeriodMs() { + public double getPeriodMs() + { return periodMs; } - public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { - try { - SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld()); - SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); - SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); + 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"); } } - } - - public static Arm getInstance() { - if(instance == null) { - instance = new Arm(); - } - return instance; } -} + + +} \ No newline at end of file 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 bc9fd7c..5402a51 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 @@ -101,8 +101,6 @@ public class Wrist extends Subsystem private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; LimitSwitchSource limitSwitchSource; - private boolean pressed; - SensorCollection isPressed; public Wrist() {