itaMotorNanotec_HRTFarc.m 22.7 KB
Newer Older
1
2
3
classdef itaMotorNanotec_HRTFarc < itaMotorNanotec
    %ITAMOTORCONTROL Summary of this class goes here
    %   Detailed explanation goes here
4

5
6
    properties(Access = protected, Hidden = true)
        mSerialObj; % the serial connection
7

8
9
10
        mMotorControl; % parent controlling this class
        sArgs_motor;
    end
11
12

    properties
13
14

    end
15

16
    properties(Constant, Hidden = true)
17

18
        sArgs_default_motor = struct( ...
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
19
            'wait',             true,       ...
20
            'speed',            1,          ...
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
21
22
23
24
25
            'VST',              'adaptiv',  ...
            'limit',            true,      ...
            'continuous',       false,      ...
            'absolut',          true,      ...
            'closed_loop',      false,       ...
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
26
27
            'acceleration_ramp',20,  ...
            'decceleration_ramp',20, ...
28
            'gear_ratio',       80,        ...
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
29
30
31
32
33
34
35
            'current',          100,        ...
            'ramp_mode',        2,           ...
            'P',                400, ...
            'I',                2.0, ...
            'D',                700, ...
            'P_nenner',         3, ...
            'I_nenner',         5,...
36
            'D_nenner',         3);
37
    end
38

39
40
41
42
43
44
    methods
        function this = itaMotorNanotec_HRTFarc(varargin)
            options =   struct('motorControl', []);
            options    =   ita_parse_arguments(options, varargin);
            this.mMotorControl = options.motorControl;
            this.mSerialObj = itaSerialDeviceInterface.getInstance();
45
46
            this.mIsReferenced = 0;

47
48
            this.motorID = 8;
            this.motorName = 'HRTFArc';
49
            this.motorLimits = [-45 330]; % the motor can do a whole rotation + ~15 deg to both sides
50
        end
51

52
        function this = init(this)
53
            this.setReferenced(false);
54
        end
55
56


57
58
59
60
        function disableReference(this,value)
            if value
                this.mMotorControl.sendControlSequenceAndPrintResults(':port_in_a=0');
            else
61
                this.mMotorControl.sendControlSequenceAndPrintResults(':port_in_a=7');
62
63
            end
        end
64
65
66
67
68
69
70
71
        function stop(this)
            % DO NOT ASK - JUST STOP ALL MOTORS!
            for i = 1:5 % repeat several times to ensure that every motor stops!
                this.mSerialObj.sendAsynch(sprintf('#%dS\r'        , this.motorID));
%                 pause(this.waitForSerialPort);
            end
            while this.mSerialObj.BytesAvailable
                ret = this.mSerialObj.recvAsynch;
72
            end
73
        end
74

75
76
77
78
%         function freeFromStopButton(this)
%            res = motorControl.sendControlSequenceAndPrintResults('Zd');
%            resp = res{end};
%            direction = str2double(resp(end));
79
%
80
81
82
83
84
85
86
87
88
89
%            if direction == 0
%               direction = -1;
%            else
%                direction = 1;
%            end
%            this.allowMoveOverRefButton(1);
%            this.prepareMove(direction*20,'absolut',false,'speed',10);
%            this.startMoveToPosition();
%            this.allowMoveOverRefButton(0);
%         end
90
91


92
93
94
        function getStatus(this)
            motorControl.add_to_commandlist(sprintf('#%d$\r'    , this.motorID));
        end
95

96
97
98
99
100
101
        function status = isActive(this)
            if this.mIsInit == false
                this.mSerialObj.sendAsynch(sprintf('#%d$\r'    , this.motorID));
            end
            status = this.mIsInit;
        end
102

103
104
105
        function setActive(this,value)
           this.mIsInit = value;
        end
106

107
108
109
110
        function id = getMotorID(this)
            id = this.motorID;
        end
        function name = getMotorName(this)
111
           name = this.motorName;
112
113
114
115
        end
        function sendConfiguration(this)
            % Set Input 1 as external Referenceswitch
            motorControl = this.mMotorControl;
116
            motorControl.add_to_commandlist(sprintf('#%d:port_in_a=7\r'  , this.motorID));
117
118
119
120
121
122
            motorControl.add_to_commandlist(sprintf('#%d:port_in_b=7\r'  , this.motorID));
            motorControl.add_to_commandlist(sprintf('#%d:port_out_a=1\r' , this.motorID));
            motorControl.add_to_commandlist(sprintf('#%d:port_out_a=2\r' , this.motorID));
            % phasenstrom im stillstand
            motorControl.add_to_commandlist(sprintf('#%dr=0\r' , this.motorID));
            % fehlerkorrekturmodus
