From 3efed01c473e1f0bf1e1821958cf8d1d75d10ac0 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 11 Feb 2019 18:11:30 -0700 Subject: [PATCH] Climber Updates WPI --- .../org/usfirst/frc4388/robot/RobotMap.java | 6 + .../robot/commands/InitiateClimber.java | 15 ++- .../frc4388/robot/subsystems/Climber.java | 105 ++++++++++++++---- 3 files changed, 97 insertions(+), 29 deletions(-) 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 64e015f..c355cee 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -24,7 +24,13 @@ public class RobotMap { //Elevator Motors public static final int ELEVATOR_MOTOR1_ID = 6; public static final int ELEVATOR_MOTOR2_ID = 7; + + + //Climber Motors public static final int CLIMBER_CAN_ID = 10; + public static final int CLIMBER_WHEEL1_ID = 9; + public static final int CLIMBER_WHEEL2_ID = 11; + public static final int CLIMBER_RATCHET_ID = 12; // Pneumatics diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java index 3be9068..c949f6e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java @@ -7,18 +7,21 @@ import edu.wpi.first.wpilibj.command.Command; public class InitiateClimber extends Command { - - public InitiateClimber() { + boolean climb; + float speed; + + public InitiateClimber(boolean Climb) { requires(Robot.climber); + this.climb = Climb; } @Override protected void initialize() { - Robot.climber.setClimbSpeed(kClimbLiftSpeed); } @Override protected void execute() { + Robot.climber.setClimbSpeed(climb, speed); } // Make this return true when this Command no longer needs to run execute() @@ -29,9 +32,9 @@ public class InitiateClimber extends Command // Called once after isFinished returns true protected void end() { } - + @Override protected void interrupted() { - + } -} \ No newline at end of file +} 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 e8046eb..a23dda2 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 @@ -1,28 +1,77 @@ 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 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; +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; - +/** + * + */ 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; + + //Time Control + static float TIME_TO_CLIMB = 1254; + static float TIME_TO_TILT = 1254; + static float TIME_TO_DRIVE = 1254; + static float TIME_TO_PULL = 1254; + + //Limit and Saftey vars + LimitSwitchSource limitSwitchSource; + SensorCollection isPressed; + boolean safteySwitch; public Climber(){ - try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - + climberBack = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + climberFront = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL1_ID); + climberFront2 = new WPI_TalonSRX(RobotMap.CLIMBER_WHEEL2_ID); + flipOutMotor = new WPI_TalonSRX(RobotMap.CLIMBER_RATCHET_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("An error occurred in the climbing constructor"); - } } @@ -31,22 +80,32 @@ public class Climber extends Subsystem{ } - @Override public void periodic() { - // Put code here to be run every loop - } - public void setClimbSpeed(double Climb) { - Climber.set(Climb); -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - + public void setClimbSpeed(boolean Climb, float speed) { + if (Climb && safteySwitch) { + if(isPressed.isFwdLimitSwitchClosed()) //If back at max height + { + climberBack.set(0); + climberFront.set(FRONT_FREQ * speed); + } + else if (isPressed.isFwdLimitSwitchClosed() == false) //If back not at max height + { + climberBack.set(BACK_FREQ * speed); + climberFront.set(FRONT_FREQ * speed); + } + } - + if (Climb == false) { + climberBack.set(0); + climberFront.set(0); + } + } } - +/*TODO + * add command code for starting climb (including button press) + * add command code for flipping front out (including button press) + * add saftey switch on second controller + */