-
Notifications
You must be signed in to change notification settings - Fork 0
/
encoder_test.ino
129 lines (102 loc) · 3.19 KB
/
encoder_test.ino
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
119
120
121
122
123
124
125
126
127
128
129
volatile long count=0;
double sensed_output_1, control_signal_1;
double setpoint_1;
double Kp_1=1; //proportional gain
double Ki_1=1; //integral gain
double Kd_1=1; //derivative gain
double sensed_output_2, control_signal_2;
double setpoint_2;
double Kp_2=1; //proportional gain
double Ki_2=1; //integral gain
double Kd_2=1;
int T=1; //sample time in milliseconds (ms)
unsigned long last_time;
double total_error, last_error;
int max_control=255;
int min_control=0;
int const RF =10;
int const RB =11;
int const RE = 6;
int interruptPin_1=3;
int const LF =8;
int const LB =9;
int const LE = 5;
int interruptPin_2=2;
unsigned long Tick_1=0, Tick_2=0;
void tick(){
count+=1;
}
int speedofmotors(int k)
{
int i,f=0;
int ticks;
i=micros();
f=micros()-i;
interrupts();
attachInterrupt(digitalPinToInterrupt(k), tick, RISING);
while(f<1000)
{
f=micros()-i; }
detachInterrupt(digitalPinToInterrupt(k));
noInterrupts();
ticks = count;
count=0;
i=0;
f=0;
return(ticks);
}
void setup() {
Serial.begin(9600);
pinMode(5,OUTPUT);
pinMode(RF,OUTPUT);
pinMode(RB,OUTPUT);
digitalWrite(RF,1);
digitalWrite(RB,0);
pinMode(4,OUTPUT);
pinMode(LF,OUTPUT);
pinMode(LB,OUTPUT);
digitalWrite(LF,1);
digitalWrite(LB,0);
setpoint_1=50;
setpoint_2=50;
}
void loop()
{
// while(Tick_1 <= 1000000 && Tick_2 <= 1000000)
/* while(1)
{
sensed_output_1=speedofmotors(interruptPin_1);
Tick_1 += sensed_output_1;
control_signal_1 = PID_Control(Kp_1,Ki_1,Kd_1,setpoint_1,sensed_output_1);
analogWrite(RE,control_signal_1);
sensed_output_2=speedofmotors(interruptPin_2);
Tick_2 += sensed_output_2;
control_signal_2 = PID_Control(Kp_2,Ki_2,Kd_2,setpoint_2,sensed_output_2); //calls the PID function every T interval and outputs a control signal
analogWrite(LE,control_signal_2);
Serial.print(sensed_output_1);
Serial.print("\t");
Serial.println(sensed_output_2);
}*/
// analogWrite(RE,0);
// analogWrite(LE,0);
sensed_output_2=speedofmotors(interruptPin_2);
Serial.println(sensed_output_2);
}
double PID_Control(double Kp, double Ki, double Kd, double setpoint, double sensed_output){
unsigned long current_time = micros();
double control_signal; //returns the number of milliseconds passed since the Arduino started running the program
int delta_time = current_time - last_time; //delta time interval
if (delta_time >= T){
double error = setpoint - sensed_output;
total_error += error; //accumalates the error - integral term
if (total_error >= max_control) total_error = max_control;
else if (total_error <= min_control) total_error = min_control;
double delta_error = error - last_error; //difference of error for derivative term
control_signal = Kp*error + (Ki*T)*total_error + (Kd/T)*delta_error; //PID control compute
if (control_signal >= max_control) control_signal = max_control;
else if (control_signal <= min_control) control_signal = min_control;
last_error = error;
last_time = current_time;
return control_signal;
}
}