clc; clear all
port='COM9';
board='Uno';


a=arduino(port, board, 'Libraries',{'Servo', 'I2C'});

% % % % % % % % % % %
%senzor part

dev = device(a,'I2CAddress','0x38');
writeRegister(dev,0X40,1 < 7 | 1 < 6)
writeRegister(dev,0X41,0x00)
writeRegister(dev,0X42,0x10)
writeRegister(dev,0X44,0x02)
writeRegister(dev,0X60,0x00)
writeRegister(dev,0X61,0x01)

size = 8;
i=0;

uint16 Redvalue;
uint16 Greenvalue;
uint16 Bluevalue;
uint16 Colorvalue;


WHITE = 1;
RED   =2;
YELLOW = 3;
GREEN =4;
BLUE  =5;
BLACK =6;

% % % % % % % % % % % % % % % % % %
% % Servomotor part

joint1 = servo(a, 'D2');
joint2 = servo(a, 'D3');
joint3 = servo(a, 'D4');
joint4 = servo(a, 'D5');
joint5 = servo(a, 'D6');


jointAngle=[130,10,130,90,160,0,0,0,0,0,0]
jointAngle=moveManipulatorToHomePosition(jointAngle,joint1,joint2,joint3,joint4,joint5);

