diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index e61c1ba3..c3ba7c4e 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -25,8 +25,8 @@ public static final class Drivetrain { public static final class Arm { //Motor port - public static final int ARM_MOTOR_PORT_1 = 7; - public final static int ARM_MOTOR_PORT_2 = 8; + public static final int MASTER_ARM_MOTOR = 7; + public final static int FOLLOW_ARM_MOTOR = 8; //all angles in rot here //TODO: finish understand why this is broken public static final Measure INTAKE_ANGLE = Degrees.to(-1); @@ -65,7 +65,7 @@ public static final class Arm { public static final double MAX_FF_VEL = 1; // rot / s public static final double MAX_FF_ACCEL = 1; // rot / s^2 //I assume to max vel and accel are in meters per second - public static TrapezoidProfile.Constraints trapConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL); + public static TrapezoidProfile.Constraints armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL); //Arm buttons public static final int raiseToSpeakerPodButton = Button.kY.value; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java index 3b71a425..b962a702 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -40,35 +40,33 @@ public class Arm extends SubsystemBase { */ // a boolean meant to tell if the arm is in a forbidden posistion AKA FORBIDDEN FLAG - private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_1, MotorConfig.NEO); - private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(ARM_MOTOR_PORT_2, MotorConfig.NEO); - private final RelativeEncoder armEncoder = armMotor1.getEncoder(); - + private final CANSparkMax masterArmMotor = MotorControllerFactory.createSparkMax(MASTER_ARM_MOTOR, MotorConfig.NEO); + private final CANSparkMax followArmMotor = MotorControllerFactory.createSparkMax(FOLLOW_ARM_MOTOR, MotorConfig.NEO); + private final RelativeEncoder armEncoder = masterArmMotor.getEncoder(); private static double kDt = 0.02; //PID, feedforward, trap profile private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV, kA); - private final SparkPIDController armPID1 = armMotor1.getPIDController(); - private final SparkPIDController armPID2 = armMotor2.getPIDController(); - private TrapezoidProfile armProfile = new TrapezoidProfile(trapConstraints); - private Timer armProfileTimer = new Timer(); - TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later + private final SparkPIDController armPID = masterArmMotor.getPIDController(); + + private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints); + + private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later // rad, rad/s //public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(0, 0) }; public Arm() { // weird math stuff - armMotor1.setInverted(motorInverted); - armMotor1.setIdleMode(IdleMode.kBrake); - armMotor2.setInverted(motorInverted); - armMotor2.setIdleMode(IdleMode.kBrake); + masterArmMotor.setInverted(motorInverted); + masterArmMotor.setIdleMode(IdleMode.kBrake); + followArmMotor.setInverted(motorInverted); + followArmMotor.setIdleMode(IdleMode.kBrake); armEncoder.setPositionConversionFactor(rotationToRad); armEncoder.setVelocityConversionFactor(rotationToRad); armEncoder.setInverted(encoderInverted); - - armMotor2.follow(armMotor1); + followArmMotor.follow(masterArmMotor); //armEncoder1.setZeroOffset(offsetRad); @@ -93,12 +91,9 @@ public void periodic() { //ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); // armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL , MAX_FF_ACCEL ); - armPID1.setP(kP); - armPID1.setI(kI); - armPID1.setD(kD); - armPID2.setP(kP); - armPID2.setI(kI); - armPID2.setD(kD); + armPID.setP(kP); + armPID.setI(kI); + armPID.setD(kD); //smart dahsboard stuff //SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID1.atSetpoint()); @@ -138,13 +133,13 @@ public void driveArm(double goalAngle){ TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState); double armFeedVolts = armFeed.calculate(goalState.velocity, 0); - armPID1.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts); + armPID.setReference(setPoint.position, CANSparkBase.ControlType.kVelocity, 0, armFeedVolts); } public void setArmTarget(double targetPos) { targetPos = getArmClampedGoal(targetPos); - armProfile = new TrapezoidProfile(trapConstraints); + armProfile = new TrapezoidProfile(armConstraints); armProfileTimer.reset(); goalState.position = targetPos; @@ -156,7 +151,7 @@ public void setArmTarget(double targetPos) { public void resetGoal() { double armPos = getArmPos(); - armProfile = new TrapezoidProfile(trapConstraints); + armProfile = new TrapezoidProfile(armConstraints); }