diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index 3cacf76..0c95756 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -4,14 +4,35 @@ package org.usfirst.frc4388.robot.subsystems; +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.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.PIDParams; +import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import edu.wpi.first.wpilibj.DoubleSolenoid; +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.livewindow.LiveWindow; +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; +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; @@ -20,20 +41,36 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; */ public class Climber extends Subsystem{ - private WPI_TalonSRX Climber; + //Motors + private WPI_TalonSRX climberBack; + private WPI_TalonSRX climberFront; + private WPI_TalonSRX climberFront2; + private WPI_TalonSRX flipOutMotor; - public boolean on; + //Frequency Control + static float BACK_FREQ = 1; + static float FRONT_FREQ; + static float FREQ_RATIO = 0.2443744576F; + + //Limit and Saftey vars + LimitSwitchSource limitSwitchSource; + SensorCollection isPressed; + boolean safetySwitch; public Climber(){ try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - - System.err.println("An error occurred in the climbing constructor"); - + climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID); + climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID); + climberFront2.set(ControlMode.Follower, climberFront.getDeviceID()); + + climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed + } + catch (Exception e) { + System.err.println("The code broke before the guard did. An error occurred in the climbing constructor"); } } @@ -49,20 +86,30 @@ public class Climber extends Subsystem{ } - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); + public void setClimbSpeed(boolean Climb, double speed) { + if (Climb && safetySwitch) { + if(isPressed.isFwdLimitSwitchClosed()){ //If leg at max height + climberBack.set(0); + climberFront.set(FRONT_FREQ * speed); + } + else if (!isPressed.isFwdLimitSwitchClosed()){ //If leg not at max height + climberBack.set(BACK_FREQ * speed); + climberFront.set(FRONT_FREQ * speed); + } } - if (Climb==false) { - Climber.set(0); - } -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - + if (!Climb || !safetySwitch) { + climberBack.set(0); + climberFront.set(0); } + } } + public void safetySwitch(boolean safetySwitch){ + this.safetySwitch = safetySwitch; + } + + public void flipRatchet(boolean flip, double speed){ + climberFront.set(FRONT_FREQ * speed); + } +}