-
Notifications
You must be signed in to change notification settings - Fork 0
/
motorSync.m
122 lines (110 loc) · 3.39 KB
/
motorSync.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
function motorSync(r,fiL,fiR)
% Pins for left motor
configurePin(r,14,'DigitalOutput');
configurePin(r,15,'DigitalOutput');
configurePin(r,23,'DigitalOutput');
configurePin(r,24,'DigitalOutput');
% Pins for right motor
configurePin(r,25,'DigitalOutput');
configurePin(r,12,'DigitalOutput');
configurePin(r,16,'DigitalOutput');
configurePin(r,20,'DigitalOutput');
ratio=64; % Gearbox reduction ratio
st_rev=64; % Steps per rev. in Half-Step operation
seqL=zeros(8,4); % memory allocation for left motor sequence
seqR=zeros(8,4); % memory allocation for right motor sequence
stepCount=8; % number of step sequences for half-step operation
% Define sequences for clockwise (+) and counterclockwise (-) movement
if fiL>0
seqL(1,:)=[1,0,0,0];
seqL(2,:)=[1,1,0,0];
seqL(3,:)=[0,1,0,0];
seqL(4,:)=[0,1,1,0];
seqL(5,:)=[0,0,1,0];
seqL(6,:)=[0,0,1,1];
seqL(7,:)=[0,0,0,1];
seqL(8,:)=[1,0,0,1];
elseif fiL<0
seqL(8,:)=[1,0,0,0];
seqL(7,:)=[1,1,0,0];
seqL(6,:)=[0,1,0,0];
seqL(5,:)=[0,1,1,0];
seqL(4,:)=[0,0,1,0];
seqL(3,:)=[0,0,1,1];
seqL(2,:)=[0,0,0,1];
seqL(1,:)=[1,0,0,1];
else
seqL=zeros(8,4);
end
if fiR>0
seqR(1,:)=[1,0,0,0];
seqR(2,:)=[1,1,0,0];
seqR(3,:)=[0,1,0,0];
seqR(4,:)=[0,1,1,0];
seqR(5,:)=[0,0,1,0];
seqR(6,:)=[0,0,1,1];
seqR(7,:)=[0,0,0,1];
seqR(8,:)=[1,0,0,1];
elseif fiR<0
seqR(8,:)=[1,0,0,0];
seqR(7,:)=[1,1,0,0];
seqR(6,:)=[0,1,0,0];
seqR(5,:)=[0,1,1,0];
seqR(4,:)=[0,0,1,0];
seqR(3,:)=[0,0,1,1];
seqR(2,:)=[0,0,0,1];
seqR(1,:)=[1,0,0,1];
else
seqR=zeros(8,4);
end
revsL=abs(fiL)/(2*pi); % conversion from rad to revolutions
stepsL=round(revsL*st_rev*ratio); % conversion from revs to number of steps
revsR=abs(fiR)/(2*pi); % conversion from rad to revolutions
stepsR=round(revsR*st_rev*ratio); % conversion from revs to number of steps
stepTime=2/1000;%1.5
if stepsL>=stepsR
dStep=round(stepsL/stepsR);
j=1;
jj=1;
for i=1:stepsR
setStepR(r,seqR(j,1),seqR(j,2),seqR(j,3),seqR(j,4));
for ii=1:dStep
setStepL(r,seqL(jj,1),seqL(jj,2),seqL(jj,3),seqL(jj,4));
delay(stepTime);
if jj==stepCount
jj=1;
else
jj=jj+1;
end
end
if j==stepCount
j=1;
else
j=j+1;
end
end
else
dStep=round(stepsR/stepsL);
j=1;
jj=1;
for i=1:stepsL
setStepL(r,seqL(j,1),seqL(j,2),seqL(j,3),seqL(j,4));
for ii=1:dStep
setStepR(r,seqR(jj,1),seqR(jj,2),seqR(jj,3),seqR(jj,4));
delay(stepTime);
if jj==stepCount
jj=1;
else
jj=jj+1;
end
end
if j==stepCount
j=1;
else
j=j+1;
end
end
end
setStepR(r,0,0,0,0); % zero the right motor pins
setStepL(r,0,0,0,0); % zero the left motor pins
end