Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

hook fixes

Browse Source
This commit is contained in:
Ryan Manley
2022-03-05 14:21:48 -07:00
parent e7358d7eb5
commit 6a639544ae
6 changed files with 114 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -5
View File
@@ -80,7 +80,7 @@ public final class Constants {
public static final int SHOULDER_ID = 1;
public static final int ELBOW_ID = 3;
public static final int GYRO_ID = -1;
// TODO Update this stuff too
public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm
public static final double LOWER_ARM_LENGTH = 27;
@@ -110,14 +110,15 @@ public final class Constants {
public static final int CLIMBER_TIMEOUT_MS = 50;
/* TODO: Update Constants */
// TODO: Update Constants
// Robot Angle
public static final double ROBOT_ANGLE_ID = 0;
}
public static final class HooksConstants {
public static final int LEFT_HOOK_ID = -1;
public static final int RIGHT_HOOK_ID = -1;
public static final class ClawConstants {
public static final int LEFT_CLAW_ID = 44;
public static final int RIGHT_CLAW_ID = 45;
public static final double OPEN_POSITION = 0;
public static final double CLOSE_POSITION = 0;
src/main/java/frc4388/robot/RobotContainer.java
+15 -12
View File
@@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Hooks;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
@@ -29,7 +29,7 @@ public class RobotContainer {
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
/* Subsystems *//*
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
@@ -39,13 +39,13 @@ public class RobotContainer {
m_robotMap.rightFrontEncoder,
m_robotMap.leftBackEncoder,
m_robotMap.rightBackEncoder
);
);*/
private final LED m_robotLED = new LED(m_robotMap.LEDController);
/*private final LED m_robotLED = new LED(m_robotMap.LEDController);
private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook);
*/
private final Claws m_claws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -64,12 +64,12 @@ public class RobotContainer {
// getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
// moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand(
/*m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(),
getOperatorController().getLeftYAxis()), m_robotClimber));
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));*/
}
/**
@@ -83,13 +83,16 @@ public class RobotContainer {
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
/*new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));*/
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whenPressed(() -> m_hooks.setOpen(true))
.whenReleased(() -> m_hooks.setOpen(false));
.whenPressed(() -> m_claws.setSpeed(0.5))
.whenReleased(() -> m_claws.setSpeed(0.0));
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.whenPressed(() -> m_claws.setSpeed(-0.5))
.whenReleased(() -> m_claws.setSpeed(0.0));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+12 -12
View File
@@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.ClimberConstants;
import frc4388.robot.Constants.HooksConstants;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -27,18 +27,18 @@ import frc4388.robot.Constants.SwerveDriveConstants;
public class RobotMap {
public RobotMap() {
configureLEDMotorControllers();
configureSwerveMotorControllers();
//configureLEDMotorControllers();
//configureSwerveMotorControllers();
}
/* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {
/*void configureLEDMotorControllers() {
}
}*/
/* Swerve Subsystem */
/* Swerve Subsystem *//*
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
@@ -99,14 +99,14 @@ public class RobotMap {
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
}/*
/* Climb Subsystem */
public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
/*public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO
public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO*/
/* Hooks Subsystem */
public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless);
public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_ID, MotorType.kBrushless);
public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
}
src/main/java/frc4388/robot/subsystems/Claws.java
+76
View File
@@ -0,0 +1,76 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants;
public class Claws extends SubsystemBase {
private CANSparkMax m_leftClaw;
private CANSparkMax m_rightClaw;
private SparkMaxLimitSwitch m_leftLimitSwitchF;
private SparkMaxLimitSwitch m_rightLimitSwitchF;
private SparkMaxLimitSwitch m_leftLimitSwitchR;
private SparkMaxLimitSwitch m_rightLimitSwitchR;
private double m_leftOffset;
private double m_rightOffset;
private boolean m_open;
public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchF.enableLimitSwitch(true);
m_rightLimitSwitchF.enableLimitSwitch(true);
m_leftLimitSwitchR.enableLimitSwitch(true);
m_rightLimitSwitchR.enableLimitSwitch(true);
leftClaw.setInverted(true);
rightClaw.setInverted(true);
m_open = false;
// m_leftClaw.set(ClawConstants.CALIBRATION_SPEED);
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
}
public void setSpeed(double speed){
m_leftClaw.set(speed);
m_rightClaw.set(speed);
}
public void setOpen(boolean open) {
if(open) {
m_leftClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_leftOffset);
m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset);
} else {
m_leftClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_leftOffset);
m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset);
}
m_open = open;
}
public boolean getOpen() {
return m_open;
}
@Override
public void periodic() {
if(m_leftLimitSwitchR.isPressed())
m_leftOffset = m_leftClaw.getEncoder().getPosition();
if(m_rightLimitSwitchR.isPressed())
m_rightOffset = m_rightClaw.getEncoder().getPosition();
}
}
src/main/java/frc4388/robot/subsystems/Climber.java
+5 -12
View File
@@ -32,6 +32,7 @@ public class Climber extends SubsystemBase {
private boolean m_groundRelative;
private double m_robotAngle;
private double m_robotPosition;
private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d};
@@ -162,18 +163,11 @@ public class Climber extends SubsystemBase {
{Math.cos(theta) - Math.sin(theta), 0 },
{Math.sin(theta) + Math.cos(theta), 0},
{0, 0, 1}
};
if (m_robotPosition < 45){
setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle);
if (m_robotPosition < m_robotAngle || m_robotPosition > m_robotAngle){
setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition);
}
if( m_robotPosition > 45){
setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle);
}
return Math.toRadians(ypr[1]); // Pitch
// multiply by pie and then divide by 180
@@ -254,11 +248,10 @@ public class Climber extends SubsystemBase {
setJointAngles(jointAngles);
}
public void setRobotAngle(double robotAngle, double robotPosition) {
public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) {
m_robotPosition = robotPosition;
m_robotAngle = robotAngle;
m_robotAngle = 45;
m_robotAngle = 45; //45 is placeholder
}
}
src/main/java/frc4388/robot/subsystems/Hooks.java
-62
View File
@@ -1,62 +0,0 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.HooksConstants;
public class Hooks extends SubsystemBase {
private CANSparkMax m_leftHook;
private CANSparkMax m_rightHook;
private SparkMaxLimitSwitch m_leftLimitSwitch;
private SparkMaxLimitSwitch m_rightLimitSwitch;
private double m_leftOffset;
private double m_rightOffset;
private boolean m_open;
public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) {
m_leftHook = leftHook;
m_rightHook = rightHook;
m_leftLimitSwitch = m_leftHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_rightLimitSwitch = m_rightHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitch.enableLimitSwitch(true);
m_rightLimitSwitch.enableLimitSwitch(true);
m_open = false;
m_leftHook.set(HooksConstants.CALIBRATION_SPEED);
m_rightHook.set(HooksConstants.CALIBRATION_SPEED);
}
public void setOpen(boolean open) {
if(open) {
m_leftHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_leftOffset);
m_rightHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_rightOffset);
} else {
m_leftHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_leftOffset);
m_rightHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_rightOffset);
}
m_open = open;
}
public boolean getOpen() {
return m_open;
}
@Override
public void periodic() {
if(m_leftLimitSwitch.isPressed())
m_leftOffset = m_leftHook.getEncoder().getPosition();
if(m_rightLimitSwitch.isPressed())
m_rightOffset = m_rightHook.getEncoder().getPosition();
}
}