diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index f13fd84..3f6ad83 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -39,4 +39,8 @@ public class RobotMap { public static final int OPEN_INTAKE_PCM_ID = 4; public static final int CLOSE_INTAKE_PCM_ID = 5; + //DIO + public static final int ARM_LIMIT_FORWARD_ID = 0; + public static final int ARM_LIMIT_REVERSE_ID = 1; + } 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 c48e94e..991a67e 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 @@ -53,7 +53,7 @@ public class Arm extends Subsystem implements ControlLoopable { private static Arm instance; - public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION}; + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION, F_SWITCH, R_SWITCH}; 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 @@ -87,11 +87,12 @@ public class Arm extends Subsystem implements ControlLoopable // Motor controllers public CANSparkMax motor1; - private CANSparkMax motor2; + //private CANSparkMax motor2; private CANPIDController motorController; private CANEncoder motorEncoder; private CANDigitalInput motorForwardLimitSwitch, motorReverseLimitSwitch; + private DigitalInput armForwardLimit, armReverseLimit; // PID controller and params //private MPTalonPIDController mpController; @@ -151,14 +152,14 @@ public class Arm extends Subsystem implements ControlLoopable // motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); motor1 = new CANSparkMax(RobotMap.ARM_MOTOR1_ID, MotorType.kBrushless); - motor2 = new CANSparkMax(RobotMap.ARM_MOTOR2_ID, MotorType.kBrushless); + //motor2 = new CANSparkMax(RobotMap.ARM_MOTOR2_ID, MotorType.kBrushless); motor1.restoreFactoryDefaults(); - motor2.restoreFactoryDefaults(); - motor2.follow(motor1); + //motor2.restoreFactoryDefaults(); + //motor2.follow(motor1); motorController = motor1.getPIDController(); - motorEncoder = motor1.getEncoder(); - motorForwardLimitSwitch = motor1.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - motorReverseLimitSwitch = motor1.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + //motorEncoder = motor1.getEncoder(); + //motorForwardLimitSwitch = motor1.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + //motorReverseLimitSwitch = motor1.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); motorController.setP(kP); motorController.setI(kI); @@ -173,6 +174,8 @@ public class Arm extends Subsystem implements ControlLoopable motorController.setSmartMotionMaxAccel(maxAcc, smartMotionSlot); motorController.setSmartMotionAllowedClosedLoopError(allowedErr, smartMotionSlot); + armForwardLimit = new DigitalInput(RobotMap.ARM_LIMIT_FORWARD_ID); + armReverseLimit = new DigitalInput(RobotMap.ARM_LIMIT_REVERSE_ID); // motor1.setInverted(true); // motor2.setInverted(true); @@ -217,15 +220,28 @@ public class Arm extends Subsystem implements ControlLoopable dPadButtons(); - if (motorReverseLimitSwitch.get()){ - resetEncoder(); + if (armForwardLimit.get() && armControlMode == ArmControlMode.JOYSTICK_MANUAL){ + setArmControlMode(ArmControlMode.F_SWITCH); } + else if (armReverseLimit.get() && armControlMode == ArmControlMode.JOYSTICK_MANUAL){ + setArmControlMode(ArmControlMode.R_SWITCH); + } + + /* if (motorReverseLimitSwitch.get()){ + resetEncoder(); + } */ // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); // System.err.println(motorController.getControlMode()); } + else if (armControlMode == ArmControlMode.F_SWITCH){ + controlManualWithJoystickFSwitch(); + } + else if (armControlMode == ArmControlMode.R_SWITCH){ + controlManualWithJoystickRSwitch(); + } //else if (!isFinished) { else { if (armControlMode == ArmControlMode.JOYSTICK_PID){ @@ -288,7 +304,7 @@ public class Arm extends Subsystem implements ControlLoopable public void setSpeed(double speed) { // motor1.set(ControlMode.PercentOutput, speed); - motorController.setReference(speed, ControlType.kVoltage); + motor1.set(speed); setArmControlMode(ArmControlMode.MANUAL); } @@ -310,6 +326,20 @@ public class Arm extends Subsystem implements ControlLoopable double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick(joyStickSpeed); } + + private void controlManualWithJoystickFSwitch(){ + double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + if (joyStickSpeed > 0) joyStickSpeed = 0; + setSpeedJoystick(joyStickSpeed); + if (!armForwardLimit.get()) setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + } + + private void controlManualWithJoystickRSwitch(){ + double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + if (joyStickSpeed < 0) joyStickSpeed = 0; + setSpeedJoystick(joyStickSpeed); + if (!armReverseLimit.get()) setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + } private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); @@ -388,7 +418,7 @@ public class Arm extends Subsystem implements ControlLoopable } public double getAverageMotorCurrent() { - return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; + return (motor1.getOutputCurrent() /*+ motor2.getOutputCurrent()*/) / 2; } public synchronized boolean isFinished() { @@ -480,7 +510,7 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println("the encoder is right after this"); try { - SmartDashboard.putNumber("Arm Ticks", motorEncoder.getPosition()); + ///SmartDashboard.putNumber("Arm Ticks", motorEncoder.getPosition()); //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); //SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity()); @@ -489,7 +519,7 @@ public class Arm extends Subsystem implements ControlLoopable SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent()); SmartDashboard.putNumber("Arm Target SM", targetPositionInchesSM); SmartDashboard.putString("Arm Control Mode", armControlMode.toString()); - //SmartDashboard.putNumber("arm output", motor1.getMotorOutputPercent()); + SmartDashboard.putNumber("arm output", motor1.getOutputCurrent()); //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)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 166c217..2931df0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -266,7 +266,7 @@ public class Drive extends Subsystem implements ControlLoopable //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); //rightDrive1.setNominalClosedLoopVoltage(12.0); rightDrive1.clearFaults(); - rightDrive1.setInverted(true);//false on comp, false on practice + rightDrive1.setInverted(false);//false on comp, false on practice //rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot //rightDrive1.setSafetyEnabled(false); rightDrive1.setIdleMode(IdleMode.kBrake); @@ -279,7 +279,7 @@ public class Drive extends Subsystem implements ControlLoopable // } - rightDrive2.setInverted(false);//True on comp 2019, false on practice + rightDrive2.setInverted(true);//True on comp 2019, false on practice //rightDrive2.setSafetyEnabled(false); rightDrive2.setIdleMode(IdleMode.kBrake); //rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); 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 1e42b45..26f4782 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 @@ -104,7 +104,7 @@ public class Wrist extends Subsystem implements ControlLoopable // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC; + private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; public double targetPositionInchesPID = 0; public double targetPositionInchesMM = 0; private boolean firstMpPoint; @@ -172,7 +172,7 @@ public class Wrist extends Subsystem implements ControlLoopable } public void setSpeedJoystick(double speed) { - wristMotor1.set(ControlMode.PercentOutput, speed); + wristMotor1.set(ControlMode.PercentOutput, speed * SPEED_VOLTAGE_RATIO); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } public void setPositionMM(double targetPositionInches){ @@ -422,6 +422,7 @@ public class Wrist extends Subsystem implements ControlLoopable // 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("Wrist Target PID", targetPositionInchesPID); + SmartDashboard.putString("Wrist Mode", wristControlMode.toString()); } catch (Exception e) { System.err.println("Drivetrain update status error" +e.getMessage());