Steamworks/PIDLoop.h at master · Techbrick/Steamworks · 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
#include "WPILib.h"
#include "Constants.h"
//#include "Filters.h"
#include <math.h>
#include <fstream>

#ifndef SRC_PIDLOOP_H
#define SRC_PIDLOOP_H

class PIDLoop {

  float k_p_Angle;
  float k_i_Angle;
  float k_d_Angle;
  float p_Angle;
  float i_Angle;
  float d_Angle;
  float angle_error;
  float last_angle_error;
  float angleOutput;
  float angleMaxError;
  float iteration_time;

  float k_p_Y = .05;
  float k_i_Y = .05;
  float k_d_Y = .05;
  float p_Y;
  float i_Y;
  float d_Y;
  float y_error;
  float last_y_error;
  float yOutput;
  float yMaxError;

  float k_p_X = .05;
  float k_i_X = .05;
  float k_d_X = .05;
  float p_X;
  float i_X = 0;
  float d_X;
  float x_error;
  float last_x_error = 0;
  float xOutput;
  float xMaxError = 0;

  float k_p_YEnc = .05;
  float k_i_YEnc = .05;
  float k_d_YEnc = .05;
  float p_YEnc;
  float i_YEnc;
  float d_YEnc;
  float yEnc_error;
  float last_yEnc_error;
  float yEncOutput;
  float yEncMaxError;


  float lastLeftUltrasonic = 0;
  float lastRightUltrasonic = 0;

//  Filters filter;

public:
	PIDLoop();
	void resetPIDAngle();
	void resetPIDX();
	void resetPIDY();
	void setAngle(float pAngleInput, float iAngleInput, float dAngleInput);
	void setX(float pXInput, float iXInput, float dXInput);
	void setY(float pYInput, float iYInput, float dYInput);
	float PIDAngle(float yaw, float desiredAngle);
	//float PIDX(float distance, float angleOffset, float cameraOffset);
	float PIDX(float angleToGear);
	float PIDY(float lDistance, float rDistance);
	float PIDYEncoder(float desiredDistance, float encoderDistance);
	void resetPIDYEnc();
	void setYEnc(float pYEncInput, float iYEncInput, float dYEncInput);
	float ultrasonicFilter(float left, float right);
};

#endif