diff --git a/Get_Dog_Postion.m b/Get_Dog_Postion.m index 9db909b..0aec8ca 100644 --- a/Get_Dog_Postion.m +++ b/Get_Dog_Postion.m @@ -1,4 +1,4 @@ -function [result] = Get_Dog_Postion(client,id) +function [time_stamp,x,z,yaw] = Get_Dog_Postion(client,id) frameData = client.GetLastFrameOfData(); @@ -40,12 +40,11 @@ end yaw = yaw*180/pi; - result = [time_stamp x z yaw]; + else time_stamp=0; z=0; x=0; yaw=0; - result = [time_stamp x z yaw]; end diff --git a/Main.m b/Main.m index d1e0350..8a27582 100644 --- a/Main.m +++ b/Main.m @@ -25,17 +25,17 @@ % Porportional constant on velocity action K_P_x = 0.6; K_P_z = 0.6; -K_P_yaw = 0.5; +K_P_yaw = 0.07; % Integral K_I_x = 1.25; K_I_z = 1.25; -K_I_yaw = 0.03; +K_I_yaw = 0.01; % Derivative K_D_x = 0.08; K_D_z = 0.08; -K_D_yaw = 0.05; +K_D_yaw = 0.01; % limit propostional_x_limit = 0.7; % m/s @@ -178,14 +178,14 @@ breakflag = 0; Dog_Pos_Record=[]; Dog_Pos_Record_Index = 1; -[Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); -time = Dog_Pos(1); +[time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); +init_time = time; %% Main Loop while true % get position from camera - [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] - Dog_Pos_Record=[Dog_Pos_Record;Dog_Pos Dog_Pos(1)-time]; + [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + Dog_Pos_Record=[Dog_Pos_Record; time, x, z, yaw, time-init_time]; Dog_Pos_Record_Index = Dog_Pos_Record_Index +1; if Control_Mode == 2 Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; @@ -193,130 +193,31 @@ %% Feedback control % Calculate Distance - Point_Dog = [Dog_Pos(2) Dog_Pos(3)]; %[x,z] + Point_Dog = [x z]; %[x,z] Vector_PD_TP = Target_Point-Point_Dog; % Get vector Norm_Vector = norm(Vector_PD_TP); % Calculate norm % rotation matrix to turn vector to robot dog frame - Rotation_matrix = [cosd(Dog_Pos(4)), -sind(Dog_Pos(4)) ; sind(Dog_Pos(4)),cosd(Dog_Pos(4)) ]; + Rotation_matrix = [cosd(yaw), -sind(yaw) ; sind(yaw),cosd(yaw) ]; if Control_Mode==1 || Norm_Vector > Distance_Threshold %vector in robot dog frame = error_x error_z Vector_rotated = Rotation_matrix*Vector_PD_TP'; - if yaw_set == -1 % not control yaw - error_yaw_command=0; + Error_Yaw=Yaw_Controllor(yaw_set,yaw,Vector_PD_TP); + %% PID + Control_x=PID_Controllor(K_P_x,K_I_x,K_D_x,dt,Vector_rotated(1),integral_x,previous_error_x,propostional_x_limit,integral_x_limit,derivative_x_limit); + Control_z=PID_Controllor(K_P_z,K_I_z,K_D_z,dt,Vector_rotated(2),integral_z,previous_error_z,propostional_z_limit,integral_z_limit,derivative_z_limit); + Control_yaw=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,dt,Error_Yaw,integral_yaw,previous_error_yaw,propostional_yaw_limit,integral_yaw_limit,derivative_yaw_limit); - elseif yaw_set == -2 % control yaw to motion direction - angle_r = atan2(Vector_PD_TP(1),Vector_PD_TP(2)); - yaw_set_mode2 = rad2deg(angle_r); - if yaw_set_mode2 < 0 - yaw_set_mode2 = yaw_set_mode2+360; - end - error_yaw = yaw_set_mode2-Dog_Pos(4); - - if error_yaw<-180 - error_yaw_command=(360+error_yaw); - elseif error_yaw > 180 - error_yaw_command=error_yaw-360; - else - error_yaw_command=error_yaw; - end - - elseif yaw_set >=0 && yaw_set<360 % control yaw - error_yaw = yaw_set-Dog_Pos(4); - if error_yaw<-180 - error_yaw_command=(360+error_yaw); - elseif error_yaw > 180 - error_yaw_command=error_yaw-360; - else - error_yaw_command=error_yaw; - end - end - - - %% propostional propostional_x = Vector_rotated(1) * K_P_x; propostional_z = Vector_rotated(2) * K_P_z; - propostional_yaw = error_yaw_command*pi/180 * K_P_yaw; - - % propostional limit - if propostional_x > propostional_x_limit - propostional_x = propostional_x_limit; - elseif propostional_x < -propostional_x_limit - propostional_x = -propostional_x_limit; - end - - if propostional_z > propostional_z_limit - propostional_z = propostional_z_limit; - elseif propostional_z < -propostional_z_limit - propostional_z = -propostional_z_limit; - end - - if propostional_yaw > propostional_yaw_limit - propostional_yaw = propostional_yaw_limit; - elseif propostional_yaw < -propostional_yaw_limit - propostional_yaw = -propostional_yaw_limit; - end - - %% integral - integral_x = integral_x + K_I_x * Vector_rotated(1) * dt; - integral_z = integral_z + K_I_z * Vector_rotated(2) * dt; - integral_yaw = integral_yaw + K_I_yaw * error_yaw_command * dt; - - % integral limit - if integral_x > integral_x_limit - integral_x = integral_x_limit; - elseif integral_x < -integral_x_limit - integral_x = -integral_x_limit; - end - - if integral_z > integral_z_limit - integral_z = integral_z_limit; - elseif integral_z < -integral_z_limit - integral_z = -integral_z_limit; - end - - if integral_yaw > integral_yaw_limit - integral_yaw = integral_yaw_limit; - elseif integral_yaw < -integral_yaw_limit - integral_yaw = -integral_yaw_limit; - end - - - %% derivative - derivative_x = K_D_x * (Vector_rotated(1) - previous_error_x) /dt; - derivative_z = K_D_z * (Vector_rotated(2) - previous_error_z) /dt; - derivative_yaw = K_D_yaw * (error_yaw_command - previous_error_yaw)/dt; - - previous_error_x = Vector_rotated(1); - previous_error_z = Vector_rotated(2); - previous_error_yaw = error_yaw_command; - - % derivative limit - if derivative_x > derivative_x_limit - derivative_x = derivative_x_limit; - elseif derivative_x < -derivative_x_limit - derivative_x = -derivative_x_limit; - end - - if derivative_z > derivative_z_limit - derivative_z = derivative_z_limit; - elseif derivative_z < -derivative_z_limit - derivative_z = -derivative_z_limit; - end - - if derivative_yaw > derivative_yaw_limit - derivative_yaw = derivative_yaw_limit; - elseif derivative_yaw < -derivative_yaw_limit - derivative_yaw = -derivative_yaw_limit; - end - + propostional_yaw = Error_Yaw*pi/180 * K_P_yaw; %% set command - Control_Command(10) = propostional_x + integral_x + derivative_x; %x - Control_Command(9) = propostional_z + integral_z + derivative_z; %z - Control_Command(11) = propostional_yaw + integral_yaw + derivative_yaw; %yaw + Control_Command(10) = Control_x; %x + Control_Command(9) = Control_z; %z + Control_Command(11) = Control_yaw; %yaw else % switch point % clean integral @@ -341,7 +242,6 @@ disp(Control_Command); % send command to vitrual machine Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - %% draw figure plot(ax,circle_x,circle_y,'b-'); xlabel('X') @@ -351,9 +251,9 @@ plot(ax,Target_Point(1),Target_Point(2),'.','Color','r','MarkerSize',20); plot(ax,Way_Points_x,Way_Points_z,'o'); ax.DataAspectRatio=[1 1 1]; - dy=arrow_length*cosd(Dog_Pos(4)); - dx=arrow_length*sind(Dog_Pos(4)); - quiver(Dog_Pos(2),Dog_Pos(3),dx,dy,'r','LineWidth',0.2,'MaxHeadSize',2); + dy=arrow_length*cosd(yaw); + dx=arrow_length*sind(yaw); + quiver(x,z,dx,dy,'r','LineWidth',0.2,'MaxHeadSize',2); plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3),'Color','r'); set(gca,'XDir','reverse'); xlim(ax,[-3,3]); diff --git a/Motive_test.m b/Motive_test.m new file mode 100644 index 0000000..c5e3c1d --- /dev/null +++ b/Motive_test.m @@ -0,0 +1,58 @@ +%% Clean +clc; +clear; +close all; + +%% Instantiate client object to run Motive API commands + +% Check list: +% 1.Broadcast Frame Date +% 2.Network Interface: Local Loopback + +% https://optitrack.com/software/natnet-sdk/ +% Create Motive client object +dllPath = fullfile('d:','StDroneControl','NatNetSDK','lib','x64','NatNetML.dll'); +assemblyInfo = NET.addAssembly(dllPath); % Add API function calls +theClient = NatNetML.NatNetClientML(0); + +% Create connection to localhost, data is now being streamed through client object +HostIP = '127.0.0.1'; +theClient.Initialize(HostIP, HostIP); + +Dog_ID = 1; % Rigid body ID of the drone from Motive + +%% initialized + +real_time=0; +run_time=1; + +time=0; + +[init_time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] +Dog_Pos_Record = [0,x,z,yaw]; +i=1; +%% Main Loop +while true + [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + real_time=time-init_time; + if ~isequal(Dog_Pos_Record(end,:),[real_time,x,z,yaw]) + i=i+1; + Dog_Pos_Record=[Dog_Pos_Record ; real_time,x,z,yaw]; + real_time = Dog_Pos_Record(i-1,1)-Dog_Pos_Record(1,1); + if real_time>run_time + break + end + end + +end + +%% figure + +figure; +plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)); +xlabel('X'); +ylabel('Z'); +set(gca,'XDir','reverse'); +xlim([-3,3]); +ylim([-2,2]); +daspect([1 1 1]); diff --git a/PID_Controllor.m b/PID_Controllor.m index e176454..69c9e66 100644 --- a/PID_Controllor.m +++ b/PID_Controllor.m @@ -1,4 +1,4 @@ -function [Control_Output] = PID_Controllor(K_P,K_I,K_D,D_t,error,integral,previous_error,P_l,I_l,D_l) +function [Control_Output,integral] = PID_Controllor(K_P,K_I,K_D,D_t,error,integral,previous_error,P_l,I_l,D_l) %UNTITLED Summary of this function goes here % Detailed explanation goes here %Input: diff --git a/speed_control.m b/speed_control.m index 267377e..9ab0583 100644 --- a/speed_control.m +++ b/speed_control.m @@ -1,5 +1,6 @@ %% Clean clc; +clear; close all; %% Robot Dog Network Parameters @@ -7,51 +8,52 @@ Robot_Dog_IP = '192.168.123.161'; Robot_Dog_Port = 1145; -%% Robot Dog Command Initialized +% Robot Dog Command Initialized Control_Command = zeros(1,11,'single'); %velocity walking Control_Command(1)=2; -if isempty(gcp('nocreate')) - parpool; -end -futureResult = parallel.FevalFuture; +% if isempty(gcp('nocreate')) +% parpool; +% end +% futureResult = parallel.FevalFuture; %% Feedback Control Parameters % Porportional constant on velocity action -K_P_x = 1; -K_P_z = 1; -K_P_yaw = 0.5; +K_P_x = 0.6; +K_P_z = 0.6; +K_P_yaw = 0.07; % Integral -K_I_x = 0.15; -K_I_z = 0.15; -K_I_yaw = 0.03; +K_I_x = 2; +K_I_z = 2; +K_I_yaw = 0; % Derivative -K_D_x = 0.08; -K_D_z = 0.08; -K_D_yaw = 0.05; +K_D_x = 0.02; +K_D_z = 0.02; +K_D_yaw = 0; % limit p_x_limit = 0; % m/s p_z_limit = 0; % m/s p_yaw_limit = 40*pi/180; % rad/s -i_x_limit = 0; -i_z_limit = 0; +i_x_limit = 1; +i_z_limit = 1; i_yaw_limit = 5*pi/180; -d_x_limit = 0; -d_z_limit = 0; +d_x_limit = 0.2; +d_z_limit = 0.2; d_yaw_limit = 5*pi/180; + %% Control Setting % MODE % Mode 1: Target point. % Mode 2: Dog will follow Way_Points. % Mode 3: Dog will follow the path Control_Mode=1; -Control_Speed=0.5; -Stop_Distance=0.5*Control_Speed^2; +Control_Speed=0.6; +Stop_Distance=0.25*Control_Speed^2; % Target Point %[x,z] Target_Point=[0 0]; @@ -103,18 +105,7 @@ Dog_ID = 1; % Rigid body ID of the drone from Motive -%% figure for movtion track -fig = figure(); -ax = axes('Parent',fig); - -arrow_length=0.2; -%circle for draw -circle_center =[0,0]; -circle_radius =1.65; -circle_theta = linspace(0,2*pi,100); -circle_x=circle_center(1)+circle_radius*cos(circle_theta); -circle_y=circle_center(2)+circle_radius*sin(circle_theta); -%% Robot dog command +% Robot dog command % Control_Command() % % +(11) +(9) -(11) @@ -123,7 +114,8 @@ % | % -(9) % -%% Motive coordiate frame + +% Motive coordiate frame % wall wall wall wall wall % ^ z % | @@ -134,8 +126,6 @@ % wall computer wall %% Init Parameters -% index for way points -Way_Point_index=1; integral_x = 0; integral_z = 0; @@ -145,58 +135,85 @@ previous_error_z = 0; previous_error_yaw = 0; - +Dog_Speed_Record=[]; Dog_Pos_Record=[]; -Dog_Pos_Record_Index = 1; -i=0; +while true + [init_time, x, z, yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw]; + Dog_Pos_Record=[0, x, z, yaw]; + i=1; + if init_time ~= 0 + break + end +end %% Main Loop while true % get position from camera % async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] - if i==0 - i=1; - Dog_Pos_Record=[Dog_Pos_Record ; Dog_Pos]; - else - if ~isequal(Dog_Pos_Record(end,:),Dog_Pos) %if not the same values - i=i+1; - Dog_Pos_Record=[Dog_Pos_Record ; Dog_Pos]; - Rotation_matrix = [cosd(Dog_Pos(4)), -sind(Dog_Pos(4)) ; sind(Dog_Pos(4)),cosd(Dog_Pos(4)) ]; - - d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); %[dtime, dx, dz, dyaw] - real_time = Dog_Pos_Record(i-1,1)-Dog_Pos_Record(1,1); - Real_Dog_Speed_Room = [d_dog_pos(2)/d_dog_pos(1), d_dog_pos(3)/d_dog_pos(1)]; - Real_Dog_Speed_Dog = Rotation_matrix*Real_Dog_Speed_Room'; - %Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_R]; %[rtime, z_speed, x_speed] - - if Control_Mode==1 - Vector_D_T = Target_Point-[Dog_Pos(2) Dog_Pos(3)]; % Get vector - Norm_VDT = norm(Vector_D_T); % Calculate Distance - scale=Control_Speed/Norm_VDT; - Ref_Speed_Room=scale*Vector_D_T; - Ref_Speed_Dog = Rotation_matrix*Ref_Speed_Room'; - Error_Speed_Dog = Ref_Speed_Dog-Real_Dog_Speed_Dog; - %error yaw calculate - Error_Yaw=Yaw_Controllor(yaw_set,Dog_Pos(4),Vector_D_T); - %PID Control - Control_x=PID_Controllor(K_P_x,K_I_x,K_D_x,d_dog_pos(1),Error_Speed_Dog(1),integral_x,previous_error_x,p_x_limit,i_x_limit,d_x_limit); - Control_z=PID_Controllor(K_P_z,K_I_z,K_D_z,d_dog_pos(1),Error_Speed_Dog(2),integral_z,previous_error_z,p_z_limit,i_z_limit,d_z_limit); - Control_yaw=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,d_dog_pos(1),Error_Yaw,integral_yaw,previous_error_yaw,p_yaw_limit,i_yaw_limit,d_yaw_limit); - if Norm_VDT> Stop_Distance - Control_Command(10) = Control_x; %x - Control_Command(9) = Control_z; %z - Control_Command(11) = Control_yaw; %yaw - else - Control_Command = zeros(1,11,'single'); - %velocity walking - Control_Command(1)=2; - %break; - end - - end - + [time, x, z, yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + real_time = time-init_time; + if ~isequal(Dog_Pos_Record(end,:), [real_time, x, z, yaw]) %if not the same values + i=i+1; + Dog_Pos_Record=[Dog_Pos_Record ; real_time, x, z, yaw]; + Rotation_matrix = [cosd(yaw), -sind(yaw) ; sind(yaw),cosd(yaw) ]; + + d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); %[dtime, dx, dz, dyaw] + + + Real_Dog_Speed_Room = [d_dog_pos(2)/d_dog_pos(1), d_dog_pos(3)/d_dog_pos(1)]; + Real_Dog_Speed_Dog = Rotation_matrix*Real_Dog_Speed_Room'; + + Vector_D_T = Target_Point-[x z]; % Get vector + Norm_VDT = norm(Vector_D_T); % Calculate Distance + scale=Control_Speed/Norm_VDT; + Ref_Speed_Room=scale*Vector_D_T; + Ref_Speed_Dog = Rotation_matrix*Ref_Speed_Room'; + + Error_Speed_Dog = Ref_Speed_Dog-Real_Dog_Speed_Dog; + + + %[rtime, real_x_speed, real_z_speed, ref_x_speed, ref_z_speed, error_x, error_z] + + %error yaw calculate + Error_Yaw=Yaw_Controllor(yaw_set,yaw,Vector_D_T); + %PID Control + [Control_x,integral_x]=PID_Controllor(K_P_x,K_I_x,K_D_x,d_dog_pos(1),Error_Speed_Dog(1),integral_x,previous_error_x,p_x_limit,i_x_limit,d_x_limit); + [Control_z,integral_z]=PID_Controllor(K_P_z,K_I_z,K_D_z,d_dog_pos(1),Error_Speed_Dog(2),integral_z,previous_error_z,p_z_limit,i_z_limit,d_z_limit); + [Control_yaw,integral_yaw]=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,d_dog_pos(1),Error_Yaw,integral_yaw,previous_error_yaw,p_yaw_limit,i_yaw_limit,d_yaw_limit); + + previous_error_x = Error_Speed_Dog(1); + previous_error_z = Error_Speed_Dog(2); + previous_error_yaw = Error_Yaw; + disp([Real_Dog_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,Control_yaw]) + + Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_Dog',Ref_Speed_Dog',Error_Speed_Dog',Control_x,Control_z]; + + if Norm_VDT> 0.2 + Control_Command(10) = Control_x; %x + Control_Command(9) = Control_z; %z + Control_Command(11) = Control_yaw; %yaw + else + Control_Command = zeros(1,11,'single'); + %velocity walking + Control_Command(1)=2; + %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + break; end + %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); end - async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); end +figure; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,2),'DisplayName','Measure Speed'); +title('Robot Dog X Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,4),'LineWidth',1,'DisplayName','Ref Speed'); + +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,6),'LineWidth',1,'DisplayName','Error'); + +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,8),'LineWidth',1,'DisplayName','Control'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off;