123
            motorControl.add_to_commandlist(sprintf('#%dU=0\r' , this.motorID));
124
125
126
            % ausschwingzeit
            motorControl.add_to_commandlist(sprintf('#%dO=1\r' , this.motorID));
            % umkehrspiel
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
127
            motorControl.add_to_commandlist(sprintf('#%dz=5\r'          , this.motorID));
128

129
130
131
            % automatisches senden des status deaktivieren
            motorControl.add_to_commandlist(sprintf('#%dJ=0\r'          , this.motorID));
            % endschalterverhalten: the ref manual is not very clear. bit 0
132
            % is the most important bit. all not listed bits are 0
133
            % defValue bin2dec('0100010000100010') = 17442
134
            this.allowMoveOverRefButton(0);
135

136
            % set lower speed to 1 Hz/sec (lowest value)
137
            motorControl.add_to_commandlist(sprintf('#%du3\r'          , this.motorID));
138

139
        end
140

141
142
143
144
145
        function this = moveToReferencePosition(this)
            % Prepare reference move (turntable)
            motorControl = this.mMotorControl;
            % Turn + some degrees in case we are already at the end of the
            % reference switch or already passed it:
146
147
148
149
150
151
152
153
154
155
%             this.prepareMove(20,'absolut',false,'speed',10);
%             this.startMoveToPosition();
%             if this.mMotorControl.send_commandlist(5);
%                 ita_verbose_info('HRTFarc started move...',2);
%             end
%             tmpWait = motorControl.wait;
%             motorControl.wait = true;
%             motorControl.wait4everything;
%             motorControl.wait = tmpWait;
            this.disableReference(0);
156
157
158
159
160
            % Call Reference-Mode:
            motorControl.add_to_commandlist(sprintf('#%dp=4\r'          , this.motorID));
            % Set direction:
            motorControl.add_to_commandlist(sprintf('#%dd=1\r'          , this.motorID));
            % Calculate and set lower speed:
161
162
            stepspersecond      =   round(this.sArgs_default_motor.speed/0.9*this.sArgs_default_motor.gear_ratio);
            motorControl.add_to_commandlist(sprintf('#%du=%d\r'       , this.motorID, stepspersecond));
163
164
165
            % Calculate and set upper speed:
            stepspersecond      =   round(this.sArgs_default_motor.speed/0.9*this.sArgs_default_motor.gear_ratio);
            motorControl.add_to_commandlist(sprintf('#%do=%d\r'       , this.motorID, stepspersecond));
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
166
167
            % set decel to a high value so the switch is not overrun
            motorControl.add_to_commandlist(sprintf('#%d:decel1%.0f\r'              , this.motorID,100));
168

169
170
171
172
173
            % Start reference move:
            motorControl.add_to_commandlist(sprintf('#%dA\r'            , this.motorID));


            this.old_position = itaCoordinates(1);
174

175
        end
176

177
178
179
        function this = startMoveToPosition(this)
             this.mMotorControl.add_to_commandlist(sprintf('#%dA\r'        , this.motorID));
        end
180
181


182
183
184
185
        function started = prepareMove(this,position,varargin)
           sArgs = this.sArgs_default_motor;
           sArgs.continuous = false;
           if ~isempty(varargin)
186
               [sArgs,~] = ita_parse_arguments(sArgs,varargin);
187
188
           end
           if sArgs.continuous
189
                ret = this.prepare_move(position, sArgs);
190
191
192
193
194
195
196
197
198
199
200
201
202
                started = true;
           else
                   % if only the phi angle is given
                   if ~isa(position,'itaCoordinates')
                      tmpPosition = this.old_position;
                      if ~isnan(position)
                          tmpPosition.phi_deg = position;
                      end
                      position = tmpPosition;
                   end
               if sArgs.absolut == 1
                   if this.old_position.phi ~= position.phi

Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
203
                        angle = mod(position.phi(1)/2/pi*360+360, 721)-360;
204
                        ret = this.prepare_move(angle, sArgs);
205
206
207
208
209
210
211
212
213
                        this.old_position = position;
                        started = true;
                   else
                        started = false;
                   end
               else
                    % in relative positioning mode, the angle is calculated
                    % and added to the old position
                    angle = position.phi_deg(1);
214
                    ret = this.prepare_move(angle, sArgs);
215
216
217
218
219
220
221
                    this.old_position = this.old_position + position;
                    started = true;
               end
           end
        end

    end
