CommonDecode.java

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.DcMotor;


public class CommonDecode {

    private HardwareMap hardwareMap = null;
    private Servo ballDrop = null;
    private Servo rotator = null;
    private DcMotorEx shooter = null;
    private DcMotorEx intake = null;
    
    enum ColorOrder { GPP, PGP, PPG }
    enum ShooterState { Off, SpinningUp, On }
    enum ShootAllBallsState {
        Off,
        StartSpinning,
        RotateTo1,
        RotateTo2,
        RotateTo3,
        StopSpinning,
        Done }
    
    // states
    private ColorOrder colorOrder = ColorOrder.GPP;
    private int currentBallSlot = 1;   // 1, 2 or 3
    private ShooterState shooterState = ShooterState.Off;
    private ShootAllBallsState shootAllBallsState = ShootAllBallsState.Off;
    private double shootAllBallsDelay = 0.0;
    private ElapsedTime shootAllBallsTimer = new ElapsedTime();
    private ElapsedTime spinningUpTime = new ElapsedTime();
    
    // constants
    private final double intakePower = 1.0;
    private final double retrieveVelocity = 725.0;
    private final double shootVelocity = 1650.0;
    
    private final double ballDropDown = 0.3;
    private final double ballDropUp = 0.85;
    private final double ballSlot1 = 1.0;
    private final double ballSlot2 = 0.53;
    private final double ballSlot3 = 0.08;

    public void initialize (HardwareMap hardwareMap)
    {
        this.hardwareMap = hardwareMap;

        ballDrop = hardwareMap.get(Servo.class, "balldrop");
        rotator = hardwareMap.get(Servo.class, "rotator");
        shooter = hardwareMap.get(DcMotorEx.class, "shooter");
        intake = hardwareMap.get(DcMotorEx.class, "intake");

        shooter.setDirection(DcMotor.Direction.FORWARD);
        intake.setDirection(DcMotor.Direction.FORWARD);
        
        // raise the ball drop
        setBallDropDown (false);
        
        // start in slot 1
        rotateToSlot (1);
    }

    public int getCurrentBallSlot ()
    {
        return currentBallSlot;
    }

    public ShootAllBallsState getShootAllBallsState ()
    {
        return shootAllBallsState;
    }

    public void checkShooterState ()
    {
        double rotateDelay = 0;

        // if finished spinning up, drop the ball
        if (shooterState == ShooterState.SpinningUp &&
            spinningUpTime.seconds () > 1.0)
        {
            shooterState = ShooterState.On;
            setBallDropDown (true);
        }
        
        if (shootAllBallsTimer.seconds () > shootAllBallsDelay)
        {
            shootAllBallsTimer.reset ();

            switch (shootAllBallsState)
            {
                case Off:
                    break;

                case StartSpinning:
                    shootOn (true);

                    switch (colorOrder)
                    {
                        case GPP:
                            rotateDelay = rotateToSlot (1);
                            shootAllBallsState = ShootAllBallsState.RotateTo2;
                            break;
                        case PGP:
                            rotateDelay = rotateToSlot (2);
                            shootAllBallsState = ShootAllBallsState.RotateTo1;
                            break;
                        case PPG:
                            rotateDelay = rotateToSlot (3);
                            shootAllBallsState = ShootAllBallsState.RotateTo2;
                            break;
                    }
                    shootAllBallsDelay = 2.0;
                    break;

                case RotateTo1:
                    rotateDelay = rotateToSlot (1);

                    switch (colorOrder)
                    {
                        case GPP:
                            shootAllBallsState = ShootAllBallsState.RotateTo2;
                            break;
                        case PGP:
                            shootAllBallsState = ShootAllBallsState.RotateTo3;
                            break;
                        case PPG:
                            shootAllBallsState = ShootAllBallsState.StopSpinning;
                            break;
                    }
                    shootAllBallsDelay = 1.0 + rotateDelay;
                    break;

                case RotateTo2:
                    rotateDelay = rotateToSlot (2);

                    switch (colorOrder)
                    {
                        case GPP:
                            shootAllBallsState = ShootAllBallsState.RotateTo3;
                            break;
                        case PGP:
                            shootAllBallsState = ShootAllBallsState.RotateTo1;
                            break;
                        case PPG:
                            shootAllBallsState = ShootAllBallsState.RotateTo1;
                            break;
                    }
                    shootAllBallsDelay = 1.0 + rotateDelay;
                    break;

                case RotateTo3:
                    rotateDelay = rotateToSlot (3);

                    switch (colorOrder)
                    {
                        case GPP:
                            shootAllBallsState = ShootAllBallsState.StopSpinning;
                            break;
                        case PGP:
                            shootAllBallsState = ShootAllBallsState.StopSpinning;
                            break;
                        case PPG:
                            shootAllBallsState = ShootAllBallsState.RotateTo2;
                            break;
                    }
                    shootAllBallsDelay = 1.0 + rotateDelay;
                    break;

                case StopSpinning:
                    rotateToSlot (1);
                    shootOn (false);

                    shootAllBallsState = ShootAllBallsState.Done;
                    shootAllBallsDelay = 1.0;
                    break;

                case Done:
                    shootAllBallsState = ShootAllBallsState.Off;
                    shootAllBallsDelay = 0.0;
                    break;
            }
        }
    }

