% Control de WIT motion servo controller 16 chanels mediante MatLab
% Comunicación mediante USB-serial externo (con el interno no funciona)

% Tramas de datos
% | 0xFF          | 0x(XX)         | 0x(XX) | 0x(XX) | 0x(XX) |
% |Byte           |Byte            | # de   | Data 2 | Data 1 |
% |de             |de              | servo  |        |        |
% |Confirmación   |acción          | [0-15] |        |        |
% |Siempre 0xFF   |0x01 velocidad  |        |        |        |
% |               |0x02 posición   |        |        |        |
% |               |0x09 en grupo   |        |        |        |
% |               |0x0b emergencia |        |        |        |

% Conectar el puerto 'COMX' con MatLab para acceso serie (RS232)
s = serial('COM3');  % Por defecto: 9.600bps, 8bits, sin paridad, 1 stop
% Abrir la comunicación serie con el puerto
fopen(s);

% Parámetros de control
% Número de serovo a controlar, de 0 a 15
servo = 2;

% Velocidad de giro en grados por segundo, de 9º/s a 180º/s
velocidad = 90;
% Debe convertirse en un valor entre 1 (9º/s) y 20 (180º/s)
velocidad = round((velocidad*10)/90);

% Posición en grados, de 0º a 180º
posicion = 90;
% Debe convertirse a un valor de control entre 500 y 2500
% realmente se envía el ancho del pulso de control en microsegundos
posicion = round(500+(111*(posicion/10)));

% Los datos se deben enviar en dos bytes
% Ejemplo: para enviar el valor 2498 que equivale a 0x09C2
% se sebe enviar primero 0xC2 y luego 0x09 
Data2 = posicion - 256*fix(posicion/256);
Data1 = fix(posicion/256);

% Enviar velocidad de movimiento
fwrite(s,[hex2dec('ff') hex2dec('01') servo velocidad hex2dec('00')]);
% Enviar la posición destino
fwrite(s,[hex2dec('ff') hex2dec('02') servo Data2 Data1]);
pause(2);

% Finalizar la comunicación
fclose(s); % Cerrar la comunicación serie (se puede volver a abrir)
delete(s); % Desconectar definitivamente el puerto 'COMX' de MatLab