% Arduino Mega + Adafruit MotorShield V2.3
%% Arduino objekti loomine koos MotorShieldV2 teegiga
a = arduino('COM7', 'Mega2560', 'Libraries', {'I2C','SPI','Adafruit\MotorShieldV2','rotaryEncoder','Servo'})
% oota, uuendab serveri koodi Arduinos 

% MotorShieldV2 objekti loomine
shield = addon(a, 'Adafruit\MotorShieldV2')

%% Servomootori objekti loomine (spetsiaalses) 1.-s pordis (pin D10)
% Hitec HS-311 Standard servo PWM Signal min to max [s]
s = servo(shield, 1, 'MinPulseDuration', 575*10^-6, 'MaxPulseDuration', 2460*10^-6)
angle=0.5; % 0...1 on nurk 0...180 kraadi
writePosition(s, angle)
current_pos = readPosition(s) % 

%% DC mootori objekti loomine M4 pordis
dcmot = dcmotor(shield, 4)
dcmot.Speed = 0.3;
start(dcmot);
pause(2);
dcmot.Speed = -0.2;
pause(2);
stop(dcmot);

%% Enkoodri objekti loomine, kanalid A ja B on kas D# või A#
ppr=360; % pulses per revolution LEGO NXT mootoril 360
encoder = rotaryEncoder(a,'D2','D3',ppr)
rpm = readSpeed(encoder)% revolutions per minute 
resetCount(encoder,0); % set to 0
[count,time] = readCount(encoder,'reset',true) % X4 decoding < 2^32