diff --git a/.idea/misc.xml b/.idea/misc.xml index 3ea5c51..7766c02 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,7 +1,7 @@ - + diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 651aeb5..d5ac2ab 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; @@ -17,8 +15,8 @@ import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { - private static final ArmInputsAutoLogged inputs = new ArmInputsAutoLogged(); private static Arm INSTANCE; + private final ArmInputs inputs = new ArmInputs(); private final ArmIO io; private final Mechanism2d mechanism = new Mechanism2d( 3, 3 @@ -28,24 +26,24 @@ public class Arm extends SubsystemBase { new MechanismLigament2d("Shoulder", ArmConstants.SHOULDER_LENGTH, 0) ); private final MechanismLigament2d elbow = shoulder.append( - new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0, 10, new Color8Bit(Color.kPurple)) + new MechanismLigament2d("Elbow", ArmConstants.ELBOW_LENGTH, 0) ); private Command lastCommand = null; private Command currentCommand = null; private boolean changedToDefaultCommand = false; - private Arm(ArmIO io) { - this.io = io; + + private Arm() { + if (Robot.isReal()) { + io = new ArmIOReal(); + } else { + io = new ArmIOReal(); + } } public static Arm getINSTANCE() { if (INSTANCE == null) { - if (Robot.isReal()) { - INSTANCE = new Arm(new ArmIOSim(inputs)); - - } else { - INSTANCE = new Arm(new ArmIOSim(inputs)); - } + INSTANCE = new Arm(); } return INSTANCE; } @@ -94,7 +92,7 @@ public Translation2d getEndPosition() { return ArmIO.armKinematics.forwardKinematics(shoulderAngle, shoulderAngle + elbowAngle - Math.PI); } - public ArmIO.ArmInputs getInputs() { + public ArmInputs getInputs() { return inputs; } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java index 2f8db4c..22c6600 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java @@ -1,6 +1,11 @@ package frc.robot.subsystems.arm; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXInvertType; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -34,8 +39,32 @@ public class ArmConstants { public static final double TICKS_PER_RADIAN_SHOULDER = SHOULDER_FALCON_TICKS_PER_REVOLUTION / (Math.PI * 2); public static final double ELBOW_FALCON_TICKS_PER_REVOLUTION = 2048 * ELBOW_GEARING; public static final double TICKS_PER_RADIAN_ELBOW = ELBOW_FALCON_TICKS_PER_REVOLUTION / (Math.PI * 2); - public static final TalonFXInvertType MAIN_CLOCKWISE = TalonFXInvertType.Clockwise; + public static final boolean MAIN_CLOCKWISE = true; public static final TalonFXInvertType AUX_CLOCKWISE = TalonFXInvertType.Clockwise; + public static final TalonFXConfiguration shoulderMainMotorConfig = new TalonFXConfiguration(); + public static final TalonFXConfiguration shoulderAuxMotorConfig = new TalonFXConfiguration(); + public static final TalonFXConfiguration elbowMainMotorConfig = new TalonFXConfiguration(); + public static final TalonFXConfiguration elbowAuxMotorConfig = new TalonFXConfiguration(); + public static final double SHOULDER_FEED_FORWARD_MULTIPLIER = 0; + public static final double ELBOW_FEED_FORWARD_MULTIPLIER = 0; + public static final int SHOULDER_VELOCITY = 0; + public static final int ELBOW_VELOCITY = 0; + + public static final Slot0Configs SHOULDER_PID= new Slot0Configs() + .withKP(ArmConstants.shoulderP) + .withKI(ArmConstants.shoulderI) + .withKD(ArmConstants.shoulderD); + + public static final Slot1Configs ELBOW_PID = new Slot1Configs() + .withKP(ArmConstants.elbowP) + .withKI(ArmConstants.elbowI) + .withKD(ArmConstants.elbowD); + + public static final double SENSOR_TO_MECHANISM_RATIO= 1; + public static final double ROTOR_TO_SENSOR_RATIO= 1; + + + public static final double ELBOW_ZERO_POSITION = 360 - 53.33; //[degrees] public static final double SHOULDER_ZERO_POSITION = 180 - 65.53; //[degrees] @@ -45,6 +74,8 @@ public class ArmConstants { public static final double END_POSITION_LOWER_X_LIMIT = 0; //[cm] public static final double SHOULDER_ABSOLUTE_ENCODER_OFFSET = 0.4090 - SHOULDER_ZERO_POSITION / 360.0; public static final double ELBOW_ABSOLUTE_ENCODER_OFFSET = 0.57396956434 - ELBOW_ZERO_POSITION / 360.0; + public static final double CURRENT_LIMIT = 10; + //PID public static final double shoulderP = 4.0; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 3c76219..53485ba 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,9 +1,10 @@ package frc.robot.subsystems.arm; import edu.wpi.first.math.geometry.Translation2d; -import org.littletonrobotics.junction.AutoLog; public interface ArmIO { + double currentShoulderAngle = 0; + double currentElbowAngle = 0; ArmKinematics armKinematics = new ArmKinematics(ArmConstants.SHOULDER_LENGTH, ArmConstants.ELBOW_LENGTH); @@ -29,27 +30,12 @@ default void setElbowP(double kP) { default void updateInputs() { } + String getSubsystemName(); + + void updateInputs(ArmInputs inputs); + enum ControlMode { PRECENT_OUTPUT, POSITION } - - @AutoLog - class ArmInputs { - - public double shoulderAngleSetPoint = 0; - double shoulderAppliedVoltage = 0; - double elbowAppliedVoltage = 0; - double shoulderAppliedCurrent = 0; - double elbowAppliedCurrent = 0; - double[] shoulderTipPose = new double[2]; - double[] endEffectorPositionSetPoint = new double[2]; - double shoulderAngle = 0; - double elbowAngleSetpoint = 0; - double elbowAngleRelative = 0; - double elbowAngleAbsolute = 0; - Translation2d endEffectorPose; - ControlMode shoulderControlMode = ControlMode.PRECENT_OUTPUT; - ControlMode elbowControlMode = ControlMode.PRECENT_OUTPUT; - } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java new file mode 100644 index 0000000..495219e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java @@ -0,0 +1,318 @@ +package frc.robot.subsystems.arm; + + +import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Ports; +import utils.math.AngleUtil; + +import static frc.robot.subsystems.arm.ArmConstants.*; + + +public class ArmIOReal implements ArmIO { + + private final TalonFX shoulderMainMotor; + private final TalonFX shoulderAuxMotor; + private final TalonFX elbowMainMotor; + private final TalonFX elbowAuxMotor; + private final DutyCycleEncoder shoulderEncoder; + private final DutyCycleEncoder elbowEncoder; + + + private static ArmIOReal INSTANCE = null; + + private final ArmKinematics kinematics = new ArmKinematics(ArmConstants.SHOULDER_ARM_LENGTH, ArmConstants.ELBOW_ARM_LENGTH); + + private double shoulderOffset = ArmConstants.SHOULDER_ABSOLUTE_ENCODER_OFFSET; + private double elbowOffset = ArmConstants.ELBOW_ABSOLUTE_ENCODER_OFFSET; + + private double shoulderFeedforward; + private double elbowFeedforward; + + private Command lastCommand; + private boolean changedToDefaultCommand = false; + + private double ySetpoint = 0; + + private ControlRequest motorControlRequest = null; + + + public ArmIOReal() { + + this.shoulderEncoder = new DutyCycleEncoder(Ports.ArmPorts.SHOULDER_ENCODER); + this.elbowEncoder = new DutyCycleEncoder(Ports.ArmPorts.ELBOW_ENCODER); + + this.shoulderMainMotor = new TalonFX(Ports.ArmPorts.SHOULDER_MAIN_MOTOR); + + this.shoulderAuxMotor = new TalonFX(Ports.ArmPorts.SHOULDER_AUX_MOTOR); + + this.elbowMainMotor = new TalonFX(Ports.ArmPorts.ELBOW_MAIN_MOTOR); + + this.elbowAuxMotor = new TalonFX(Ports.ArmPorts.ELBOW_AUX_MOTOR); + + + shoulderAuxMotor.setControl(new StrictFollower(shoulderMainMotor.getDeviceID())); + elbowAuxMotor.setControl(new StrictFollower(elbowMainMotor.getDeviceID())); + + + shoulderMainMotorConfig.Voltage.PeakForwardVoltage = ArmConstants.VOLT_COMP_SATURATION; + shoulderMainMotorConfig.Voltage.PeakReverseVoltage = -ArmConstants.VOLT_COMP_SATURATION; + shoulderMainMotorConfig.Slot0 = SHOULDER_PID; + elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT; + elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT; + shoulderMainMotorConfig.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM_RATIO; + shoulderMainMotorConfig.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR_RATIO; + shoulderMainMotor.getConfigurator().apply(shoulderMainMotorConfig); + + shoulderMainMotor.setNeutralMode(NeutralModeValue.Brake); + shoulderMainMotor.setInverted(MAIN_CLOCKWISE); + + elbowMainMotorConfig.Voltage.PeakForwardVoltage = ArmConstants.VOLT_COMP_SATURATION; + elbowMainMotorConfig.Voltage.PeakReverseVoltage = -ArmConstants.VOLT_COMP_SATURATION; + elbowMainMotorConfig.Slot1 = ELBOW_PID; + elbowMainMotorConfig.CurrentLimits.SupplyCurrentLimit = ArmConstants.CURRENT_LIMIT; + elbowMainMotorConfig.CurrentLimits.StatorCurrentLimit = ArmConstants.CURRENT_LIMIT; + elbowMainMotorConfig.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM_RATIO; + elbowMainMotorConfig.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR_RATIO; + elbowMainMotor.getConfigurator().apply(elbowMainMotorConfig); + + elbowMainMotor.setNeutralMode(NeutralModeValue.Brake); + elbowMainMotor.setInverted(MAIN_CLOCKWISE); + + } + + /** + * Get the instance of the arm subsystem. + * + * @return Arm instance + */ + public static ArmIOReal getInstance() { + if (INSTANCE == null) { + INSTANCE = new ArmIOReal(); + } + return INSTANCE; + } + + + /** + * Sets the power of the shoulder motors. + * + * @param power desired power. [-1,1] + */ + public void setShoulderJointPower(double power) { + shoulderMainMotor.setControl(new DutyCycleOut(power)); + } + + public double getShoulderJointPower() { + return shoulderMainMotor.get(); + } + + public double getElbowJointPower() { + return elbowMainMotor.get(); + } + + /** + * Sets the power of the elbow motors. + * + * @param power desired power. [-1,1] + */ + public void setElbowJointPower(double power) { + elbowMainMotor.setControl(new DutyCycleOut(power)); + } + + + /** + * Sets the angle of the shoulder joint. + * + * @param angle desired angle. [degrees] + */ + public void setShoulderJointAngle(Rotation2d angle) { + setShoulderJointAngle(angle.getRadians(), SHOULDER_FEED_FORWARD_MULTIPLIER, SHOULDER_VELOCITY); + } + + + private void setShoulderJointAngle(double angle, double ffMultiplier, int velocity) { + angle = AngleUtil.normalize(angle); + Rotation2d error = new Rotation2d(angle).minus(new Rotation2d(currentShoulderAngle)); + if (shoulderEncoder.isConnected()) { + motorControlRequest = new PositionVoltage(angle); + shoulderMainMotor.setControl(new PositionDutyCycle(angle, velocity, true, + shoulderFeedforward * ffMultiplier, 0, true) + .withEnableFOC(true)); + } else { + shoulderMainMotor.stopMotor(); + } + } + + + public Rotation2d getShoulderJointAngle() { + return Rotation2d.fromRotations(shoulderEncoder.getAbsolutePosition() - shoulderOffset); + } + + public Rotation2d getElbowJointAngle() { + return Rotation2d.fromRotations(elbowEncoder.getAbsolutePosition() - elbowOffset); + } + + /** + * Sets the angle of the elbow joint. + * + * @param angle desired angle. [degrees] + */ + public void setElbowJointAngle(Rotation2d angle) { + setElbowJointAngle(angle.getRadians(), ELBOW_FEED_FORWARD, ELBOW_VELOCITY); + } + + /** + * Sets the angle of the elbow joint. + * + * @param angle desired angle. [degrees] + */ + private void setElbowJointAngle(double angle, double ffMultiplier, int velocity) { + angle = AngleUtil.normalize(angle); + Rotation2d error = new Rotation2d(angle).minus(new Rotation2d(currentShoulderAngle)); + if (elbowEncoder.isConnected()) { + motorControlRequest = new PositionVoltage(angle); + elbowMainMotor.setControl(new PositionDutyCycle(angle, velocity, true, + elbowFeedforward * ffMultiplier, 1, true) + .withEnableFOC(true)); + } else { + elbowMainMotor.stopMotor(); + } + } + + /** + * Calculates the position of the end of the arm. + * + * @return Translation2d of the position. + */ + public Translation2d getEndPosition() { + Rotation2d shoulderAngle = getShoulderJointAngle(); + Rotation2d elbowAngle = getElbowJointAngle(); + return kinematics.forwardKinematics(shoulderAngle.getRadians(), shoulderAngle.getRadians() + elbowAngle.getRadians() - Math.PI); + } + + /** + * Sets the position of the end of the arm. + * + * @param armLocation Translation2d of the desired location. + */ + public void setEndPosition(Translation2d armLocation) { + setEndPosition(armLocation, 0, 0); + } + + /** + * Sets the position of the end of the arm. + * + * @param armLocation Translation2d of the desired location. + */ + public void setEndPosition(Translation2d armLocation, double shoulderFFMultiplier, double elbowFFMultiplier) { + var angles = kinematics.inverseKinematics(armLocation); + setShoulderJointAngle(Math.toDegrees(angles.shoulderAngle), shoulderFFMultiplier, 0); + setElbowJointAngle(Math.toDegrees(angles.elbowAngle), elbowFFMultiplier, 0); + ySetpoint = armLocation.getY(); + } + + /** + * Gets the velocity of the shoulder motors. + * + * @return shoulder motors velocity. [rad/sec] + */ + public double getShoulderMotorVelocity() { + return shoulderMainMotor.getVelocity().getValue(); + } + + public void setFinalSetpointAngles(Translation2d position, ArmInputs inputs) { + var solution = kinematics.inverseKinematics(position); + inputs.finalSetpointAngles = new Translation2d(Math.toDegrees(solution.shoulderAngle), Math.toDegrees(solution.elbowAngle)); + } + + /** + * Gets the velocity of the elbow motors. + * + * @return elbow motor velocity. [rad/sec] + */ + public double getElbowMotorVelocity() { + return elbowMainMotor.getVelocity().getValue(); + } + + public void resetArmEncoders() { + double elbowAngleReset = elbowEncoder.getAbsolutePosition() * 360.0 - ArmConstants.ELBOW_ZERO_POSITION; + double shoulderAngleReset = shoulderEncoder.getAbsolutePosition() * 360.0 - ArmConstants.SHOULDER_ZERO_POSITION; + shoulderOffset = AngleUtil.normalize(shoulderAngleReset) / 360.0; + elbowOffset = AngleUtil.normalize(elbowAngleReset) / 360.0; + } + + public Translation2d getElbowJointPosition() { + Rotation2d shoulderAngle = getShoulderJointAngle(); + return new Translation2d( + SHOULDER_ARM_LENGTH, + shoulderAngle + ); + } + + public boolean armIsOutOfFrame() { + Translation2d elbowJoint = getElbowJointPosition(), endPosition = getEndPosition(); + return !(elbowJoint.getX() < 0) || !(endPosition.getX() < 0); + } + + public boolean armIsInRobot() { + return getEndPosition().getY() < 0 && getShoulderJointAngle().getDegrees() > 90; + } + + public void setVelocity(Translation2d velocity) { + var armVelocities = kinematics.getVelocities(getEndPosition(), velocity); + setShoulderJointPower(armVelocities.shoulderAngle / Math.PI); + setElbowJointPower(armVelocities.elbowAngle / (2 * Math.PI)); + } + + public boolean changedToDefaultCommand() { + return changedToDefaultCommand; + } + + /** + * Stops the motors. + */ + public void stop() { + shoulderMainMotor.stopMotor(); + elbowMainMotor.stopMotor(); + } + + @Override + public String getSubsystemName() { + return "Arm"; + } + + + @Override + public void updateInputs(ArmInputs inputs) { + inputs.shoulderAngle = getShoulderJointAngle().getDegrees(); + inputs.elbowAngle = getElbowJointAngle().getDegrees(); + inputs.shoulderMotorPower = getShoulderJointPower(); + inputs.elbowMotorPower = getElbowJointPower(); + inputs.shoulderEncoderPosition = shoulderEncoder.getAbsolutePosition(); + inputs.elbowEncoderPosition = elbowEncoder.getAbsolutePosition(); + inputs.shoulderVelocity = getShoulderMotorVelocity(); + inputs.elbowVelocity = getElbowMotorVelocity(); + Translation2d position = getEndPosition(); + inputs.armPosition[0] = position.getX(); + inputs.armPosition[1] = position.getY(); + ArmKinematics.InverseKinematicsSolution kinematicsSolution = kinematics.inverseKinematics(position); + inputs.inverseKinematicsSolution[0] = Math.toDegrees(kinematicsSolution.shoulderAngle); + inputs.inverseKinematicsSolution[1] = Math.toDegrees(kinematicsSolution.elbowAngle); + inputs.ySetpoint = ySetpoint; + + inputs.feedforward[0] = shoulderFeedforward; + inputs.feedforward[1] = elbowFeedforward; + + } + + public ArmKinematics getKinematics() { + return kinematics; + } +} + diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java index 4a5d095..13b0e31 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java @@ -38,7 +38,6 @@ public void setElbowPower(double power) { public void setShoulderAngle(double angle) { inputs.shoulderAppliedVoltage = shoulderController.calculate(inputs.shoulderAngle, angle); shoulder.setInputVoltage(inputs.shoulderAppliedVoltage); - } @Override @@ -58,8 +57,14 @@ public void setElbowP(double kP) { elbowController.setP(kP); } + + @Override + public String getSubsystemName() { + return "arm"; + } + @Override - public void updateInputs() { + public void updateInputs(ArmInputs inputs) { shoulder.update(0.02); elbow.update(0.02); inputs.shoulderAngle = shoulder.getAngleRads(); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInputs.java b/src/main/java/frc/robot/subsystems/arm/ArmInputs.java new file mode 100644 index 0000000..7aa3418 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmInputs.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +public class ArmInputs implements LoggableInputs { + + public double shoulderAngle; + public double elbowAngle; + public double shoulderMotorPower; + public double elbowMotorPower; + public double shoulderSetpoint; + public double elbowSetpoint; + public double shoulderEncoderPosition; + public double elbowEncoderPosition; + public double shoulderVelocity; + public double elbowVelocity; + public double[] armPosition; + public double[] inverseKinematicsSolution; + public double[] feedforward; + public double shoulderAcceleration; + public double elbowAcceleration; + public Rotation2d shoulderError; + public Rotation2d elbowError; + public double shoulderOutputVoltage; + public double elbowOutputVoltage; + public Translation2d finalSetpointAngles; + public double ySetpoint; + public double xSetpoint; + public double shoulderAngleSetPoint = 0; + double shoulderAppliedVoltage; + double elbowAppliedVoltage; + double shoulderAppliedCurrent; + double elbowAppliedCurrent; + double[] shoulderTipPose; + double[] endEffectorPositionSetPoint; + double elbowAngleSetpoint; + double elbowAngleRelative; + double elbowAngleAbsolute; + Translation2d endEffectorPose; + ArmIO.ControlMode shoulderControlMode; + ArmIO.ControlMode elbowControlMode; + + + + public void toLog(LogTable table) { + table.put("shoulderAngle", shoulderAngle); + table.put("elbowAngle", elbowAngle); + table.put("shoulderMotorPower", shoulderMotorPower); + table.put("elbowMotorPower", elbowMotorPower); + table.put("shoulderSetpoint", shoulderSetpoint); + table.put("elbowSetpoint", elbowSetpoint); + table.put("shoulderEncoderPosition", shoulderEncoderPosition); + table.put("elbowEncoderPosition", elbowEncoderPosition); + table.put("shoulderVelocity", shoulderVelocity); + table.put("armPosition", armPosition); + table.put("inverseKinematicsSolution", inverseKinematicsSolution); + table.put("feedforward", feedforward); + table.put("shoulderAcceleration", shoulderAcceleration); + table.put("elbowAcceleration", elbowAcceleration); + table.put("shoulderError", shoulderError); + table.put("elbowError", elbowError); + table.put("shoulderOutputVoltage", shoulderOutputVoltage); + table.put("elbowOutputVoltage", elbowOutputVoltage); + table.put("finalSetpointAngles", finalSetpointAngles); + table.put("ySetpoint", ySetpoint); + table.put("xSetpoint", xSetpoint); + table.put("shoulderAppliedVoltage", shoulderAppliedVoltage); + table.put("elbowAppliedVoltage", elbowAppliedVoltage); + table.put("shoulderAppliedCurrent", shoulderAppliedCurrent); + table.put("elbowAppliedCurrent", elbowAppliedCurrent); + table.put("shoulderTipPose", shoulderTipPose); + table.put("endEffectorPositionSetPoint", endEffectorPositionSetPoint); + table.put("elbowAngleSetpoint", elbowAngleSetpoint); + table.put("endEffectorPose", endEffectorPose); + table.put("shoulderControlMode", shoulderControlMode); + table.put("elbowControlMode", elbowControlMode); + } + + @Override + public void fromLog(LogTable table) { + elbowMotorPower = table.get("elbowPower", elbowMotorPower); + elbowMotorPower = table.get("shoulderPower", shoulderMotorPower); + elbowAngle = table.get("elbowAngle", elbowAngle); + shoulderAngle = table.get("shoulderAngle", shoulderAngle); + elbowVelocity = table.get("elbowVelocity", elbowVelocity); + shoulderVelocity = table.get("shoulderVelocity", shoulderVelocity); + elbowAcceleration = table.get("elbowAcceleration", elbowAcceleration); + shoulderAcceleration = table.get("shoulderAcceleration", shoulderAcceleration); + elbowSetpoint = table.get("elbowSetPoint", elbowSetpoint); + shoulderSetpoint = table.get("shoulderSetPoint", shoulderSetpoint); + elbowEncoderPosition = table.get("elbowEncoderPosition", elbowEncoderPosition); + shoulderEncoderPosition = table.get("shoulderEncoderPosition", shoulderEncoderPosition); + armPosition = table.get("armPosition", armPosition); + inverseKinematicsSolution = table.get("inverseKinematicsSolution", inverseKinematicsSolution); + feedforward = table.get("feedforward", feedforward); + shoulderOutputVoltage = table.get("shoulderOutputVoltage", shoulderOutputVoltage); + elbowOutputVoltage = table.get("elbowOutputVoltage", elbowOutputVoltage); + finalSetpointAngles = table.get("finalSetpointAngles", finalSetpointAngles); + ySetpoint = table.get("ySetpoint", ySetpoint); + xSetpoint = table.get("xSetpoint", xSetpoint); + elbowAppliedVoltage = table.get("elbowAppliedVoltage", elbowAppliedVoltage); + shoulderAppliedVoltage = table.get("shoulderAppliedVoltage", shoulderAppliedVoltage); + elbowAppliedCurrent = table.get("elbowAppliedCurrent", elbowAppliedCurrent); + shoulderAppliedCurrent = table.get("shoulderAppliedCurrent", shoulderAppliedCurrent); + shoulderTipPose = table.get("shoulderTipPose", shoulderTipPose); + endEffectorPositionSetPoint = table.get("endEffectorPositionSetPoint", endEffectorPositionSetPoint); + elbowControlMode = table.get("elbowControlMode", elbowControlMode); + shoulderControlMode = table.get("shoulderControlMode", shoulderControlMode); + + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPosition.java b/src/main/java/frc/robot/subsystems/arm/ArmPosition.java index fe16c1a..964995b 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmPosition.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmPosition.java @@ -25,7 +25,7 @@ public enum ArmPosition { this.conePose = conePose; } - public ArmPosition nextState(ArmIO.ArmInputs armInputs, ArmPosition desiredPosition) { + public ArmPosition nextState(ArmInputs armInputs, ArmPosition desiredPosition) { switch (desiredPosition) { case FEEDER: if (armInputs.endEffectorPose.getY() < 0.2 && armInputs.endEffectorPose.getX() < 0) { @@ -63,7 +63,7 @@ public ArmPosition nextState(ArmIO.ArmInputs armInputs, ArmPosition desiredPosit } } - public QuinticBezierSpline getSpline(ArmIO.ArmInputs inputs, ArmPosition desiredPosition) { + public QuinticBezierSpline getSpline(ArmInputs inputs, ArmPosition desiredPosition) { var middleMan = desiredPosition.nextState(inputs, desiredPosition); Translation2d middlePose2, middlePose1; if (middleMan == NEUTRAL && desiredPosition != NEUTRAL) {