Climber Updates WPI

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

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() {
-			
+
 	}
-}
+}

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