From 13c5b892fc2999125bba12140a30ca2fcc5e7f61 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Fri, 15 Mar 2019 00:18:33 -0600 Subject: [PATCH] fix fixed code so it builds, added toggle to opperator controlor for hatch and ball --- .../java/org/usfirst/frc4388/robot/OI.java | 22 +- .../commands/{ArmStow.java => StowArm.java} | 6 +- .../frc4388/robot/constants/LEDPatterns.java | 39 +-- .../usfirst/frc4388/robot/subsystems/Arm.java | 303 ++++++++---------- .../usfirst/frc4388/robot/subsystems/LED.java | 6 +- 5 files changed, 156 insertions(+), 220 deletions(-) rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{ArmStow.java => StowArm.java} (93%) 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 e5f43d9..8cec3ba 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -7,7 +7,7 @@ import buttons.XBoxTriggerButton; import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.constants.LEDPatterns; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.GenericHID; @@ -50,11 +50,10 @@ public class OI JoystickButton Expand = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.Y_BUTTON); Expand.whenPressed(new WristPlacement(true)); - Expand.whenPressed(new setLEDPattern(LEDPatterns.SOLID_RED)); JoystickButton Contract = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.A_BUTTON); - Contract.whenPressed(new WristPlacement(false)); - Contract.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); + Contract.whenPressed(new WristPlacement(false)); + JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); liftBothIntake.whenPressed(new HatchAndBallUp()); @@ -69,9 +68,7 @@ public class OI JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); safteySwitch.whenPressed(new setClimberSafety(true)); - safteySwitch.whenPressed(new setLEDPattern(LEDPatterns.SOLID_YELLOW)); safteySwitch.whenReleased(new setClimberSafety(false)); - safteySwitch.whenReleased(new setLEDPattern(LEDPatterns.C1_HEARTBEAT_FAST)); JoystickButton climbUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS); JoystickButton climbDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_TRIGGER_AXIS); @@ -83,10 +80,11 @@ public class OI JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); shiftUp.whenPressed(new DriveSpeedShift(true)); + //shiftUp.whenPressed(new LEDIndicators(true)); JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - shiftDown.whenPressed(new DriveSpeedShift(false)); - + shiftDown.whenPressed(new DriveSpeedShift(false)); + // shiftDown.whenPressed(new LEDIndicators(false)); //Operator Xbox /* JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); @@ -96,12 +94,8 @@ public class OI JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ - - JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - stow.whenPressed(new ArmStow()); - - JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_JOYSTICK_BUTTON); - lowHeight.whenPressed(new GrabFromLoadingSatation()); + JoystickButton lowheight = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + lowheight.whenPressed(new GrabFromLoadingSatation()); SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java similarity index 93% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java index e07d9a6..209cac2 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmStow.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StowArm.java @@ -9,16 +9,16 @@ package org.usfirst.frc4388.robot.commands; import edu.wpi.first.wpilibj.command.CommandGroup; -public class ArmStow extends CommandGroup { +public class StowArm extends CommandGroup { /** * Add your docs here. */ - public ArmStow() { + public StowArm() { addSequential(new WristSetPositionPID(0), 1); addParallel(new HatchFlip(false)); addParallel(new WristPlacement(true)); addSequential(new ArmSetPositionMM(10)); - + // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); // these will run in order. diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/constants/LEDPatterns.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/constants/LEDPatterns.java index 4976514..44ecddc 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/constants/LEDPatterns.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/constants/LEDPatterns.java @@ -1,38 +1,9 @@ package org.usfirst.frc4388.robot.constants; public enum LEDPatterns { - // PALLETTE PATTERNS - RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), - RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), - PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), - PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), - RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f), - RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f), - RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f), - BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), - GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), + RED(0.61f), BLACK(0.99f); - // COLOR 1 PATTERNS - C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), - C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), - - // COLOR 2 PATTERNS - C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), - C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), - - // COLOR 1 AND 2 PATTERNS - C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), - C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), - - // SOLID COLORS - SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), - SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), - SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), - SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), - SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); - - // GETTERS/SETTERS - private final float id; - LEDPatterns(float id) { this.id = id; } - public float getValue() { return id; } -} \ No newline at end of file + private final float id; + LEDPatterns(float id) { this.id = id; } + public float getValue() { return id; } + } \ No newline at end of file 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 912b234..00b9501 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 @@ -12,6 +12,7 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; @@ -31,12 +32,11 @@ public class Arm extends Subsystem implements ControlLoopable { private static Arm instance; - public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; - public static enum ArmPositionMode { CARGO, HATCH }; - - // 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); + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; + // 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,7 +48,7 @@ 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 = 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; @@ -57,16 +57,7 @@ public class Arm extends Subsystem implements ControlLoopable public static final double MAX_POSITION_INCHES = 3900; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - public static final double HATCH_LOW_POSITION_WORLD = 135; - public static final double HATCH_MID_POSITION_WORLD = 2400; - public static final double HATCH_HIGH_POSITION_WORLD = 3800; - public static final double CARGO_LOW_POSITION_WORLD = 1500; - public static final double CARGO_MID_POSITION_WORLD = 2900; - public static final double CARGO_HIGH_POSITION_WORLD = 4300; - public static final double CARGO_PICKUP_POSITION_WORLD = 0; - public static final double HATCH_PICKUP_POSITION_WORLD = 0; - - /*public static final double SWITCH_POSITION_INCHES = 24.0; + public static final double SWITCH_POSITION_INCHES = 24.0; public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR public static final double SCALE_LOW_POSITION_INCHES = 56.0; public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0 @@ -74,70 +65,83 @@ public class Arm extends Subsystem implements ControlLoopable public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES; public static final double CLIMB_BAR_POSITION_INCHES = 70.0; public static final double CLIMB_HIGH_POSITION_INCHES = 10.0; - public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0;*/ + 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 ArrayList motorControllers = new ArrayList(); private TalonSRXEncoder motor1; private TalonSRX motor2; - + // PID controller and params private MPTalonPIDController mpController; public static int PID_SLOT = 0; - public static int MP_SLOT = 1; + 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); + 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; + public static final double I_Value = 0.0005;// 0.00030; + public static final double D_Value = 100;// 200; + public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms) + public static final double maxGravityComp = 0.08; + public static final double RampRate = 0;// 0.0; + public static final int A_value = 450; + public static final int CV_value = 740; + + + - 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 = 2; - public static final double I_Value = 0.00030; - public static final double D_Value = 200; - public static final double RampRate = 0.0; private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 5; LimitSwitchSource limitSwitchSource; - + // Pneumatics private Solenoid speedShift; - //DPAD - public static final double DPAD_UP = 0; - public static final double DPAD_RIGHT = 90; - public static final double DPAD_DOWN = 180; - public static final double DPAD_LEFT = 270; - public static final double DPAD_RELEASED = -1; - // 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.JOYSTICK_MANUAL; - public ArmPositionMode armPositionMode = ArmPositionMode.HATCH; private double targetPositionInchesPID = 0; + private 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); + motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); + + motor1.selectProfileSlot(MM_SLOT, 0); + motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); @@ -145,8 +149,9 @@ public class Arm extends Subsystem implements ControlLoopable motor2.setNeutralMode(NeutralMode.Brake); motor1.enableCurrentLimit(true); motorControllers.add(motor1); - - + //motor1.setSelectedSensorPosition(0, , ); + + } catch (Exception e) { System.err.println("An error occurred in the Arm constructor"); @@ -156,55 +161,79 @@ 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); } - + private synchronized void setArmControlMode(ArmControlMode controlMode) { this.armControlMode = controlMode; } - + private synchronized ArmControlMode getArmControlMode() { return this.armControlMode; } - public synchronized void setArmPositionMode(ArmPositionMode controlMode) { - this.armPositionMode = controlMode; - } - - private synchronized ArmPositionMode getArmPositionMode() { - return this.armPositionMode; - } - public void setSpeed(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.MANUAL); } - + public void setSpeedJoystick(double speed) { motor1.set(ControlMode.PercentOutput, speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } + public void setPositionMM(double targetPositionInches){ + motor1.set(ControlMode.MotionMagic, targetPositionInches); + System.err.println(motor1.getControlMode()); + motor1.selectProfileSlot(MM_SLOT, 0); + setArmControlMode(ArmControlMode.MOTION_MAGIC); + updatePositionMM(targetPositionInches); + setFinished(false); + + + } + + public double calcGravityCompensationAtCurrentPosition() { + int ticks = motor1.getSelectedSensorPosition(); + double degreesFromDown = (ticks+920)*(360.0/(4096*3)); + double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); + System.err.println("comp(" + degreesFromDown + "^) = " + compensation); + return compensation; + } + public void updatePositionMM(double targetPositionInches){ + targetPositionInchesMM = limitPosition(targetPositionInches); + //double startPositionInches = motor1.getPositionWorld(); + double compensation = calcGravityCompensationAtCurrentPosition(); + //System.err.println("compensation = " + compensation); + // motor1.set(ControlMode.MotionMagic, targetPositionInches); + motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation); + //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); @@ -216,15 +245,15 @@ 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) { return MIN_POSITION_INCHES; @@ -232,7 +261,7 @@ public class Arm extends Subsystem implements ControlLoopable else if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } - + return targetPosition; } @Override @@ -243,51 +272,6 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPIDSlot(PID_SLOT); this.periodMs = periodMs; } - /*@Override - public void onStart(double timestamp) { - 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.setPIDSlot(PID_SLOT); - } - - @Override - public void onStop(double timestamp) { - // TODO Auto-generated method stub - - } - - @Override - public void onLoop(double timestamp) { - synchronized (Arm.this) { - switch( getElevatorControlMode() ) { - case JOYSTICK_PID: - controlPidWithJoystick(); - break; - case JOYSTICK_MANUAL: - controlManualWithJoystick(); - break; - case MOTION_PROFILE: - if (!isFinished()) { - if (firstMpPoint) { - mpController.setPIDSlot(MP_SLOT); - firstMpPoint = false; - } - setFinished(mpController.controlLoopUpdate()); - if (isFinished()) { - mpController.setPIDSlot(PID_SLOT); - } - } - break; - default: - break; - } - } - } - -*/ @@ -300,7 +284,7 @@ public class Arm extends Subsystem implements ControlLoopable lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; } lastControlLoopUpdateTimestamp = currentTimestamp; - + if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ resetEncoder(); } @@ -311,69 +295,58 @@ 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){ - //System.err.println(motor1.getControlMode()); - int dPadAngle = Robot.oi.getOperatorController().getDpadAngle(); - if (dPadAngle == DPAD_RELEASED){ controlPidWithJoystick(); - } else { - controlPidWithDPad(dPadAngle); - } + System.err.println(motor1.getControlMode()); } - + if (armControlMode == ArmControlMode.MOTION_MAGIC){ + 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); }*/ } } - private void controlPidWithDPad(int dPadAngle){ - if (armPositionMode == ArmPositionMode.HATCH){ - if (dPadAngle == DPAD_UP){ - updatePositionPID(HATCH_HIGH_POSITION_WORLD); - } else if (dPadAngle == DPAD_RIGHT){ - updatePositionPID(HATCH_MID_POSITION_WORLD); - } else if (dPadAngle == DPAD_DOWN){ - setPositionPID(HATCH_LOW_POSITION_WORLD); - } else if (dPadAngle == DPAD_LEFT){ - //updatePositionPID(HATCH_PICKUP_POSITION_WORLD); - } - } else if (armPositionMode == ArmPositionMode.CARGO){ - if (dPadAngle == DPAD_UP){ - updatePositionPID(CARGO_HIGH_POSITION_WORLD); - } else if (dPadAngle == DPAD_RIGHT){ - updatePositionPID(CARGO_MID_POSITION_WORLD); - } else if (dPadAngle == DPAD_DOWN){ - updatePositionPID(CARGO_LOW_POSITION_WORLD); - } else if (dPadAngle == DPAD_LEFT){ - //updatePositionPID(CARGO_PICKUP_POSITION_WORLD); - } - } - } + + + + + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); } + private void controlMMWithJoystick() { + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesMM = targetPositionInchesMM + deltaPosition; + updatePositionMM(targetPositionInchesMM); + + + } private void ControlWithJoystickhold(){ double holdPosition = motor1.getPositionWorld(); updatePositionPID(holdPosition); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick((joyStickSpeed*.30)+.1); + setSpeedJoystick((joyStickSpeed*.3)+.08); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -388,7 +361,7 @@ public class Arm extends Subsystem implements ControlLoopable mpController.setPID(pidPIDParamsLoGear, PID_SLOT); } } - + public ElevatorSpeedShiftState getShiftState() { return shiftState; } @@ -396,49 +369,51 @@ 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", motor1.getPositionWorld()); - SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); - SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); + + SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); + //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); + //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); + SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity()); SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError()); SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent()); - + SmartDashboard.putNumber("Arm Target MM Position", targetPositionInchesMM); + //SmartDashboard.putNumber("arm output", motor1.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("Arm Target PID Position", targetPositionInchesPID); } catch (Exception e) { - System.err.println("Drivetrain update status error" +e.getMessage()); + 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/LED.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java index 0ef11ea..9c03553 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/LED.java @@ -9,12 +9,10 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.HashMap; -import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.constants.LEDPatterns; import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Add your docs here. @@ -26,17 +24,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public static Spark LEDController = new Spark(LED_SPARK_ID); public LED(){ - setPattern(LEDPatterns.C1_HEARTBEAT_FAST); + setPattern(LEDPatterns.RED); } public void periodic() { LEDController.set(currentLED); - SmartDashboard.putNumber("LED", currentLED); } public void setPattern(LEDPatterns pattern){ currentLED = pattern.getValue(); - LEDController.set(currentLED); } @Override