
 
%% pripojeni k V-REP
 clc
 clear all
 disp('Program started');
    % vytvoreni spojeni s V-REP
  vrep=remApi('remoteApi');  %nahrani remote API funkci                           
  vrep.simxFinish(-1);
  id = vrep.simxStart('127.0.0.1', 19997, true, true, 2000, 5);  % definice spojeni a start simulace

    % pro pripad ze se spojeni nepovede
  if id < 0,
    disp('Ztrata spojedni k remote API server. Ukoncuji.');
    vrep.delete();
    return;
  end
  fprintf('otevreno spojeni s %d remote API server.\n', id);
 % ukonceni spojeni kdykoliv je skript prerusen
  cleanupObj = onCleanup(@() cleanup_vrep(vrep, id));
  
  % prikaz pro tvorbu synchonizaci s V-REP, programy se synchronizuji po zadani prikazu  vrep.simxSynchronousTrigger(id)
  vrep.simxSynchronous(id,true)
  % toto bude fungovat jen pro  "continuous remote API server service"
  % Vice na  http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
  ab = vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait);vrchk(vrep, ab, true);
  
    % zahajeni synchronizace
   vrep.simxSynchronousTrigger(id)
  
 
  
%% Inicializace  
  
        % prirazeni parametru vazby do promenne / ab - pripadna vracena chyba, steer_handle - promena, id- pripojeni k V-rep, nazev vazby
        % ve stringu, vrep.simx_opmode_oneshot_wait - mod prenosu prikazu do V-rep
        [ab, steer_handle] = vrep.simxGetObjectHandle(id, 'zataceni' ,vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_handle0] = vrep.simxGetObjectHandle(id,'zataceni#0',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_handle1] = vrep.simxGetObjectHandle(id,'zataceni#1',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_handle2] = vrep.simxGetObjectHandle(id,'zataceni#2',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
	
		[ab, steer_speed]  = vrep.simxGetObjectHandle(id,'motor',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_speed0] = vrep.simxGetObjectHandle(id,'motor#0',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_speed1] = vrep.simxGetObjectHandle(id,'motor#1',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
		[ab, steer_speed2] = vrep.simxGetObjectHandle(id,'motor#2',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
        
        [ab, proxi_sensor_1] = vrep.simxGetObjectHandle(id,'Proximity_sensor_1',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);  % prirazeni parametru proxi senzoru do promenne
        [ab, vis_sensor_1] = vrep.simxGetObjectHandle(id,'Vision_sensor_2',vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);       % prirazeni parametru visual senzoru do promenne
	
        %inicializace konstant a promenych   
	
	l=0.4965;
	d=0.3745;
	max_angle = 0.50;
	speed = 0;
	max_speed = 30;
	
    % promene pro inkrementy v cyklech
    i = 0;
    u = 1;
    
  joy = vrjoystick(1);          % inicializace joysticku
  
  % test detekce proximity sensoru a visual senzoru, nyni budou vraceny hodnoty jen kdyz dojde ke zmene, diky simx_opmode_streaming
  
  [ab detection coordinates senzor normal_vector] = vrep.simxReadProximitySensor(id,proxi_sensor_1,vrep.simx_opmode_streaming); vrchk(vrep, ab, true);
  [ab, resolution, image] = vrep.simxGetVisionSensorImage(id, vis_sensor_1,1,vrep.simx_opmode_streaming);vrchk(vrep, ab, true);
  
  
 %% vzpocetni cast programu 
 vrep.simxSynchronousTrigger(id)        % opet synchonizace pred zahajenim cyklu 
 tic
  while 1
    
      %     definice zatoceni pri pouziti joysticku
    y = -axis(joy, 1);
    x = -axis(joy, 2);
    b = button(joy, 1);
    %pause(0.1);       
        
    if x~=0                     %jizda dopredu
        speed = speed + x*4;
    else
        speed = 0;
    end
    
    if (speed > 0) && (x < 0)   %zastaveni pri zmene smeru pohybu
       speed = 0;
    end
    
    if (speed < 0) && (x > 0)
        speed = 0;
    end
    
    steer_angle = abs(y)*0.65 ; % zataceni
    
    if speed >= max_speed,      % omezeni maximalni rychlosti a uhlu natoceni
        speed = max_speed;
    end
     if speed <= -max_speed,
        speed = -max_speed;
    end
    if steer_angle >= max_angle
        steer_angle = max_angle;
    end
    
    if b == 1                   % pri stisku tlacitka na joysticku zastaveni simulace
        ab = vrep.simxStopSimulation(id,vrep.simx_opmode_oneshot_wait); vrchk(vrep, ab, true);
        vrep.simxFinish(-1);
        clc
        return;
    end
                        % vypocet rychlisti a zatoceni s vyuzitim Ackerman kinematics
    Asteer_angle = steer_angle;     % vnitrni uhel zatoceni
	steeringAngle = atan((l/2)/(d+(l/2)/tan(Asteer_angle)));    % vnejsi uhel zatoceni
    v_in = speed*(((l/2)/sin(Asteer_angle))/((d/2)+((l/2)/tan(Asteer_angle))));    % rychlost na vnitrnich kolech
    v_out = speed*(((l/2)/sin(steeringAngle))/((d/2)+((l/2)/tan(Asteer_angle))));   % rychlost na vnejsich kolech
    
    % poslani vypoctu do danych vazeb pro zataceni a rychlost
    if y > 0

		ab = vrep.simxSetJointTargetPosition(id,steer_handle,steeringAngle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle0,Asteer_angle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle1,-Asteer_angle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle2,-steeringAngle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
        
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed, v_in,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed0, -v_out,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed1, -v_out,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed2, v_in,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		
    elseif y < 0
		
		ab = vrep.simxSetJointTargetPosition(id,steer_handle,-Asteer_angle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle0,-steeringAngle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle1,steeringAngle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetPosition(id,steer_handle2,Asteer_angle,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
        
        ab = vrep.simxSetJointTargetVelocity(id,steer_speed, v_out,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed0, -v_in,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed1, -v_in,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed2, v_out,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		
    elseif y == 0
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed, speed,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed0, -speed,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed1, -speed,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
		ab = vrep.simxSetJointTargetVelocity(id,steer_speed2, speed,vrep.simx_opmode_oneshot); vrchk(vrep, ab, true);
     end
    
    
    
  %% Sber dat
  
  % sber dat s proximity senzoru, diky simx_opmode_buffer posila data jen pri zmene
  [ab, detection, coordinates, sensor, normal_vector] = vrep.simxReadProximitySensor(id, proxi_sensor_1, vrep.simx_opmode_buffer);vrchk(vrep, ab, true); 

    % ukladani techto dat
    i = i+1;
     pozice = coordinates;
     data(i) = pozice(1,3);
      
     
     
     % jestlize je hodnota na proxi senzoru mezi 1 metrem a 1.1 metru, udelej obrazovou matici s vyuzitin vision senzoru 
     if data(i)>=1 && data(i)<=1.1
        [ab, resolution, image] = vrep.simxGetVisionSensorImage2(id, vis_sensor_1,1,vrep.simx_opmode_buffer);vrchk(vrep, ab, true);
        figure(u)
        imshow(image)
        u = u+1;
     end
  
vrep.simxSynchronousTrigger(id)  % opet sychronizace vzdy na konci cyklu
  
end

