Climber Updates WIP

WIP

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"

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

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

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
+ */