-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrobotProje.m
127 lines (115 loc) · 4.21 KB
/
robotProje.m
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
%tic
%-------ROBOT-------
myRobot=mlrobot;
mlrobotstart(myRobot);
%-------------------
Kp = 1; % proportional
Ki = 0.25; % Integral
Kd = 0.01; % derivative // dt azaldıkça aşım azalır.
dt = 0.01; % örnekleme zamanı
desired = 270; % dönmesini istediğimiz açı
Time = 2; % toplam simulasyon suresi
n = round(Time/dt); % ornek sayisi
Prop(1:n+1) = 0; Der(1:n+1) = 0; Int(1:n+1) = 0; I(1:n+1) = 0;
%PID(1:n+1) = 0;
%Output(1:n+1) = 0;
%Error(1:n+1) = 0;
%FeedBack(1:n+1) = 0;
color = rand(1,3);
for i = 1:n
if desired<=180
%------------PID------------
FeedBack(i)=mlrobotreadcompassangle(myRobot);
Error(i+1) = desired - FeedBack(i);
Prop(i+1) = Error(i+1);
Der(i+1) = (Error(i+1) - Error(i))/dt;
Int(i+1) = (Error(i+1) + Error(i))*dt/2;
I(i+1) = sum(Int);
PID(i+1) = Kp*Prop(i) + Ki*I(i+1)+ Kd*Der(i);
%-----differential drive-----
r = 0.1; %Tekerlek yarıçapı(m)
L = 0.5; %Tekerlekler arası uzaklık(m)
W = PID(i);
V = 10;
vRight(i) = ( ( (2*V)+(PID(i)*L) ) / (2*r) );
vLeft(i) = ( ( (2*V)-(PID(i)*L) ) / (2*r) );
nvRight(i) = ( (1/150)*vRight(i) )*2;
nvLeft(i) = ( (1/150)*vLeft(i) )*2;
if(nvRight(i)>5) nvRight(i)=5; end
if(nvRight(i)<-5) nvRight(i)=-5; end
if(nvLeft(i)>5) nvLeft(i)=5; end
if(nvLeft(i)<-5) nvLeft(i)=5; end
mlrobotsetmotorvoltage(myRobot,'left',-nvLeft(i));
mlrobotsetmotorvoltage(myRobot,'right',nvRight(i));
%----------------------------
wheelSpeedLeft(i) = mlrobotreadwheelspeed(myRobot,'left');
wheelSpeedRight(i) = mlrobotreadwheelspeed(myRobot,'right');
else
inverseDesired = 360 - desired;
%------------PID------------
FeedBack(i)=mlrobotreadcompassangle(myRobot);
Error(i+1) = inverseDesired - FeedBack(i);
Prop(i+1) = Error(i+1);
Der(i+1) = (Error(i+1) - Error(i))/dt;
Int(i+1) = (Error(i+1) + Error(i))*dt/2;
I(i+1) = sum(Int);
PID(i+1) = Kp*Prop(i) + Ki*I(i+1)+ Kd*Der(i);
%-----differential drive-----
r = 0.1; %Tekerlek yarıçapı(m)
L = 0.5; %Tekerlekler arası uzaklık(m)
W = PID(i);
V = 10;
vRight(i) = ( (2*V)+(W*L) )/ (2*r);
vLeft(i) = ( (2*V)-(W*L) )/ (2*r);
nvRight(i) = ( (1/150)*vRight(i) )*2;
nvLeft(i) = ( (1/150)*vLeft(i) )*2;
if(nvRight(i)>5) nvRight(i)=5; end
if(nvRight(i)<-5) nvRight(i)=-5; end
if(nvLeft(i)>5) nvLeft(i)=5; end
if(nvLeft(i)<-5) nvLeft(i)=5; end
mlrobotsetmotorvoltage(myRobot,'left',-nvRight(i));
mlrobotsetmotorvoltage(myRobot,'right',nvLeft(i));
%----------------------------
wheelSpeedLeft(i) = mlrobotreadwheelspeed(myRobot,'left');
wheelSpeedRight(i) = mlrobotreadwheelspeed(myRobot,'right');
end
pause(0.1);% Bir sonraki robot açısını okumadan önce 0,1 saniye bekleme
end
%-----------GRAFİKLER--------------------------
T200 = 1:i;
T201 = 1:i+1;
subplot(3,2,1)
line(T200, FeedBack*pi/180,'color',color )
grid on; hold on;
title('Açı [rad]')
xlabel('t(ms)')
subplot(3,2,2)
line(T201, Error*pi/180,'color',color)
grid on; hold on;
title('Hata [rad]')
xlabel('t(ms)')
subplot(3,2,3)
%tekerlek açısal hız, w=v/r
line(T200, wheelSpeedLeft/r,'color',color )
grid on; hold on;
line(T200, wheelSpeedRight/r,'color',color )
title('Tekerlek Açısal Hızları [rad/s]')
xlabel('t(ms)')
subplot(3,2,4)
line(T201, PID*pi/180,'color',color )
grid on; hold on;
title('Kontrol Sinyali (u)')
xlabel('t(ms)')
subplot(3,2,5)
line(T200, nvLeft,'color',color )
grid on; hold on;
title('vl[volt]')
xlabel('t(ms)')
subplot(3,2,6)
line(T200, nvRight,'color',color )
grid on; hold on;
title('vr[volt]')
xlabel('t(ms)')
%---------------------------------
mlrobotstop(myRobot);
%tsim = toc