PIDTeaching/ChassisPIDControl.java at master · team3238/PIDTeaching · 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
package org.usfirst.frc.team3238.robot;

import edu.wpi.first.wpilibj.Timer;

public class ChassisPIDControl
{
    static double cummulativeError = 0;
    static double motorPower;
    static double time;
    static double oldTime = 0;
    static double timeDifference;

    static double getMotorValue(double error, double pConstant,
        double Iconstant)
    {
        time = Timer.getFPGATimestamp();
        if(oldTime == 0)
        {
            timeDifference = 0;
        } else
        {
            timeDifference = time - oldTime;
        }
        cummulativeError += error;
        if(error > 0)
        {
            motorPower = -(error * pConstant + cummulativeError * Iconstant
                    * timeDifference);
        }
        if(error < 0)
        {
            motorPower = 0;
        }
        oldTime = time;
        return motorPower;
    }

    static void reinit()
    {
        cummulativeError = 0;
        oldTime = 0;
    }
}