-
Notifications
You must be signed in to change notification settings - Fork 0
/
step_motor.cpp
221 lines (213 loc) · 13.5 KB
/
step_motor.cpp
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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
#include "step_motor.h"
#include "drv8711.h"
#include "QDebug"
STEP_MOTOR::STEP_MOTOR()
{
CTRL_REG.DTIME = (DRV8711_Registrs[0] & (~DRV8711_CTRL_DTIME_MASK)) >> DRV8711_CTRL_DTIME_POS;
CTRL_REG.ISGAIN = (DRV8711_Registrs[0] & (~DRV8711_CTRL_ISGAIN_MASK)) >> DRV8711_CTRL_ISGAIN_POS;
CTRL_REG.EXSTALL = (DRV8711_Registrs[0] & (~DRV8711_CTRL_EXSTALL_MASK)) >> DRV8711_CTRL_EXSTALL_POS;
CTRL_REG.MODE = (DRV8711_Registrs[0] & (~DRV8711_CTRL_MODE_MASK)) >> DRV8711_CTRL_MODE_POS;
CTRL_REG.RSTEP = (DRV8711_Registrs[0] & (~DRV8711_CTRL_RSTEP_MASK)) >> DRV8711_CTRL_RSTEP_POS;
CTRL_REG.RDIR = (DRV8711_Registrs[0] & (~DRV8711_CTRL_RDIR_MASK)) >> DRV8711_CTRL_RDIR_POS;
CTRL_REG.ENBL = (DRV8711_Registrs[0] & (~DRV8711_CTRL_ENBL_MASK)) >> DRV8711_CTRL_ENBL_POS;
TORQUE_REG.SIMPLTH = (DRV8711_Registrs[1] & (~DRV8711_TORQUE_SIMPLTH_MASK)) >> DRV8711_TORQUE_SIMPLTH_POS;
TORQUE_REG.TORQUE = (DRV8711_Registrs[1] & (~DRV8711_TORQUE_TORQUE_MASK)) >> DRV8711_TORQUE_TORQUE_POS;
OFF_REG.TOFF = (DRV8711_Registrs[2] & (~DRV8711_OFF_TOFF_MASK)) >> DRV8711_OFF_TOFF_POS;
OFF_REG.PWMMODE = (DRV8711_Registrs[2] & (~DRV8711_OFF_PWMMODE_MASK)) >> DRV8711_OFF_PWMMODE_POS;
BLANK_REG.ABT = (DRV8711_Registrs[3] & (~DRV8711_BLANK_ABT_MASK)) >> DRV8711_BLANK_ABT_POS;
BLANK_REG.TBLANK = (DRV8711_Registrs[3] & (~DRV8711_BLANK_TBLANK_MASK)) >> DRV8711_BLANK_TBLANK_POS;
DECAY_REG.DECMOD = (DRV8711_Registrs[4] & (~DRV8711_DECAY_DECMOD_MASK)) >> DRV8711_DECAY_DECMOD_POS;
DECAY_REG.TDECAY = (DRV8711_Registrs[4] & (~DRV8711_DECAY_TDECAY_MASK)) >> DRV8711_DECAY_TDECAY_POS;
STALL_REG.VDIV = (DRV8711_Registrs[5] & (~DRV8711_STALL_VDIV_MASK)) >> DRV8711_STALL_VDIV_POS;
STALL_REG.SDCNT = (DRV8711_Registrs[5] & (~DRV8711_STALL_SDCNT_MASK)) >> DRV8711_STALL_SDCNT_POS;
STALL_REG.SDTHR = (DRV8711_Registrs[5] & (~DRV8711_STALL_SDTHR_MASK)) >> DRV8711_STALL_SDTHR_POS;
DRIVE_REG.IDRIVEP = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_IDRIVEP_MASK)) >> DRV8711_DRIVE_IDRIVEP_POS;
DRIVE_REG.IDRIVEN = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_IDRIVEN_MASK)) >> DRV8711_DRIVE_IDRIVEN_POS;
DRIVE_REG.TDRIVEP = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_TDRIVEP_MASK)) >> DRV8711_DRIVE_TDRIVEP_POS;
DRIVE_REG.TDRIVEN = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_TDRIVEN_MASK)) >> DRV8711_DRIVE_TDRIVEN_POS;
DRIVE_REG.OCPDEG = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_OCPDEG_MASK)) >> DRV8711_DRIVE_OCPDEG_POS;
DRIVE_REG.OCPTH = (DRV8711_Registrs[6] & (~DRV8711_DRIVE_OCPTH_MASK)) >> DRV8711_DRIVE_OCPTH_POS;
STATUS_REG.OTS = (DRV8711_Registrs[7] & (~DRV8711_STATUS_OTS_MASK)) >> DRV8711_STATUS_OTS_POS;
STATUS_REG.AOCP = (DRV8711_Registrs[7] & (~DRV8711_STATUS_AOCP_MASK)) >> DRV8711_STATUS_AOCP_POS;
STATUS_REG.BOCP = (DRV8711_Registrs[7] & (~DRV8711_STATUS_BOCP_MASK)) >> DRV8711_STATUS_BOCP_POS;
STATUS_REG.APDF = (DRV8711_Registrs[7] & (~DRV8711_STATUS_APDF_MASK)) >> DRV8711_STATUS_APDF_POS;
STATUS_REG.BPDF = (DRV8711_Registrs[7] & (~DRV8711_STATUS_BPDF_MASK)) >> DRV8711_STATUS_BPDF_POS;
STATUS_REG.UVLO = (DRV8711_Registrs[7] & (~DRV8711_STATUS_UVLO_MASK)) >> DRV8711_STATUS_UVLO_POS;
STATUS_REG.STD = (DRV8711_Registrs[7] & (~DRV8711_STATUS_STD_MASK)) >> DRV8711_STATUS_STD_POS;
STATUS_REG.STDLAT = (DRV8711_Registrs[7] & (~DRV8711_STATUS_STDLAT_MASK)) >> DRV8711_STATUS_STDLAT_POS;
}
quint16 STEP_MOTOR::GetREG(quint8 addr)
{
return (DRV8711_Registrs[addr]);
}
void STEP_MOTOR::SetREG(quint8 addr, quint16 data)
{
DRV8711_Registrs[addr]=data;
switch (addr)
{
case DRV8711_CTRL:
CTRL_REG.DTIME = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_DTIME_MASK)) >> DRV8711_CTRL_DTIME_POS;
CTRL_REG.ISGAIN = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_ISGAIN_MASK)) >> DRV8711_CTRL_ISGAIN_POS;
CTRL_REG.EXSTALL = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_EXSTALL_MASK)) >> DRV8711_CTRL_EXSTALL_POS;
CTRL_REG.MODE = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_MODE_MASK)) >> DRV8711_CTRL_MODE_POS;
CTRL_REG.RSTEP = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_RSTEP_MASK)) >> DRV8711_CTRL_RSTEP_POS;
CTRL_REG.RDIR = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_RDIR_MASK)) >> DRV8711_CTRL_RDIR_POS;
CTRL_REG.ENBL = (DRV8711_Registrs[DRV8711_CTRL] & (~DRV8711_CTRL_ENBL_MASK)) >> DRV8711_CTRL_ENBL_POS;
break;
case DRV8711_TORQUE:
{
TORQUE_REG.SIMPLTH = (DRV8711_Registrs[DRV8711_TORQUE] & (~DRV8711_TORQUE_SIMPLTH_MASK)) >> DRV8711_TORQUE_SIMPLTH_POS;
TORQUE_REG.TORQUE = (DRV8711_Registrs[DRV8711_TORQUE] & (~DRV8711_TORQUE_TORQUE_MASK)) >> DRV8711_TORQUE_TORQUE_POS;
break;
}
case DRV8711_OFF:
{
OFF_REG.TOFF = (DRV8711_Registrs[DRV8711_OFF] & (~DRV8711_OFF_TOFF_MASK)) >> DRV8711_OFF_TOFF_POS;
OFF_REG.PWMMODE = (DRV8711_Registrs[DRV8711_OFF] & (~DRV8711_OFF_PWMMODE_MASK)) >> DRV8711_OFF_PWMMODE_POS;
break;
}
case DRV8711_BLANK:
{
BLANK_REG.ABT = (DRV8711_Registrs[DRV8711_BLANK] & (~DRV8711_BLANK_ABT_MASK)) >> DRV8711_BLANK_ABT_POS;
BLANK_REG.TBLANK = (DRV8711_Registrs[DRV8711_BLANK] & (~DRV8711_BLANK_TBLANK_MASK)) >> DRV8711_BLANK_TBLANK_POS;
break;
}
case DRV8711_DECAY:
{
DECAY_REG.DECMOD = (DRV8711_Registrs[DRV8711_DECAY] & (~DRV8711_DECAY_DECMOD_MASK)) >> DRV8711_DECAY_DECMOD_POS;
DECAY_REG.TDECAY = (DRV8711_Registrs[DRV8711_DECAY] & (~DRV8711_DECAY_TDECAY_MASK)) >> DRV8711_DECAY_TDECAY_POS;
break;
}
case DRV8711_STALL:
{
STALL_REG.VDIV = (DRV8711_Registrs[DRV8711_STALL] & (~DRV8711_STALL_VDIV_MASK)) >> DRV8711_STALL_VDIV_POS;
STALL_REG.SDCNT = (DRV8711_Registrs[DRV8711_STALL] & (~DRV8711_STALL_SDCNT_MASK)) >> DRV8711_STALL_SDCNT_POS;
STALL_REG.SDTHR = (DRV8711_Registrs[DRV8711_STALL] & (~DRV8711_STALL_SDTHR_MASK)) >> DRV8711_STALL_SDTHR_POS;
break;
}
case DRV8711_DRIVE:
{
DRIVE_REG.IDRIVEP = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_IDRIVEP_MASK)) >> DRV8711_DRIVE_IDRIVEP_POS;
DRIVE_REG.IDRIVEN = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_IDRIVEN_MASK)) >> DRV8711_DRIVE_IDRIVEN_POS;
DRIVE_REG.TDRIVEP = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_TDRIVEP_MASK)) >> DRV8711_DRIVE_TDRIVEP_POS;
DRIVE_REG.TDRIVEN = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_TDRIVEN_MASK)) >> DRV8711_DRIVE_TDRIVEN_POS;
DRIVE_REG.OCPDEG = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_OCPDEG_MASK)) >> DRV8711_DRIVE_OCPDEG_POS;
DRIVE_REG.OCPTH = (DRV8711_Registrs[DRV8711_DRIVE] & (~DRV8711_DRIVE_OCPTH_MASK)) >> DRV8711_DRIVE_OCPTH_POS;
break;
}
case DRV8711_STATUS:
{
STATUS_REG.OTS = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_OTS_MASK)) >> DRV8711_STATUS_OTS_POS;
STATUS_REG.AOCP = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_AOCP_MASK)) >> DRV8711_STATUS_AOCP_POS;
STATUS_REG.BOCP = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_BOCP_MASK)) >> DRV8711_STATUS_BOCP_POS;
STATUS_REG.APDF = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_APDF_MASK)) >> DRV8711_STATUS_APDF_POS;
STATUS_REG.BPDF = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_BPDF_MASK)) >> DRV8711_STATUS_BPDF_POS;
STATUS_REG.UVLO = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_UVLO_MASK)) >> DRV8711_STATUS_UVLO_POS;
STATUS_REG.STD = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_STD_MASK)) >> DRV8711_STATUS_STD_POS;
STATUS_REG.STDLAT = (DRV8711_Registrs[DRV8711_STATUS] & (~DRV8711_STATUS_STDLAT_MASK)) >> DRV8711_STATUS_STDLAT_POS;
break;
}
}
emit UpdateRegister(addr);
}
void STEP_MOTOR::update_single(int addr)
{
switch (addr)
{
case DRV8711_CTRL:
DRV8711_Registrs[addr] &=(DRV8711_CTRL_DTIME_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.DTIME << DRV8711_CTRL_DTIME_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_ISGAIN_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.ISGAIN << DRV8711_CTRL_ISGAIN_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_EXSTALL_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.EXSTALL << DRV8711_CTRL_EXSTALL_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_MODE_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.MODE << DRV8711_CTRL_MODE_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_RSTEP_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.RSTEP << DRV8711_CTRL_RSTEP_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_RDIR_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.RDIR << DRV8711_CTRL_RDIR_POS );
DRV8711_Registrs[addr] &=(DRV8711_CTRL_ENBL_MASK);
DRV8711_Registrs[addr] |=(CTRL_REG.ENBL << DRV8711_CTRL_ENBL_POS );
break;
case DRV8711_TORQUE:
{
DRV8711_Registrs[addr] &=(DRV8711_TORQUE_SIMPLTH_MASK);
DRV8711_Registrs[addr] |=(TORQUE_REG.SIMPLTH << DRV8711_TORQUE_SIMPLTH_POS );
DRV8711_Registrs[addr] &=(DRV8711_TORQUE_TORQUE_MASK);
DRV8711_Registrs[addr] |=(TORQUE_REG.TORQUE << DRV8711_TORQUE_TORQUE_POS );
break;
}
case DRV8711_OFF:
{
DRV8711_Registrs[addr] &=(DRV8711_OFF_TOFF_MASK);
DRV8711_Registrs[addr] |=(OFF_REG.TOFF << DRV8711_OFF_TOFF_POS);
DRV8711_Registrs[addr] &=(DRV8711_OFF_PWMMODE_MASK);
DRV8711_Registrs[addr] |=(OFF_REG.PWMMODE << DRV8711_OFF_PWMMODE_POS);
break;
}
case DRV8711_BLANK:
{
DRV8711_Registrs[addr] &=(DRV8711_BLANK_ABT_MASK);
DRV8711_Registrs[addr] |=(BLANK_REG.ABT << DRV8711_BLANK_ABT_POS);
DRV8711_Registrs[addr] &=(DRV8711_BLANK_TBLANK_MASK);
DRV8711_Registrs[addr] |=(BLANK_REG.TBLANK << DRV8711_BLANK_TBLANK_POS);
break;
}
case DRV8711_DECAY:
{
DRV8711_Registrs[addr] &=(DRV8711_DECAY_DECMOD_MASK);
DRV8711_Registrs[addr] |=(DECAY_REG.DECMOD << DRV8711_DECAY_DECMOD_POS);
DRV8711_Registrs[addr] &=(DRV8711_DECAY_TDECAY_MASK);
DRV8711_Registrs[addr] |=(DECAY_REG.TDECAY << DRV8711_DECAY_TDECAY_POS);
break;
}
case DRV8711_STALL:
{
DRV8711_Registrs[addr] &=(DRV8711_STALL_SDCNT_MASK);
DRV8711_Registrs[addr] |=(STALL_REG.SDCNT << DRV8711_STALL_SDCNT_POS);
DRV8711_Registrs[addr] &=(DRV8711_STALL_SDTHR_MASK);
DRV8711_Registrs[addr] |=(STALL_REG.SDTHR << DRV8711_STALL_SDTHR_POS);
DRV8711_Registrs[addr] &=(DRV8711_STALL_VDIV_MASK);
DRV8711_Registrs[addr] |=(STALL_REG.VDIV << DRV8711_STALL_VDIV_POS);
break;
}
case DRV8711_DRIVE:
{
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_IDRIVEN_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.IDRIVEN << DRV8711_DRIVE_IDRIVEN_POS);
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_IDRIVEP_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.IDRIVEP << DRV8711_DRIVE_IDRIVEP_POS);
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_OCPDEG_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.OCPDEG << DRV8711_DRIVE_OCPDEG_POS);
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_OCPTH_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.OCPTH << DRV8711_DRIVE_OCPTH_POS);
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_TDRIVEN_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.TDRIVEN << DRV8711_DRIVE_TDRIVEN_POS);
DRV8711_Registrs[addr] &=(DRV8711_DRIVE_TDRIVEP_MASK);
DRV8711_Registrs[addr] |=(DRIVE_REG.TDRIVEP << DRV8711_DRIVE_TDRIVEP_POS);
break;
}
case DRV8711_STATUS:
{
DRV8711_Registrs[addr] &=(DRV8711_STATUS_OTS_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.OTS << DRV8711_STATUS_OTS_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_AOCP_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.AOCP << DRV8711_STATUS_AOCP_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_BOCP_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.BOCP << DRV8711_STATUS_BOCP_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_APDF_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.APDF << DRV8711_STATUS_APDF_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_BPDF_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.BPDF << DRV8711_STATUS_BPDF_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_UVLO_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.UVLO << DRV8711_STATUS_UVLO_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_STD_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.STD << DRV8711_STATUS_STD_POS);
DRV8711_Registrs[addr] &=(DRV8711_STATUS_STDLAT_MASK);
DRV8711_Registrs[addr] |=(STATUS_REG.STDLAT << DRV8711_STATUS_STDLAT_POS);
break;
}
}
emit UpdateRegister(addr);
}