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

climber default command not IK

Browse Source
This commit is contained in:
aarav18
2022-03-06 12:57:41 -07:00
parent 0b75f5793f
commit b5345a53cb
2 changed files with 15 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+7 -2
View File
@@ -137,8 +137,13 @@ 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().getLeftX(),
getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), getOperatorController().getLeftY()),
m_robotClimber));
// IK command
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
// getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Turret default command
//m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
src/main/java/frc4388/robot/subsystems/Climber.java
+8 -4
View File
@@ -22,8 +22,8 @@ import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase {
private WPI_TalonFX m_shoulder;
private WPI_TalonFX m_elbow;
public WPI_TalonFX m_shoulder;
public WPI_TalonFX m_elbow;
private double m_shoulderOffset;
private double m_elbowOffset;
@@ -33,8 +33,6 @@ public class Climber extends SubsystemBase {
private double m_robotAngle;
private double m_robotPosition;
private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d};
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
@@ -142,6 +140,12 @@ public class Climber extends SubsystemBase {
angles[1] = elbowAngle;
return angles;
}
public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput);
m_elbow.set(elbowOutput);
}
/* Rotation Matrix
R = [cos(0) -sin(0) \n sin(0) cos(0)]
Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]