Aufgrund einer Wartung wird GitLab am 29.10. zwischen 9:00 und 10:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 29.10. between 9:00 and 10:00 am.

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 186 187 188
        function started = prepareMove(this,position,varargin)
           sArgs = this.sArgs_default_motor;
           sArgs.continuous = false;
           if ~isempty(varargin)
               sArgs = ita_parse_arguments(sArgs,varargin);
           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