From 5bba588af98222cfc89f058a9098f0a40489b5d2 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 11 Feb 2019 17:10:17 -0700 Subject: [PATCH] Climber Updates WIP WIP --- 2019robot/build.gradle | 2 +- .../org/usfirst/frc4388/robot/RobotMap.java | 5 + .../robot/commands/InitiateClimber.java | 2 +- .../frc4388/robot/subsystems/Climber.java | 111 ++++++++++++------ 4 files changed, 84 insertions(+), 36 deletions(-) diff --git a/2019robot/build.gradle b/2019robot/build.gradle index ea2312b..23028c6 100644 --- a/2019robot/build.gradle +++ b/2019robot/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" + id "edu.wpi.first.GradleRIO" version "2019.2.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" 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 0ef7d9e..55e22a3 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,12 @@ public class RobotMap { //Arm Motors public static final int ARM_MOTOR1_ID = 6; public static final int ARM_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 8a6e37d..6b01543 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 @@ -15,7 +15,7 @@ public class InitiateClimber extends Command @Override protected void initialize() { - Robot.climber.setClimbSpeed(climb); + Robot.climber.setClimbSpeed(climb, speed); } @Override 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..acf990b 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,40 +1,78 @@ - - - - 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; - /** * */ public class Climber extends Subsystem{ - private WPI_TalonSRX Climber; + private WPI_TalonSRX climberBack; + private WPI_TalonSRX climberFront; - 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); + + climber.set(ControlMode.Follower, carriageLeft.getDeviceID()); + + climberBack.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + climberBack.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + } + catch (Exception e) { System.err.println("An error occurred in the climbing constructor"); - } + FRONT_FREQ = BACK_FREQ * FREQ_RATIO; // Sets the front motor speed to ~1/4 the back motor speed } @Override @@ -42,27 +80,32 @@ public class Climber extends Subsystem{ } - @Override public void periodic() { - // Put code here to be run every loop - } - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); - } - if (Climb==false) { - Climber.set(0); - } -} - // 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 + */