├── presentation.pdf ├── 2.Q_fuzzy controller ├── Goal Reach │ ├── image.png │ ├── benchmark.png │ ├── Zer5_H 49 rule │ │ ├── image.png │ │ ├── MAIN_RL3_ZMOH5.m │ │ ├── Error.m │ │ ├── trianglar_fct.m │ │ ├── Odometry.m │ │ ├── Reward_function1.m │ │ ├── Diff_Robot_Model.m │ │ ├── Reward_function2.m │ │ └── Rules_act_deg_49.m │ └── 20 rules │ │ ├── 1.Learning │ │ ├── qtable500.mat │ │ ├── qtable1000.mat │ │ ├── qtable1500.mat │ │ ├── qtable2000.mat │ │ ├── qtable2500.mat │ │ ├── qtable3000.mat │ │ ├── qtable3500.mat │ │ ├── qtable4000.mat │ │ ├── qtable4500.mat │ │ ├── Error.m │ │ ├── trianglar_fct.m │ │ ├── Odometry.m │ │ ├── Reward_function1.m │ │ ├── Diff_Robot_Model.m │ │ ├── Rules_act_deg.m │ │ └── MAIN_RL3_ZMO4.m │ │ ├── 3.FLC vs Q_FLC │ │ ├── image.png │ │ ├── qtable2000.mat │ │ ├── qtable4000.mat │ │ ├── Q_policy.m │ │ ├── Error.m │ │ ├── trianglar_fct.m │ │ ├── Odometry.m │ │ ├── Diff_Robot_Model.m │ │ ├── Act_deg_conclusion_tab.m │ │ ├── fuzzy_controller_RL.m │ │ ├── Rules_act_deg.m │ │ ├── fuzzy_controller_simple.m │ │ └── MAIN_FLC_VS_QFLC.m │ │ └── 2.test itetrations │ │ ├── MAIN_test.m │ │ ├── qtable2000.mat │ │ ├── qtable4000.mat │ │ ├── Q_policy.m │ │ ├── Error.m │ │ ├── trianglar_fct.m │ │ ├── Odometry.m │ │ ├── Diff_Robot_Model.m │ │ ├── Act_deg_conclusion_tab.m │ │ ├── fuzzy_controller_RL.m │ │ ├── Rules_act_deg.m │ │ └── Q_FLC.m └── Obstacle avoidance │ └── version 2 │ ├── qtable.mat │ ├── MAIN_obs_RL1.m │ ├── OBFLC-zer.xlsx │ ├── qtable1000.mat │ ├── qtable500.mat │ ├── environment_type2.m │ ├── sensors_pos.m │ ├── trianglar_fct.m │ ├── sensor_value_obs.m │ ├── Odometry.m │ ├── Reward_function_obs.m │ ├── Diff_Robot_Model.m │ ├── visualization_vect.m │ ├── draw_PFE2.m │ ├── sensor_value.m │ ├── Rules_act_deg_obs.m │ └── InterX.m ├── 1.fuzzy controller ├── 1.Goal Reach │ ├── fuzzy_controller_w.m │ ├── fuzzy_controller_simple.m │ ├── fuzzy controllers │ │ ├── official │ │ │ ├── fuzzy_controller_w.m │ │ │ ├── other │ │ │ │ └── fuzzy_controller_w.m │ │ │ └── membership function plot │ │ │ │ ├── trianglar_fct.m │ │ │ │ └── FLC_w.m │ │ └── simple │ │ │ ├── fuzzy_controller_simple.m │ │ │ └── membership function plot │ │ │ ├── trianglar_fct.m │ │ │ └── FLC_w.m │ ├── Error.m │ ├── trianglar_fct.m │ ├── Odometry.m │ ├── Diff_Robot_Model.m │ ├── visualization_vect.m │ └── MAIN.m ├── 2.Obstacle avoidance │ ├── obstacle_avoidance_FLC_corr.m │ ├── ~$OBFLC.xlsx │ ├── environment_type2.m │ ├── sensors_pos.m │ ├── trianglar_fct.m │ ├── sensor_value_obs.m │ ├── Odometry.m │ ├── Diff_Robot_Model.m │ ├── visualization_vect.m │ ├── draw_PFE2.m │ ├── sensor_value.m │ ├── MAIN.m │ └── InterX.m └── 4.Goal Reach and Obstacle avoidance ( chamber ) │ ├── fuzzy_controller_w.m │ ├── obstacle_avoidance_FLC_corr.m │ ├── goal_reach+obs_avoidance │ ├── fuzzy_controller_w.m │ ├── obstacle_avoidance_FLC_corr.m │ ├── standalone controller │ │ ├── obstacle_avoidance_FLC_corr.m │ │ └── trianglar_fct.m │ ├── Error.m │ ├── trianglar_fct.m │ ├── Odometry.m │ ├── sensors_pos.m │ ├── Diff_Robot_Model.m │ ├── sensor_value_chamber.m │ ├── sensor_value.m │ ├── MAIN.m │ ├── draw_PFE3.m │ ├── obstacles_chamber.m │ ├── visualization_vect.m │ └── InterX.m │ ├── Error.m │ ├── trianglar_fct.m │ ├── Odometry.m │ ├── sensors_pos.m │ ├── Diff_Robot_Model.m │ ├── sensor_value_chamber.m │ ├── sensor_value.m │ ├── MAIN.m │ ├── draw_PFE3.m │ ├── obstacles_chamber.m │ ├── visualization_vect.m │ └── InterX.m ├── README.md └── LICENSE /presentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/presentation.pdf -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/image.png -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/benchmark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/benchmark.png -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy_controller_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/1.Goal Reach/fuzzy_controller_w.m -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy_controller_simple.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/1.Goal Reach/fuzzy_controller_simple.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/image.png -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable500.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/image.png -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/MAIN_RL3_ZMOH5.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/MAIN_RL3_ZMOH5.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/MAIN_obs_RL1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Obstacle avoidance/version 2/MAIN_obs_RL1.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/OBFLC-zer.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Obstacle avoidance/version 2/OBFLC-zer.xlsx -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable1000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable1000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Obstacle avoidance/version 2/qtable500.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable1000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable1000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable1500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable1500.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable2000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable2000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable2500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable2500.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable3000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable3000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable3500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable3500.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable4000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable4000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable4500.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/qtable4500.mat -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/obstacle_avoidance_FLC_corr.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/2.Obstacle avoidance/obstacle_avoidance_FLC_corr.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/qtable2000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/qtable2000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/qtable4000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/qtable4000.mat -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/~$OBFLC.xlsx: -------------------------------------------------------------------------------- 1 | El El -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/MAIN_test.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/MAIN_test.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/qtable2000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/qtable2000.mat -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/qtable4000.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/qtable4000.mat -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/fuzzy_controller_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/fuzzy_controller_w.m -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/simple/fuzzy_controller_simple.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/1.Goal Reach/fuzzy controllers/simple/fuzzy_controller_simple.m -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/other/fuzzy_controller_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/other/fuzzy_controller_w.m -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/fuzzy_controller_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/fuzzy_controller_w.m -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/obstacle_avoidance_FLC_corr.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/obstacle_avoidance_FLC_corr.m -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Q_policy.m: -------------------------------------------------------------------------------- 1 | function policy=Q_policy(Q) 2 | policy=zeros(1,20); 3 | %the final states are 1, 5, 9, 13, 17 4 | for i=1:20 5 | [Q_max action]=max(Q(i,:)); 6 | policy(i)=action; 7 | end 8 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Q_policy.m: -------------------------------------------------------------------------------- 1 | function policy=Q_policy(Q) 2 | policy=zeros(1,20); 3 | %the final states are 1, 5, 9, 13, 17 4 | for i=1:20 5 | [Q_max action]=max(Q(i,:)); 6 | policy(i)=action; 7 | end 8 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/fuzzy_controller_w.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/fuzzy_controller_w.m -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/obstacle_avoidance_FLC_corr.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/obstacle_avoidance_FLC_corr.m -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/standalone controller/obstacle_avoidance_FLC_corr.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zeryabmoussaoui/fuzzy-qlearning-robot/HEAD/1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/standalone controller/obstacle_avoidance_FLC_corr.m -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | 3 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 4 | 5 | %atan(X) returns values in the interval [-pi/2,pi/2] 6 | % atan2(a,b) maps in [-pi +pi] 7 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 8 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | 3 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 4 | 5 | %atan(X) returns values in the interval [-pi/2,pi/2] 6 | % atan2(a,b) maps in [-pi +pi] 7 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 8 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | % distance error between the desired posiotion and the robot position (m) 3 | % angllur error between the desidered position and the robot position (rad) 4 | % there is a problem concerning the angle atan ( -1,-1 or -1,1) 5 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 6 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 7 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | % distance error between the desired posiotion and the robot position (m) 3 | % angllur error between the desidered position and the robot position (rad) 4 | % there is a problem concerning the angle atan ( -1,-1 or -1,1) 5 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 6 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 7 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | % distance error between the desired posiotion and the robot position (m) 3 | % angllur error between the desidered position and the robot position (rad) 4 | % there is a problem concerning the angle atan ( -1,-1 or -1,1) 5 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 6 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 7 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | % distance error between the desired posiotion and the robot position (m) 3 | % angllur error between the desidered position and the robot position (rad) 4 | % there is a problem concerning the angle atan ( -1,-1 or -1,1) 5 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 6 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 7 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Error.m: -------------------------------------------------------------------------------- 1 | function [d_error, theta_error]= Error(x_cur, y_cur, theta_cur, x_des, y_des) 2 | % distance error between the desired posiotion and the robot position (m) 3 | % angllur error between the desidered position and the robot position (rad) 4 | % there is a problem concerning the angle atan ( -1,-1 or -1,1) 5 | d_error= sqrt( (x_des-x_cur)^2+(y_des-y_cur)^2); 6 | theta_error = atan2((y_des-y_cur),(x_des-x_cur))-theta_cur; 7 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/environment_type2.m: -------------------------------------------------------------------------------- 1 | function [ wall_u wall_d] = environment_type2() 2 | pt=0:0.01:2*pi; % use 0.01 becuase there are some flaws when using 0.05 3 | xr=0; 4 | yr=0; 5 | ru=3; 6 | rd=4.2; 7 | xu= xr+ru*cos(pt); 8 | yu= yr+ru*sin(pt); 9 | xd= xr+rd*cos(pt); 10 | yd= yr+rd*sin(pt); 11 | 12 | wall_u=[xu; yu]; 13 | wall_d=[xd; yd]; 14 | 15 | plot(wall_u(1,:),wall_u(2,:),'b',wall_d(1,:),wall_d(2,:),'b'); 16 | axis equal 17 | hold on 18 | 19 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/environment_type2.m: -------------------------------------------------------------------------------- 1 | function [ wall_u wall_d] = environment_type2() 2 | pt=0:0.01:2*pi; % use 0.01 becuase there are some flaws when using 0.05 3 | xr=0; 4 | yr=0; 5 | ru=3; 6 | rd=4.2; 7 | xu= xr+ru*cos(pt); 8 | yu= yr+ru*sin(pt); 9 | xd= xr+rd*cos(pt); 10 | yd= yr+rd*sin(pt); 11 | 12 | wall_u=[xu; yu]; 13 | wall_d=[xd; yd]; 14 | 15 | plot(wall_u(1,:),wall_u(2,:),'b',wall_d(1,:),wall_d(2,:),'b'); 16 | axis equal 17 | hold on 18 | 19 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/sensors_pos.m: -------------------------------------------------------------------------------- 1 | function [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range) 2 | pt=min_range:0.03:max_range; 3 | 4 | % I made a mistake her concernig the direction 5 | R_sensor= [ x+pt*cos(-pi/3+theta); y+pt*sin(-pi/3+theta)]; 6 | L_sensor= [x+pt*cos(theta+pi/3); y+pt*sin(theta+pi/3)]; 7 | F_sensor= [x+pt*cos(theta);y+pt*sin(theta)]; 8 | 9 | %plot(R_sensor(1,:),R_sensor(2,:),'r',L_sensor(1,:),L_sensor(2,:),'b',F_sensor(1,:),F_sensor(2,:),'g'); 10 | %hold on 11 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/sensors_pos.m: -------------------------------------------------------------------------------- 1 | function [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range) 2 | pt=min_range:0.03:max_range; 3 | 4 | % I made a mistake her concernig the direction 5 | R_sensor= [ x+pt*cos(-pi/3+theta); y+pt*sin(-pi/3+theta)]; 6 | L_sensor= [x+pt*cos(theta+pi/3); y+pt*sin(theta+pi/3)]; 7 | F_sensor= [x+pt*cos(theta);y+pt*sin(theta)]; 8 | 9 | %plot(R_sensor(1,:),R_sensor(2,:),'r',L_sensor(1,:),L_sensor(2,:),'b',F_sensor(1,:),F_sensor(2,:),'g'); 10 | %hold on 11 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/simple/membership function plot/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/membership function plot/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/sensor_value_obs.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value_obs(x, y, theta,min_range,max_range, obs1, obs2) %sensor_distance(ENV) 2 | 3 | 4 | [L1_dis, R1_dis, F1_dis, detect_1] = sensor_value(x, y, theta,min_range,max_range, obs1); 5 | [L2_dis, R2_dis, F2_dis, detect_2] = sensor_value(x, y, theta,min_range,max_range, obs2); 6 | 7 | L_dis=min([L1_dis L2_dis]); 8 | R_dis=min([R1_dis R2_dis]); 9 | F_dis=min([F1_dis F2_dis]); 10 | if ((detect_1==1) | (detect_2==1)) 11 | detect=1; 12 | else 13 | detect=0; 14 | end 15 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/sensor_value_obs.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value_obs(x, y, theta,min_range,max_range, obs1, obs2) %sensor_distance(ENV) 2 | 3 | 4 | [L1_dis, R1_dis, F1_dis, detect_1] = sensor_value(x, y, theta,min_range,max_range, obs1); 5 | [L2_dis, R2_dis, F2_dis, detect_2] = sensor_value(x, y, theta,min_range,max_range, obs2); 6 | 7 | L_dis=min([L1_dis L2_dis]); 8 | R_dis=min([R1_dis R2_dis]); 9 | F_dis=min([F1_dis F2_dis]); 10 | if ((detect_1==1) | (detect_2==1)) 11 | detect=1; 12 | else 13 | detect=0; 14 | end 15 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/standalone controller/trianglar_fct.m: -------------------------------------------------------------------------------- 1 | % max(min((X-(a))/((b)-(a)),((c)-X)/((c)-(b))), 0) __a/b\c___ 2 | 3 | % min(max((X-a)/(b-a),0),1) % ____(a)/(b)''''''''' 4 | 5 | % max(min((b-X)/(b-a),1),0) % '''''''(a)\(d)____ 6 | function val=trianglar_fct(a,b,c,x,type) 7 | switch type 8 | case 'tria' 9 | val= max(min((x-(a))/((b)-(a)),((c)-x)/((c)-(b))),0); 10 | case 'end' 11 | val= min(max((x-a)/(b-a),0),1); 12 | case 'start' 13 | val= max(min((b-x)/(b-a),1),0); 14 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/Odometry.m: -------------------------------------------------------------------------------- 1 | function [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,robot_par ) 2 | R=robot_par(1); 3 | L=robot_par(2); 4 | xn_pos= xp_pos + sim_delta*(R/2*(w_l+w_r)*cos(thetap)); 5 | yn_pos= yp_pos + sim_delta*(R/2*(w_l+w_r)*sin(thetap)); 6 | %thetan=thetap + sim_delta*(R/L*(w_r-w_l)); 7 | theta_inf= thetap + sim_delta*(R/L*(w_r-w_l)); 8 | if (theta_inf<-2*pi) 9 | thetan = theta_inf+2*pi; 10 | else 11 | if(theta_inf> 2*pi) 12 | thetan = theta_inf-2*pi; 13 | else 14 | thetan = theta_inf; 15 | 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/sensors_pos.m: -------------------------------------------------------------------------------- 1 | function [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range) 2 | pt=min_range:0.03:max_range; 3 | 4 | % I made a mistake her concernig the direction 5 | R_sensor= [ x+pt*cos(-pi/3+theta); y+pt*sin(-pi/3+theta)]; 6 | L_sensor= [x+pt*cos(theta+pi/3); y+pt*sin(theta+pi/3)]; 7 | F_sensor= [x+pt*cos(theta);y+pt*sin(theta)]; 8 | 9 | %plot(R_sensor(1,:),R_sensor(2,:),'r',L_sensor(1,:),L_sensor(2,:),'b',F_sensor(1,:),F_sensor(2,:),'g'); 10 | %hold on 11 | end 12 | 13 | % R_sensor= [ x+pt*cos(theta); y+pt*sin(theta)]; 14 | % L_sensor= [x+pt*cos(theta+pi); y+pt*sin(theta+pi)]; 15 | % F_sensor= [x+pt*cos(theta+pi/2);y+pt*sin(theta+pi/2)]; -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/sensors_pos.m: -------------------------------------------------------------------------------- 1 | function [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range) 2 | pt=min_range:0.03:max_range; 3 | 4 | % I made a mistake her concernig the direction 5 | R_sensor= [ x+pt*cos(-pi/3+theta); y+pt*sin(-pi/3+theta)]; 6 | L_sensor= [x+pt*cos(theta+pi/3); y+pt*sin(theta+pi/3)]; 7 | F_sensor= [x+pt*cos(theta);y+pt*sin(theta)]; 8 | 9 | %plot(R_sensor(1,:),R_sensor(2,:),'r',L_sensor(1,:),L_sensor(2,:),'b',F_sensor(1,:),F_sensor(2,:),'g'); 10 | %hold on 11 | end 12 | 13 | % R_sensor= [ x+pt*cos(theta); y+pt*sin(theta)]; 14 | % L_sensor= [x+pt*cos(theta+pi); y+pt*sin(theta+pi)]; 15 | % F_sensor= [x+pt*cos(theta+pi/2);y+pt*sin(theta+pi/2)]; -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Reward_function1.m: -------------------------------------------------------------------------------- 1 | % the reward function 2 | % the reward is given with regards to the distance error and angle error 3 | % beteen the robot position and the orientation 4 | function [Reward, Episode_end]= Reward_function1(xp, yp, thetap, xn, yn, thetan, x_d, y_d) 5 | [d_errorp, theta_errorp]= Error(xp, yp, thetap, x_d, y_d); 6 | [d_errorn, theta_errorn]= Error(xn, yn, thetan, x_d, y_d); 7 | e_theta=abs(theta_errorn)-abs(theta_errorp); 8 | e_d=d_errorn-d_errorp; 9 | Episode_end=1; 10 | Reward=-1; 11 | if(e_d<=0 || e_theta<=0) 12 | Reward= Reward+2; 13 | else 14 | Reward= Reward-2; 15 | end 16 | 17 | 18 | if( d_errorn <= 0.03) 19 | Reward=10; 20 | Episode_end=0; 21 | end 22 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/Reward_function1.m: -------------------------------------------------------------------------------- 1 | % the reward function 2 | % the reward is given with regards to the distance error and angle error 3 | % beteen the robot position and the orientation 4 | function [Reward, Episode_end]= Reward_function1(xp, yp, thetap, xn, yn, thetan, x_d, y_d) 5 | [d_errorp, theta_errorp]= Error(xp, yp, thetap, x_d, y_d); 6 | [d_errorn, theta_errorn]= Error(xn, yn, thetan, x_d, y_d); 7 | e_theta=abs(theta_errorn)-abs(theta_errorp); 8 | e_d=d_errorn-d_errorp; 9 | Episode_end=1; 10 | Reward=-1; 11 | if(e_d<=0 || e_theta<=0) 12 | Reward= Reward+2; 13 | else 14 | Reward= Reward-2; 15 | end 16 | 17 | 18 | if( d_errorn <= 0.03) 19 | Reward=10; 20 | Episode_end=0; 21 | end 22 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/Reward_function_obs.m: -------------------------------------------------------------------------------- 1 | function [Reward, Episode_end]= Reward_function_obs(xp,yp,L_sensor_n, R_sensor_n, F_sensor_n,L_sensor_p, R_sensor_p, F_sensor_p, wall_u, wall_d) 2 | 3 | e_L=L_sensor_n-L_sensor_p; 4 | e_F=F_sensor_n-F_sensor_p; 5 | e_R=R_sensor_n-R_sensor_p; 6 | 7 | if(e_L>0 || e_F>0 || e_R>0) 8 | Reward=+1; 9 | else 10 | Reward=-1; 11 | end 12 | Episode_end=1; 13 | 14 | %% check if the robot has crushed 15 | r=0.1; 16 | pt=0:0.2:2*pi; 17 | x=xp+r*cos(pt); 18 | y=yp+r*sin(pt); 19 | robot=[x;y]; 20 | 21 | P_u = InterX(robot,wall_u); 22 | P_d = InterX(robot,wall_d); 23 | if(length(P_u)==0 && length(P_d)==0 ) 24 | Episode_end=1; 25 | else 26 | Reward=-10; 27 | Episode_end=0; 28 | end 29 | 30 | 31 | 32 | 33 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Reward_function2.m: -------------------------------------------------------------------------------- 1 | % the reward function 2 | % the reward is given with regards to the distance error and angle error 3 | % beteen the robot position and the orientation 4 | function [Reward, Episode_end]= Reward_function2(xp, yp, thetap, xn, yn, thetan, x_d, y_d,t) 5 | [d_errorp, theta_errorp]= Error(xp, yp, thetap, x_d, y_d); 6 | [d_errorn, theta_errorn]= Error(xn, yn, thetan, x_d, y_d); 7 | e_theta=abs(theta_errorn)-abs(theta_errorp); 8 | e_theta=wrapToPi(e_theta); % always do if we use this value for comparaison 9 | e_d=d_errorn-d_errorp; 10 | Episode_end=1; 11 | if(e_d<=0) 12 | Reward=1; 13 | elseif (e_theta<=0) % TODO : try to keep it more simple (only one case) 14 | Reward=0.5; 15 | else 16 | Reward=-1-t; % TODO : try to keep it more simple ( no t) 17 | end 18 | 19 | if( d_errorn <= 0.05) 20 | Reward=10; 21 | Episode_end=0; 22 | end 23 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/Diff_Robot_Model.m: -------------------------------------------------------------------------------- 1 | function [ w_l,w_r] = Diff_Robot_Model(wd_l, wd_r, sim_delta) 2 | %% DC Motor parameters 3 | J =0.01; b = 0.1; K = 0.01; R = 1; L = 0.5; 4 | 5 | A = [-b/J K/J ; -K/L -R/L]; 6 | B = [ 0 ; 1/L]; 7 | C = [ 1 , 0]; 8 | K_f=[-0.0100 3.0000]; 9 | 10 | Max_speed = 0.025; % (rad/s); 11 | 12 | 13 | % left motor 14 | xl1=[0;0]; xr1=[0;0]; 15 | persistent xl; 16 | if isempty(xl) 17 | xl=[0;0]; 18 | yl=C*xl; 19 | w_l=yl(1); 20 | else 21 | Ul= wd_l/Max_speed; 22 | ul = -K_f*xl1+Ul; 23 | xl=xl+sim_delta*(A*xl+B*ul); 24 | yl=C*xl; 25 | w_l=yl(1); 26 | end 27 | % right motor 28 | 29 | persistent xr; 30 | if isempty(xr) 31 | xr=[0;0]; 32 | yr=C*xr; 33 | w_r=yr(1); 34 | else 35 | Ur= wd_r/Max_speed; 36 | ur = -K_f*xr1+Ur; 37 | xr=xr+sim_delta*(A*xr+B*ur); 38 | yr=C*xr; 39 | w_r=yr(1); 40 | end 41 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/visualization_vect.m: -------------------------------------------------------------------------------- 1 | function visualization_vect(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,min_range,max_range, uwall, dwall) 2 | 3 | close all; 4 | i_final=length(vect_x_cur); 5 | t_max=vect_t(length(vect_t)); 6 | 7 | for i=1:5:i_final 8 | % the position and trajectory 9 | 10 | % plot the trajectory 11 | plot(vect_x_cur(1:i), vect_y_cur(1:i)); 12 | xlabel('X coordinates (m)'); 13 | ylabel('Y coordinates (m)') 14 | hold on 15 | 16 | % Robot plotting (x,y) the robot radius is 0.09 m 17 | rectangle('Position',[vect_x_cur(i)-0.09 vect_y_cur(i)-0.09 0.18 0.18]','Curvature',[1 1],'FaceColor','c'); 18 | 19 | % plot sensor 20 | [L_sensor R_sensor F_sensor]= sensors_pos(vect_x_cur(i),vect_y_cur(i),vect_theta_cur(i),min_range,max_range); 21 | plot(F_sensor(1,:),F_sensor(2,:),'g--',R_sensor(1,:),R_sensor(2,:),'g--',L_sensor(1,:),L_sensor(2,:),'g--'); 22 | 23 | % plot env 24 | plot(dwall(1,:),dwall(2,:),'b',uwall(1,:),uwall(2,:),'b'); 25 | %axis([x0-0.5 xf+0.5 y0-0.5 yf+0.5]); 26 | 27 | % plotting theta 28 | pt_rot=0:0.01:0.09; 29 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 3); 30 | axis equal 31 | %grid on 32 | %grid minor 33 | hold off; 34 | 35 | pause(0.00000000001) 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/visualization_vect.m: -------------------------------------------------------------------------------- 1 | function visualization_vect(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,min_range,max_range, uwall, dwall) 2 | 3 | close all; 4 | i_final=length(vect_x_cur); 5 | t_max=vect_t(length(vect_t)); 6 | 7 | for i=1:5:i_final 8 | % the position and trajectory 9 | 10 | % plot the trajectory 11 | plot(vect_x_cur(1:i), vect_y_cur(1:i)); 12 | xlabel('X coordinates (m)'); 13 | ylabel('Y coordinates (m)') 14 | hold on 15 | 16 | % Robot plotting (x,y) the robot radius is 0.09 m 17 | rectangle('Position',[vect_x_cur(i)-0.09 vect_y_cur(i)-0.09 0.18 0.18]','Curvature',[1 1],'FaceColor','c'); 18 | 19 | % plot sensor 20 | [L_sensor R_sensor F_sensor]= sensors_pos(vect_x_cur(i),vect_y_cur(i),vect_theta_cur(i),min_range,max_range); 21 | plot(F_sensor(1,:),F_sensor(2,:),'g--',R_sensor(1,:),R_sensor(2,:),'g--',L_sensor(1,:),L_sensor(2,:),'g--'); 22 | 23 | % plot env 24 | plot(dwall(1,:),dwall(2,:),'b',uwall(1,:),uwall(2,:),'b'); 25 | %axis([x0-0.5 xf+0.5 y0-0.5 yf+0.5]); 26 | 27 | % plotting theta 28 | pt_rot=0:0.01:0.09; 29 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 3); 30 | axis equal 31 | %grid on 32 | %grid minor 33 | hold off; 34 | 35 | pause(0.00000000001) 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/sensor_value_chamber.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value_chamber(x, y, theta,min_range,max_range,wall_d, wall_u, wall_l,wall_r, obs1, obs2, obs3, obs4) %sensor_distance(ENV) 2 | 3 | [L1_dis, R1_dis, F1_dis, detect_1] = sensor_value(x, y, theta,min_range,max_range, obs1); 4 | [L2_dis, R2_dis, F2_dis, detect_2] = sensor_value(x, y, theta,min_range,max_range, obs2); 5 | [L3_dis, R3_dis, F3_dis, detect_3] = sensor_value(x, y, theta,min_range,max_range, obs3); 6 | [L4_dis, R4_dis, F4_dis, detect_4] = sensor_value(x, y, theta,min_range,max_range, obs4); 7 | % 8 | [Ld_dis, Rd_dis, Fd_dis, detect_5] = sensor_value(x, y, theta,min_range,max_range, wall_d); 9 | [Lu_dis, Ru_dis, Fu_dis, detect_6] = sensor_value(x, y, theta,min_range,max_range, wall_u); 10 | [Ll_dis, Rl_dis, Fl_dis, detect_7] = sensor_value(x, y, theta,min_range,max_range, wall_l); 11 | [Lr_dis, Rr_dis, Fr_dis, detect_8] = sensor_value(x, y, theta,min_range,max_range, wall_r); 12 | 13 | L_dis=min([L1_dis L2_dis L3_dis L4_dis Ld_dis Lu_dis Ll_dis Lr_dis ]); 14 | R_dis=min([R1_dis R2_dis R3_dis R4_dis Rd_dis Ru_dis Rl_dis Rr_dis ]); 15 | F_dis=min([F1_dis F2_dis F3_dis F4_dis Fd_dis Fu_dis Fl_dis Fr_dis]); 16 | if ((detect_1==1) | (detect_2==1) | (detect_3==1) | (detect_4==1) | (detect_5==1) | (detect_6==1) | (detect_7==1) | (detect_8==1)) 17 | detect=1; 18 | else 19 | detect=0; 20 | end 21 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/sensor_value_chamber.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value_chamber(x, y, theta,min_range,max_range,wall_d, wall_u, wall_l,wall_r, obs1, obs2, obs3, obs4) %sensor_distance(ENV) 2 | 3 | [L1_dis, R1_dis, F1_dis, detect_1] = sensor_value(x, y, theta,min_range,max_range, obs1); 4 | [L2_dis, R2_dis, F2_dis, detect_2] = sensor_value(x, y, theta,min_range,max_range, obs2); 5 | [L3_dis, R3_dis, F3_dis, detect_3] = sensor_value(x, y, theta,min_range,max_range, obs3); 6 | [L4_dis, R4_dis, F4_dis, detect_4] = sensor_value(x, y, theta,min_range,max_range, obs4); 7 | % 8 | [Ld_dis, Rd_dis, Fd_dis, detect_5] = sensor_value(x, y, theta,min_range,max_range, wall_d); 9 | [Lu_dis, Ru_dis, Fu_dis, detect_6] = sensor_value(x, y, theta,min_range,max_range, wall_u); 10 | [Ll_dis, Rl_dis, Fl_dis, detect_7] = sensor_value(x, y, theta,min_range,max_range, wall_l); 11 | [Lr_dis, Rr_dis, Fr_dis, detect_8] = sensor_value(x, y, theta,min_range,max_range, wall_r); 12 | 13 | L_dis=min([L1_dis L2_dis L3_dis L4_dis Ld_dis Lu_dis Ll_dis Lr_dis ]); 14 | R_dis=min([R1_dis R2_dis R3_dis R4_dis Rd_dis Ru_dis Rl_dis Rr_dis ]); 15 | F_dis=min([F1_dis F2_dis F3_dis F4_dis Fd_dis Fu_dis Fl_dis Fr_dis]); 16 | if ((detect_1==1) | (detect_2==1) | (detect_3==1) | (detect_4==1) | (detect_5==1) | (detect_6==1) | (detect_7==1) | (detect_8==1)) 17 | detect=1; 18 | else 19 | detect=0; 20 | end 21 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/draw_PFE2.m: -------------------------------------------------------------------------------- 1 | function draw_PFE2(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,x_init,y_init) 2 | 3 | % draw the chamber 4 | xr=0; yr=0; rd=3 ; ru=4.2; rdp=2.8 ; rup=4.4; 5 | rectangle('Position',[xr-rup yr-rup 2*rup 2*rup]','Curvature',[1 1],'FaceColor','k'); % lower border 6 | hold on 7 | rectangle('Position',[xr-ru yr-ru 2*ru 2*ru]','Curvature',[1 1],'FaceColor','w'); 8 | hold on 9 | rectangle('Position',[xr-rd yr-rd 2*rd 2*rd]','Curvature',[1 1],'FaceColor','k'); 10 | hold on 11 | rectangle('Position',[xr-rdp yr-rdp 2*rdp 2*rdp]','Curvature',[1 1],'FaceColor','w'); 12 | hold on 13 | 14 | axis equal 15 | hold on 16 | 17 | % draw the initial positon and the final position 18 | rectangle('Position',[x_init-0.1 y_init-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','g'); 19 | hold on 20 | 21 | % draw the robot's trajectory and orientation 22 | for i=1:5:length(vect_t) 23 | 24 | % Robot plotting (x,y) the robot radius is 0.09 m 25 | % filled circle 26 | %rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 27 | % empty circle 28 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'EdgeColor','b','LineWidth',0.5) 29 | 30 | % orientation 31 | pt_rot=0:0.01:0.09; 32 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 1); 33 | hold on 34 | 35 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/draw_PFE2.m: -------------------------------------------------------------------------------- 1 | function draw_PFE2(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,x_init,y_init) 2 | 3 | % draw the chamber 4 | xr=0; yr=0; rd=3 ; ru=4.2; rdp=2.8 ; rup=4.4; 5 | rectangle('Position',[xr-rup yr-rup 2*rup 2*rup]','Curvature',[1 1],'FaceColor','k'); % lower border 6 | hold on 7 | rectangle('Position',[xr-ru yr-ru 2*ru 2*ru]','Curvature',[1 1],'FaceColor','w'); 8 | hold on 9 | rectangle('Position',[xr-rd yr-rd 2*rd 2*rd]','Curvature',[1 1],'FaceColor','k'); 10 | hold on 11 | rectangle('Position',[xr-rdp yr-rdp 2*rdp 2*rdp]','Curvature',[1 1],'FaceColor','w'); 12 | hold on 13 | 14 | axis equal 15 | hold on 16 | 17 | % draw the initial positon and the final position 18 | rectangle('Position',[x_init-0.1 y_init-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','g'); 19 | hold on 20 | 21 | % draw the robot's trajectory and orientation 22 | for i=1:5:length(vect_t) 23 | 24 | % Robot plotting (x,y) the robot radius is 0.09 m 25 | % filled circle 26 | %rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 27 | % empty circle 28 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'EdgeColor','b','LineWidth',0.5) 29 | 30 | % orientation 31 | pt_rot=0:0.01:0.09; 32 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 1); 33 | hold on 34 | 35 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/visualization_vect.m: -------------------------------------------------------------------------------- 1 | function visualization_vect(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t) 2 | 3 | close all; 4 | i_final=length(vect_x_cur); 5 | t_max=vect_t(length(vect_t)); 6 | 7 | for i=1:5:i_final 8 | % the position and trajectory 9 | subplot(1,2,1); 10 | % plot the trajectory 11 | plot(vect_x_cur(1:i), vect_y_cur(1:i)); 12 | axis([-1.2 1.2 -1.2 3.5]); 13 | xlabel('X coordinates (m)'); 14 | ylabel('Y coordinates (m)') 15 | hold on 16 | 17 | % Robot plotting (x,y) the robot radius is 0.09 m 18 | rectangle('Position',[vect_x_cur(i)-0.09 vect_y_cur(i)-0.09 0.18 0.18]','Curvature',[1 1],'FaceColor','c'); 19 | 20 | % direction polotting theta 21 | pt_rot=0:0.01:0.09; 22 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 3); 23 | axis equal 24 | grid on 25 | grid minor 26 | hold off; 27 | 28 | %% plot x(t) 29 | subplot(3,2,2); 30 | plot(vect_t(1:i), vect_x_cur(1:i)); 31 | axis([0 t_max 0 1.2]); 32 | xlabel('t (s)'); 33 | ylabel('x (m)') 34 | hold off 35 | 36 | %% plot y(t) 37 | subplot(3,2,4); 38 | plot(vect_t(1:i), vect_y_cur(1:i)); 39 | axis([0 t_max 0 1.2]); 40 | xlabel('t (s)'); 41 | ylabel('y (m)') 42 | hold off 43 | 44 | 45 | %% plot theta(t) 46 | 47 | subplot(3,2,6); 48 | plot(vect_t(1:i), vect_theta_cur(1:i)); 49 | axis([0 t_max -pi pi]); 50 | xlabel('t (s)'); 51 | ylabel('theta (m)') 52 | hold off 53 | 54 | pause(0.00000000001) 55 | end 56 | end 57 | -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/sensor_value.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value(x, y, theta,min_range,max_range, ENV) %sensor_distance(ENV) 2 | % the coorinates of the range of detection of the three sensors 3 | [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range); 4 | 5 | % intersection ENV with L_sensor 6 | P_L = InterX(ENV,L_sensor); 7 | if(length(P_L)==0) 8 | L_dis= max_range; 9 | else 10 | if(size(P_L)==[2 1]) 11 | L_dis=sqrt((P_L(1)-x)^2+(P_L(2)-y)^2); 12 | % in case there is 2 or more intersections 13 | else 14 | pt_1=sqrt((P_L(1,1)-x)^2+(P_L(2,1)-y)^2) 15 | pt_2=sqrt((P_L(2,1)-x)^2+(P_L(2,2)-y)^2) 16 | L_dis= min([pt_1 pt_2]); 17 | end 18 | end 19 | 20 | P_R = InterX(ENV,R_sensor); 21 | if(length(P_R)==0) 22 | R_dis= max_range; 23 | else 24 | if(size(P_R)==[2 1]) 25 | R_dis=sqrt((P_R(1)-x)^2+(P_R(2)-y)^2); 26 | % in case there is 2 or more intersections ( assigne a value ) 27 | else 28 | pt_1=sqrt((P_R(1,1)-x)^2+(P_R(2,1)-y)^2); 29 | pt_2=sqrt((P_R(2,1)-x)^2+(P_R(2,2)-y)^2); 30 | R_dis= min([pt_1 pt_2]); 31 | end 32 | end 33 | 34 | P_F = InterX(ENV,F_sensor); 35 | if(length(P_F)==0) 36 | F_dis= max_range; 37 | else 38 | if(size(P_F)==[2 1]) 39 | F_dis=sqrt((P_F(1)-x)^2+(P_F(2)-y)^2); 40 | % in case there is 2 or more intersections 41 | else 42 | pt_1=sqrt((P_F(1,1)-x)^2+(P_F(2,1)-y)^2); 43 | pt_2=sqrt((P_F(2,1)-x)^2+(P_F(2,2)-y)^2); 44 | F_dis= min([pt_1 pt_2]); 45 | end 46 | end 47 | 48 | if ( (F_dis==max_range) & (L_dis==max_range) & (R_dis==max_range)) 49 | detect=0; 50 | else 51 | detect=1; 52 | end 53 | 54 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/sensor_value.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value(x, y, theta,min_range,max_range, ENV) %sensor_distance(ENV) 2 | % the coorinates of the range of detection of the three sensors 3 | [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range); 4 | 5 | % intersection ENV with L_sensor 6 | P_L = InterX(ENV,L_sensor); 7 | if(length(P_L)==0) 8 | L_dis= max_range; 9 | else 10 | if(size(P_L)==[2 1]) 11 | L_dis=sqrt((P_L(1)-x)^2+(P_L(2)-y)^2); 12 | % in case there is 2 or more intersections 13 | else 14 | pt_1=sqrt((P_L(1,1)-x)^2+(P_L(2,1)-y)^2) 15 | pt_2=sqrt((P_L(2,1)-x)^2+(P_L(2,2)-y)^2) 16 | L_dis= min([pt_1 pt_2]); 17 | end 18 | end 19 | 20 | P_R = InterX(ENV,R_sensor); 21 | if(length(P_R)==0) 22 | R_dis= max_range; 23 | else 24 | if(size(P_R)==[2 1]) 25 | R_dis=sqrt((P_R(1)-x)^2+(P_R(2)-y)^2); 26 | % in case there is 2 or more intersections ( assigne a value ) 27 | else 28 | pt_1=sqrt((P_R(1,1)-x)^2+(P_R(2,1)-y)^2); 29 | pt_2=sqrt((P_R(2,1)-x)^2+(P_R(2,2)-y)^2); 30 | R_dis= min([pt_1 pt_2]); 31 | end 32 | end 33 | 34 | P_F = InterX(ENV,F_sensor); 35 | if(length(P_F)==0) 36 | F_dis= max_range; 37 | else 38 | if(size(P_F)==[2 1]) 39 | F_dis=sqrt((P_F(1)-x)^2+(P_F(2)-y)^2); 40 | % in case there is 2 or more intersections 41 | else 42 | pt_1=sqrt((P_F(1,1)-x)^2+(P_F(2,1)-y)^2); 43 | pt_2=sqrt((P_F(2,1)-x)^2+(P_F(2,2)-y)^2); 44 | F_dis= min([pt_1 pt_2]); 45 | end 46 | end 47 | 48 | if ( (F_dis==max_range) & (L_dis==max_range) & (R_dis==max_range)) 49 | detect=0; 50 | else 51 | detect=1; 52 | end 53 | 54 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Act_deg_conclusion_tab.m: -------------------------------------------------------------------------------- 1 | % return the activation degrees of the outputs 2 | % the encountered problem is that the conclusions changes over iteration 3 | % so we need a function to associate each rul with each conclusion every 4 | % time 5 | function [A_L_Z, A_L_M, A_L_H, A_R_Z, A_R_M, A_R_H]= Act_deg_conclusion_tab(Ad, Actions) 6 | 7 | NStates=20; 8 | A=Ad(:); 9 | 10 | Tab=zeros(NStates,2); 11 | % state 1 ( row order ) : A(1) actions(1) 12 | for i=1:20 13 | Tab(i,1)=A(i); 14 | Tab(i,2)=Actions(i); 15 | end 16 | 17 | % the conclusion L_Z exists the the actions 1 2 and 3 18 | vec=[]; 19 | for i=1:NStates 20 | vec=[vec Tab(i,1)*(Tab(i,2)==1 | Tab(i,2)==2 | Tab(i,2)==3)]; 21 | end 22 | A_L_Z=max(vec); 23 | 24 | % the conclusion L_M exists the the actions 4 5 and 6 25 | vec=[]; 26 | for i=1:NStates 27 | vec=[vec Tab(i,1)*(Tab(i,2)==4 | Tab(i,2)==5 | Tab(i,2)==6)]; 28 | end 29 | A_L_M=max(vec); 30 | 31 | % the conclusion L_H exists the the actions 7 8 and 9 32 | vec=[]; 33 | for i=1:NStates 34 | vec=[vec Tab(i,1)*(Tab(i,2)==7 | Tab(i,2)==8 | Tab(i,2)==9)]; 35 | end 36 | A_L_H=max(vec); 37 | 38 | % the conclusion R_Z exists the the actions 1 4 and 7 39 | vec=[]; 40 | for i=1:NStates 41 | vec=[vec Tab(i,1)*(Tab(i,2)==1 | Tab(i,2)==4 | Tab(i,2)==7)]; 42 | end 43 | A_R_Z=max(vec); 44 | 45 | % the conclusion R_M exists the the actions 2 5 and 8 46 | vec=[]; 47 | for i=1:NStates 48 | vec=[vec Tab(i,1)*(Tab(i,2)==2 | Tab(i,2)==5 | Tab(i,2)==8)]; 49 | end 50 | A_R_M=max(vec); 51 | 52 | % the conclusion R_H exists the the actions 3 6 and 9 53 | vec=[]; 54 | for i=1:NStates 55 | vec=[vec Tab(i,1)*(Tab(i,2)==3 | Tab(i,2)==6 | Tab(i,2)==9)]; 56 | end 57 | A_R_H=max(vec); 58 | 59 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Act_deg_conclusion_tab.m: -------------------------------------------------------------------------------- 1 | % return the activation degrees of the outputs 2 | % the encountered problem is that the conclusions changes over iteration 3 | % so we need a function to associate each rul with each conclusion every 4 | % time 5 | function [A_L_Z, A_L_M, A_L_H, A_R_Z, A_R_M, A_R_H]= Act_deg_conclusion_tab(Ad, Actions) 6 | 7 | NStates=20; 8 | A=Ad(:); 9 | 10 | Tab=zeros(NStates,2); 11 | % state 1 ( row order ) : A(1) actions(1) 12 | for i=1:20 13 | Tab(i,1)=A(i); 14 | Tab(i,2)=Actions(i); 15 | end 16 | 17 | % the conclusion L_Z exists the the actions 1 2 and 3 18 | vec=[]; 19 | for i=1:NStates 20 | vec=[vec Tab(i,1)*(Tab(i,2)==1 | Tab(i,2)==2 | Tab(i,2)==3)]; 21 | end 22 | A_L_Z=max(vec); 23 | 24 | % the conclusion L_M exists the the actions 4 5 and 6 25 | vec=[]; 26 | for i=1:NStates 27 | vec=[vec Tab(i,1)*(Tab(i,2)==4 | Tab(i,2)==5 | Tab(i,2)==6)]; 28 | end 29 | A_L_M=max(vec); 30 | 31 | % the conclusion L_H exists the the actions 7 8 and 9 32 | vec=[]; 33 | for i=1:NStates 34 | vec=[vec Tab(i,1)*(Tab(i,2)==7 | Tab(i,2)==8 | Tab(i,2)==9)]; 35 | end 36 | A_L_H=max(vec); 37 | 38 | % the conclusion R_Z exists the the actions 1 4 and 7 39 | vec=[]; 40 | for i=1:NStates 41 | vec=[vec Tab(i,1)*(Tab(i,2)==1 | Tab(i,2)==4 | Tab(i,2)==7)]; 42 | end 43 | A_R_Z=max(vec); 44 | 45 | % the conclusion R_M exists the the actions 2 5 and 8 46 | vec=[]; 47 | for i=1:NStates 48 | vec=[vec Tab(i,1)*(Tab(i,2)==2 | Tab(i,2)==5 | Tab(i,2)==8)]; 49 | end 50 | A_R_M=max(vec); 51 | 52 | % the conclusion R_H exists the the actions 3 6 and 9 53 | vec=[]; 54 | for i=1:NStates 55 | vec=[vec Tab(i,1)*(Tab(i,2)==3 | Tab(i,2)==6 | Tab(i,2)==9)]; 56 | end 57 | A_R_H=max(vec); 58 | 59 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/Rules_act_deg_obs.m: -------------------------------------------------------------------------------- 1 | function A=Rules_act_deg_obs(L_sensor, R_sensor, F_sensor) 2 | 3 | L_N= trianglar_fct( 0+0.1, 0.1+0.1 ,0 , L_sensor, 'start'); 4 | L_M= trianglar_fct( 0.05+0.1,0.15+0.1 ,0.25+0.1 , L_sensor, 'tria'); 5 | L_F= trianglar_fct( 0.25+0.1, 0.3+0.1 ,0 , L_sensor, 'end'); 6 | 7 | R_N= trianglar_fct( 0+0.1, 0.1+0.1 ,0 , R_sensor, 'start'); 8 | R_M= trianglar_fct( 0.05+0.1,0.15+0.1 ,0.25+0.1 , R_sensor, 'tria'); 9 | R_F= trianglar_fct( 0.25+0.1, 0.3+0.1 ,0 , R_sensor, 'end'); 10 | 11 | 12 | F_N= trianglar_fct( 0+0.1, 0.1+0.1 ,0 , F_sensor, 'start'); 13 | F_M= trianglar_fct( 0.05+0.1,0.15+0.1 ,0.25+0.1 , F_sensor, 'tria'); 14 | F_F= trianglar_fct( 0.25+0.1, 0.3+0.1 ,0 , F_sensor, 'end'); 15 | 16 | % activation degree 17 | Ad=zeros(27,1); 18 | Ad(1)=min([L_N F_N R_N]); 19 | Ad(2)=min([L_N F_N R_M]); 20 | Ad(3)=min([L_N F_N R_F]); 21 | Ad(4)=min([L_N F_M R_N]); 22 | Ad(5)=min([L_N F_M R_M]); 23 | Ad(6)=min([L_N F_M R_F]); 24 | Ad(7)=min([L_N F_F R_N]); 25 | Ad(8)=min([L_N F_F R_M]); 26 | Ad(9)=min([L_N F_F R_F]); 27 | 28 | Ad(10)=min([L_M F_N R_N]); 29 | Ad(11)=min([L_M F_N R_M]); 30 | Ad(12)=min([L_M F_N R_F]); 31 | Ad(13)=min([L_M F_M R_N]); 32 | Ad(14)=min([L_M F_M R_M]); 33 | Ad(15)=min([L_M F_M R_F]); 34 | Ad(16)=min([L_M F_F R_N]); 35 | Ad(17)=min([L_M F_F R_M]); 36 | Ad(18)=min([L_M F_F R_F]); 37 | 38 | Ad(19)=min([L_F F_N R_N]); 39 | Ad(20)=min([L_F F_N R_M]); 40 | Ad(21)=min([L_F F_N R_F]); 41 | Ad(22)=min([L_F F_M R_N]); 42 | Ad(23)=min([L_F F_M R_M]); 43 | Ad(24)=min([L_F F_M R_F]); 44 | Ad(25)=min([L_F F_F R_N]); 45 | Ad(26)=min([L_F F_F R_M]); 46 | Ad(27)=min([L_F F_F R_F]); 47 | 48 | A=Ad(:); 49 | 50 | 51 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/sensor_value.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value(x, y, theta,min_range,max_range, ENV) %sensor_distance(ENV) 2 | % the coorinates of the range of detection of the three sensors 3 | % the value returned is the distance between the robot position center and 4 | % the point of intersections so the fuzzy controller must be adjusted ( 5 | % x+min_range ..) 6 | 7 | [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range); 8 | 9 | % intersection ENV with L_sensor 10 | P_L = InterX(ENV,L_sensor); 11 | if(length(P_L)==0) 12 | L_dis= max_range; 13 | else 14 | if(size(P_L)==[2 1]) 15 | L_dis=sqrt((P_L(1)-x)^2+(P_L(2)-y)^2); 16 | % in case there is 2 or more intersections 17 | else 18 | pt_1=sqrt((P_L(1,1)-x)^2+(P_L(2,1)-y)^2) 19 | pt_2=sqrt((P_L(2,1)-x)^2+(P_L(2,2)-y)^2) 20 | L_dis= min([pt_1 pt_2]); 21 | end 22 | end 23 | 24 | P_R = InterX(ENV,R_sensor); 25 | if(length(P_R)==0) 26 | R_dis= max_range; 27 | else 28 | if(size(P_R)==[2 1]) 29 | R_dis=sqrt((P_R(1)-x)^2+(P_R(2)-y)^2); 30 | % in case there is 2 or more intersections ( assigne a value ) 31 | else 32 | pt_1=sqrt((P_R(1,1)-x)^2+(P_R(2,1)-y)^2); 33 | pt_2=sqrt((P_R(2,1)-x)^2+(P_R(2,2)-y)^2); 34 | R_dis= min([pt_1 pt_2]); 35 | end 36 | end 37 | 38 | P_F = InterX(ENV,F_sensor); 39 | if(length(P_F)==0) 40 | F_dis= max_range; 41 | else 42 | if(size(P_F)==[2 1]) 43 | F_dis=sqrt((P_F(1)-x)^2+(P_F(2)-y)^2); 44 | % in case there is 2 or more intersections 45 | else 46 | pt_1=sqrt((P_F(1,1)-x)^2+(P_F(2,1)-y)^2); 47 | pt_2=sqrt((P_F(2,1)-x)^2+(P_F(2,2)-y)^2); 48 | F_dis= min([pt_1 pt_2]); 49 | end 50 | end 51 | 52 | if ( (F_dis==max_range) & (L_dis==max_range) & (R_dis==max_range)) 53 | detect=0; 54 | else 55 | detect=1; 56 | end 57 | 58 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/sensor_value.m: -------------------------------------------------------------------------------- 1 | function [L_dis, R_dis, F_dis, detect] = sensor_value(x, y, theta,min_range,max_range, ENV) %sensor_distance(ENV) 2 | % the coorinates of the range of detection of the three sensors 3 | % the value returned is the distance between the robot position center and 4 | % the point of intersections so the fuzzy controller must be adjusted ( 5 | % x+min_range ..) 6 | 7 | [L_sensor R_sensor F_sensor]= sensors_pos(x,y,theta,min_range,max_range); 8 | 9 | % intersection ENV with L_sensor 10 | P_L = InterX(ENV,L_sensor); 11 | if(length(P_L)==0) 12 | L_dis= max_range; 13 | else 14 | if(size(P_L)==[2 1]) 15 | L_dis=sqrt((P_L(1)-x)^2+(P_L(2)-y)^2); 16 | % in case there is 2 or more intersections 17 | else 18 | pt_1=sqrt((P_L(1,1)-x)^2+(P_L(2,1)-y)^2) 19 | pt_2=sqrt((P_L(2,1)-x)^2+(P_L(2,2)-y)^2) 20 | L_dis= min([pt_1 pt_2]); 21 | end 22 | end 23 | 24 | P_R = InterX(ENV,R_sensor); 25 | if(length(P_R)==0) 26 | R_dis= max_range; 27 | else 28 | if(size(P_R)==[2 1]) 29 | R_dis=sqrt((P_R(1)-x)^2+(P_R(2)-y)^2); 30 | % in case there is 2 or more intersections ( assigne a value ) 31 | else 32 | pt_1=sqrt((P_R(1,1)-x)^2+(P_R(2,1)-y)^2); 33 | pt_2=sqrt((P_R(2,1)-x)^2+(P_R(2,2)-y)^2); 34 | R_dis= min([pt_1 pt_2]); 35 | end 36 | end 37 | 38 | P_F = InterX(ENV,F_sensor); 39 | if(length(P_F)==0) 40 | F_dis= max_range; 41 | else 42 | if(size(P_F)==[2 1]) 43 | F_dis=sqrt((P_F(1)-x)^2+(P_F(2)-y)^2); 44 | % in case there is 2 or more intersections 45 | else 46 | pt_1=sqrt((P_F(1,1)-x)^2+(P_F(2,1)-y)^2); 47 | pt_2=sqrt((P_F(2,1)-x)^2+(P_F(2,2)-y)^2); 48 | F_dis= min([pt_1 pt_2]); 49 | end 50 | end 51 | 52 | if ( (F_dis==max_range) & (L_dis==max_range) & (R_dis==max_range)) 53 | detect=0; 54 | else 55 | detect=1; 56 | end 57 | 58 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/MAIN.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all; 3 | clc; 4 | 5 | % robot parameters 6 | R = 0.035; L = 0.28; shape_r=0.1; 7 | min_range=0.1; max_range=0.4; 8 | 9 | % simulation parameters 10 | sim_delta = 0.1; 11 | t=0; 12 | t_final=200; 13 | 14 | % var for initialization 15 | x_init=0; y_init=3.6; theta_init=45*pi/180; 16 | xp_pos=x_init ;yp_pos=y_init; thetap=theta_init; 17 | 18 | % vectors for visualization 19 | vect_t=[]; 20 | vect_x=[]; 21 | vect_y=[]; 22 | vect_theta=[]; 23 | 24 | vect_L_dis=[]; 25 | vect_R_dis=[]; 26 | vect_F_dis=[]; 27 | 28 | 29 | % vector of the enivronment points 30 | [ wall_u wall_d] = environment_type2(); 31 | 32 | while(1) 33 | 34 | % returns the distances measured and detect(0 and 1 if there is obs ) 35 | [L_dis, R_dis, F_dis, detect] = sensor_value_obs(xp_pos, yp_pos, thetap,min_range,max_range, wall_u, wall_d) 36 | 37 | % vectors used for the animation 38 | vect_L_dis=[vect_L_dis L_dis]; 39 | vect_R_dis=[vect_R_dis R_dis]; 40 | vect_F_dis=[vect_F_dis F_dis]; 41 | 42 | if (detect == 0) 43 | w_lc=0.6; w_rc=0.6; 44 | else 45 | [w_lc, w_rc]= obstacle_avoidance_FLC_corr(L_dis, R_dis, F_dis); 46 | end 47 | 48 | % robot cmd input 49 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 50 | 51 | % odometry 52 | [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,[R L]); 53 | xp_pos=xn_pos ;yp_pos=yn_pos; thetap=thetan; 54 | 55 | % filling the plot vectors 56 | vect_t=[vect_t t]; 57 | vect_x=[vect_x xp_pos]; 58 | vect_y=[vect_y yp_pos]; 59 | vect_theta=[vect_theta thetap]; 60 | 61 | t=t+sim_delta; 62 | if(t>t_final) 63 | break 64 | end 65 | end 66 | 67 | figure(1) 68 | plot(vect_x,vect_y); title('robot position'); xlabel('x(m)'); ylabel('y(m)'); 69 | figure(2) 70 | draw_PFE2(vect_x, vect_y,vect_theta,vect_t,x_init,y_init); 71 | %pause() 72 | %visualization_vect(vect_x, vect_y,vect_theta, vect_t,min_range,max_range,wall_u, wall_d); -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/fuzzy_controller_RL.m: -------------------------------------------------------------------------------- 1 | function [W_Lc,W_Rc ]= fuzzy_controller_RL(d_error,theta_error,Actions) 2 | 3 | % fuzzyfication 4 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 5 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 6 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 7 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 8 | 9 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 10 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 11 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 12 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 13 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 14 | 15 | % activation degree 16 | Ad=zeros(4,5); 17 | Ad(1,1)=min([ D_Z theta_N]); 18 | Ad(1,2)=min([ D_Z theta_SN]); 19 | Ad(1,3)=min([ D_Z theta_Z]); 20 | Ad(1,4)=min([ D_Z theta_SP]); 21 | Ad(1,5)=min([ D_Z theta_P]); 22 | 23 | 24 | Ad(2,1)=min([ D_NZ theta_N]); 25 | Ad(2,2)=min([ D_NZ theta_SN]); 26 | Ad(2,3)=min([ D_NZ theta_Z]); 27 | Ad(2,4)=min([ D_NZ theta_SP]); 28 | Ad(2,5)=min([ D_NZ theta_P]); 29 | 30 | 31 | Ad(3,1)=min([ D_M theta_N]); 32 | Ad(3,2)=min([ D_M theta_SN]); 33 | Ad(3,3)=min([ D_M theta_Z]); 34 | Ad(3,4)=min([ D_M theta_SP]); 35 | Ad(3,5)=min([ D_M theta_P]); 36 | 37 | 38 | Ad(4,1)=min([ D_F theta_N]); 39 | Ad(4,2)=min([ D_F theta_SN]); 40 | Ad(4,3)=min([ D_F theta_Z]); 41 | Ad(4,4)=min([ D_F theta_SP]); 42 | Ad(4,5)=min([ D_F theta_P]); 43 | 44 | [A_L_Z, A_L_M, A_L_H, A_R_Z, A_R_M, A_R_H]=Act_deg_conclusion_tab(Ad, Actions); 45 | 46 | R_Z=0; 47 | R_M=1.5; 48 | R_H=4; 49 | 50 | 51 | L_Z= R_Z; 52 | L_M= R_M; 53 | L_H= R_H; 54 | 55 | 56 | W_Rc= (A_L_Z*L_Z + A_L_M*L_M + A_L_H*L_H )/3; 57 | W_Lc= (A_R_Z*R_Z + A_R_M*R_M + A_R_H*R_H )/3; 58 | 59 | 60 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/fuzzy_controller_RL.m: -------------------------------------------------------------------------------- 1 | function [W_Lc,W_Rc ]= fuzzy_controller_RL(d_error,theta_error,Actions) 2 | 3 | % fuzzyfication 4 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 5 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 6 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 7 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 8 | 9 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 10 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 11 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 12 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 13 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 14 | 15 | % activation degree 16 | Ad=zeros(4,5); 17 | Ad(1,1)=min([ D_Z theta_N]); 18 | Ad(1,2)=min([ D_Z theta_SN]); 19 | Ad(1,3)=min([ D_Z theta_Z]); 20 | Ad(1,4)=min([ D_Z theta_SP]); 21 | Ad(1,5)=min([ D_Z theta_P]); 22 | 23 | 24 | Ad(2,1)=min([ D_NZ theta_N]); 25 | Ad(2,2)=min([ D_NZ theta_SN]); 26 | Ad(2,3)=min([ D_NZ theta_Z]); 27 | Ad(2,4)=min([ D_NZ theta_SP]); 28 | Ad(2,5)=min([ D_NZ theta_P]); 29 | 30 | 31 | Ad(3,1)=min([ D_M theta_N]); 32 | Ad(3,2)=min([ D_M theta_SN]); 33 | Ad(3,3)=min([ D_M theta_Z]); 34 | Ad(3,4)=min([ D_M theta_SP]); 35 | Ad(3,5)=min([ D_M theta_P]); 36 | 37 | 38 | Ad(4,1)=min([ D_F theta_N]); 39 | Ad(4,2)=min([ D_F theta_SN]); 40 | Ad(4,3)=min([ D_F theta_Z]); 41 | Ad(4,4)=min([ D_F theta_SP]); 42 | Ad(4,5)=min([ D_F theta_P]); 43 | 44 | [A_L_Z, A_L_M, A_L_H, A_R_Z, A_R_M, A_R_H]=Act_deg_conclusion_tab(Ad, Actions); 45 | 46 | R_Z=0; 47 | R_M=1.5; 48 | R_H=4; 49 | 50 | 51 | L_Z= R_Z; 52 | L_M= R_M; 53 | L_H= R_H; 54 | 55 | 56 | W_Rc= (A_L_Z*L_Z + A_L_M*L_M + A_L_H*L_H )/3; 57 | W_Lc= (A_R_Z*R_Z + A_R_M*R_M + A_R_H*R_H )/3; 58 | 59 | 60 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/MAIN.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all; 3 | clc; 4 | 5 | % robot parameters 6 | R = 0.035; L = 0.28; 7 | 8 | % simulation parameters 9 | sim_delta = 0.1; 10 | t=0; 11 | t_final=100; 12 | 13 | % var for initialization 14 | x_init=0; y_init=0; theta_init=0; 15 | xp=x_init ;yp=y_init; thetap=theta_init; 16 | 17 | % vectors for plotting 18 | vect_t=[]; 19 | vect_wl=[]; 20 | vect_wr=[]; 21 | vect_x=[]; 22 | vect_y=[]; 23 | vect_theta=[]; 24 | 25 | % desired position 26 | x_d=5; y_d=0; 27 | 28 | 29 | while(1) 30 | % error calclation 31 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 32 | 33 | % fuzzy controller ( wheel speed) 34 | %[w_lc,w_rc ]= fuzzy_controller_simple(d_error,theta_error); 35 | [w_lc,w_rc ]= fuzzy_controller_w(d_error,theta_error); 36 | 37 | 38 | % robot cmd input 39 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 40 | 41 | 42 | % odometry 43 | [xn,yn,thetan]= Odometry(w_l, w_r, sim_delta,xp,yp, thetap,[R L]); 44 | xp=xn ;yp=yn; thetap=thetan; 45 | 46 | 47 | 48 | % filling the plot vectors 49 | vect_wl=[vect_wl w_l]; 50 | vect_wr=[vect_wr w_r]; 51 | vect_t=[vect_t t]; 52 | vect_x=[vect_x xp]; 53 | vect_y=[vect_y yp]; 54 | vect_theta=[vect_theta thetap]; 55 | 56 | t=t+sim_delta; 57 | if(t>t_final) 58 | break 59 | end 60 | end 61 | 62 | figure(1) 63 | subplot(2,1,1); plot(vect_t,vect_wr,'r'); title('speed conrol r'); xlabel('w'); ylabel('t'); 64 | subplot(2,1,2); plot(vect_t,vect_wl,'r'); title('speed conrol l'); xlabel('w'); ylabel('t'); 65 | 66 | figure(2) 67 | subplot(3,1,1); plot(vect_t,vect_x,'r'); title('position x'); xlabel('x(m)'); ylabel('t'); 68 | subplot(3,1,2); plot(vect_t,vect_y,'r'); title('positon y'); xlabel('y(m)'); ylabel('t'); 69 | subplot(3,1,3); plot(vect_t,vect_theta,'r'); title('angle theta'); xlabel('theta(rad)'); ylabel('t'); 70 | 71 | figure(3) 72 | plot(vect_x,vect_y); 73 | title('robot position'); 74 | xlabel('x(m)'); 75 | ylabel('y(m)'); 76 | 77 | %pause() 78 | %visualization_vect(vect_x, vect_y,vect_theta,vect_t) -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/Rules_act_deg.m: -------------------------------------------------------------------------------- 1 | % the function returns the order of the state according to 2 | % the table in the excel file 3 | function Rules_deg= Rules_act_deg(xp, yp, thetap, x_d, y_d) 4 | 5 | 6 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 7 | 8 | % D_error = {Z, NZ, M, F} 9 | % theta_error = {N, SN, Z, SP , P} 10 | % L = {Z, M, H} 11 | % R = {Z, M, H} 12 | 13 | % fuzzyfication 14 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 15 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 16 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 17 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 18 | 19 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 20 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 21 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 22 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 23 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 24 | 25 | 26 | % activation degree of the rules 27 | Ad=zeros(4,5); 28 | Ad(1,1)=min([ D_Z theta_N]); 29 | Ad(1,2)=min([ D_Z theta_SN]); 30 | Ad(1,3)=min([ D_Z theta_Z]); 31 | Ad(1,4)=min([ D_Z theta_SP]); 32 | Ad(1,5)=min([ D_Z theta_P]); 33 | 34 | 35 | Ad(2,1)=min([ D_NZ theta_N]); 36 | Ad(2,2)=min([ D_NZ theta_SN]); 37 | Ad(2,3)=min([ D_NZ theta_Z]); 38 | Ad(2,4)=min([ D_NZ theta_SP]); 39 | Ad(2,5)=min([ D_NZ theta_P]); 40 | 41 | 42 | Ad(3,1)=min([ D_M theta_N]); 43 | Ad(3,2)=min([ D_M theta_SN]); 44 | Ad(3,3)=min([ D_M theta_Z]); 45 | Ad(3,4)=min([ D_M theta_SP]); 46 | Ad(3,5)=min([ D_M theta_P]); 47 | 48 | 49 | Ad(4,1)=min([ D_F theta_N]); 50 | Ad(4,2)=min([ D_F theta_SN]); 51 | Ad(4,3)=min([ D_F theta_Z]); 52 | Ad(4,4)=min([ D_F theta_SP]); 53 | Ad(4,5)=min([ D_F theta_P]); 54 | 55 | Rules_deg=Ad(:); % Ad(:) is a vector in which the elements a11 a12 .. a1n a21 a22 .... a2n 56 | 57 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/Rules_act_deg.m: -------------------------------------------------------------------------------- 1 | % the function returns the order of the state according to 2 | % the table in the excel file 3 | function Rules_deg= Rules_act_deg(xp, yp, thetap, x_d, y_d) 4 | 5 | 6 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 7 | 8 | % D_error = {Z, NZ, M, F} 9 | % theta_error = {N, SN, Z, SP , P} 10 | % L = {Z, M, H} 11 | % R = {Z, M, H} 12 | 13 | % fuzzyfication 14 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 15 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 16 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 17 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 18 | 19 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 20 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 21 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 22 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 23 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 24 | 25 | 26 | % activation degree of the rules 27 | Ad=zeros(4,5); 28 | Ad(1,1)=min([ D_Z theta_N]); 29 | Ad(1,2)=min([ D_Z theta_SN]); 30 | Ad(1,3)=min([ D_Z theta_Z]); 31 | Ad(1,4)=min([ D_Z theta_SP]); 32 | Ad(1,5)=min([ D_Z theta_P]); 33 | 34 | 35 | Ad(2,1)=min([ D_NZ theta_N]); 36 | Ad(2,2)=min([ D_NZ theta_SN]); 37 | Ad(2,3)=min([ D_NZ theta_Z]); 38 | Ad(2,4)=min([ D_NZ theta_SP]); 39 | Ad(2,5)=min([ D_NZ theta_P]); 40 | 41 | 42 | Ad(3,1)=min([ D_M theta_N]); 43 | Ad(3,2)=min([ D_M theta_SN]); 44 | Ad(3,3)=min([ D_M theta_Z]); 45 | Ad(3,4)=min([ D_M theta_SP]); 46 | Ad(3,5)=min([ D_M theta_P]); 47 | 48 | 49 | Ad(4,1)=min([ D_F theta_N]); 50 | Ad(4,2)=min([ D_F theta_SN]); 51 | Ad(4,3)=min([ D_F theta_Z]); 52 | Ad(4,4)=min([ D_F theta_SP]); 53 | Ad(4,5)=min([ D_F theta_P]); 54 | 55 | Rules_deg=Ad(:); % Ad(:) is a vector in which the elements a11 a12 .. a1n a21 a22 .... a2n 56 | 57 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Rules_act_deg.m: -------------------------------------------------------------------------------- 1 | % the function returns the order of the state according to 2 | % the table in the excel file 3 | function Rules_deg= Rules_act_deg(xp, yp, thetap, x_d, y_d) 4 | 5 | 6 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 7 | 8 | % D_error = {Z, NZ, M, F} 9 | % theta_error = {N, SN, Z, SP , P} 10 | % L = {Z, M, H} 11 | % R = {Z, M, H} 12 | 13 | % fuzzyfication 14 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 15 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 16 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 17 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 18 | 19 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 20 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 21 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 22 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 23 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 24 | 25 | 26 | % activation degree of the rules 27 | Ad=zeros(4,5); 28 | Ad(1,1)=min([ D_Z theta_N]); 29 | Ad(1,2)=min([ D_Z theta_SN]); 30 | Ad(1,3)=min([ D_Z theta_Z]); 31 | Ad(1,4)=min([ D_Z theta_SP]); 32 | Ad(1,5)=min([ D_Z theta_P]); 33 | 34 | 35 | Ad(2,1)=min([ D_NZ theta_N]); 36 | Ad(2,2)=min([ D_NZ theta_SN]); 37 | Ad(2,3)=min([ D_NZ theta_Z]); 38 | Ad(2,4)=min([ D_NZ theta_SP]); 39 | Ad(2,5)=min([ D_NZ theta_P]); 40 | 41 | 42 | Ad(3,1)=min([ D_M theta_N]); 43 | Ad(3,2)=min([ D_M theta_SN]); 44 | Ad(3,3)=min([ D_M theta_Z]); 45 | Ad(3,4)=min([ D_M theta_SP]); 46 | Ad(3,5)=min([ D_M theta_P]); 47 | 48 | 49 | Ad(4,1)=min([ D_F theta_N]); 50 | Ad(4,2)=min([ D_F theta_SN]); 51 | Ad(4,3)=min([ D_F theta_Z]); 52 | Ad(4,4)=min([ D_F theta_SP]); 53 | Ad(4,5)=min([ D_F theta_P]); 54 | 55 | Rules_deg=Ad(:); % Ad(:) is a vector in which the elements a11 a12 .. a1n a21 a22 .... a2n 56 | 57 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fuzzy Q-Learning Robot 2 | **Authors :** [El Hacene Chabane](https://www.linkedin.com/in/el-hacene-chabane-9855b6123/), [Oussama Derouiche](https://www.linkedin.com/in/oussama-derouiche-aab0a4136/), [Zeryab Moussaoui](https://fr.linkedin.com/in/zeryab-moussaoui-9a728029) 3 | 4 | This project provides a proof of concept for Fuzzy Q Learning applied to mobile robots. 5 | Simulations and environment only require GNU Octave, this should also work on Matlab, but we advice you to use GNU Octave. 6 | The main propose is to be able to benchmark classical and Reinforced Fuzzy logics 7 | 8 | Warning : source code needs a serious refactoring. 9 | 10 | ![](./2.Q_fuzzy%20controller/Goal%20Reach/benchmark.png) 11 | 12 | 13 | ## Installation 14 | * Download the github project (zip) 15 | * Unzip and copy all files in your GNU Octave current directory. 16 | 17 | ## How to 18 | 19 | Multiple controllers for several environment can be executed : 20 | 21 | | Environment | Purpose | Controller | File to execute | 22 | | ------------- | ------------- | ------------- | ------------- | 23 | | Free | Goal Reach | FLC49 | [MAIN.m](./1.fuzzy%20controller/1.Goal%20Reach/MAIN.m") | 24 | | 4 walls | Obstacle Avoidance | FLC27 |[MAIN.m](./1.fuzzy%20controller/2.Obstacle%20avoidance/MAIN.m)| 25 | | 4 walls and 4 obstacles | Goal Reach and Obstacle Avoidance | FLC49/27 | [MAIN.m](./1.fuzzy%20controller/4.Goal%20Reach%20and%20Obstacle%20avoidance%20(%20chamber%20)/MAIN.m)| 26 | | Free | Learning Goal Reach | R-FLC20 | [MAIN_RL3_ZMO4.m](./2.Q_fuzzy%20controller/Goal%20Reach/20%20rules/1.Learning/MAIN_RL3_ZMO4.m)| 27 | | Free | Goal Reach | RL-FLC20 | [MAIN_test.m](./2.Q_fuzzy%20controller/Goal%20Reach/20%20rules/2.test%20itetrations/MAIN_test.m)| 28 | | Free | Learning Goal Reach | RL-FLC49 | [MAIN_RL3_ZMOH5.m](./2.Q_fuzzy%20controller/Goal%20Reach/Zer5_H%2049%20rule/MAIN_RL3_ZMOH5.m)| 29 | | 4 walls | Learning Obstacle Avoidance | RL-FLC27 | [MAIN_obs_RL1.m](./2.Q_fuzzy%20controller/Obstacle%20avoidance/version%202/MAIN_obs_RL1.m)| 30 | 31 | ## Related Publications: 32 | 33 | El Hacene Chabane, Oussama Derouiche. *National Polytechnical School of Algiers June 2018*. **Implementation of a Fuzzy Reinforcement Learning Control using 2D VSLAM** **[PDF (in french)](./presentation.pdf)** 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/MAIN.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | clc; 4 | 5 | % robot parameters 6 | R = 0.04; L = 0.15; 7 | min_range=0.1; max_range=0.4; 8 | 9 | % simulation parameters 10 | sim_delta = 0.1; 11 | t=0; 12 | t_final=200; 13 | 14 | % var for initialization 15 | x_init=0.5; y_init=0.5; theta_init=0*pi/180; 16 | 17 | xp_pos=x_init ;yp_pos=y_init; thetap=theta_init; % inilial position and orientation 18 | 19 | % vectors for plotting 20 | vect_x=[]; 21 | vect_y=[]; 22 | vect_theta=[]; 23 | vect_t=[]; 24 | 25 | % vector of the enivronment points 26 | [wall_d wall_u wall_l wall_r obs1 obs2 obs3 obs4 ]= obstacles_chamber(); 27 | 28 | % desired position 29 | x_d=4; y_d=4; 30 | 31 | 32 | while(1) 33 | 34 | % returns the distances measured and detect(0 and 1 if there is obs ) 35 | [L_dis, R_dis, F_dis, detect] = sensor_value_chamber(xp_pos, yp_pos, thetap,min_range,max_range, wall_d, wall_u, wall_l,wall_r, obs1, obs2, obs3, obs4); 36 | 37 | if (detect == 0) 38 | % error calclation 39 | [d_error, theta_error]= Error(xp_pos, yp_pos, thetap, x_d, y_d); 40 | 41 | % fuzzy controller 42 | [w_lc,w_rc ]= fuzzy_controller_w(d_error,theta_error); 43 | else 44 | [w_lc, w_rc]= obstacle_avoidance_FLC_corr(L_dis, R_dis, F_dis); 45 | end 46 | 47 | % robot cmd input 48 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 49 | 50 | % odometry 51 | [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,[R L]); 52 | xp_pos=xn_pos ;yp_pos=yn_pos; thetap=thetan; 53 | 54 | % filling the plot vectors 55 | vect_x=[vect_x xp_pos]; 56 | vect_y=[vect_y yp_pos]; 57 | vect_theta=[vect_theta thetap]; 58 | vect_t=[vect_t t]; 59 | 60 | 61 | t=t+sim_delta; 62 | if(t>t_final) 63 | break 64 | end 65 | end 66 | 67 | figure(1) 68 | plot(vect_x,vect_y); 69 | title('robot position'); 70 | xlabel('x(m)'); 71 | ylabel('y(m)'); 72 | 73 | draw_PFE3(vect_x, vect_y,vect_theta,vect_t,x_init,y_init) 74 | 75 | %pause() 76 | %visualization_vect(vect_x, vect_y,vect_theta,vect_t,min_range,max_range, obs1, obs2, obs3, obs4) 77 | 78 | -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/MAIN.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | clc; 4 | 5 | % robot parameters 6 | R = 0.04; L = 0.15; 7 | min_range=0.1; max_range=0.4; 8 | 9 | % simulation parameters 10 | sim_delta = 0.1; 11 | t=0; 12 | t_final=200; 13 | 14 | % var for initialization 15 | x_init=0.5; y_init=0.5; theta_init=0*pi/180; 16 | 17 | xp_pos=x_init ;yp_pos=y_init; thetap=theta_init; % inilial position and orientation 18 | 19 | % vectors for plotting 20 | vect_x=[]; 21 | vect_y=[]; 22 | vect_theta=[]; 23 | vect_t=[]; 24 | 25 | % vector of the enivronment points 26 | [wall_d wall_u wall_l wall_r obs1 obs2 obs3 obs4 ]= obstacles_chamber(); 27 | 28 | % desired position 29 | x_d=4; y_d=4; 30 | 31 | 32 | while(1) 33 | 34 | % returns the distances measured and detect(0 and 1 if there is obs ) 35 | [L_dis, R_dis, F_dis, detect] = sensor_value_chamber(xp_pos, yp_pos, thetap,min_range,max_range, wall_d, wall_u, wall_l,wall_r, obs1, obs2, obs3, obs4); 36 | 37 | if (detect == 0) 38 | % error calclation 39 | [d_error, theta_error]= Error(xp_pos, yp_pos, thetap, x_d, y_d); 40 | 41 | % fuzzy controller 42 | [w_lc,w_rc ]= fuzzy_controller_w(d_error,theta_error); 43 | else 44 | [w_lc, w_rc]= obstacle_avoidance_FLC_corr(L_dis, R_dis, F_dis); 45 | end 46 | 47 | % robot cmd input 48 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 49 | 50 | % odometry 51 | [xn_pos,yn_pos,thetan]= Odometry(w_l, w_r, sim_delta,xp_pos,yp_pos, thetap,[R L]); 52 | xp_pos=xn_pos ;yp_pos=yn_pos; thetap=thetan; 53 | 54 | % filling the plot vectors 55 | vect_x=[vect_x xp_pos]; 56 | vect_y=[vect_y yp_pos]; 57 | vect_theta=[vect_theta thetap]; 58 | vect_t=[vect_t t]; 59 | 60 | 61 | t=t+sim_delta; 62 | if(t>t_final) 63 | break 64 | end 65 | end 66 | 67 | figure(1) 68 | plot(vect_x,vect_y); 69 | title('robot position'); 70 | xlabel('x(m)'); 71 | ylabel('y(m)'); 72 | 73 | draw_PFE3(vect_x, vect_y,vect_theta,vect_t,x_init,y_init) 74 | 75 | %pause() 76 | %visualization_vect(vect_x, vect_y,vect_theta,vect_t,min_range,max_range, obs1, obs2, obs3, obs4) 77 | 78 | -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/draw_PFE3.m: -------------------------------------------------------------------------------- 1 | function draw_PFE3(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,x_init,y_init) 2 | 3 | % draw the chamber 4 | d=0.1; x0=0; xf=4.5; y0=0; yf=4.5; 5 | rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 6 | rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 7 | rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 8 | rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 9 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 10 | axis equal 11 | hold on 12 | 13 | % draw the initial positon and the final position 14 | rectangle('Position',[x_init-0.1 y_init-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','g'); 15 | hold on 16 | 17 | % draw obstacles 18 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 19 | %x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; %% r of the obstacles must be more than 0.2 20 | x_ob2=3.3; y_ob2=2.7; r_ob2=0.28; 21 | %x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 22 | x_ob3=2.2; y_ob3=1.5; r_ob3=0.28; 23 | x_ob4=2.1; y_ob4=2.7; r_ob4=0.25; 24 | 25 | rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 26 | hold on 27 | rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 28 | hold on 29 | rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 30 | hold on 31 | rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 32 | hold on 33 | 34 | % draw the robot trajectory 35 | for i=1:4:length(vect_t) 36 | 37 | % Robot plotting (x,y) the robot radius is 0.09 m 38 | % filled circle 39 | %rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 40 | % empty circle 41 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'EdgeColor','b','LineWidth',0.5) 42 | 43 | % orientation 44 | pt_rot=0:0.01:0.09; 45 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 1); 46 | hold on 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/draw_PFE3.m: -------------------------------------------------------------------------------- 1 | function draw_PFE3(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,x_init,y_init) 2 | 3 | % draw the chamber 4 | d=0.1; x0=0; xf=4.5; y0=0; yf=4.5; 5 | rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 6 | rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 7 | rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 8 | rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 9 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 10 | axis equal 11 | hold on 12 | 13 | % draw the initial positon and the final position 14 | rectangle('Position',[x_init-0.1 y_init-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','g'); 15 | hold on 16 | 17 | % draw obstacles 18 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 19 | %x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; %% r of the obstacles must be more than 0.2 20 | x_ob2=3.3; y_ob2=2.7; r_ob2=0.28; 21 | %x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 22 | x_ob3=2.2; y_ob3=1.5; r_ob3=0.28; 23 | x_ob4=2.1; y_ob4=2.7; r_ob4=0.25; 24 | 25 | rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 26 | hold on 27 | rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 28 | hold on 29 | rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 30 | hold on 31 | rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 32 | hold on 33 | 34 | % draw the robot trajectory 35 | for i=1:4:length(vect_t) 36 | 37 | % Robot plotting (x,y) the robot radius is 0.09 m 38 | % filled circle 39 | %rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 40 | % empty circle 41 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'EdgeColor','b','LineWidth',0.5) 42 | 43 | % orientation 44 | pt_rot=0:0.01:0.09; 45 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 1); 46 | hold on 47 | end 48 | 49 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/2.test itetrations/Q_FLC.m: -------------------------------------------------------------------------------- 1 | function [W_Lc,W_Rc ]=Q_FLC(d_error,theta_error, Conclusions) 2 | 3 | % fuzzyfication 4 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 5 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 6 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 7 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 8 | 9 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 10 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 11 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 12 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 13 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 14 | 15 | % activation degree 16 | Ad=zeros(4,5); 17 | Ad(1,1)=min([ D_Z theta_N]); 18 | Ad(1,2)=min([ D_Z theta_SN]); 19 | Ad(1,3)=min([ D_Z theta_Z]); 20 | Ad(1,4)=min([ D_Z theta_SP]); 21 | Ad(1,5)=min([ D_Z theta_P]); 22 | 23 | 24 | Ad(2,1)=min([ D_NZ theta_N]); 25 | Ad(2,2)=min([ D_NZ theta_SN]); 26 | Ad(2,3)=min([ D_NZ theta_Z]); 27 | Ad(2,4)=min([ D_NZ theta_SP]); 28 | Ad(2,5)=min([ D_NZ theta_P]); 29 | 30 | 31 | Ad(3,1)=min([ D_M theta_N]); 32 | Ad(3,2)=min([ D_M theta_SN]); 33 | Ad(3,3)=min([ D_M theta_Z]); 34 | Ad(3,4)=min([ D_M theta_SP]); 35 | Ad(3,5)=min([ D_M theta_P]); 36 | 37 | 38 | Ad(4,1)=min([ D_F theta_N]); 39 | Ad(4,2)=min([ D_F theta_SN]); 40 | Ad(4,3)=min([ D_F theta_Z]); 41 | Ad(4,4)=min([ D_F theta_SP]); 42 | Ad(4,5)=min([ D_F theta_P]); 43 | 44 | W_Rules_deg=Ad(:); 45 | 46 | % Action management 47 | 48 | R_Z=0; 49 | R_M=1.5; 50 | R_H=4; 51 | 52 | L_Z= R_Z; 53 | L_M= R_M; 54 | L_H= R_H; 55 | 56 | Actions=[R_Z L_Z ; R_Z L_M ; R_Z L_H ; R_M L_Z; R_M L_M ; R_M L_H ; R_H L_Z ; R_H L_M ; R_H L_H]; % each row corresponds to the action index 57 | R_Actions=Actions(:,1); 58 | L_Actions=Actions(:,2); 59 | 60 | %%% 61 | NRules=20; 62 | %%% 63 | selected_WR=zeros(NRules,1); % selected actions from each rules in an iteration 64 | selected_WL=zeros(NRules,1); 65 | 66 | selected_WR=R_Actions(Conclusions); 67 | selected_WL=L_Actions(Conclusions); 68 | 69 | W_Rc= selected_WR'*W_Rules_deg/3; % weigthed sum using scalar product 70 | W_Lc= selected_WL'*W_Rules_deg/3; 71 | 72 | 73 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/obstacles_chamber.m: -------------------------------------------------------------------------------- 1 | function [wall_d wall_u wall_l wall_r obs1 obs2 obs3 obs4 ]= obstacles_chamber() 2 | 3 | 4 | %% the borders 5 | x0=0; 6 | xf=4.5; 7 | y0=0; 8 | yf=4.5; 9 | delta=0.1; 10 | 11 | % the lower borders 12 | x_wall_d=x0:delta:xf; 13 | y_wall_d=y0*ones(1,length(x_wall_d)); 14 | wall_d=[x_wall_d;y_wall_d]; 15 | 16 | % the upper borders 17 | x_wall_u=x0:delta:xf; 18 | y_wall_u=yf*ones(1,length(x_wall_u)); 19 | wall_u=[x_wall_u;y_wall_u]; 20 | 21 | % the left wall 22 | y_wall_l= y0:0.1:yf; 23 | x_wall_l= x0*ones(1,length(y_wall_l)); 24 | wall_l=[x_wall_l;y_wall_l]; 25 | 26 | % the right wall 27 | y_wall_r=y0:0.1:yf; 28 | x_wall_r=xf*ones(1,length(y_wall_r)); 29 | wall_r=[x_wall_r; y_wall_r]; 30 | 31 | % plot 32 | plot(wall_d(1,:),wall_d(2,:),wall_u(1,:),wall_u(2,:),wall_r(1,:),wall_r(2,:),wall_l(1,:),wall_l(2,:)); 33 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 34 | 35 | %% obstacles 36 | pt=0:0.01:2*pi; % use 0.01 becuase there are some flaws when using 0.05 37 | 38 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 39 | %x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; 40 | x_ob2=3.3; y_ob2=2.7; r_ob2=0.28; 41 | %x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 42 | x_ob3=2.2; y_ob3=1.5; r_ob3=0.28; 43 | x_ob4=2.1; y_ob4=2.7; r_ob4=0.25; 44 | 45 | 46 | obs1=[x_ob1+r_ob1*cos(pt); y_ob1+r_ob1*sin(pt)]; 47 | obs2=[x_ob2+r_ob2*cos(pt); y_ob2+r_ob2*sin(pt)]; 48 | obs3=[x_ob3+r_ob3*cos(pt); y_ob3+r_ob3*sin(pt)]; 49 | obs4=[x_ob4+r_ob4*cos(pt); y_ob4+r_ob4*sin(pt)]; 50 | 51 | plot(obs1(1,:),obs1(2,:),obs2(1,:),obs2(2,:),obs3(1,:),obs3(2,:),obs4(1,:),obs4(2,:)); 52 | 53 | 54 | 55 | %% draw rectangles 56 | % borders 57 | d=0.1; 58 | % rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 59 | % rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 60 | % rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 61 | % rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 62 | 63 | % obstacles 64 | % rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 65 | % rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 66 | % rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 67 | % rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 68 | 69 | axis equal 70 | hold on 71 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/obstacles_chamber.m: -------------------------------------------------------------------------------- 1 | function [wall_d wall_u wall_l wall_r obs1 obs2 obs3 obs4 ]= obstacles_chamber() 2 | 3 | 4 | %% the borders 5 | x0=0; 6 | xf=4.5; 7 | y0=0; 8 | yf=4.5; 9 | delta=0.1; 10 | 11 | % the lower borders 12 | x_wall_d=x0:delta:xf; 13 | y_wall_d=y0*ones(1,length(x_wall_d)); 14 | wall_d=[x_wall_d;y_wall_d]; 15 | 16 | % the upper borders 17 | x_wall_u=x0:delta:xf; 18 | y_wall_u=yf*ones(1,length(x_wall_u)); 19 | wall_u=[x_wall_u;y_wall_u]; 20 | 21 | % the left wall 22 | y_wall_l= y0:0.1:yf; 23 | x_wall_l= x0*ones(1,length(y_wall_l)); 24 | wall_l=[x_wall_l;y_wall_l]; 25 | 26 | % the right wall 27 | y_wall_r=y0:0.1:yf; 28 | x_wall_r=xf*ones(1,length(y_wall_r)); 29 | wall_r=[x_wall_r; y_wall_r]; 30 | 31 | % plot 32 | plot(wall_d(1,:),wall_d(2,:),wall_u(1,:),wall_u(2,:),wall_r(1,:),wall_r(2,:),wall_l(1,:),wall_l(2,:)); 33 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 34 | 35 | %% obstacles 36 | pt=0:0.01:2*pi; % use 0.01 becuase there are some flaws when using 0.05 37 | 38 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 39 | %x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; 40 | x_ob2=3.3; y_ob2=2.7; r_ob2=0.28; 41 | %x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 42 | x_ob3=2.2; y_ob3=1.5; r_ob3=0.28; 43 | x_ob4=2.1; y_ob4=2.7; r_ob4=0.25; 44 | 45 | 46 | obs1=[x_ob1+r_ob1*cos(pt); y_ob1+r_ob1*sin(pt)]; 47 | obs2=[x_ob2+r_ob2*cos(pt); y_ob2+r_ob2*sin(pt)]; 48 | obs3=[x_ob3+r_ob3*cos(pt); y_ob3+r_ob3*sin(pt)]; 49 | obs4=[x_ob4+r_ob4*cos(pt); y_ob4+r_ob4*sin(pt)]; 50 | 51 | plot(obs1(1,:),obs1(2,:),obs2(1,:),obs2(2,:),obs3(1,:),obs3(2,:),obs4(1,:),obs4(2,:)); 52 | 53 | 54 | 55 | %% draw rectangles 56 | % borders 57 | d=0.1; 58 | % rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 59 | % rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 60 | % rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 61 | % rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 62 | 63 | % obstacles 64 | % rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 65 | % rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 66 | % rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 67 | % rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 68 | 69 | axis equal 70 | hold on 71 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/visualization_vect.m: -------------------------------------------------------------------------------- 1 | function visualization_vect(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,min_range,max_range, obs1, obs2, obs3, obs4) 2 | 3 | close all; 4 | i_final=length(vect_x_cur); 5 | t_max=vect_t(length(vect_t)); 6 | 7 | for i=1:5:i_final 8 | % the position and trajectory 9 | 10 | % plot the trajectory 11 | plot(vect_x_cur(1:i), vect_y_cur(1:i)); 12 | xlabel('X coordinates (m)'); 13 | ylabel('Y coordinates (m)') 14 | hold on 15 | 16 | % Robot plotting (x,y) the robot radius is 0.09 m 17 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 18 | 19 | % plot sensor 20 | [L_sensor R_sensor F_sensor]= sensors_pos(vect_x_cur(i),vect_y_cur(i),vect_theta_cur(i),min_range,max_range); 21 | plot(F_sensor(1,:),F_sensor(2,:),'g--',R_sensor(1,:),R_sensor(2,:),'g--',L_sensor(1,:),L_sensor(2,:),'g--'); 22 | 23 | % plot env 24 | plot(obs1(1,:),obs1(2,:),'b',obs2(1,:),obs2(2,:),'b',obs3(1,:),obs3(2,:),'b',obs4(1,:),obs4(2,:),'b'); 25 | 26 | 27 | % plotting theta 28 | pt_rot=0:0.01:0.09; 29 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 3); 30 | axis equal 31 | %grid on 32 | %grid minor 33 | hold off; 34 | 35 | % plot env 36 | d=0.1; 37 | x0=0; 38 | xf=4.5; 39 | y0=0; 40 | yf=4.5; 41 | rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 42 | rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 43 | rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 44 | rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 45 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 46 | 47 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 48 | x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; %% r of the obstacles must be more than 0.2 49 | x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 50 | x_ob4=2.3; y_ob4=3; r_ob4=0.25; 51 | 52 | rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 53 | rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 54 | rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 55 | rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 56 | pause(0.00000000001) 57 | end 58 | end 59 | -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/visualization_vect.m: -------------------------------------------------------------------------------- 1 | function visualization_vect(vect_x_cur, vect_y_cur,vect_theta_cur,vect_t,min_range,max_range, obs1, obs2, obs3, obs4) 2 | 3 | close all; 4 | i_final=length(vect_x_cur); 5 | t_max=vect_t(length(vect_t)); 6 | 7 | for i=1:5:i_final 8 | % the position and trajectory 9 | 10 | % plot the trajectory 11 | plot(vect_x_cur(1:i), vect_y_cur(1:i)); 12 | xlabel('X coordinates (m)'); 13 | ylabel('Y coordinates (m)') 14 | hold on 15 | 16 | % Robot plotting (x,y) the robot radius is 0.09 m 17 | rectangle('Position',[vect_x_cur(i)-0.1 vect_y_cur(i)-0.1 0.2 0.2]','Curvature',[1 1],'FaceColor','c'); 18 | 19 | % plot sensor 20 | [L_sensor R_sensor F_sensor]= sensors_pos(vect_x_cur(i),vect_y_cur(i),vect_theta_cur(i),min_range,max_range); 21 | plot(F_sensor(1,:),F_sensor(2,:),'g--',R_sensor(1,:),R_sensor(2,:),'g--',L_sensor(1,:),L_sensor(2,:),'g--'); 22 | 23 | % plot env 24 | plot(obs1(1,:),obs1(2,:),'b',obs2(1,:),obs2(2,:),'b',obs3(1,:),obs3(2,:),'b',obs4(1,:),obs4(2,:),'b'); 25 | 26 | 27 | % plotting theta 28 | pt_rot=0:0.01:0.09; 29 | plot(vect_x_cur(i)+pt_rot*cos(vect_theta_cur(i)),vect_y_cur(i)+pt_rot*sin(vect_theta_cur(i)),'r','LineWidth', 3); 30 | axis equal 31 | %grid on 32 | %grid minor 33 | hold off; 34 | 35 | % plot env 36 | d=0.1; 37 | x0=0; 38 | xf=4.5; 39 | y0=0; 40 | yf=4.5; 41 | rectangle('Position',[x0-d y0-d (yf-y0)+2*d d]','FaceColor','k'); % lower border 42 | rectangle('Position',[x0-d yf (yf-y0)+2*d d]','FaceColor','k'); % upper border 43 | rectangle('Position',[x0-d y0-d d (yf-y0)+2*d ]','FaceColor','k'); % left boder 44 | rectangle('Position',[xf y0-d d (yf-y0)+2*d ]','FaceColor','k'); % right border 45 | axis([x0-0.2 xf+0.2 y0-0.2 yf+0.2]); 46 | 47 | x_ob1=1; y_ob1=1.5; r_ob1=0.25; 48 | x_ob2=3.3; y_ob2=2.9; r_ob2=0.25; %% r of the obstacles must be more than 0.2 49 | x_ob3=2.2; y_ob3=1.7; r_ob3=0.25; 50 | x_ob4=2.3; y_ob4=3; r_ob4=0.25; 51 | 52 | rectangle('Position',[x_ob1-r_ob1 y_ob1-r_ob1 2*r_ob1 2*r_ob1]','Curvature',[1 1],'FaceColor','k'); 53 | rectangle('Position',[x_ob2-r_ob2 y_ob2-r_ob2 2*r_ob2 2*r_ob2]','Curvature',[1 1],'FaceColor','k'); 54 | rectangle('Position',[x_ob3-r_ob3 y_ob3-r_ob3 2*r_ob3 2*r_ob3]','Curvature',[1 1],'FaceColor','k'); 55 | rectangle('Position',[x_ob4-r_ob4 y_ob4-r_ob4 2*r_ob4 2*r_ob4]','Curvature',[1 1],'FaceColor','k'); 56 | pause(0.00000000001) 57 | end 58 | end 59 | -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/fuzzy_controller_simple.m: -------------------------------------------------------------------------------- 1 | %% Goal_reach fuzzy controller 2 | % D_error = {Z, NZ, M, F} 3 | % theta_error = {N, SN, Z, SP , P} 4 | % L = {Z, M, H} 5 | % R = {Z, M, H} 6 | 7 | function [W_Lc,W_Rc ]= fuzzy_controller_simple(d_error,theta_error) 8 | 9 | % fuzzyfication 10 | D_Z= trianglar_fct( 0.00, 0.02,0 , d_error, 'start'); 11 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , d_error, 'tria'); 12 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 13 | D_F= trianglar_fct( 0.7, 1 ,0 , d_error, 'end'); 14 | 15 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , theta_error, 'start'); 16 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , theta_error, 'tria'); 17 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , theta_error, 'tria'); % 1 -->1.5 18 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , theta_error, 'tria'); 19 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , theta_error, 'end'); 20 | 21 | % activation degree 22 | Ad=zeros(4,5); 23 | Ad(1,1)=min([ D_Z theta_N]); 24 | Ad(1,2)=min([ D_Z theta_SN]); 25 | Ad(1,3)=min([ D_Z theta_Z]); 26 | Ad(1,4)=min([ D_Z theta_SP]); 27 | Ad(1,5)=min([ D_Z theta_P]); 28 | 29 | 30 | Ad(2,1)=min([ D_NZ theta_N]); 31 | Ad(2,2)=min([ D_NZ theta_SN]); 32 | Ad(2,3)=min([ D_NZ theta_Z]); 33 | Ad(2,4)=min([ D_NZ theta_SP]); 34 | Ad(2,5)=min([ D_NZ theta_P]); 35 | 36 | 37 | Ad(3,1)=min([ D_M theta_N]); 38 | Ad(3,2)=min([ D_M theta_SN]); 39 | Ad(3,3)=min([ D_M theta_Z]); 40 | Ad(3,4)=min([ D_M theta_SP]); 41 | Ad(3,5)=min([ D_M theta_P]); 42 | 43 | 44 | Ad(4,1)=min([ D_F theta_N]); 45 | Ad(4,2)=min([ D_F theta_SN]); 46 | Ad(4,3)=min([ D_F theta_Z]); 47 | Ad(4,4)=min([ D_F theta_SP]); 48 | Ad(4,5)=min([ D_F theta_P]); 49 | 50 | 51 | % rules activation 52 | A_L_Z= max([ Ad(1,1) Ad(1,2) Ad(1,3)]) 53 | A_L_M= max([ Ad(1,4) Ad(1,5) Ad(2,1) Ad(2,2) Ad(2,3) Ad(3,1) Ad(3,2) Ad(3,3) Ad(4,1) Ad(4,2)]) 54 | A_L_H= max([ Ad(2,4) Ad(2,5) Ad(3,4) Ad(3,5) Ad(4,3) Ad(4,4) Ad(4,5)]) 55 | 56 | A_R_Z= max([ Ad(1,3) Ad(1,4) Ad(1,5)]); 57 | A_R_M= max([ Ad(1,1) Ad(1,2) Ad(2,3) Ad(2,4) Ad(2,5) Ad(3,3) Ad(3,4) Ad(3,5) Ad(4,4) Ad(4,5)]); 58 | A_R_H= max([ Ad(2,1) Ad(2,2) Ad(3,1) Ad(3,2) Ad(4,1) Ad(4,2) Ad(4,3)]); 59 | 60 | 61 | R_Z=0; 62 | R_M=1.5; 63 | R_H=4; 64 | 65 | 66 | L_Z= R_Z; 67 | L_M= R_M; 68 | L_H= R_H; 69 | 70 | 71 | W_Rc= (A_L_Z*L_Z + A_L_M*L_M + A_L_H*L_H )/3; 72 | W_Lc= (A_R_Z*R_Z + A_R_M*R_M + A_R_H*R_H )/3; 73 | 74 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/3.FLC vs Q_FLC/MAIN_FLC_VS_QFLC.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear; 3 | clc; 4 | 5 | %% Robot parameters 6 | R = 0.035; L = 0.28; 7 | 8 | %% the Policy 9 | load('qtable4000'); 10 | policy=Q_policy(q); 11 | 12 | % simulation parameters 13 | sim_delta = 0.1; 14 | t=0; 15 | t_final=300; 16 | 17 | % var for initialization 18 | x_init=0.0; y_init=0.0; theta_init=0; 19 | xp=x_init ;yp=y_init; thetap=theta_init; % inilial position and orientation 20 | 21 | % vectors for plotting 22 | vect_t=[]; 23 | % vectors for plotting the robot's coor unsing a FLC 24 | vect_x=[]; 25 | vect_y=[]; 26 | vect_theta=[]; 27 | 28 | % vectors for plotting the robot's coor unsing Q-FLC 29 | vect_x_r=[]; 30 | vect_y_r=[]; 31 | vect_theta_r=[]; 32 | % desired position 33 | x_d=1; y_d=-0.5; 34 | 35 | %% FLC 36 | while(1) 37 | % error calclation 38 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 39 | 40 | [w_lc,w_rc ]= fuzzy_controller_RL(d_error,theta_error,policy); 41 | 42 | % robot cmd input 43 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 44 | 45 | 46 | % odometry 47 | [xn,yn,thetan]= Odometry(w_l, w_r, sim_delta,xp,yp, thetap,[R L]); 48 | xp=xn ;yp=yn; thetap=thetan; 49 | 50 | % filling the plot vectors 51 | vect_t=[vect_t t]; 52 | vect_x=[vect_x xp]; 53 | vect_y=[vect_y yp]; 54 | vect_theta=[vect_theta thetap]; 55 | 56 | t=t+sim_delta; 57 | if(t>t_final) 58 | break 59 | end 60 | end 61 | 62 | %% Q-FLC 63 | t=0; 64 | xp=x_init ;yp=y_init; thetap=theta_init; 65 | 66 | while(1) 67 | % error calclation 68 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 69 | 70 | [w_lc,w_rc ]= fuzzy_controller_simple(d_error,theta_error); 71 | 72 | % robot cmd input 73 | [ w_l,w_r] = Diff_Robot_Model(w_lc, w_rc, sim_delta); 74 | 75 | 76 | % odometry 77 | [xn,yn,thetan]= Odometry(w_l, w_r, sim_delta,xp,yp, thetap,[R L]); 78 | xp=xn ;yp=yn; thetap=thetan; 79 | 80 | % filling the plot vectors 81 | vect_x_r=[vect_x_r xp]; 82 | vect_y_r=[vect_y_r yp]; 83 | vect_theta_r=[vect_theta_r thetap]; 84 | 85 | t=t+sim_delta; 86 | if(t>t_final) 87 | break 88 | end 89 | end 90 | 91 | figure(1) 92 | subplot(3,1,1); plot(vect_t,vect_x,'r',vect_t,vect_x_r,'b'); title('position x'); xlabel('t'); ylabel('x(m)');legend('Q-FLC','FLC'); 93 | subplot(3,1,2); plot(vect_t,vect_y,'r',vect_t,vect_y_r,'b'); title('positon y'); xlabel('t'); ylabel('y(m)'); 94 | subplot(3,1,3); plot(vect_t,vect_theta,'r',vect_t,vect_theta_r,'b'); title('angle theta'); xlabel('theta(rad)'); ylabel('t'); 95 | 96 | figure(2) 97 | plot(vect_x,vect_y,'r',vect_x_r,vect_y_r,'b'); title('robot position'); xlabel('x(m)'); ylabel('y(m)');legend('Q-FLC','FLC') 98 | -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/simple/membership function plot/FLC_w.m: -------------------------------------------------------------------------------- 1 | %% Goal_reach fuzzy controller 2 | % D_error = {Z, NZ, M, F} 3 | % theta_error = {N, SN, Z, SP , P} 4 | % L = {Z, M, H} 5 | % R = {Z, M, H} 6 | 7 | clear 8 | clc 9 | 10 | 11 | %% plot the membership functions of the output 12 | % D_error 13 | vect_D_Z= []; 14 | vect_D_NZ= []; 15 | vect_D_M= []; 16 | vect_D_F= []; 17 | 18 | % theta error 19 | vect_theta_N=[]; 20 | vect_theta_SN=[]; 21 | vect_theta_Z=[]; 22 | vect_theta_SP=[]; 23 | vect_theta_P=[]; 24 | 25 | 26 | vect_td=[]; 27 | vect_ta=[]; 28 | for td=0:0.01:2 29 | 30 | D_Z= trianglar_fct( 0.00, 0.02,0 , td, 'start'); 31 | D_NZ= trianglar_fct( 0.01 ,0.15,0.3 , td, 'tria'); 32 | D_M= trianglar_fct( 0.25,0.5 ,0.75 , td, 'tria'); 33 | D_F= trianglar_fct( 0.7, 1 ,0 , td, 'end'); 34 | 35 | 36 | 37 | 38 | vect_D_Z = [vect_D_Z D_Z ]; 39 | vect_D_NZ= [vect_D_NZ D_NZ]; 40 | vect_D_M= [vect_D_M D_M ]; 41 | vect_D_F= [vect_D_F D_F ]; 42 | 43 | vect_td=[vect_td td]; 44 | end 45 | 46 | for ta=-90:0.1:90 47 | vect_ta=[vect_ta ta]; 48 | % deg -> 49 | ta=ta*(pi/180); 50 | 51 | theta_N= trianglar_fct( -30*(pi/180), -10*(pi/180),0 , ta, 'start'); 52 | theta_SN= trianglar_fct( -15*(pi/180), -4*(pi/180), -1*(pi/180) , ta, 'tria'); 53 | theta_Z= trianglar_fct( -1.5*(pi/180), 0, 1.5*(pi/180) , ta, 'tria'); % 1 -->1.5 54 | theta_SP= trianglar_fct( 1*(pi/180), 4*(pi/180), 15*(pi/180) , ta, 'tria'); 55 | theta_P= trianglar_fct( 10*(pi/180), 30*(pi/180), 0 , ta, 'end'); 56 | 57 | vect_theta_N=[vect_theta_N theta_N]; 58 | vect_theta_SN=[vect_theta_SN theta_SN]; 59 | vect_theta_Z=[vect_theta_Z theta_Z]; 60 | vect_theta_SP=[vect_theta_SP theta_SP]; 61 | vect_theta_P=[vect_theta_P theta_P]; 62 | 63 | 64 | end 65 | 66 | 67 | % theta_error = {N, SN, Z, SP , P} 68 | 69 | figure(1) 70 | plot(vect_td, vect_D_Z, vect_td, vect_D_NZ,'k',vect_td, vect_D_M, vect_td, vect_D_F) 71 | legend('Z','NZ','M','F'); 72 | axis([0 2 0 1.2]); 73 | xlabel('distance error (m)') 74 | 75 | 76 | figure(2) 77 | plot(vect_ta,vect_theta_N,vect_ta, vect_theta_SN,vect_ta, vect_theta_Z,vect_ta,vect_theta_SP,vect_ta,vect_theta_P) 78 | legend('N','SN','Z','SP','P'); 79 | axis([-90 90 0 1.2]) 80 | xlabel('Angle error') 81 | 82 | %% plot the membership functions of the output 83 | R_Z=0; 84 | R_M=2; 85 | R_H=4; 86 | 87 | 88 | L_Z= R_Z; 89 | L_M= R_M; 90 | L_H= R_H; 91 | 92 | 93 | y=0:0.1:1; 94 | x=ones(1,length(y)); 95 | 96 | figure(3) 97 | plot(R_Z.*x, y,R_M.*x,y, R_H.*x, y); 98 | legend('Z','M','H'); 99 | axis([-0.1 16 0 1.2]); 100 | xlabel(' right wheel velocity membership functions ') 101 | 102 | figure(4) 103 | plot(L_Z.*x, y, L_M.*x, y, L_H.*x, y); 104 | legend('Z','M','H'); 105 | axis([-0.1 16 0 1.2]); 106 | xlabel(' left wheel velocity membership functions ') 107 | 108 | -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/Zer5_H 49 rule/Rules_act_deg_49.m: -------------------------------------------------------------------------------- 1 | function Rules_deg= Rules_act_deg_49(xp, yp, thetap, x_d, y_d) 2 | 3 | [d_error, theta_error]= Error(xp, yp, thetap, x_d, y_d); 4 | 5 | % fuzzyfication 6 | D_Z= trianglar_fct( 0, 0.05,0 , d_error, 'start'); 7 | D_NZ= trianglar_fct( 0.01,0.25,0.5 , d_error, 'tria'); 8 | D_P= trianglar_fct( 0.25,0.5 ,0.75 , d_error, 'tria'); 9 | D_M= trianglar_fct( 0.5,0.75 ,1 , d_error, 'tria'); 10 | D_NF= trianglar_fct( 0.75,1,1.25 , d_error, 'tria'); 11 | D_F= trianglar_fct( 1 , 1.25, 1.5 , d_error, 'tria'); 12 | D_VF= trianglar_fct( 1.25,1.5 ,0 , d_error, 'end'); 13 | 14 | theta_N= trianglar_fct( -60*(pi/180), -40*(pi/180),0 , theta_error, 'start'); 15 | theta_SN= trianglar_fct( -50*(pi/180), -30*(pi/180), -15*(pi/180) , theta_error, 'tria'); 16 | theta_NNZ= trianglar_fct( -20*(pi/180), -5*(pi/180), -0.2*(pi/180) , theta_error, 'tria'); 17 | theta_Z= trianglar_fct( -2*(pi/180), 0, 2*(pi/180) , theta_error, 'tria'); 18 | theta_NPZ= trianglar_fct( 0.2*(pi/180), 5*(pi/180), 20*(pi/180) , theta_error, 'tria'); 19 | theta_SP= trianglar_fct( 15*(pi/180), 30*(pi/180), 50*(pi/180) , theta_error, 'tria'); 20 | theta_P= trianglar_fct( 40*(pi/180), 60*(pi/180), 0 , theta_error, 'end'); 21 | 22 | % activation degree rules 23 | Ad=zeros(7,7); 24 | Ad(1,1)=min([D_Z theta_N]); 25 | Ad(1,2)=min([D_Z theta_SN]); 26 | Ad(1,3)=min([D_Z theta_NNZ]); 27 | Ad(1,4)=min([D_Z theta_Z]); 28 | Ad(1,5)=min([D_Z theta_NPZ]); 29 | Ad(1,6)=min([D_Z theta_SP]); 30 | Ad(1,7)=min([D_Z theta_P]); 31 | 32 | Ad(2,1)=min([D_NZ theta_N]); 33 | Ad(2,2)=min([D_NZ theta_SN ]); 34 | Ad(2,3)=min([D_NZ theta_NNZ]); 35 | Ad(2,4)=min([D_NZ theta_Z]); 36 | Ad(2,5)=min([D_NZ theta_NPZ]); 37 | Ad(2,6)=min([D_NZ theta_SP]); 38 | Ad(2,7)=min([D_NZ theta_P]); 39 | 40 | Ad(3,1)=min([D_P theta_N]); 41 | Ad(3,2)=min([D_P theta_SN]); 42 | Ad(3,3)=min([D_P theta_NNZ]); 43 | Ad(3,4)=min([D_P theta_Z]); 44 | Ad(3,5)=min([D_P theta_NPZ]); 45 | Ad(3,6)=min([D_P theta_SP]); 46 | Ad(3,7)=min([D_P theta_P]); 47 | 48 | Ad(4,1)=min([D_M theta_N]); 49 | Ad(4,2)=min([D_M theta_SN]); 50 | Ad(4,3)=min([D_M theta_NNZ]); 51 | Ad(4,4)=min([D_M theta_Z]); 52 | Ad(4,5)=min([D_M theta_NPZ]); 53 | Ad(4,6)=min([D_M theta_SP]); 54 | Ad(4,7)=min([D_M theta_P]); 55 | 56 | Ad(5,1)=min([D_NF theta_N]); 57 | Ad(5,2)=min([D_NF theta_SN]); 58 | Ad(5,3)=min([D_NF theta_NNZ]); 59 | Ad(5,4)=min([D_NF theta_Z]); 60 | Ad(5,5)=min([D_NF theta_NPZ]); 61 | Ad(5,6)=min([D_NF theta_SP]); 62 | Ad(5,7)=min([D_NF theta_P]); 63 | 64 | Ad(6,1)=min([D_F theta_N]); 65 | Ad(6,2)=min([D_F theta_SN]); 66 | Ad(6,3)=min([D_F theta_NNZ]); 67 | Ad(6,4)=min([D_F theta_Z]); 68 | Ad(6,5)=min([D_F theta_NPZ]); 69 | Ad(6,6)=min([D_F theta_SP]); 70 | Ad(6,7)=min([D_F theta_P]); 71 | 72 | Ad(7,1)=min([D_VF theta_N]); 73 | Ad(7,2)=min([D_VF theta_SN]); 74 | Ad(7,3)=min([D_VF theta_NNZ]); 75 | Ad(7,4)=min([D_VF theta_Z]); 76 | Ad(7,5)=min([D_VF theta_NPZ]); 77 | Ad(7,6)=min([D_VF theta_SP]); 78 | Ad(7,7)=min([D_VF theta_P]); 79 | 80 | Rules_deg=Ad(:); 81 | 82 | end -------------------------------------------------------------------------------- /1.fuzzy controller/2.Obstacle avoidance/InterX.m: -------------------------------------------------------------------------------- 1 | function P = InterX(L1,varargin) 2 | %INTERX Intersection of curves 3 | % P = INTERX(L1,L2) returns the intersection points of two curves L1 4 | % and L2. The curves L1,L2 can be either closed or open and are described 5 | % by two-row-matrices, where each row contains its x- and y- coordinates. 6 | % The intersection of groups of curves (e.g. contour lines, multiply 7 | % connected regions etc) can also be computed by separating them with a 8 | % column of NaNs as for example 9 | % 10 | % L = [x11 x12 x13 ... NaN x21 x22 x23 ...; 11 | % y11 y12 y13 ... NaN y21 y22 y23 ...] 12 | % 13 | % P has the same structure as L1 and L2, and its rows correspond to the 14 | % x- and y- coordinates of the intersection points of L1 and L2. If no 15 | % intersections are found, the returned P is empty. 16 | % 17 | % P = INTERX(L1) returns the self-intersection points of L1. To keep 18 | % the code simple, the points at which the curve is tangent to itself are 19 | % not included. P = INTERX(L1,L1) returns all the points of the curve 20 | % together with any self-intersection points. 21 | % 22 | % Example: 23 | % t = linspace(0,2*pi); 24 | % r1 = sin(4*t)+2; x1 = r1.*cos(t); y1 = r1.*sin(t); 25 | % r2 = sin(8*t)+2; x2 = r2.*cos(t); y2 = r2.*sin(t); 26 | % P = InterX([x1;y1],[x2;y2]); 27 | % plot(x1,y1,x2,y2,P(1,:),P(2,:),'ro') 28 | 29 | % Author : NS 30 | % Version: 3.0, 21 Sept. 2010 31 | 32 | % Two words about the algorithm: Most of the code is self-explanatory. 33 | % The only trick lies in the calculation of C1 and C2. To be brief, this 34 | % is essentially the two-dimensional analog of the condition that needs 35 | % to be satisfied by a function F(x) that has a zero in the interval 36 | % [a,b], namely 37 | % F(a)*F(b) <= 0 38 | % C1 and C2 exactly do this for each segment of curves 1 and 2 39 | % respectively. If this condition is satisfied simultaneously for two 40 | % segments then we know that they will cross at some point. 41 | % Each factor of the 'C' arrays is essentially a matrix containing 42 | % the numerators of the signed distances between points of one curve 43 | % and line segments of the other. 44 | 45 | %...Argument checks and assignment of L2 46 | error(nargchk(1,2,nargin)); 47 | if nargin == 1, 48 | L2 = L1; hF = @lt; %...Avoid the inclusion of common points 49 | else 50 | L2 = varargin{1}; hF = @le; 51 | end 52 | 53 | %...Preliminary stuff 54 | x1 = L1(1,:)'; x2 = L2(1,:); 55 | y1 = L1(2,:)'; y2 = L2(2,:); 56 | dx1 = diff(x1); dy1 = diff(y1); 57 | dx2 = diff(x2); dy2 = diff(y2); 58 | 59 | %...Determine 'signed distances' 60 | S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1); 61 | S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1); 62 | 63 | C1 = feval(hF,D(bsxfun(@times,dx1,y2)-bsxfun(@times,dy1,x2),S1),0); 64 | C2 = feval(hF,D((bsxfun(@times,y1,dx2)-bsxfun(@times,x1,dy2))',S2'),0)'; 65 | 66 | %...Obtain the segments where an intersection is expected 67 | [i,j] = find(C1 & C2); 68 | if isempty(i),P = zeros(2,0);return; end; 69 | 70 | %...Transpose and prepare for output 71 | i=i'; dx2=dx2'; dy2=dy2'; S2 = S2'; 72 | L = dy2(j).*dx1(i) - dy1(i).*dx2(j); 73 | i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0 74 | 75 | %...Solve system of eqs to get the common points 76 | P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ... 77 | dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')'; 78 | 79 | function u = D(x,y) 80 | u = bsxfun(@minus,x(:,1:end-1),y).*bsxfun(@minus,x(:,2:end),y); 81 | end 82 | end -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Obstacle avoidance/version 2/InterX.m: -------------------------------------------------------------------------------- 1 | function P = InterX(L1,varargin) 2 | %INTERX Intersection of curves 3 | % P = INTERX(L1,L2) returns the intersection points of two curves L1 4 | % and L2. The curves L1,L2 can be either closed or open and are described 5 | % by two-row-matrices, where each row contains its x- and y- coordinates. 6 | % The intersection of groups of curves (e.g. contour lines, multiply 7 | % connected regions etc) can also be computed by separating them with a 8 | % column of NaNs as for example 9 | % 10 | % L = [x11 x12 x13 ... NaN x21 x22 x23 ...; 11 | % y11 y12 y13 ... NaN y21 y22 y23 ...] 12 | % 13 | % P has the same structure as L1 and L2, and its rows correspond to the 14 | % x- and y- coordinates of the intersection points of L1 and L2. If no 15 | % intersections are found, the returned P is empty. 16 | % 17 | % P = INTERX(L1) returns the self-intersection points of L1. To keep 18 | % the code simple, the points at which the curve is tangent to itself are 19 | % not included. P = INTERX(L1,L1) returns all the points of the curve 20 | % together with any self-intersection points. 21 | % 22 | % Example: 23 | % t = linspace(0,2*pi); 24 | % r1 = sin(4*t)+2; x1 = r1.*cos(t); y1 = r1.*sin(t); 25 | % r2 = sin(8*t)+2; x2 = r2.*cos(t); y2 = r2.*sin(t); 26 | % P = InterX([x1;y1],[x2;y2]); 27 | % plot(x1,y1,x2,y2,P(1,:),P(2,:),'ro') 28 | 29 | % Author : NS 30 | % Version: 3.0, 21 Sept. 2010 31 | 32 | % Two words about the algorithm: Most of the code is self-explanatory. 33 | % The only trick lies in the calculation of C1 and C2. To be brief, this 34 | % is essentially the two-dimensional analog of the condition that needs 35 | % to be satisfied by a function F(x) that has a zero in the interval 36 | % [a,b], namely 37 | % F(a)*F(b) <= 0 38 | % C1 and C2 exactly do this for each segment of curves 1 and 2 39 | % respectively. If this condition is satisfied simultaneously for two 40 | % segments then we know that they will cross at some point. 41 | % Each factor of the 'C' arrays is essentially a matrix containing 42 | % the numerators of the signed distances between points of one curve 43 | % and line segments of the other. 44 | 45 | %...Argument checks and assignment of L2 46 | error(nargchk(1,2,nargin)); 47 | if nargin == 1, 48 | L2 = L1; hF = @lt; %...Avoid the inclusion of common points 49 | else 50 | L2 = varargin{1}; hF = @le; 51 | end 52 | 53 | %...Preliminary stuff 54 | x1 = L1(1,:)'; x2 = L2(1,:); 55 | y1 = L1(2,:)'; y2 = L2(2,:); 56 | dx1 = diff(x1); dy1 = diff(y1); 57 | dx2 = diff(x2); dy2 = diff(y2); 58 | 59 | %...Determine 'signed distances' 60 | S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1); 61 | S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1); 62 | 63 | C1 = feval(hF,D(bsxfun(@times,dx1,y2)-bsxfun(@times,dy1,x2),S1),0); 64 | C2 = feval(hF,D((bsxfun(@times,y1,dx2)-bsxfun(@times,x1,dy2))',S2'),0)'; 65 | 66 | %...Obtain the segments where an intersection is expected 67 | [i,j] = find(C1 & C2); 68 | if isempty(i),P = zeros(2,0);return; end; 69 | 70 | %...Transpose and prepare for output 71 | i=i'; dx2=dx2'; dy2=dy2'; S2 = S2'; 72 | L = dy2(j).*dx1(i) - dy1(i).*dx2(j); 73 | i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0 74 | 75 | %...Solve system of eqs to get the common points 76 | P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ... 77 | dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')'; 78 | 79 | function u = D(x,y) 80 | u = bsxfun(@minus,x(:,1:end-1),y).*bsxfun(@minus,x(:,2:end),y); 81 | end 82 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/InterX.m: -------------------------------------------------------------------------------- 1 | function P = InterX(L1,varargin) 2 | %INTERX Intersection of curves 3 | % P = INTERX(L1,L2) returns the intersection points of two curves L1 4 | % and L2. The curves L1,L2 can be either closed or open and are described 5 | % by two-row-matrices, where each row contains its x- and y- coordinates. 6 | % The intersection of groups of curves (e.g. contour lines, multiply 7 | % connected regions etc) can also be computed by separating them with a 8 | % column of NaNs as for example 9 | % 10 | % L = [x11 x12 x13 ... NaN x21 x22 x23 ...; 11 | % y11 y12 y13 ... NaN y21 y22 y23 ...] 12 | % 13 | % P has the same structure as L1 and L2, and its rows correspond to the 14 | % x- and y- coordinates of the intersection points of L1 and L2. If no 15 | % intersections are found, the returned P is empty. 16 | % 17 | % P = INTERX(L1) returns the self-intersection points of L1. To keep 18 | % the code simple, the points at which the curve is tangent to itself are 19 | % not included. P = INTERX(L1,L1) returns all the points of the curve 20 | % together with any self-intersection points. 21 | % 22 | % Example: 23 | % t = linspace(0,2*pi); 24 | % r1 = sin(4*t)+2; x1 = r1.*cos(t); y1 = r1.*sin(t); 25 | % r2 = sin(8*t)+2; x2 = r2.*cos(t); y2 = r2.*sin(t); 26 | % P = InterX([x1;y1],[x2;y2]); 27 | % plot(x1,y1,x2,y2,P(1,:),P(2,:),'ro') 28 | 29 | % Author : NS 30 | % Version: 3.0, 21 Sept. 2010 31 | 32 | % Two words about the algorithm: Most of the code is self-explanatory. 33 | % The only trick lies in the calculation of C1 and C2. To be brief, this 34 | % is essentially the two-dimensional analog of the condition that needs 35 | % to be satisfied by a function F(x) that has a zero in the interval 36 | % [a,b], namely 37 | % F(a)*F(b) <= 0 38 | % C1 and C2 exactly do this for each segment of curves 1 and 2 39 | % respectively. If this condition is satisfied simultaneously for two 40 | % segments then we know that they will cross at some point. 41 | % Each factor of the 'C' arrays is essentially a matrix containing 42 | % the numerators of the signed distances between points of one curve 43 | % and line segments of the other. 44 | 45 | %...Argument checks and assignment of L2 46 | error(nargchk(1,2,nargin)); 47 | if nargin == 1, 48 | L2 = L1; hF = @lt; %...Avoid the inclusion of common points 49 | else 50 | L2 = varargin{1}; hF = @le; 51 | end 52 | 53 | %...Preliminary stuff 54 | x1 = L1(1,:)'; x2 = L2(1,:); 55 | y1 = L1(2,:)'; y2 = L2(2,:); 56 | dx1 = diff(x1); dy1 = diff(y1); 57 | dx2 = diff(x2); dy2 = diff(y2); 58 | 59 | %...Determine 'signed distances' 60 | S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1); 61 | S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1); 62 | 63 | C1 = feval(hF,D(bsxfun(@times,dx1,y2)-bsxfun(@times,dy1,x2),S1),0); 64 | C2 = feval(hF,D((bsxfun(@times,y1,dx2)-bsxfun(@times,x1,dy2))',S2'),0)'; 65 | 66 | %...Obtain the segments where an intersection is expected 67 | [i,j] = find(C1 & C2); 68 | if isempty(i),P = zeros(2,0);return; end; 69 | 70 | %...Transpose and prepare for output 71 | i=i'; dx2=dx2'; dy2=dy2'; S2 = S2'; 72 | L = dy2(j).*dx1(i) - dy1(i).*dx2(j); 73 | i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0 74 | 75 | %...Solve system of eqs to get the common points 76 | P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ... 77 | dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')'; 78 | 79 | function u = D(x,y) 80 | u = bsxfun(@minus,x(:,1:end-1),y).*bsxfun(@minus,x(:,2:end),y); 81 | end 82 | end -------------------------------------------------------------------------------- /1.fuzzy controller/4.Goal Reach and Obstacle avoidance ( chamber )/goal_reach+obs_avoidance/InterX.m: -------------------------------------------------------------------------------- 1 | function P = InterX(L1,varargin) 2 | %INTERX Intersection of curves 3 | % P = INTERX(L1,L2) returns the intersection points of two curves L1 4 | % and L2. The curves L1,L2 can be either closed or open and are described 5 | % by two-row-matrices, where each row contains its x- and y- coordinates. 6 | % The intersection of groups of curves (e.g. contour lines, multiply 7 | % connected regions etc) can also be computed by separating them with a 8 | % column of NaNs as for example 9 | % 10 | % L = [x11 x12 x13 ... NaN x21 x22 x23 ...; 11 | % y11 y12 y13 ... NaN y21 y22 y23 ...] 12 | % 13 | % P has the same structure as L1 and L2, and its rows correspond to the 14 | % x- and y- coordinates of the intersection points of L1 and L2. If no 15 | % intersections are found, the returned P is empty. 16 | % 17 | % P = INTERX(L1) returns the self-intersection points of L1. To keep 18 | % the code simple, the points at which the curve is tangent to itself are 19 | % not included. P = INTERX(L1,L1) returns all the points of the curve 20 | % together with any self-intersection points. 21 | % 22 | % Example: 23 | % t = linspace(0,2*pi); 24 | % r1 = sin(4*t)+2; x1 = r1.*cos(t); y1 = r1.*sin(t); 25 | % r2 = sin(8*t)+2; x2 = r2.*cos(t); y2 = r2.*sin(t); 26 | % P = InterX([x1;y1],[x2;y2]); 27 | % plot(x1,y1,x2,y2,P(1,:),P(2,:),'ro') 28 | 29 | % Author : NS 30 | % Version: 3.0, 21 Sept. 2010 31 | 32 | % Two words about the algorithm: Most of the code is self-explanatory. 33 | % The only trick lies in the calculation of C1 and C2. To be brief, this 34 | % is essentially the two-dimensional analog of the condition that needs 35 | % to be satisfied by a function F(x) that has a zero in the interval 36 | % [a,b], namely 37 | % F(a)*F(b) <= 0 38 | % C1 and C2 exactly do this for each segment of curves 1 and 2 39 | % respectively. If this condition is satisfied simultaneously for two 40 | % segments then we know that they will cross at some point. 41 | % Each factor of the 'C' arrays is essentially a matrix containing 42 | % the numerators of the signed distances between points of one curve 43 | % and line segments of the other. 44 | 45 | %...Argument checks and assignment of L2 46 | error(nargchk(1,2,nargin)); 47 | if nargin == 1, 48 | L2 = L1; hF = @lt; %...Avoid the inclusion of common points 49 | else 50 | L2 = varargin{1}; hF = @le; 51 | end 52 | 53 | %...Preliminary stuff 54 | x1 = L1(1,:)'; x2 = L2(1,:); 55 | y1 = L1(2,:)'; y2 = L2(2,:); 56 | dx1 = diff(x1); dy1 = diff(y1); 57 | dx2 = diff(x2); dy2 = diff(y2); 58 | 59 | %...Determine 'signed distances' 60 | S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1); 61 | S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1); 62 | 63 | C1 = feval(hF,D(bsxfun(@times,dx1,y2)-bsxfun(@times,dy1,x2),S1),0); 64 | C2 = feval(hF,D((bsxfun(@times,y1,dx2)-bsxfun(@times,x1,dy2))',S2'),0)'; 65 | 66 | %...Obtain the segments where an intersection is expected 67 | [i,j] = find(C1 & C2); 68 | if isempty(i),P = zeros(2,0);return; end; 69 | 70 | %...Transpose and prepare for output 71 | i=i'; dx2=dx2'; dy2=dy2'; S2 = S2'; 72 | L = dy2(j).*dx1(i) - dy1(i).*dx2(j); 73 | i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0 74 | 75 | %...Solve system of eqs to get the common points 76 | P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ... 77 | dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')'; 78 | 79 | function u = D(x,y) 80 | u = bsxfun(@minus,x(:,1:end-1),y).*bsxfun(@minus,x(:,2:end),y); 81 | end 82 | end -------------------------------------------------------------------------------- /1.fuzzy controller/1.Goal Reach/fuzzy controllers/official/membership function plot/FLC_w.m: -------------------------------------------------------------------------------- 1 | % D_error = {Z, NZ, P, M, NF, F, VF} 2 | % theta_error = {N, SN, NNZ, Z, P, NPZ, SP, P} 3 | % L = {Z, S, NM, M, NH, H, VH} 4 | % R = {Z, S, NM, M, NH, H, VH} 5 | 6 | clear 7 | clc 8 | 9 | 10 | %% plot the membership functions of the output 11 | % D_error 12 | vect_D_Z= []; 13 | vect_D_NZ= []; 14 | vect_D_P= []; 15 | vect_D_M= []; 16 | vect_D_NF= []; 17 | vect_D_F= []; 18 | vect_D_VF= []; 19 | 20 | % theta error 21 | vect_theta_N=[]; 22 | vect_theta_SN=[]; 23 | vect_theta_NNZ=[]; 24 | vect_theta_Z=[]; 25 | vect_theta_NPZ=[]; 26 | vect_theta_SP=[]; 27 | vect_theta_P=[]; 28 | 29 | 30 | vect_td=[]; 31 | vect_ta=[]; 32 | for td=0:0.01:2 33 | 34 | D_Z= trianglar_fct( 0, 0.05,0 , td, 'start'); 35 | D_NZ= trianglar_fct( 0.01,0.25,0.5 , td, 'tria'); 36 | D_P= trianglar_fct( 0.25,0.5 ,0.75 , td, 'tria'); 37 | D_M= trianglar_fct( 0.5,0.75 ,1 , td, 'tria'); 38 | D_NF= trianglar_fct( 0.75,1,1.25 , td, 'tria'); 39 | D_F= trianglar_fct( 1 , 1.25, 1.5 , td, 'tria'); 40 | D_VF= trianglar_fct( 1.25,1.5 ,0 , td, 'end'); 41 | 42 | 43 | 44 | 45 | vect_D_Z = [vect_D_Z D_Z ]; 46 | vect_D_NZ= [vect_D_NZ D_NZ]; 47 | vect_D_P= [vect_D_P D_P]; 48 | vect_D_M= [vect_D_M D_M ]; 49 | vect_D_NF= [vect_D_NF D_NF]; 50 | vect_D_F= [vect_D_F D_F ]; 51 | vect_D_VF= [vect_D_VF D_VF]; 52 | 53 | vect_td=[vect_td td]; 54 | end 55 | 56 | for ta=-90:0.1:90 57 | vect_ta=[vect_ta ta]; 58 | % deg -> 59 | ta=ta*(pi/180); 60 | 61 | theta_N= trianglar_fct( -60*(pi/180), -40*(pi/180),0 , ta, 'start'); 62 | theta_SN= trianglar_fct( -50*(pi/180), -30*(pi/180), -15*(pi/180) , ta, 'tria'); 63 | theta_NNZ= trianglar_fct( -20*(pi/180), -5*(pi/180), -0.2*(pi/180) , ta, 'tria'); 64 | theta_Z= trianglar_fct( -2*(pi/180), 0, 2*(pi/180) , ta, 'tria'); 65 | theta_NPZ= trianglar_fct( 0.2*(pi/180), 5*(pi/180), 20*(pi/180) , ta, 'tria'); 66 | theta_SP= trianglar_fct( 15*(pi/180), 30*(pi/180), 50*(pi/180) , ta, 'tria'); 67 | theta_P= trianglar_fct( 40*(pi/180), 60*(pi/180), 0 , ta, 'end'); 68 | 69 | vect_theta_N=[vect_theta_N theta_N]; 70 | vect_theta_SN=[vect_theta_SN theta_SN]; 71 | vect_theta_NNZ=[vect_theta_NNZ theta_NNZ ]; 72 | vect_theta_Z=[vect_theta_Z theta_Z]; 73 | vect_theta_NPZ=[vect_theta_NPZ theta_NPZ]; 74 | vect_theta_SP=[vect_theta_SP theta_SP]; 75 | vect_theta_P=[vect_theta_P theta_P]; 76 | 77 | 78 | end 79 | figure(1) 80 | plot(vect_td, vect_D_Z, vect_td, vect_D_NZ,'k', vect_td,vect_D_P, vect_td, vect_D_M, vect_td, vect_D_NF, vect_td, vect_D_F,vect_td,vect_D_VF) 81 | legend('Z','NZ','P','M','NF','F','VF'); 82 | axis([0 2 0 1.2]); 83 | xlabel('distance error (m)') 84 | 85 | 86 | figure(2) 87 | plot(vect_ta,vect_theta_N,vect_ta, vect_theta_SN,vect_ta, vect_theta_NNZ,vect_ta, vect_theta_Z,vect_ta,vect_theta_NPZ,vect_ta,vect_theta_SP,vect_ta,vect_theta_P) 88 | legend('N','SN','NNZ','Z','NPZ','SP','P'); 89 | axis([-90 90 0 1.2]) 90 | xlabel('Angle error') 91 | 92 | %% plot the membership functions of the output 93 | R_Z=0; R_S=1.5; R_NM=3; R_M=5; R_NH=7; R_H=10; R_VH=15; 94 | L_Z= R_Z; L_S= R_S; L_NM= R_NM; L_M= R_M; L_NH= R_NH; L_H= R_H; L_VH= R_VH; 95 | y=0:0.1:1; 96 | x=ones(1,length(y)); 97 | 98 | figure(3) 99 | plot(R_Z.*x, y, R_S.*x ,y ,R_NM.*x,y ,R_M.*x,y, R_NH.*x, y, R_H.*x, y, R_H.*x, y, R_VH.*x,y ); 100 | legend('Z','S','NM','M','M','NH','H','VH'); 101 | axis([-0.1 16 0 1.2]); 102 | xlabel(' right wheel velocity membership functions ') 103 | 104 | figure(4) 105 | plot(L_Z.*x, y, L_S.*x ,y ,L_NM.*x,y ,L_M.*x,y, L_NH.*x, y, L_H.*x, y, L_H.*x, y, L_VH.*x,y ); 106 | legend('Z','S','NM','M','M','NH','H','VH'); 107 | axis([-0.1 16 0 1.2]); 108 | xlabel(' left wheel velocity membership functions ') 109 | 110 | -------------------------------------------------------------------------------- /2.Q_fuzzy controller/Goal Reach/20 rules/1.Learning/MAIN_RL3_ZMO4.m: -------------------------------------------------------------------------------- 1 | % Inspired by : 2 | % [1] pooyanjamshidi Fuzzy QL pseudocode 3 | % {2] Reinforcement Distribution in Fuzzy Q-learning 4 | close all 5 | clear; 6 | clc; 7 | 8 | %% Robot parameters 9 | R = 0.035; L = 0.28; 10 | 11 | 12 | %% Fuzzy Q learning Algorithm 13 | 14 | % Action management 15 | 16 | R_Z=0; 17 | R_M=1.5; 18 | R_H=4; 19 | 20 | L_Z= R_Z; 21 | L_M= R_M; 22 | L_H= R_H; 23 | 24 | Actions=[R_Z L_Z ; R_Z L_M ; R_Z L_H ; R_M L_Z; R_M L_M ; R_M L_H ; R_H L_Z ; R_H L_M ; R_H L_H]; % each row corresponds to the action index 25 | R_Actions=Actions(:,1); 26 | L_Actions=Actions(:,2); 27 | 28 | %%% 29 | NRules=20; 30 | %%% 31 | selected_WR=zeros(NRules,1); % selected actions from each rules in an iteration 32 | selected_WL=zeros(NRules,1); 33 | 34 | % Q learning coeff 35 | alpha0=1; % learning rate 36 | gamma=0.7; % discount factor 37 | epsilon0=1; % Epsilon greedy coeffi ( exploration rate ) 38 | decr = 0.998; % decrease factor 39 | 40 | % Actions and states 41 | NConclusions=9; % 3*3 actions for each wheel 42 | NRules=20; % 4*5 MFs combinaison 43 | 44 | % 1-Initialisation of q table 45 | q = zeros(NRules, NConclusions); 46 | 47 | % conclusions 48 | Conclusions=zeros(NRules,1); 49 | 50 | % Loop 51 | NEpisodes=8000; 52 | 53 | %Full exploitation params (Uncomment if necessary) 54 | NEpisodes=1; 55 | alpha0=0; % no learning 56 | epsilon0=0; 57 | load('qtable4000'); % also comment q=zeros(,) in step1 58 | 59 | 60 | x_d=1; y_d=1; 61 | 62 | % simulation parameters 63 | t_final=150; 64 | 65 | vect_cumReward=[]; 66 | for Episode = 1 : NEpisodes 67 | t=0; 68 | vect_t=[]; 69 | vect_x=[]; 70 | vect_y=[]; 71 | vect_theta=[]; 72 | 73 | cumReward=0; % cumulative Rewerd for each episode 74 | 75 | % starting position 76 | x_init=rand*2; y_init=rand*2 ; theta_init=0; % Begin at a random place between [0 2] to avoid overfitting 77 | xp=x_init ;yp=y_init; thetap=theta_init; 78 | 79 | % desired position 80 | 81 | 82 | Episode_end=1; % 0 if the end of episode 83 | sim_delta = 0.1; % sample Time 84 | % Initialise Q learning params 85 | alphaI=alpha0; 86 | epsilonI=epsilon0; 87 | while ( (Episode_end~=0) && (t < t_final) ) % loop until find goal state 88 | alphaI=alphaI*decr; % decrease learning rate 89 | epsilonI=epsilonI*decr; 90 | t=t+sim_delta; 91 | cumReward=0; 92 | %% 2 select an action for each fired rule 93 | for rule=1:NRules 94 | if rand > epsilonI % exploitation 95 | [QValue, Curr_Conclusion] = max(q(rule,:)); % the q table is in order 96 | 97 | else % exploration 98 | Curr_Conclusion=round((NConclusions-1)*rand+1); 99 | end 100 | Conclusions(rule)=Curr_Conclusion; % update the conclusion of each rule 101 | end 102 | 103 | %% 3-calculate the control action by the fuzzy controller 104 | 105 | Rules_deg0 = Rules_act_deg(xp, yp, thetap, x_d, y_d); % rule activation "alpha") 106 | W_Rules_deg0 = Rules_deg0/sum(Rules_deg0); % weighted rule "psi" ( seen from : [2] "Action selection") 107 | 108 | selected_WR=R_Actions(Conclusions); 109 | selected_WL=L_Actions(Conclusions); 110 | 111 | W_Rc= selected_WR'*W_Rules_deg0; % weigthed sum using scalar product 112 | W_Lc= selected_WL'*W_Rules_deg0; 113 | 114 | 115 | %% 4-approxiame the Q function from the q-values and the firing levels of the rule 116 | 117 | Q0=0; 118 | for i=1:NRules 119 | Q0=Q0+W_Rules_deg0(i)*q(i,Conclusions(i)); % seen from [2] fig3 120 | end 121 | 122 | cumReward=cumReward+Q0; 123 | %% 5-take action a and let the system goes to the next state 124 | 125 | [ w_l,w_r] = Diff_Robot_Model(W_Lc, W_Rc, sim_delta); 126 | [xn,yn,thetan]= Odometry(w_l, w_r, sim_delta,xp,yp, thetap,[R L]); 127 | 128 | %% 6- observe the reinforcement signal r(t+1) and compute the value for new state 129 | [Reward, Episode_end]= Reward_function1(xp, yp, thetap, xn, yn, thetan, x_d, y_d); 130 | 131 | Rules_deg1= Rules_act_deg(xn, yn, thetan, x_d, y_d); 132 | W_Rules_deg1 = Rules_deg1/sum(Rules_deg1); 133 | V=0; 134 | for i=1:NRules 135 | V=V+(W_Rules_deg1(i)*max(q(i,:))); 136 | end 137 | 138 | %% 7- calculate the error signal 139 | delta_Q= Reward + gamma*V-Q0; 140 | 141 | %% 8- update q-values 142 | for i=1:NRules 143 | q(i,Conclusions(i))=q(i,Conclusions(i))+ alphaI*delta_Q*W_Rules_deg0(i); 144 | end 145 | 146 | xp=xn ;yp=yn; thetap=thetan; 147 | 148 | % Log trajectory 149 | vect_t=[vect_t t]; 150 | vect_x=[vect_x xp]; 151 | vect_y=[vect_y yp]; 152 | vect_theta=[vect_theta thetap]; 153 | 154 | 155 | end 156 | fprintf('Episode %i termin�. Le robot a fait %i It�rations. La r�comence cumul�e est %i.\n', Episode, t, cumReward); 157 | 158 | if mod(Episode,500)==0 159 | save(strcat('qtable',int2str(Episode)),'q') 160 | end 161 | end 162 | 163 | %save('qtable','q'); 164 | 165 | figure(1) 166 | subplot(3,1,1); plot(vect_t,vect_x,'r'); title('position x'); xlabel('x(m)'); ylabel('t'); 167 | subplot(3,1,2); plot(vect_t,vect_y,'r'); title('positon y'); xlabel('y(m)'); ylabel('t'); 168 | subplot(3,1,3); plot(vect_t,vect_theta,'r'); title('angle theta'); xlabel('theta(rad)'); ylabel('t'); 169 | 170 | figure(2) 171 | plot(vect_x,vect_y); title('robot position'); xlabel('x(m)'); ylabel('y(m)'); 172 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------