2013RobotCodeCpp/PIDPositionMotor.cpp at master · project691/2013RobotCodeCpp · 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
//PIDPositionMotor.cpp

//Includes
#include <ctime>
#include <cmath>
#include "PIDPositionMotor.h"

//Constructor
PIDPositionMotor::PIDPositionMotor(char * _name,
                                   Victor &_vic,
                                   Encoder &_encoder,
                                   const double _posPID[],
                                   const double _velPID[]) : name(_name),
                                   	   	   	   	   motor(_name, _vic, _encoder, _velPID),
                                                   encoder(_encoder),
                                                   position(0.0),
                                                   target(0.0),
                                                   error(0.0),
                                                   deltaTime(0.0),
                                                   kp(_posPID[0]),
                                                   ki(_posPID[1]),
                                                   kd(_posPID[2]),
                                                   maxVel(_velPID[3]),
                                                   integral(0.0),
												   derivative(0.0),
												   out(0.0),
												   lastError(0.0),
												   lastTime(0)
{

}

//Destructor
PIDPositionMotor::~PIDPositionMotor() {}

//Functions

//PIDPositionMotor control
void PIDPositionMotor::run() {
    if(time(NULL) - 10 > lastTime) {
        position = encoder.GetDistance();
        while(position > 360 || position < -360) {
            position /= 360;
        }
        error = target - encoder.GetDistance();
        if(target == encoder.GetDistance()) {
            integral = 0.0;
        }
        deltaTime = time(NULL) - lastTime;

        integral += error * deltaTime;
        derivative = (error - lastError) / deltaTime;
        out = (kp * error) + (ki * integral) + (kd * derivative);
        out *= maxVel;
        if(out >= maxVel) {
            motor.run(maxVel);
        } else if(out <= -maxVel) {
            motor.run(-maxVel);
        } else {
            motor.run(out);
        }
        printf("Name: %s KP: %f Target: %f CurrentPos: %f Error: %f Get(): %d Out: %f\n", name, kp, target, encoder.GetDistance(), error, encoder.Get(), out);

        lastError = error;
        lastTime = time(NULL);
    }
}

//PIDPositionMotor control
void PIDPositionMotor::run(double pos) {
    target = pos;
    run();
}