From e4ab27db28327783919d966547e394c499bd9731 Mon Sep 17 00:00:00 2001 From: XI LUO Date: Tue, 23 May 2023 16:24:41 -0700 Subject: [PATCH] add follow --- Follow.m | 235 +++++++++++++++++++++++++++++++++++++++++++++++ Yaw_Controllor.m | 3 +- speed_control.m | 99 ++++++++++++++------ 3 files changed, 307 insertions(+), 30 deletions(-) create mode 100644 Follow.m diff --git a/Follow.m b/Follow.m new file mode 100644 index 0000000..70d96a5 --- /dev/null +++ b/Follow.m @@ -0,0 +1,235 @@ +%% Clean +clc; +clear; +close all; + +%% Robot Dog Network Parameters +% this IP is the vm ip +Robot_Dog_IP = '192.168.123.161'; +Robot_Dog_Port = 1145; + +% 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; +%% Feedback Control Parameters + +% Porportional constant on velocity action +K_P_x = 0.6; +K_P_z = 0.6; +K_P_yaw = 0.07; + +% Integral +K_I_x = 1.8; +K_I_z = 1.8; +K_I_yaw = 0.05; + +% Derivative +K_D_x = 0.02; +K_D_z = 0.02; +K_D_yaw =0.005; + +% limit +p_x_limit = 0; % m/s +p_z_limit = 0; % m/s +p_yaw_limit = 20*pi/180; % rad/s + +i_x_limit = 1; +i_z_limit = 1; +i_yaw_limit = 40*pi/180; + +d_x_limit = 0.2; +d_z_limit = 0.2; +d_yaw_limit = 2*pi/180; + +%% Control Setting + +Control_Speed=0.8; + +Switch_Distance=0.1; + +% Target Point +%[x,z] +Target_Point=[0 0]; + +% YAW +% [0,360) +% -1: Disable yaw control +% -2: Control Yaw to the motion direction + +% Yaw +% wall wall wall wall wall +% 0,359.9.. +% ^ z +% | +% | +% 90 x <----O 270 +% +% 180 +% +% wall computer wall +yaw_set = 0; + + +%% 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 +Drone_ID = 2; +% Robot dog command +% Control_Command() +% +% +(11) +(9) -(11) +% | +% +(10) dog -(10) +% | +% -(9) +% + +% Motive coordiate frame +% wall wall wall wall wall +% ^ z +% | +% | +% x <----O y(pointing up) +% +% +% wall computer wall + +%% Init Parameters + +integral_x = 0; +integral_z = 0; +integral_yaw = 0; + +previous_error_x = 0; +previous_error_z = 0; +previous_error_yaw = 0; + +Dog_Speed_Record=[]; +Dog_Pos_Record=[]; +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); + [time, x, z, yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + [Drone_time, Drone_x, Drone_z, Drone_yaw] = Get_Dog_Postion(theClient, Drone_ID); + Target_Point = [Drone_x,Drone_z]; + if norm(Target_Point)>1.5 + Target_Point = [0,0]; + end + + 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; + + %error yaw calculate + [Error_Yaw,Mode2_Yaw_Ref]=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,yaw,Mode2_Yaw_Ref,Control_yaw]; + %[rtime, real_x_speed, real_z_speed, ref_x_speed, ref_z_speed, error_x, error_z] + + %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + if Norm_VDT> Switch_Distance + Control_Command(10) = Control_x; %x + Control_Command(9) = Control_z; %z + Control_Command(11) = Control_yaw; %yaw + else + Control_Command(10) = 0; %x + Control_Command(9) = 0; %z + Control_Command(11) = 0; %yaw + integral_z=0; + integral_x=0; + integral_yaw=0; + previous_error_yaw=0; + previous_error_z=0; + previous_error_x=0; + end + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + end +end +%% Speed figures +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; + +figure; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,3),'DisplayName','Measure Speed'); +title('Robot Dog Z Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,5),'LineWidth',1,'DisplayName','Ref Speed'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,7),'LineWidth',1,'DisplayName','Error'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,9),'LineWidth',1,'DisplayName','Control'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off; + +figure; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,10),'DisplayName','Measure Yaw'); +title('Robot Dog Z Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,11),'LineWidth',1,'DisplayName','Yaw Ref'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,12),'LineWidth',1,'DisplayName','Control_yaw'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off; diff --git a/Yaw_Controllor.m b/Yaw_Controllor.m index e737af7..8609104 100644 --- a/Yaw_Controllor.m +++ b/Yaw_Controllor.m @@ -1,4 +1,4 @@ -function [error_yaw_command] = Yaw_Controllor(yaw_set,Dog_yaw,Vector) +function [error_yaw_command,yaw_set_mode2] = Yaw_Controllor(yaw_set,Dog_yaw,Vector) %YAW_CONTROLLOR Summary of this function goes here % Detailed explanation goes here % Yaw @@ -12,6 +12,7 @@ % 180 % % wall computer wall +yaw_set_mode2=0; if yaw_set == -1 % not control yaw error_yaw_command=0; elseif yaw_set == -2 % control yaw to motion direction diff --git a/speed_control.m b/speed_control.m index 9ab0583..4cc26a2 100644 --- a/speed_control.m +++ b/speed_control.m @@ -24,36 +24,39 @@ K_P_yaw = 0.07; % Integral -K_I_x = 2; -K_I_z = 2; -K_I_yaw = 0; +K_I_x = 1.8; +K_I_z = 1.8; +K_I_yaw = 0.05; % Derivative K_D_x = 0.02; K_D_z = 0.02; -K_D_yaw = 0; +K_D_yaw =0.005; % limit p_x_limit = 0; % m/s p_z_limit = 0; % m/s -p_yaw_limit = 40*pi/180; % rad/s +p_yaw_limit = 20*pi/180; % rad/s i_x_limit = 1; i_z_limit = 1; -i_yaw_limit = 5*pi/180; +i_yaw_limit = 40*pi/180; d_x_limit = 0.2; d_z_limit = 0.2; -d_yaw_limit = 5*pi/180; +d_yaw_limit = 2*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_Mode=2; + Control_Speed=0.6; -Stop_Distance=0.25*Control_Speed^2; + +Switch_Distance=0.5*Control_Speed^2; + % Target Point %[x,z] Target_Point=[0 0]; @@ -80,9 +83,9 @@ % Distance Threshold to switch to next way point Distance_Threshold = 0.15; -% Cricel way points 18 +% Cricel way points Way_Points_center =[0,0]; -Way_Points_radius =1.65; +Way_Points_radius =1.5; Way_Points_theta = linspace(0,2*pi,50); Way_Points_x=Way_Points_center(1)+Way_Points_radius*cos(Way_Points_theta); Way_Points_z=Way_Points_center(2)+Way_Points_radius*sin(Way_Points_theta); @@ -126,6 +129,7 @@ % wall computer wall %% Init Parameters +Way_Point_index=1; integral_x = 0; integral_z = 0; @@ -145,6 +149,10 @@ break end end + +if Control_Mode == 2 + Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; +end %% Main Loop while true % get position from camera @@ -158,23 +166,19 @@ 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); + [Error_Yaw,Mode2_Yaw_Ref]=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); @@ -185,35 +189,72 @@ 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]; + Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_Dog',Ref_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,yaw,Mode2_Yaw_Ref,Control_yaw]; + %[rtime, real_x_speed, real_z_speed, ref_x_speed, ref_z_speed, error_x, error_z] - if Norm_VDT> 0.2 + if Norm_VDT> Switch_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; - %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - break; + if Control_Mode == 1 + 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; + elseif Control_Mode == 2 + if length(Way_Points_z)>Way_Point_index + Way_Point_index=Way_Point_index+1; + Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; + 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 + end + end %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); end end - +%% Speed figures 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; + +figure; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,3),'DisplayName','Measure Speed'); +title('Robot Dog Z Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,5),'LineWidth',1,'DisplayName','Ref Speed'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,7),'LineWidth',1,'DisplayName','Error'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,9),'LineWidth',1,'DisplayName','Control'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off; + +figure; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,10),'DisplayName','Measure Yaw'); +title('Robot Dog Z Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,11),'LineWidth',1,'DisplayName','Yaw Ref'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,12),'LineWidth',1,'DisplayName','Control_yaw'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off;