-
Notifications
You must be signed in to change notification settings - Fork 0
/
wheel_enc_pid.c
118 lines (102 loc) · 2.56 KB
/
wheel_enc_pid.c
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
#include "debug_frmwrk.h"
#include "LPC17xx.h"
#include "modules.h"
#include "mechanics.h"
#include "m3pi.h"
#include "wheel_enc_pid.h"
/*working variables*/
unsigned long lastTime;
float Input, Output, Setpoint;
float ITerm, lastInput;
float kp, ki, kd;
int SampleTime = 1000; //1 sec
float outMin, outMax;
uint8_t inAuto = 0;
int controllerDirection = DIRECT;
void Compute()
{
if(!inAuto) return;
uint64_t now = millis();
uint32_t timeChange = (now - lastTime);
if(timeChange >= SampleTime)
{
Input = right_wheel_enc.ticks - left_wheel_enc.ticks;
_DBG("Input: ");_DBD16(right_wheel_enc.ticks);_DBG(" - ");
_DBD16(left_wheel_enc.ticks);_DBG_("");
/*Compute all the working error variables*/
float error = Input;
ITerm += (ki * error);
if(ITerm > outMax) ITerm = outMax;
else if(ITerm < outMin) ITerm = outMin;
float dInput = (Input - lastInput);
/*Compute PID Output*/
Output = kp * error + ITerm - kd * dInput;
if(Output > outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
/*Set Motors*/
if(Output < 0) {
left_motor(outMax + Output);
right_motor(outMax);
} else {
left_motor(outMax);
right_motor(outMax - Output);
}
/*Remember some variables for next time*/
lastInput = Input;
lastTime = now;
}
}
void SetTunings(float Kp, float Ki, float Kd)
{
if (Kp<0 || Ki<0|| Kd<0) return;
float SampleTimeInSec = ((float)SampleTime)/1000;
kp = Kp;
ki = Ki * SampleTimeInSec;
kd = Kd / SampleTimeInSec;
if(controllerDirection == REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
void SetSampleTime(uint32_t NewSampleTime)
{
if (NewSampleTime > 0)
{
float ratio = (float)NewSampleTime
/ (float)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
void SetOutputLimits(uint8_t limit)
{
outMin = -limit;
outMax = limit;
if(Output > outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
void Initialize()
{
lastInput = Input;
ITerm = Output;
if(ITerm > outMax) ITerm= outMax;
else if(ITerm < outMin) ITerm= outMin;
}
void SetMode(uint8_t Mode)
{
uint8_t newAuto = (Mode == AUTOMATIC);
if(newAuto == !inAuto)
{ /*we just went from manual to auto*/
Initialize();
}
inAuto = newAuto;
}
void SetControllerDirection(uint8_t Direction)
{
controllerDirection = Direction;
}