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

fixed motor controls

Browse Source
This commit is contained in:
aarav18
2022-01-22 11:25:39 -07:00
parent ab2a3f6866
commit 050d60ef36
5 changed files with 34 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
build.gradle
+1 -1
View File
@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2022.1.1"
id "edu.wpi.first.GradleRIO" version "2022.2.1"
}
sourceCompatibility = JavaVersion.VERSION_11
src/main/java/frc4388/robot/Constants.java
+4 -4
View File
@@ -77,8 +77,8 @@ public final class Constants {
public static final class ClimberConstants {
/* TODO: Update motor IDS */
public static final int SHOULDER_ID = -1;
public static final int ELBOW_ID = -1;
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
@@ -88,7 +88,7 @@ public final class Constants {
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 MOVE_SPEED = .2; // cm per second
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
@@ -103,7 +103,7 @@ public final class Constants {
public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
public static final int CLIMBER_TIMEOUT_MS = 100;
public static final int CLIMBER_TIMEOUT_MS = 50;
}
public static final class HooksConstants {
src/main/java/frc4388/robot/RobotContainer.java
+9 -10
View File
@@ -43,9 +43,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 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 Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -59,15 +59,14 @@ public class RobotContainer {
/* Default Commands */
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
// m_robotSwerveDrive.setDefaultCommand(
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
// getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
// 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(),
getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), m_robotClimber));
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));
@@ -88,9 +87,9 @@ public class RobotContainer {
.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));
// new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
// .whenPressed(() -> m_hooks.setOpen(true))
// .whenReleased(() -> m_hooks.setOpen(false));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+3 -3
View File
@@ -99,9 +99,9 @@ public class RobotMap {
/* Climb Subsystem */
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
public final WPI_PigeonIMU gyro = null;//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);
//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
+17 -7
View File
@@ -24,10 +24,11 @@ public class Climber extends SubsystemBase {
private WPI_TalonFX m_elbow;
private WPI_PigeonIMU m_gyro;
private boolean m_groundRelative;
private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d};
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
m_shoulder = shoulder;
m_shoulder.configFactoryDefault();
m_shoulder.setNeutralMode(NeutralMode.Brake);
@@ -36,7 +37,12 @@ public class Climber extends SubsystemBase {
m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake);
m_gyro = gyro;
setClimberGains();
if(groundRelative)
m_gyro = gyro;
m_groundRelative = groundRelative;
}
/* getJointAngles
@@ -93,6 +99,8 @@ public class Climber extends SubsystemBase {
elbowAngle += Math.PI;
}
angles[0] = shoulderAngle;
angles[1] = elbowAngle;
return angles;
}
@@ -119,11 +127,14 @@ public class Climber extends SubsystemBase {
}
public void setJointAngles(double[] angles) {
System.out.println(angles);
setJointAngles(angles[0], angles[1]);
}
public void setJointAngles(double shoulderAngle, double elbowAngle) {
// Convert radians to ticks
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
@@ -131,14 +142,13 @@ public class Climber extends SubsystemBase {
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
}
public void controlWithInput(double xInput, double yInput, boolean safety) {
if(!safety)
return;
public void controlWithInput(double xInput, double yInput) {
m_position[0] += xInput * ClimberConstants.MOVE_SPEED;
m_position[1] += yInput * ClimberConstants.MOVE_SPEED;
double tiltAngle = getRobotTilt();
System.out.println("x: " + m_position[0] + " y: " + m_position[1]);
double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d;
double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle);
setJointAngles(jointAngles);