rmr2019/Elevator.java at master · lshrack/rmr2019 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
package frc.robot;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;


public class Elevator extends Component
{
    //declares motor
    WPI_TalonSRX elevMotor = new WPI_TalonSRX(RobotMap.ELEVATOR);

    //Variable used to set speed
    double speed = 0.0;

    //In situations where limit switches are desired, it is necessary to declare them. YEET SPAGHEET
    public DigitalInput limitSwitchTop;
    public DigitalInput limitSwitchBottom;

    //Declares Encoder
    public Encoder encoder;

    //Gets Encoder Data
    private int encData;

    /*private double distance;
    private double period;
    private double rate;
    private boolean direction;
    private boolean stopped;*/

    //Constants declaring top and bottom values for encoder *NOT TESTED(change values or ded)
    private int TOP_MAX = 180;
    private int BOTTOM_MAX = 0;
    private int MIDDLE_MAX = 90;

    //Current position and whether or not we are moving
    private String position = "BOTTOM";
    private boolean isMoving = false;


    //Constructor
    public Elevator()
    {
        limitSwitchTop = new DigitalInput(RobotMap.LIMIT_TOP);
        limitSwitchBottom = new DigitalInput(RobotMap.LIMIT_BOTTOM);
	    encoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
    }


    //Updates Robot Position every frame
    public void update()
    {
        //TODO: test Robot.driveController.getRawButton(XboxMap.D_PAD_VERT)" MAY NOT BE CORRECT DPAD; CHECK IN FUTURE - try Robot.driveController.getRawButton(XboxMap.D_PAD_VERT) < 0
        //if the D-pad is pressed, we're not already at the top, and not already moving somewhere, then move UP ONE POSITION
        if(RobotMap.manipController.getRawButton(XboxMap.D_PAD_VERT) && !limitSwitchTop.get() && !isMoving)
		{
            //updates moving and speed
            isMoving = true;
            speed = 0.25;
            //updates the position of the elevator
            if (position.equals("BOTTOM"))
            {
                position = "MIDDLE";
            }
            else if(position.equals("MIDDLE"))
            {
                position = "TOP";
            }
        }

        //TODO: probably needs to be changed to D_PAD_VERT somehow
        //move DOWN ONE POSITION
        if(RobotMap.manipController.getRawButton(XboxMap.D_PAD_HORIZ) && !limitSwitchBottom.get() && !isMoving)
		{
            //updates moving and speed
            isMoving = true;
            speed = -0.25;
            //updates the position of the elevator
            if (position.equals("TOP"))
            {
                position = "MIDDLE";
            }
            else if(position.equals("MIDDLE"))
            {
                position = "BOTTOM";
            }
        }

        //if position matches encoder, then stop
        if ((position.equals("TOP") && encData == TOP_MAX) || (position.equals("MIDDLE") && encData == MIDDLE_MAX) || (position.equals("BOTTOM") && encData == BOTTOM_MAX))
        {
            speed = 0;
        }

        //Set the motor to the speed as defined by the if's above
        elevMotor.set(speed);
        encData = encoder.get();

        /*distance = encoder.getDistance();
        period = encoder.getPeriod();
        rate = encoder.getRate();
        direction = encoder.getDirection();
        stopped = encoder.getStopped();*/

    }


    public void autoUpdate()
    {
        //TODO: Probably needs update() here
    }


    public void disable()
    {

    }
}