% MATLAB controller for Webots

% File: cherokey_test.m
% Date:
% Description:
% Author:
% Modifications:

% uncomment the next two lines if you want to use

TIME_STEP =1;%15;
enconders=[];
enconders_names=[ "wheel1sensor", "wheel2sensor", "wheel3sensor", "wheel4sensor" ];
wheels = [];
wheels_names = [ "wheel1", "wheel2", "wheel3", "wheel4" ];
robot_node = wb_supervisor_node_get_from_def('gps');
torque_node = wb_supervisor_node_get_from_def('Cherokey');
% enconder_node = wb_supervisor_node_get_from_def('Enconder');

for i = 1:4
enconders(i)=wb_robot_get_device(convertStringsToChars(enconders_names(i)));
wheels(i) = wb_robot_get_device(convertStringsToChars(wheels_names(i)));
wb_position_sensor_enable(enconders(i), TIME_STEP);
end

gyroscope=wb_robot_get_device(convertStringsToChars("gyro"));
accelerometer=wb_robot_get_device(convertStringsToChars("accelerometer"));
gps=wb_robot_get_device(convertStringsToChars("gps"));
inertial_unit=wb_robot_get_device(convertStringsToChars("inertial unit"));
% enconder=wb_robot_get_device(convertStringsToChars("enconder"));
wb_gyro_enable(gyroscope, TIME_STEP);
wb_accelerometer_enable(accelerometer, TIME_STEP);
wb_gps_enable(gps, TIME_STEP);
wb_inertial_unit_enable(inertial_unit,TIME_STEP);
% wb_gyro_enable(enconder, TIME_STEP);

j=1;
k=[1,1,1,1];
enconder_position(j,1)={[0, 0, 0, 0]};
angular_velocity_wheels(j,1)={[0, 0, 0, 0]};
rad_change(j,1)={[0, 0, 0, 0]}; %%t=0 é j=1
gyro_values(j,1)={[0, 0, 0]};
accel_values(j,1)={[0, 0, 0]};
position(j,1)={[0, 0, 0]};
speed(j,1)={0};
orientation(j,1)={[1 0 0;0 1 0;0 0 1]};
velocity(j,1)={[0,0,0]};
angles(j,1)={[0, 0, 0]};
% wheel_1_torque=signal1;
% % wheel_1_torque=[wheel_1_torque,0*wheel_1_torque,-
wheel_1_torque,0*wheel_1_torque];
% wheel_2_torque=signal1;
% wheel_3_torque=signal1;
% wheel_4_torque=signal1_thesis;
% lt=signal2;
value=[];
enconder_values(j,1)={[0, 0, 0]};
tko_unit(j,1)={[1 0 0;0 1 0;0 0 1]};
% load Controller.mat
% load Cherokey_thesis_controller_feedback
% load Cherokey_thesis_controller3.mat
% load Cherokey_thesis_controller_feedback2.mat
% load Cherokey_thesis_controller8.mat
% % Cherokey_controller.layers{4}.transferFcn ='purelin';
% load Trajectory_thesis_report_1.mat
% load PS.mat
% load gains.mat
% % [Y,Xf,Af]=Inverse_closed([0;0;0]);
% % [Y,Xf,Af]=Cherokey_controller([0;0;0;0;0;0]);
% Y=[0;0;0;0;0;0];
% % velocity_acell_derived=[0;0;0];
% % position_acell_derived=[0;0;0];
% % accel_global=[0;0;0];
%
% % velocity_trajectory=[zeros(6,1),velocity_trajectory];
% % error_total=[0;0;0];
% % kc=380;
% % Pc=0.009;
% % T=0.001;
% % Ti=0.85*Pc;
% % kp=0.45*kc;
% % ki=kp*(T/Ti);
% % kp=[kp 0 0;0 kp 0;0 0 kp];
% % ki=[ki 0 0;0 ki 0;0 0 ki];
% % kp=[0 0 0;0 0 0;0 0 0];
% % ki=[0 0 0;0 0 0;0 0 0];
%
% % k1=100;
% % k2=100;
% % k3=100;
%
% k1=100;
% k2=100;
% k3=100;
%
%
% dist=[ones(1,250)*1,ones(1,500)*0,ones(1,250)*(-
1),ones(1,250)*2,ones(1,500)*0,ones(1,250)*(-
2),ones(1,250)*3,ones(1,500)*0,ones(1,250)*(-
3),ones(1,250)*4,ones(1,500)*0,ones(1,250)*(-
4),ones(1,250)*5,ones(1,500)*0,ones(1,251)*(-5)];
% E_IM_filter=[0;0;0];

