2015Robot/Autonomous.java at master · team3238/2015Robot · 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
120
121
122
123
124
125
126
package org.usfirst.frc.team3238.robot;


/**
 *
 * @author Anders Sjoboen
 */
public class Autonomous
{
    int m_autoState;
    int m_collectCount;
    double m_infraredDistanceTrigger;
    long m_timeStamp;
    long m_timeIgnore;
    int m_timeDriveBack;
    int m_timeDriveAway;
    double m_moveBackSpeed;

    public static class AutoState
    {
        public static final int driveOffRamp = 0, pickUpCan = 1,
                translateRight = 2, waitToTranslate = 3, done = 4,
                goBack = 5;
    }

    Autonomous(Chassis chassis)
    {

    }

    /**
     * Re-initializes the autonomous period so that the variables are set up to
     * be at the beginning of the autonomous
     *
     */
    void init()
    {
        m_timeStamp = System.currentTimeMillis();
        m_autoState = AutoState.driveOffRamp;
    }

    /**
     * Handles all of the cases for the autonomous. This must be called every
     * loop for the autonomous to operate
     *
     * @param chassis
     *            Chassis object to move the robot
     * @param reflectSensorRear
     *            The reflectivity sensor in the back of the robot
     * @param reflectSensorFront
     *            The reflectivity sensor in the front of the back of the robot
     * @param gyroValue
     *            The value for how fast the robot is turning
     * @param spinThreshold
     *            Value for the threshold for how fast you need to be going for
     *            the corrector to not correct anymore
     * @param accelerometer
     *            The accelerometer object built in to the roboRIO
     * @param infraredDistance
     *            The distance that the infra sensor is reading
     */
    void idle(Chassis chassis, double gyroValue,
            double spinThreshold, ToteLifter toteLifter, Grabber grabber,
            double gyroPConstant, double gyroIConstant)
    {
        switch(m_autoState)
        {
            case (AutoState.driveOffRamp):
                if(System.currentTimeMillis() - m_timeStamp < 375)
                {
                    chassis.setJoystickData(0, -0.5, 0);
                }
                else
                {
                    chassis.setJoystickData(0, 0, 0);
                    m_autoState = AutoState.pickUpCan;
                }
                break;

            case (AutoState.pickUpCan):
                grabber.grabStepCanAuto();
                m_autoState = AutoState.waitToTranslate;
                break;

            case (AutoState.waitToTranslate):
                chassis.setJoystickData(0, 0, 0);
                if(grabber.m_finishedAutoGrab)
                {
                    System.out.println("*****************************************");
                    m_autoState = AutoState.goBack;
                    m_timeStamp = System.currentTimeMillis();
                }
                break;

            case (AutoState.goBack):
                chassis.setJoystickData(0, 0.5, 0);
                if(System.currentTimeMillis() - m_timeStamp > 250)
                {
                    m_autoState = AutoState.translateRight;
                    m_timeStamp = System.currentTimeMillis();
                }
                break;

            case (AutoState.translateRight):
                System.out.println("Trying to translate!!!!!!!!!!!!!!!!!!!!!!!!!!");
                double adjustedRotation = GyroDrive.getAdjustedRotationValue(
                        0.9, 0.1, 0, gyroPConstant, gyroIConstant,
                        spinThreshold, gyroValue - 2.37);
                chassis.setJoystickData(0.9, 0.1, adjustedRotation);
                if(System.currentTimeMillis() - m_timeStamp > 1000)
                {
                    m_autoState = AutoState.done;
                }

                break;

            case (AutoState.done):
                chassis.setJoystickData(0, 0, 0);
                break;

            default:
                chassis.setJoystickData(0, 0, 0);
                break;
        }
    }
}