mirror of
https://github.com/Team4388/2019-Hit-or-Miss.git
synced 2026-06-09 08:38:06 -06:00
Merge branch 'climber-merge' into develop
This commit is contained in:
Keenan D. Buckley
@@ -51,9 +51,6 @@ public class OI
|
||||
|
||||
//JoystickButton endEfector = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON);
|
||||
//endEfector.toggleWhenActive(new WristPlacement(true));
|
||||
|
||||
|
||||
|
||||
|
||||
JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
liftHatchIntake.whenPressed(new LiftHatchDropBall());
|
||||
@@ -64,10 +61,14 @@ public class OI
|
||||
liftBallIntake.whenPressed(new LiftBallDropHatch());
|
||||
|
||||
|
||||
|
||||
|
||||
// climbUp.whenPressed(new InitiateClimber(true));
|
||||
//climbUp.whenReleased(new InitiateClimber(false));
|
||||
JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
|
||||
double speed = m_driverXbox.getRightTriggerAxis();
|
||||
climbUp.whenPressed(new InitiateClimber(true, speed));
|
||||
climbUp.whenReleased(new InitiateClimber(false, speed));
|
||||
|
||||
JoystickButton ratchetFlip = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_TRIGGER_AXIS);
|
||||
ratchetFlip.whenPressed(new ratchetFlip(true, speed));
|
||||
ratchetFlip.whenReleased(new ratchetFlip(false, speed));
|
||||
|
||||
JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
shiftUp.whenPressed(new DriveSpeedShift(true));
|
||||
@@ -84,10 +85,13 @@ public class OI
|
||||
openIntake.whenPressed(new IntakePosition(true));
|
||||
|
||||
JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON);
|
||||
CloseIntake.whenPressed(new IntakePosition(false));
|
||||
CloseIntake.whenPressed(new IntakePosition(false));
|
||||
*/
|
||||
JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON);
|
||||
safteySwitch.whenPressed(new setClimberSafety(true));
|
||||
safteySwitch.whenReleased(new setClimberSafety(false));
|
||||
|
||||
SmartDashboard.putData("PRE GAME!!!!", new PreGame());
|
||||
*/
|
||||
//SmartDashboard.putData("PRE GAME!!!!", new PreGame());
|
||||
} catch (Exception e) {
|
||||
System.err.println("An error occurred in the OI constructor");
|
||||
}
|
||||
|
||||
@@ -23,7 +23,12 @@ public class RobotMap {
|
||||
//Elevator Motors
|
||||
public static final int ELEVATOR_MOTOR1_ID = 6;
|
||||
public static final int ELEVATOR_MOTOR2_ID = 7;
|
||||
public static final int CLIMBER_CAN_ID = 10;
|
||||
|
||||
|
||||
//Climber Motors
|
||||
public static final int CLIMBER_CAN_ID = 11;
|
||||
public static final int CLIMBER_WHEEL1_ID = 12;
|
||||
public static final int CLIMBER_WHEEL2_ID = 13;
|
||||
|
||||
|
||||
// Pneumatics
|
||||
|
||||
@@ -1,28 +1,36 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class InitiateClimber extends Command
|
||||
{
|
||||
boolean climb;
|
||||
|
||||
public InitiateClimber(boolean climb) {
|
||||
this.climb=climb;
|
||||
double speed;
|
||||
|
||||
public InitiateClimber(boolean climb, double speed) {
|
||||
requires(Robot.climber);
|
||||
this.climb = climb;
|
||||
this.speed = speed;
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void initialize() {
|
||||
Robot.climber.setClimbSpeed(climb);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void execute() {
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
if(climb){ //If climb button is pressed
|
||||
Robot.climber.setClimbSpeed(climb, speed);
|
||||
}
|
||||
else{
|
||||
Robot.climber.setClimbSpeed(false, 0);
|
||||
}
|
||||
}
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
protected boolean isFinished() {
|
||||
return true;
|
||||
}
|
||||
@@ -30,9 +38,9 @@ public class InitiateClimber extends Command
|
||||
// Called once after isFinished returns true
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class ratchetFlip extends Command {
|
||||
boolean flip;
|
||||
double speed;
|
||||
public ratchetFlip(boolean flip, double speed) {
|
||||
requires(Robot.climber);
|
||||
this.flip = flip;
|
||||
this.speed = speed;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
@Override
|
||||
protected void initialize() {
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
@Override
|
||||
protected void execute() {
|
||||
if(flip){
|
||||
Robot.climber.flipRatchet(true, speed);
|
||||
}
|
||||
else{
|
||||
Robot.climber.flipRatchet(false, speed);
|
||||
}
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
@Override
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
@Override
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
package org.usfirst.frc4388.robot.commands;
|
||||
|
||||
import org.usfirst.frc4388.robot.Robot;
|
||||
import org.usfirst.frc4388.robot.Constants;
|
||||
|
||||
import edu.wpi.first.wpilibj.command.Command;
|
||||
|
||||
public class setClimberSafety extends Command {
|
||||
boolean safety;
|
||||
public setClimberSafety(boolean safety) {
|
||||
requires(Robot.climber);
|
||||
this.safety = safety;
|
||||
}
|
||||
|
||||
// Called just before this Command runs the first time
|
||||
@Override
|
||||
protected void initialize() {
|
||||
}
|
||||
|
||||
// Called repeatedly when this Command is scheduled to run
|
||||
@Override
|
||||
protected void execute() {
|
||||
if(safety){ //If saftey button is pressed
|
||||
Robot.climber.safetySwitch(true);
|
||||
}
|
||||
else{
|
||||
Robot.climber.safetySwitch(false);
|
||||
}
|
||||
}
|
||||
|
||||
// Make this return true when this Command no longer needs to run execute()
|
||||
@Override
|
||||
protected boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Called once after isFinished returns true
|
||||
@Override
|
||||
protected void end() {
|
||||
}
|
||||
|
||||
// Called when another command which requires one or more of the same
|
||||
// subsystems is scheduled to run
|
||||
@Override
|
||||
protected void interrupted() {
|
||||
}
|
||||
}
|
||||
@@ -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,28 @@ 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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user