222

223
    methods(Hidden = true)
224

225
226
227
        function this = allowMoveOverRefButton(this,value)
           motorControl = this.mMotorControl;
           if value
228
              motorControl.sendControlSequenceAndPrintResults('l17442');
229
           else
230
              motorControl.sendControlSequenceAndPrintResults('l5154');
231
           end
232
%            Frei r�ckw�rts
233
234
235
236
%            5154
%            Stop
%            9250
        end
237
238
239



240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
        function ret = prepare_move(this, angle, varargin)
            %   This function prepares the moves of the turntable, counterclockwise for a negative
            %   angle and clockwise for a positive angle.
            %
            %   The rules for the commands sent via RS232 are the following:
            %
            %   Each command starts with it's startsign '#' followed by the
            %   motor number and ends with an '\r'. All other elements are ascii signs.
            %   A '*' sends the command to all motors.
            %
            %   The motor controller will respons with an echo of the
            %   command, but without the '#'. Invalid commands are marked
            %   with an '?' at the end of the echo.
            %
            %   Long commands start with an '#' followed by the motor ID
            %   and then an ':'. The command is read by just sending the
            %   command and set by '=<value>'.
            %
            % -----------------------------------------------------------------------------------------------
259

260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
            % -------------------------------------------------------------
            % Meaning:
            %
            % Wait              =   Stop matlab until motor reaches final position!
            % Speed             =   Grad/s of the turntable
            % VST               =   Microstep divider. Values: 1, 2, 4, 5, 8, 10, 16, 32,
            %                       64. 254="Vorschubkonstantenmodus", 255=Adaptiv steps
            % Limit             =   Position only allowed between -180 and 360 degree if true!
            % Continuous        =   Turn continuously with a given speed
            % Absolut           =   Go to absolut positions
            % Closed_loop       =   Turn on the closed loop regulation
            % Acceleration_ramp =   Value in Hz/ms
            % Gear_ratio        =   Gear ratio between motor and turntable (be careful!)
            % Current           =   Maximum current in percent
            % Ramp_mode         =   0=trapez, 1=sinus-ramp, 2=jerkfree-ramp
            % -------------------------------------------------------------
276

277
            motorControl = this.mMotorControl;
278

279
280
281
            this.sArgs_motor = ita_parse_arguments(varargin{:});
            % Assign wait to global wait:
%
282

283
284
285
286
287
288
289
290
            if (this.sArgs_motor.speed == 0) && ((angle == 0) && (~this.sArgs_motor.continuous) && (~this.sArgs_motor.absolut))
                % This means: STOP!
                motorControl.add_to_commandlist(sprintf('#%dS\r'        , this.motorID));
%                 ret             =   false;
%                 pause(0.1);
%                 fgetl(this.mSerialObj);
                return
            end
291

292
293
294
295
296
297
298
299
300
            if (this.sArgs_motor.limit == true)
                % Check if the position is too far away...
                if this.sArgs_motor.continuous == true
                    % It will lead to problems if limit is on AND continuous
                    % is true...
                    error('Please turn off limit if you want to turn continuous! Please make also sure that no cable or other stuff can coil!');
                elseif this.sArgs_motor.absolut == true
                    % This case is easy because the given absolut angle
                    % shoud be between -180 and 360
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
301
                    if (angle > this.motorLimits(2)) || (angle < this.motorLimits(1))
302
                        % It's not in the allowed range... :-(
303
                        error('Limit is on! Only positions between %d and %d degree are allowed!',this.motorLimits)
304
305
306
307
                    end
                else
                    % Limit is on and relative positioning is on... this case
                    % is a bit more complex!
308
309
310
311
                    % Get position: motorposition does not work. using
                    % saved old position instead
                    % in the init case, old_position is not set.
                    if isnan(this.old_position.phi_deg)
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
312
                        this.mSerialObj.sendAsynch(sprintf('#%dC\r'      , this.motorID));
313
314
315
316
                        act_pos       =   this.mSerialObj.recvAsynch();
                        act_pos       =   str2double(act_pos(3:end));
                        % Now multiply with 0.9 and divide by gear_ratio to get
                        % the position angle of the turntable:
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
317
                        act_pos     =   -act_pos*0.9/this.sArgs_motor.gear_ratio;
318
319
320
                    else
                        act_pos       =   this.old_position.phi_deg;
                    end
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
321
322
                    % Check if old position would be in the allowed range:
                    if ((act_pos) > this.motorLimits(2)) || ((act_pos) < this.motorLimits(1))
323
                        % No, it's not....
