/
RobotController.m
80 lines (73 loc) · 2.88 KB
/
RobotController.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
classdef RobotController < handle
properties
hMuran;
hOmnikit;
numRobots;
angleFeedbackController;
xyFeedbackController;
end
methods
function obj=RobotController(hMuran,hOmnikit,numRobots,fps)
assert(isa(hOmnikit,'utotch.arduino.OmnikitRemote'));
obj.hMuran=hMuran;
obj.hOmnikit=hOmnikit;
obj.numRobots=numRobots;
% GainA=[0.058 0.002 0.006 0.055];
GainA=[0.058 0.002 0.006 0.060];
% GainXY=[0.76 0.006 0.113 0.065];
GainXY=[0.80 0.006 0.113 0.065];
obj.angleFeedbackController=[];
obj.xyFeedbackController=[];
for id=1:numRobots
% obj.angleFeedbackController(id)=PIDController(GainA,fps,[-pi pi],[-1.0 1.0],true);
% obj.angleFeedbackController(id).setTargetValue(NaN);
% obj.xyFeedbackController(id)=PIDController(GainXY,fps,[-1 1],[0 1.0],false);
% obj.xyFeedbackController(id).setTargetValue(0.0);
obj.angleFeedbackController=[obj.angleFeedbackController PIDController(GainA,fps,[-pi pi],[-1.0 1.0],true)];
obj.angleFeedbackController(id).setTargetValue(NaN);
obj.xyFeedbackController=[obj.xyFeedbackController PIDController(GainXY,fps,[-1 1],[0 1.0],false)];
obj.xyFeedbackController(id).setTargetValue(0.0);
end
end
function doFeedbackControl(obj)
for id=1:obj.numRobots
if obj.angleFeedbackController(id).isEnable
val=obj.angleFeedbackController(id).doSampleProcess(obj.hMuran.robotStateNormal(3,id));
if obj.angleFeedbackController(id).isFinished
obj.angleFeedbackController(id).doFinish;
obj.angleFeedbackController(id).setTargetValue(NaN);
obj.hOmnikit.motorStop(id);
obj.hOmnikit.flush(id);
set(obj.hMuran.hRobotsText(id),'BackgroundColor','c');
else
obj.hOmnikit.rotate(id,val);
obj.hOmnikit.flush(id);
set(obj.hMuran.hRobotsText(id),'BackgroundColor','r');
end
else
set(obj.hMuran.hRobotsText(id),'BackgroundColor','c');
end
if obj.xyFeedbackController(id).isEnable
if ~isnan(obj.hMuran.robotTargetPointNormal(1,id))
NX=obj.hMuran.robotTargetPointNormal(:,id)-obj.hMuran.robotStateNormal([1 2],id);
if obj.xyFeedbackController(id).isFinished
obj.xyFeedbackController(id).doFinish;
obj.hOmnikit.motorStop(id);
obj.hOmnikit.flush(id);
obj.hMuran.resetRobotTarget(id);
set(obj.hMuran.hRobotsText(id),'BackgroundColor','c');
else
[Y,~]=obj.hMuran.normalVectorToMotorControlVector(id,NX);
vec=obj.xyFeedbackController(id).doSampleProcess(-norm(Y),Y);
obj.hOmnikit.moveToDirection(id,vec(1),vec(2));
obj.hOmnikit.flush(id);
set(obj.hMuran.hRobotsText(id),'BackgroundColor','r');
end
end
else
set(obj.hMuran.hRobotsText(id),'BackgroundColor','c');
end
end
end
end
end