<commit_before>package com.edinarobotics.zed.subsystems;

import com.edinarobotics.utils.commands.MaintainStateCommand;
import com.edinarobotics.utils.pid.PIDConfig;
import com.edinarobotics.utils.pid.PIDTuningManager;
import com.edinarobotics.utils.subsystems.Subsystem1816;
import edu.wpi.first.wpilibj.CANJaguar;

public class Lifter extends Subsystem1816 {
    private static final int ANGLE_POTENTIOMETER_TURNS = 1;
    private final double P_LIFTER_1 = 1;
    private final double I_LIFTER_1 = 0;
    private final double D_LIFTER_1 = 0;
    private final double P_LIFTER_2 = 1;
    private final double I_LIFTER_2 = 0;
    private final double D_LIFTER_2 = 0;
    private final int NUM_RETRIES = 10;
    private PIDConfig firstLifterPIDConfig;
    private PIDConfig secondLifterPIDConfig;

    private double position;
    private CANJaguar lifterJaguarFirst;
    private CANJaguar lifterJaguarSecond;
    
    public Lifter(int firstLifterJaguarNum, int secondLifterJaguarNum) {
        super("Lifter");
        lifterJaguarFirst = createCANJaguar(firstLifterJaguarNum,
                CANJaguar.PositionReference.kPotentiometer,
                ANGLE_POTENTIOMETER_TURNS,
                CANJaguar.ControlMode.kPosition,
                P_LIFTER_1, I_LIFTER_1, D_LIFTER_1,
                NUM_RETRIES);
        lifterJaguarSecond = createCANJaguar(secondLifterJaguarNum,
                CANJaguar.PositionReference.kPotentiometer,
                ANGLE_POTENTIOMETER_TURNS,
                CANJaguar.ControlMode.kPosition,
                P_LIFTER_2, I_LIFTER_2, D_LIFTER_2,
                NUM_RETRIES);
        firstLifterPIDConfig = PIDTuningManager.getInstance().getPIDConfig("First Lifter");
        secondLifterPIDConfig = PIDTuningManager.getInstance().getPIDConfig("Second Lifter");
    }

    protected void initDefaultCommand() {
        setDefaultCommand(new MaintainStateCommand(this));
    }
    
    public void setShooterPosition(double position) {
        this.position = position;
        update();
    }
    
    public void update() {
        try {
            if(lifterJaguarFirst != null) {
                    lifterJaguarFirst.setX(position);
                    //PID tuning code
                    lifterJaguarFirst.setPID(firstLifterPIDConfig.getP(P_LIFTER_1),
                            firstLifterPIDConfig.getI(I_LIFTER_1),
                            firstLifterPIDConfig.getD(D_LIFTER_1));
                    firstLifterPIDConfig.setSetpoint(position);
                    firstLifterPIDConfig.setValue(lifterJaguarFirst.getPosition());
            }
            if(lifterJaguarSecond != null) {
                    lifterJaguarSecond.setX(position);
                    //PID tuning code
                    lifterJaguarSecond.setPID(secondLifterPIDConfig.getP(P_LIFTER_2),
                            secondLifterPIDConfig.getI(I_LIFTER_2),
                            secondLifterPIDConfig.getD(D_LIFTER_2));
                    secondLifterPIDConfig.setSetpoint(position);
                    secondLifterPIDConfig.setValue(lifterJaguarSecond.getPosition());
            }<commit_msg>Fixed bug in Lifter class' setLifterPosition method.<commit_after>package com.edinarobotics.zed.subsystems;

import com.edinarobotics.utils.commands.MaintainStateCommand;
import com.edinarobotics.utils.pid.PIDConfig;
import com.edinarobotics.utils.pid.PIDTuningManager;
import com.edinarobotics.utils.subsystems.Subsystem1816;
import edu.wpi.first.wpilibj.CANJaguar;

public class Lifter extends Subsystem1816 {
    private static final int ANGLE_POTENTIOMETER_TURNS = 1;
    private final double P_LIFTER_1 = 1;
    private final double I_LIFTER_1 = 0;
    private final double D_LIFTER_1 = 0;
    private final double P_LIFTER_2 = 1;
    private final double I_LIFTER_2 = 0;
    private final double D_LIFTER_2 = 0;
    private final int NUM_RETRIES = 10;
    private PIDConfig firstLifterPIDConfig;
    private PIDConfig secondLifterPIDConfig;

    private double position;
    private CANJaguar lifterJaguarFirst;
    private CANJaguar lifterJaguarSecond;
    
    public Lifter(int firstLifterJaguarNum, int secondLifterJaguarNum) {
        super("Lifter");
        lifterJaguarFirst = createCANJaguar(firstLifterJaguarNum,
                CANJaguar.PositionReference.kPotentiometer,
                ANGLE_POTENTIOMETER_TURNS,
                CANJaguar.ControlMode.kPosition,
                P_LIFTER_1, I_LIFTER_1, D_LIFTER_1,
                NUM_RETRIES);
        lifterJaguarSecond = createCANJaguar(secondLifterJaguarNum,
                CANJaguar.PositionReference.kPotentiometer,
                ANGLE_POTENTIOMETER_TURNS,
                CANJaguar.ControlMode.kPosition,
                P_LIFTER_2, I_LIFTER_2, D_LIFTER_2,
                NUM_RETRIES);
        firstLifterPIDConfig = PIDTuningManager.getInstance().getPIDConfig("First Lifter");
        secondLifterPIDConfig = PIDTuningManager.getInstance().getPIDConfig("Second Lifter");
    }

    protected void initDefaultCommand() {
        setDefaultCommand(new MaintainStateCommand(this));
    }
    
    public void setLifterPosition(double position) {
        this.position = position;
        update();
    }
    
    public void update() {
        try {
            if(lifterJaguarFirst != null) {
                    lifterJaguarFirst.setX(position);
                    //PID tuning code
                    lifterJaguarFirst.setPID(firstLifterPIDConfig.getP(P_LIFTER_1),
                            firstLifterPIDConfig.getI(I_LIFTER_1),
                            firstLifterPIDConfig.getD(D_LIFTER_1));
                    firstLifterPIDConfig.setSetpoint(position);
                    firstLifterPIDConfig.setValue(lifterJaguarFirst.getPosition());
            }
            if(lifterJaguarSecond != null) {
                    lifterJaguarSecond.setX(position);
                    //PID tuning code
                    lifterJaguarSecond.setPID(secondLifterPIDConfig.getP(P_LIFTER_2),
                            secondLifterPIDConfig.getI(I_LIFTER_2),
                            secondLifterPIDConfig.getD(D_LIFTER_2));
                    secondLifterPIDConfig.setSetpoint(position);
                    secondLifterPIDConfig.setValue(lifterJaguarSecond.getPosition());
            }
{'fraction_non_alphanumeric': 0.06793650793650793, 'fraction_numerical': 0.011111111111111112, 'mean_word_length': 2.428182807399347, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6195266', 'n_tokens_mistral': 2060, 'n_tokens_neox': 1910, 'n_words': 288}