324
                        ita_verbose_info('Warning: Could not determine a sensible position. Doing reference anyway.',0)
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
325
326
327
328
                    else
                        % Check if new position would be in the allowed range:
                        if ((act_pos+angle) > this.motorLimits(2)) || ((act_pos+angle) < this.motorLimits(1))
                            % No, it's not....
329
                            error('Limit is on! Only positions between %d and %d degree are allowed!',this.motorLimits)
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
330
                        end
331
332
333
334
335
336
337
338
339
340
341
342
343
344
                    end
                end
            end
            % Set microstep-divider:
%             if strcmpi(this.sArgs_motor.VST, 'adaptiv')
%                 motorControl.add_to_commandlist(sprintf('#%dg255\r'     , this.motorID));
%             else
%                 motorControl.add_to_commandlist(sprintf(['#%dg' this.sArgs_motor.VST '\r']  , this.motorID));
%             end
            % Set maximum current to 100%:
            motorControl.add_to_commandlist(sprintf('#%di%.0f\r'       , this.motorID, this.sArgs_motor.current));
            % Choose ramp mode: (0=trapez, 1=sinus-ramp, 2=jerkfree-ramp):
            motorControl.add_to_commandlist(sprintf('#%d:ramp_mode=%d\r', this.motorID, this.sArgs_motor.ramp_mode));
%             % Set maximum acceleration jerk:
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
345
             motorControl.add_to_commandlist(sprintf('#%d:b=4\r'       , this.motorID));
346
%             % Use acceleration jerk as braking jerk:
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
347
            motorControl.add_to_commandlist(sprintf('#%d:B=0\r'         , this.motorID));
348
349
350
351
352
353
354
            % Closed_loop?
            %this.sArgs_motor.closed_loop = true; % DEBUG!
            if this.sArgs_motor.closed_loop == true
                % JEAR! Without this the new motor would be nonsense!
                % Activate CL during movement:
                motorControl.add_to_commandlist(sprintf('#%d:CL_enable=2\r' , this.motorID));
                % Nice values for the speed closed loop control:
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
%                 pos     =   [0.5 1 2 3 4 8 12 16 25 32 40 50];
%                 vecP    =   [0.5 1.5 2.5 3.5 4.5 4.5 5.5 2.5 2.0 1.3 1.3 1.3];
%                 vecI    =   [0.05 0.1 0.2 0.3 0.4 0.8 1.2 1.6 2.0 2.5 2.5 2.5];
%                 vecD    =   [9 6 4 3 2 1 1 3 6 10 10 10];
%                 pP      =   polyfit(pos,vecP,5);
%                 pI      =   polyfit(pos,vecI,5);
%                 pD      =   polyfit(pos,vecD,5);
%                 P       =   polyval(pP,this.sArgs_motor.speed);
%                 I       =   polyval(pI,this.sArgs_motor.speed);
%                 D       =   polyval(pD,this.sArgs_motor.speed);
%                 P_nenner    =   5;
%                 I_nenner    =   5;
%                 D_nenner    =   5;
                P           =   this.sArgs_motor.P;% (400 = default)
                I           =   this.sArgs_motor.I;% (2 = default)
                D           =   this.sArgs_motor.D;% (700 = default)
                P_nenner    =   this.sArgs_motor.P_nenner;
                I_nenner    =   this.sArgs_motor.I_nenner;
                D_nenner    =   this.sArgs_motor.D_nenner;
