% Copyright 2012 jeffrey laut
% Uses RWTHMindStorms toolbox (freely available)
% and a logitech gamepad

joy   = vrjoystick(1);
left  = axis(joy,4);
right = axis(joy, 2 );

handle = COM_OpenNXT('bluetooth.ini');
COM_SetDefaultNXT(handle);

StopMotor(MOTOR_A, 'off');
StopMotor(MOTOR_B, 'off');

motorA = NXTMotor('A');%, 'Power', 0);
motorB = NXTMotor('B');%, 'Power', 0);

posA_old = 0;
posB_old = 0;

ratioA = 1/3;
ratioB = 15;

while button(joy,2) == 0
    tic;
    left  = axis(joy,4);
    right = axis(joy, 2);
    
    DirectMotorCommand(MOTOR_A, left*100,  0, 'on', 'off', 0, 'off');
    DirectMotorCommand(MOTOR_B, right*100, 0, 'on', 'off', 0, 'off');
    
    if left == 0
        StopMotor(MOTOR_A, 'off');
    end
    if right == 0
        StopMotor(MOTOR_B, 'off');
    end
    
    dataA = motorA.ReadFromNXT();
    dataB = motorB.ReadFromNXT();

    dt = toc;
    velA = (dataA.Position-posA_old)/dt;
    velB = (dataB.Position-posB_old)/dt;
    
    posA_old = dataA.Position;
    posB_old = dataB.Position;
    
%     
%     figure(1)
%     bar([velA, velA*ratioA]);
%     ylim([-1600, 1600])
%     
%     figure(2)
%     bar([velB, velB*ratioB]);
%     ylim([-1600*10, 1600*10])
    
%     
%     figure(3)
%     bar([velA, velA*ratioA, velB, velB*ratioB]);
%     ylim([-1600, 1600])
%     
    pause(0.05);
end

NXT_SendKeepAlive('dontreply');
StopMotor(MOTOR_A, 'off');
StopMotor(MOTOR_B, 'off');

COM_CloseNXT all;
clear all;
close all;
clc;