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

Added hooks and did code review stuff

Browse Source 
Arm will no longer teleport
This commit is contained in:
66945
2022-01-21 17:20:31 -07:00
parent e01ffd3329
commit ab2a3f6866
5 changed files with 85 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+15 -2
View File
@@ -21,6 +21,8 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final double TICKS_PER_ROTATION_FX = 2048;
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 0.1;
public static final double WHEEL_SPEED = 0.1;
@@ -83,11 +85,14 @@ public final class Constants {
public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm
public static final double LOWER_ARM_LENGTH = 50;
public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH;
public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH);
public static final double MAX_ARM_LENGTH = 100;
public static final double MIN_ARM_LENGTH = 5;
public static final double MOVE_SPEED = 50; // cm per second
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 1;
@@ -100,6 +105,14 @@ public final class Constants {
public static final int CLIMBER_TIMEOUT_MS = 100;
}
public static final class HooksConstants {
public static final int LEFT_HOOK_ID = -1;
public static final int RIGHT_HOOK_ID = -1;
public static final double OPEN_POSITION = 0;
public static final double CLOSE_POSITION = 0;
}
/**
* The OIConstants class contains the ID for the XBox controllers
*/
src/main/java/frc4388/robot/RobotContainer.java
+10 -1
View File
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Hooks;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
@@ -43,6 +44,9 @@ public class RobotContainer {
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);
private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -62,7 +66,8 @@ public class RobotContainer {
// moves climber in xy space with two-axis input from the operator controller
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(),
getOperatorController().getLeftYAxis()), m_robotClimber));
getOperatorController().getLeftYAxis(),
getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), 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));
@@ -82,6 +87,10 @@ public class RobotContainer {
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
.whenPressed(() -> m_hooks.setOpen(true))
.whenReleased(() -> m_hooks.setOpen(false));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+6
View File
@@ -8,8 +8,10 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
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.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -98,4 +100,8 @@ public class RobotMap {
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 = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO
/* Hooks Subsystem */
public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID);
public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID);
}
src/main/java/frc4388/robot/subsystems/Climber.java
+20 -13
View File
@@ -5,8 +5,7 @@
/*
Safety
Hooks
Add
Overextension when lower arm is resting check in processing
Add
*/
package frc4388.robot.subsystems;
@@ -17,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
@@ -25,7 +25,7 @@ public class Climber extends SubsystemBase {
private WPI_PigeonIMU m_gyro;
private double[] position = {ClimberConstants.MIN_ARM_LENGTH, 0.d};
private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d};
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
m_shoulder = shoulder;
@@ -77,16 +77,20 @@ public class Climber extends SubsystemBase {
elbowAngle = Math.PI - elbowAngle;
// If the climber is resting on the robot base, rotate the upper arm in the direction of the target
if(shoulderAngle <= 0.d) {
shoulderAngle = 0.d;
if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH;
elbowAngle = Math.atan(-yTarget / xDiff);
if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
elbowAngle = Math.PI - elbowAngle;
if(xDiff >= 0.d) {
// Deal with negative wraparound
if(xDiff >= 0.d)
elbowAngle += Math.PI;
}
}
return angles;
@@ -120,20 +124,23 @@ public class Climber extends SubsystemBase {
public void setJointAngles(double shoulderAngle, double elbowAngle) {
// Convert radians to ticks
double shoulderPosition = (shoulderAngle * 1024.d) / Math.PI;
double elbowPosition = (elbowAngle * 1024.d) / Math.PI;
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
}
public void controlWithInput(double xInput, double yInput) {
position[0] += xInput * ClimberConstants.MOVE_SPEED;
position[1] += yInput * ClimberConstants.MOVE_SPEED;
public void controlWithInput(double xInput, double yInput, boolean safety) {
if(!safety)
return;
m_position[0] += xInput * ClimberConstants.MOVE_SPEED;
m_position[1] += yInput * ClimberConstants.MOVE_SPEED;
double tiltAngle = getRobotTilt();
double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle);
double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle);
setJointAngles(jointAngles);
}
}
src/main/java/frc4388/robot/subsystems/Hooks.java
+34
View File
@@ -0,0 +1,34 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.HooksConstants;
public class Hooks extends SubsystemBase {
private Servo m_leftHook;
private Servo m_rightHook;
private boolean m_open;
public Hooks(Servo leftHook, Servo rightHook) {
m_leftHook = leftHook;
m_rightHook = rightHook;
m_open = false;
setOpen(m_open);
}
public void setOpen(boolean open) {
if(open) {
m_leftHook.setPosition(HooksConstants.OPEN_POSITION);
m_rightHook.setPosition(HooksConstants.OPEN_POSITION);
} else {
m_leftHook.setPosition(HooksConstants.CLOSE_POSITION);
m_rightHook.setPosition(HooksConstants.CLOSE_POSITION);
}
}
public boolean getOpen() {
return m_open;
}
}