374
375
376
377
378
379
380
381
382
                P_zaehler   =   round(P*2^P_nenner);
                I_zaehler   =   round(I*2^I_nenner);
                D_zaehler   =   round(D*2^D_nenner);
                motorControl.add_to_commandlist(sprintf('#%d:CL_KP_v_Z=%d\r'    , this.motorID, P_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KP_v_N=%d\r'    , this.motorID, P_nenner));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KI_v_Z=%d\r'    , this.motorID, I_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KI_v_N=%d\r'    , this.motorID, I_nenner));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KD_v_Z=%d\r'    , this.motorID, D_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KD_v_N=%d\r'    , this.motorID, D_nenner));
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
383
384
385
386
387
388
389
390
%                 % Nice values for the positioning closed loop control:

                P           =   this.sArgs_motor.P;% (400 = default)
                I           =   this.sArgs_motor.I;% (2 = default)
                D           =   this.sArgs_motor.D;% (700 = default)
                P_nenner    =   this.sArgs_motor.P_nenner;
                I_nenner    =   this.sArgs_motor.I_nenner;
                D_nenner    =   this.sArgs_motor.D_nenner;
391
392
393
394
395
396
397
398
399
                P_zaehler   =   round(P*2^P_nenner);
                I_zaehler   =   round(I*2^I_nenner);
                D_zaehler   =   round(D*2^D_nenner);
                motorControl.add_to_commandlist(sprintf('#%d:CL_KP_s_Z=%d\r'    , this.motorID, P_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KP_s_N=%d\r'    , this.motorID, P_nenner));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KI_s_Z=%d\r'    , this.motorID, I_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KI_s_N=%d\r'    , this.motorID, I_nenner));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KD_s_Z=%d\r'    , this.motorID, D_zaehler));
                motorControl.add_to_commandlist(sprintf('#%d:CL_KD_s_N=%d\r'    , this.motorID, D_nenner));
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
400

401
402
403
404
405
406
407
408
                % Kask V-Regler: P = 1.2, I = 0.85, D = 0.7
                % Kask- P-Regler: P = 400 (default), I = 2 (default), D = 700 (default)
                % TODO: Send values to the motor... or we just skip the
                % kaskaded closed loop
            else
                % Use motor as classic step motor without closed loop:
                motorControl.add_to_commandlist(sprintf('#%d:CL_enable=0\r'     , this.motorID));
            end
409

410
411
412
            % JRI: unknown command?
            % Correction of the sinus-commutierung: (Should be on!)
            % motorControl.add_to_commandlist(sprintf('#%d:cal_elangle_enable=1\r', this.motorID));
413

414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
            % Set the speed:
            % Divide by 0.9 because each (half)-step is equal to 0.9 degree
            % and multiply by the gear_ratio because the given speed value
            % is for the turntable and not for the motor:
            stepspersecond  =   round((this.sArgs_motor.speed/0.9*this.sArgs_motor.gear_ratio));
            motorControl.add_to_commandlist(sprintf('#%do%d\r'               , this.motorID, stepspersecond));
            % Set mode:
            if this.sArgs_motor.continuous == true
                motorControl.add_to_commandlist(sprintf('#%dp=5\r'              , this.motorID));
                if (angle > 0)
                    % Turn right: (negative)
                    motorControl.add_to_commandlist(sprintf('#%dd=0\r'          , this.motorID));
                else
                    % Turn left: (positive)
                    motorControl.add_to_commandlist(sprintf('#%dd=1\r'          , this.motorID));
429
                end
430
                steps       =   (angle/0.9*this.sArgs_motor.gear_ratio);
431
                motorControl.add_to_commandlist(sprintf('#%ds=%d\r'       , this.motorID, round(abs(steps))));
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
            else
                % Calculate the number of steps:
                % Divide by 0.9 because each (half)-step is equal to 0.9 degree
                % and multiply by the gear_ratio because the given angle value
                % is for the turntable and not for the motor:
                steps       =   (angle/0.9*this.sArgs_motor.gear_ratio);
                % Check if absolut or relative position mode:
                if this.sArgs_motor.absolut == true
                    % Absolut position mode:
                    motorControl.add_to_commandlist(sprintf('#%dp2\r'          , this.motorID));
                    % Set position (positive/negaive relative to the
                    % reference:
                    motorControl.add_to_commandlist(sprintf('#%ds%d\r'       , this.motorID, -round(steps)));
                    % INFO: -100000000 <= steps <= +100000000!
                else
                    % Relative position mode:
                    motorControl.add_to_commandlist(sprintf('#%dp1\r'          , this.motorID));
                    motorControl.add_to_commandlist(sprintf('#%ds%d\r'       , this.motorID, round(abs(steps))));
                    % INFO:     0 < steps <= +100000000! Direction is set seperatly!
                    % Check the direction:
                    if (angle > 0) % Turn right: (negative)
                        motorControl.add_to_commandlist(sprintf('#%dd0\r'      , this.motorID));
                    else % Turn left: (positive)
                        motorControl.add_to_commandlist(sprintf('#%dd1\r'      , this.motorID));
                    end
                end
            end
            % Set acceleration ramp directly
             motorControl.add_to_commandlist(sprintf('#%d:accel%.0f\r'           , this.motorID, this.sArgs_motor.acceleration_ramp));
            % Brake ramp:
Jan-Gerrit Richter's avatar
Jan-Gerrit Richter committed
462
            motorControl.add_to_commandlist(sprintf('#%d:decel1%.0f\r'              , this.motorID,this.sArgs_motor.decceleration_ramp));
463
            % Zero menas equal to acceleration ramp!
464

465
466
            ret = true;
        end
467
468


469
470
    end

471
end