    public void setBallDropDown (boolean state)
    {
        if (state == true)
        {
            ballDrop.setPosition (ballDropDown);
        }
        else
        {
            ballDrop.setPosition (ballDropUp);
        }

    }

    public void setColorOrder (ColorOrder colorOrder)
    {
        this.colorOrder = colorOrder;
    }

    public void setIntakeOn (boolean state)
    {
        if (state == true)
        {
            intake.setPower(intakePower);
        }
        else  // turn off motor
        {
            intake.setVelocity(0.0);
        }

    }

    public void setIntakeReverse (boolean state)
    {
        if (state == true)
        {
            intake.setPower(-intakePower);
        }
        else  // turn off motor
        {
            intake.setVelocity(0.0);
        }

    }

    public void setShooterOn (boolean state)
    {
        if (state == true)
        {
            shooter.setVelocity(shootVelocity);

            // if it is already on, no change in state            
            if (shooterState == ShooterState.Off)
            {
                shooterState = ShooterState.SpinningUp;
                spinningUpTime.reset ();
            }
        }
        else  // turn off motor
        {
            shooter.setVelocity(0.0);
            shooterState = ShooterState.Off;
        }
    }
    
    public void setShooterRetrieveOn (boolean state)
    {
        if (state == true)
        {
            shooter.setVelocity(retrieveVelocity);
        }
        else  // turn off motor
        {
            shooter.setVelocity(0.0);
        }

        // all retrieve states are not shooting
        shooterState = ShooterState.Off;

    }
    
    // returns time to rotate from current slot to new slot
    public double rotateToSlot (int slot)
    {
        final double ONE_SLOT_DELAY = 0.750;  // 750 milliseconds
        double rotateDelay = Math.abs(slot - currentBallSlot) * ONE_SLOT_DELAY;
        
        switch (slot)
        {
            case 1:
                rotator.setPosition (ballSlot1);
                break;
            case 2:
                rotator.setPosition (ballSlot2);
                break;
            case 3:
                rotator.setPosition (ballSlot3);
                break;
        }
        
        currentBallSlot = slot;
        
        return rotateDelay;
    }
    
    public void rotateCW ()
    {
        if (currentBallSlot < 3)
        {
            rotateToSlot (currentBallSlot + 1);
        }
    }
    
    public void rotateCCW ()
    {
        if (currentBallSlot > 1)
        {
            rotateToSlot (currentBallSlot - 1);
        }
    }
    
    public void retrieveOn (boolean state)
    {
        setShooterRetrieveOn (state);
        setIntakeOn (state);

        // always pull up the ball drop
        setBallDropDown (false);
    }
    
    public void shootOn (boolean state)
    {
        setShooterOn (state);
        setIntakeOn (state);

        if (state == false)
        {
            // pull up the ball drop when done shooting
            setBallDropDown (false);
        }
    }

    public void shootAllBalls (boolean state)
    {
        if (state == true)
        {
            shootAllBallsState = ShootAllBallsState.StartSpinning;
        }
        else
        {
            // stop immediately
            shootAllBallsState = ShootAllBallsState.StopSpinning;
            shootAllBallsDelay = 0.0;
        }
    }
}