forked from Reinbert/ros_mecanum_robot
-
Notifications
You must be signed in to change notification settings - Fork 1
/
pid.h
37 lines (27 loc) · 1004 Bytes
/
pid.h
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
#include <PID_v1.h>
double kp=90,ki=40,kd=0;
double LF_setpoint = 0, RF_setpoint = 0, LB_setpoint = 0, RB_setpoint = 0;
PID *RF_PID, *LF_PID, *LB_PID, *RB_PID;
extern double LF_vel, RF_vel, LB_vel, RB_vel;
extern double LF_pwm, RF_pwm, LB_pwm, RB_pwm;
void setupPID(){
RF_PID = new PID(&RF_vel, &RF_pwm, &RF_setpoint, kp, ki, kd, DIRECT);
LF_PID = new PID(&LF_vel, &LF_pwm, &LF_setpoint, kp, ki, kd, DIRECT);
LB_PID = new PID(&LB_vel, &LB_pwm, &LB_setpoint, kp, ki, kd, DIRECT);
RB_PID = new PID(&RB_vel, &RB_pwm, &RB_setpoint, kp, ki, kd, DIRECT);
RF_PID->SetMode(AUTOMATIC);
RF_PID->SetOutputLimits(-255,255);
LF_PID->SetMode(AUTOMATIC);
LF_PID->SetOutputLimits(-255,255);
LB_PID->SetMode(AUTOMATIC);
LB_PID->SetOutputLimits(-255,255);
RB_PID->SetMode(AUTOMATIC);
RB_PID->SetOutputLimits(-255,255);
}
void loopPID(){
RF_PID->Compute();
LF_PID->Compute();
LB_PID->Compute();
RB_PID->Compute();
// printf("LF: %f, RF: %f, LB: %f, RB: %f\n", LF_pwm, RF_pwm, LB_pwm, RB_pwm);
}