% dist=[ones(1,7500)*10,ones(1,7501)*0];

load Torque_M_continous_25.mat

while wb_robot_step(TIME_STEP) ~= -1
 if j<=100
for i = 1:4
value(1,i)=wb_position_sensor_get_value(enconders(i));
end
enconder_position(j+1,1)={value};
left_torque=0;
right_torque=0;
wb_motor_set_torque(wheels(1), right_torque);
wb_motor_set_torque(wheels(2), right_torque);
wb_motor_set_torque(wheels(3), left_torque);
wb_motor_set_torque(wheels(4), left_torque);
rad_change(j+1,1)={cell2mat(enconder_position(j+1,1))-
cell2mat(enconder_position(j,1))};
gyro_values(j+1,1)={wb_gyro_get_values(gyroscope)}; %{[0, 0, 0]};
accel_values(j+1,1)={wb_accelerometer_get_values(accelerometer)}; %{[0, 0, 0]};
position(j+1,1)={wb_supervisor_node_get_position(torque_node)}; %{[0, 0, 0]};
speed(j+1,1)={wb_gps_get_speed(gps)}; %{[0, 0, 0]};
orientation(j+1,1)={wb_supervisor_node_get_orientation(robot_node)};
velocity(j+1,1)={wb_supervisor_node_get_velocity(torque_node)};
angles(j+1,1)={wb_inertial_unit_get_roll_pitch_yaw(inertial_unit)};
% enconder_values(j+1,1)={wb_gyro_get_values(enconder)};
% tko_unit(j+1,1)={wb_supervisor_node_get_orientation(enconder_node)};

for i=1:4
rad_change_temp=cell2mat(rad_change(j+1,1));
angular_velocity_temp=cell2mat(angular_velocity_wheels(j,1));

if rad_change_temp(1,i)~=0
time_between_change=TIME_STEP*k(1,i)/1000;
angular_velocity_wheels_1(1,i)=rad_change_temp(1,i)/time_between_change;
k(1,i)=1;
else
angular_velocity_wheels_1(1,i)=angular_velocity_temp(1,i);
k(1,i)=k(1,i)+1;
end

end

angular_velocity_wheels(j+1,1)={angular_velocity_wheels_1};

velocity_acell_derived=cell2mat(velocity(j+1,1)).';
position_acell_derived=cell2mat(position(j+1,1)).';

j=j+1;

elseif j>100 && j<=100+size(M,2)%size(wheel_1_torque,2)

if j==101
wb_supervisor_node_set_velocity(torque_node,[0,0,0,0,0,0])
end
 enconder_position(j+1,1)={value};
rad_change(j+1,1)={cell2mat(enconder_position(j+1,1))-
cell2mat(enconder_position(j,1))};
gyro_values(j+1,1)={wb_gyro_get_values(gyroscope)}; %{[0, 0, 0]};
accel_values(j+1,1)={wb_accelerometer_get_values(accelerometer)}; %{[0, 0, 0]};
position(j+1,1)={wb_supervisor_node_get_position(torque_node)}; %{[0, 0, 0]};
speed(j+1,1)={wb_gps_get_speed(gps)}; %{[0, 0, 0]};
orientation(j+1,1)={wb_supervisor_node_get_orientation(robot_node)};
velocity(j+1,1)={wb_supervisor_node_get_velocity(torque_node)};
angles(j+1,1)={wb_inertial_unit_get_roll_pitch_yaw(inertial_unit)};
%
wb_motor_set_torque(wheels(1),M(1,j-100));
wb_motor_set_torque(wheels(2),M(2,j-100));
wb_motor_set_torque(wheels(3),M(3,j-100));
wb_motor_set_torque(wheels(4),M(4,j-100));

% wb_motor_set_torque(wheels(1),0.2);
% wb_motor_set_torque(wheels(2),0.2);
% wb_motor_set_torque(wheels(3),0.2);
% wb_motor_set_torque(wheels(4),0.2);
%

% position_controller=cell2mat(position(j+1,1));
% velocity_controller=cell2mat(velocity(j+1,1));
% acceleration=cell2mat(accel_values(j+1,1));
% angular_velocity=cell2mat(gyro_values(j+1,1));
% angular_position=cell2mat(angles(j+1,1));
% angular_velocity_y=angular_velocity(1,2);
% angular_position_y=angular_position(1,3);
%
%
% % velocity_acell_derived=velocity_acell_derived+0.001*accel_global;
% % position_acell_derived=position_acell_derived+0.001*velocity_acell_derived;
% accel_local=acceleration.'+cell2mat(orientation(j,1))*[0;-9.81;0];
% accel_global=(inv(cell2mat(orientation(j,1)))*accel_local);
% velocity_acell_derived=velocity_acell_derived+0.001*accel_global;
% position_acell_derived=position_acell_derived+0.001*velocity_acell_derived;
% %
y_p=[velocity_acell_derived(1,1);velocity_acell_derived(3,1);angular_velocity_y;pos
ition_acell_derived(1,1);position_acell_derived(3,1);angular_position_y];
% %
y_p=[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y;position_
controller(1,1);position_controller(1,3);angular_position_y];
%
% position_controller=position_acell_derived.';
% velocity_controller=velocity_acell_derived.';
%
% position_acell(j+1,1)={position_controller};
% velocity_acell(j+1,1)={velocity_controller};
%
% if j==101
% error_k=[0;0;0]-
[position_controller(1,1);position_controller(1,3);angular_position_y];
% error_k_velocity=[0;0;0]-
[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% else
% error_k=velocity_trajectory(4:6,j-101)-
[position_controller(1,1);position_controller(1,3);angular_position_y];
% error_k_velocity=velocity_trajectory(1:3,j-101)-
[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% end
%
% if error_k(3,1)>pi
% error_k(3,1)= error_k(3,1)-2*pi;
%
% elseif error_k(3,1)<-pi
% error_k(3,1)= error_k(3,1)+2*pi;
% end
%
%
%
% error_k_saved(j+1,1)={error_k.'};
% error_k_velocity_saved(j+1,1)={error_k_velocity.'};
% error_k_c=[cos(angular_position_y) -sin(angular_position_y)
0;sin(angular_position_y) cos(angular_position_y) 0; 0 0 1]*error_k;
% error_k_c_velocity=[cos(angular_position_y) -sin(angular_position_y)
0;sin(angular_position_y) cos(angular_position_y) 0; 0 0 1]*error_k_velocity;
% error_k_c_saved(j+1,1)={error_k_c.'};
% error_k_c_velocity_saved(j+1,1)={error_k_c_velocity.'};
% trajectory=velocity_trajectory(:,j-100);%j-100
% r_local_velocity=[cos(trajectory(6,1)) -sin(trajectory(6,1))
0;sin(trajectory(6,1)) cos(trajectory(6,1)) 0; 0 0 1]*trajectory(1:3,1);
% u_local=[r_local_velocity(1,1)*cos(error_k_c(3,1))
+r_local_velocity(2,1)*sin(error_k_c(3,1))+k1*error_k_c(1,1);-
r_local_velocity(1,1)*sin(error_k_c(3,1))+r_local_velocity(2,1)*cos(error_k_c(3,1))
+k2*error_k_c(2,1);r_local_velocity(3,1)+k3*error_k_c(3,1)];
% % u_global=[cos(trajectory(6,1)) sin(trajectory(6,1)) 0;-sin(trajectory(6,1))
cos(trajectory(6,1)) 0; 0 0 1]*u_local;
% u_global=[cos(angular_position_y+0.001*angular_velocity_y)
sin(angular_position_y+0.001*angular_velocity_y) 0;-
sin(angular_position_y+0.001*angular_velocity_y)
cos(angular_position_y+0.001*angular_velocity_y) 0; 0 0 1]*u_local;
% % u_global=trajectory(1:3,1);
%
%
% % input_reference(1:3,1)+kp*error_k+ki*error_total;
% % trajectory=[output_pid;input_reference(4:6,1)];
%
% % wb_supervisor_node_set_velocity(robot_node,[0.001+velocity_controller(1,1) 0
0.001+velocity_controller(1,3) 0 0.001+angular_velocity_y 0]);
% %
% % y_p=[cos(angular_position_y) -sin(angular_position_y) 0;sin(angular_position_y)
cos(angular_position_y) 0; 0 0
1]*[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% % % r_local_velocity=[cos(trajectory(6,1)) -sin(trajectory(6,1))
0;sin(trajectory(6,1)) cos(trajectory(6,1)) 0; 0 0 1]*trajectory(1:3,1);
% % r_local_velocity=u_local;
% % X={r_local_velocity;y_p};
% % [Y,Xf,Af]=Cherokey_controller_feedback(X);
% % Torque=mapminmax('reverse',cell2mat(Y),PS);
% % Torque_saved(j+1,1)={Torque.'};
%
% % y_p=[cos(angular_position_y) -sin(angular_position_y) 0;sin(angular_position_y)
cos(angular_position_y) 0; 0 0
1]*[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% % % y_p=[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% % % y=cell2mat(Af(7,1));
% % %
y_p=[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y;position_
controller(1,1);position_controller(1,3);angular_position_y];
% % % y=Y;
% % y=[cos(Y(6,1)) -sin(Y(6,1)) 0;sin(Y(6,1)) cos(Y(6,1)) 0; 0 0 1]*Y(1:3,1);
%
% %%
%
% % y_p=[cos(angular_position_y) -sin(angular_position_y) 0;sin(angular_position_y)
cos(angular_position_y) 0; 0 0
1]*[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% % y=[cos(Y(6,1)) -sin(Y(6,1)) 0;sin(Y(6,1)) cos(Y(6,1)) 0; 0 0 1]*Y(1:3,1);
% Y_saved(j+1,1)={Y.'};
% y_p=[velocity_controller(1,1);velocity_controller(1,3);angular_velocity_y];
% y=Y(1:3,1);
% y_local=[cos(Y(6,1)) -sin(Y(6,1)) 0;sin(Y(6,1)) cos(Y(6,1)) 0; 0 0 1]*y;
% % trajectory=velocity_trajectory(:,j-100);
% e_IM=y_p-y;
% e_IM_filter=E_IM_filter.*[0;0;0]+e_IM.*[1;1;1];
% r_modified_prefilter=[cos(Y(6,1)+0.001*Y(3,1)) -sin(Y(6,1)+0.001*Y(3,1))
0;sin(Y(6,1)+0.001*Y(3,1)) cos(Y(6,1)+0.001*Y(3,1)) 0; 0 0 1]*(u_global-
e_IM_filter);
% % e_IM_filter=[cos(Y(6,1)+0.001*Y(3,1)) -sin(Y(6,1)+0.001*Y(3,1))
0;sin(Y(6,1)+0.001*Y(3,1)) cos(Y(6,1)+0.001*Y(3,1)) 0; 0 0 1]*e_IM_filter;
% % r_modified_prefilter=([cos(Y(6,1)+0.001*Y(3,1)) -sin(Y(6,1)+0.001*Y(3,1))
0;sin(Y(6,1)+0.001*Y(3,1)) cos(Y(6,1)+0.001*Y(3,1)) 0; 0 0 1]*trajectory(1:3,1))-
e_IM;
% % e_IM=[cos(Y(6,1)+0.001*Y(3,1)) -sin(Y(6,1)+0.001*Y(3,1))
0;sin(Y(6,1)+0.001*Y(3,1)) cos(Y(6,1)+0.001*Y(3,1)) 0; 0 0 1]*e_IM_global;
% e_IM_saved(j+1,1)={e_IM_filter.'};
% % r_modified_velocities_local=u_local-e_IM;
% % r_local_velocity_internal=[cos(Y(6,1)) -sin(Y(6,1)) 0;sin(Y(6,1)) cos(Y(6,1))
0; 0 0 1]*trajectory(1:3,1);
% % % r_local_velocity_internal=r_local_velocity;
% % r_modified_prefilter=r_local_velocity_internal-e_IM;
% % r_filter=1*r_modified_prefilter+0*r_filter;
% r_filter=r_modified_prefilter;
% r_modified_local=r_filter;
% % r_modified=r_modified_velocities_global;
% % r_modified_velocities_global=[cos(-trajectory(6,1)) -sin(-trajectory(6,1))
0;sin(-trajectory(6,1)) cos(-trajectory(6,1)) 0; 0 0
1]*r_modified_velocities_local;
% r_modified=[r_modified_local;1;1;1];
% r_modified_saved(j+1,1)={r_modified.'};
% if j==101
% [Y,Xf,Af]=Cherokey_controller(r_modified);
% else
% [Y,Xf,Af]=Cherokey_controller(r_modified,Xf,Af);
% end
% Torque=mapminmax('reverse',cell2mat(Af(4,1)),PS);
% % Torque(Torque>0.4)=0.4;
% Torque_saved(j+1,1)={Torque.'};

% velocity_controller=cell2mat(velocity(j+1,1));
% gyro_controller=cell2mat(gyro_values(j+1,1));
% angle_controller=cell2mat(angles(j+1,1));
% v_x=velocity_controller(1,1);
% v_z=velocity_controller(1,3);
% w_y=gyro_controller(1,2);
% yaw_angle=angle_controller(1,3);
% y_p=[cos(yaw_angle) -sin(yaw_angle) 0;sin(yaw_angle) cos(yaw_angle) 0; 0 0
1]*[v_x;v_z;w_y];
% y=cell2mat(Af(4,1));
% e_IM=y_p-y;
% r=[cos(velocity_trajectory(6,j-100)) -sin(velocity_trajectory(6,j-100))
0;sin(velocity_trajectory(6,j-100)) cos(velocity_trajectory(6,j-100)) 0; 0 0
1]*velocity_trajectory(1:3,j-100);
% r_modified=r-e_IM;
% [Y,Xf,Af]=Inverse_closed(r_modified,Xf,Af);
% Torque_directional=cell2mat(Af(2,1));
% A = [];
% b = [];
% Aeq = [1 1 1 1; -1 1 1 -1; 1 -1 1 -1;1 0 0 -1];
% beq =
[Torque_directional(1,1);Torque_directional(2,1);Torque_directional(3,1);0];
% f=[1 1 1 1];
% Torque(1:4,1)=linprog(f,A,b,Aeq,beq);
% Torque_saved(j+1,1)={Torque.'};

% one=wheel_1_torque(j-100);
% two=wheel_2_torque(j-100);
% three=wheel_3_torque(j-100);
% % four=wheel_4_torque(j-100);
% one=x(1,j-100);
% two=x(2,j-100);
% three=x(3,j-100);
% four=x(4,j-100);

% wb_motor_set_torque(wheels(1),Torque(1,1));
% wb_motor_set_torque(wheels(2),Torque(2,1));
% wb_motor_set_torque(wheels(3),Torque(3,1));
% wb_motor_set_torque(wheels(4),Torque(4,1));
% wb_supervisor_node_add_torque(torque_node,[0,dist(:,j-100),0],0)
% wb_supervisor_node_add_force(torque_node,[dist(:,j-100),0,dist(:,j-100)],0)
% wb_supervisor_node_add_force(torque_node,[dist(:,j-100),0,0],0)

% rad_change(j+1,1)={cell2mat(enconder_position(j+1,1))-
cell2mat(enconder_position(j,1))};
% gyro_values(j+1,1)={wb_gyro_get_values(gyroscope)}; %{[0, 0, 0]};
% accel_values(j+1,1)={wb_accelerometer_get_values(accelerometer)}; %{[0, 0, 0]};
% position(j+1,1)={wb_supervisor_node_get_position(robot_node)}; %{[0, 0, 0]};
% speed(j+1,1)={wb_gps_get_speed(gps)}; %{[0, 0, 0]};
% orientation(j+1,1)={wb_supervisor_node_get_orientation(robot_node)};
% velocity(j+1,1)={wb_supervisor_node_get_velocity(robot_node)};
% angles(j+1,1)={wb_inertial_unit_get_roll_pitch_yaw(inertial_unit)};
% enconder_values(j+1,1)={wb_gyro_get_values(enconder)};
% tko_unit(j+1,1)={wb_supervisor_node_get_orientation(enconder_node)};

for i=1:4
rad_change_temp=cell2mat(rad_change(j+1,1));
angular_velocity_temp=cell2mat(angular_velocity_wheels(j,1));

if rad_change_temp(1,i)~=0
time_between_change=TIME_STEP*k(1,i)/1000;
angular_velocity_wheels_1(1,i)=rad_change_temp(1,i)/time_between_change;
k(1,i)=1;
 else
angular_velocity_wheels_1(1,i)=angular_velocity_temp(1,i);
k(1,i)=k(1,i)+1;
end

end

angular_velocity_wheels(j+1,1)={angular_velocity_wheels_1};
j=j+1;

else
%
save('data_neural_controller_thesis_feedback_pid','gyro_values','accel_values','pos
ition','angular_velocity_wheels','orientation','speed','velocity','angles','enconde
r_position','enconder_values','tko_unit','Torque_saved')
%
save('data_neural_controller_thesis','gyro_values','accel_values','position','angul
ar_velocity_wheels','orientation','speed','velocity','angles','enconder_position','
enconder_values','tko_unit','Torque_saved')

%save('data_neural_controller_thesis_report_dist2','gyro_values','accel_values','po
sition','angular_velocity_wheels','orientation','speed','velocity','angles','encond
er_position','enconder_values','tko_unit','Torque_saved','e_IM_saved','r_modified_s
aved','Y_saved','error_k_c_saved','error_k_saved','error_k_c_velocity_saved','error
_k_velocity_saved','position_acell','velocity_acell')

save('data_neural_continous_25','gyro_values','accel_values','position','angular_ve
locity_wheels','orientation','speed','velocity','angles','enconder_position','encon
der_values','tko_unit')
quit(0);

end

end