From 2cdd55f857fa936a9ceaca40ec0f606b0c5f4ffa Mon Sep 17 00:00:00 2001 From: XI LUO Date: Mon, 20 Mar 2023 20:49:40 -0700 Subject: [PATCH] add way points control --- Main.m | 155 ++++++++++++++++++++++++++++++++------------------------- 1 file changed, 87 insertions(+), 68 deletions(-) diff --git a/Main.m b/Main.m index db60175..cc41333 100644 --- a/Main.m +++ b/Main.m @@ -1,5 +1,5 @@ %% Clean -clc; +clc; close all; %% Check List: @@ -8,7 +8,7 @@ % 3.Open Virtual Machine % 4.Running /Desktop/unitree_matlab/build/udp_link press Enter to begin % 5.Open Motive Check Broadcast and Network Interface -% 6.You are good for running +% 6.You are good for running %% Robot Dog Network Parameters % this IP is the vm ip @@ -18,7 +18,7 @@ %% Robot Dog Command Initialized Control_Command = zeros(1,11,'single'); %velocity walking -Control_Command(1)=2; +Control_Command(1)=2; %% Feedback Control Parameters K_P = 0.5; % Porportional constant on velocity action K_I = 1; % Integral @@ -26,14 +26,19 @@ % constant on velocity action K_Y = pi/180; % Proportional constant on angle action -%% Control Setting -% Mode 1: Dog will keep face to yaw_set and move to target point. -% Mode 2: Dog will turn to target point and walk to it. +%% Control Setting +% Mode 1: Dog will go to Target_point. +% Mode 2: Dog will follow Way_Points. -Control_Mode=1; +Control_Mode=2; Target_Point=[0.3 -0.2]; %[x,z] -yaw_set = -1; +yaw_set = -1; +Way_Points_center =[0,0]; +Way_Points_radius =1.65; +Way_Points_theta = linspace(0,2*pi,18); +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); % [0,360) % -1: Disable yaw control @@ -62,7 +67,7 @@ % Create connection to localhost, data is now being streamed through client object HostIP = '127.0.0.1'; -theClient.Initialize(HostIP, HostIP); +theClient.Initialize(HostIP, HostIP); Dog_ID = 1; % Rigid body ID of the drone from Motive @@ -71,7 +76,12 @@ 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 % Control_Command() % @@ -88,11 +98,17 @@ % | % x <----O y(pointing up) % wall computer wall +%% Init +Way_Point_Number=1; -%% Loop +%% Loop while true % get position from camera [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, z, x, yaw] + if Control_Mode == 2 + Target_Point = [Way_Points_x(Way_Point_Number) Way_Points_z(Way_Point_Number)]; + end + %% Feedback control @@ -102,78 +118,81 @@ Norm_Vector = norm(Vector_PD_TP); Rotation_matrix = [cosd(Dog_Pos(4)), -sind(Dog_Pos(4)) ; sind(Dog_Pos(4)),cosd(Dog_Pos(4)) ]; %% Enable Control - if Norm_Vector > 0.1 %Distance > 0.1M + if Norm_Vector > 0.15 %Distance > 0.15M %% Mode 1 - if Control_Mode == 1 - if yaw_set == -1 - error_yaw_command=0; - - elseif yaw_set >=0 && yaw_set<360 - error_yaw = yaw_set-Dog_Pos(4); - if abs(error_yaw)>180 - error_yaw_command=(360+error_yaw); - else - error_yaw_command=error_yaw; - end + + if yaw_set == -1 + error_yaw_command=0; + + elseif yaw_set >=0 && yaw_set<360 + error_yaw = yaw_set-Dog_Pos(4); + if abs(error_yaw)>180 + error_yaw_command=(360+error_yaw); + else + error_yaw_command=error_yaw; end - Vector_rotated = Rotation_matrix*Vector_PD_TP'; - - Control_Command(11) = error_yaw_command*K_Y; - Control_Command(10) = Vector_rotated(1)*K_P; %x - Control_Command(9) = Vector_rotated(2)*K_P; %z end -% %% Mode2 -% if Control_Mode == 2 -% Point_Dog = [Dog_Pos(2) Dog_Pos(3)]; -% Vector_PD_TP = Target_Point-Point_Dog; -% Norm_Vector = norm(Vector_PD_TP); -% if(Norm_Vector>0.5) -% angle_r = atan2(-Vector_PD_TP(1),Vector_PD_TP(2)); -% yaw_set = rad2deg(angle_r); -% if yaw_set >= -90 -% yaw_set = yaw_set+90; -% else -% yaw_set = 360+(yaw_set+90); -% end -% end -% -% error_yaw = yaw_set-Dog_Pos(4); -% if abs(error_yaw)>180 -% error_yaw_command=(360+error_yaw); -% else -% error_yaw_command=error_yaw; -% end -% error_Z = Target_Point(1)-Dog_Pos(2); -% -% error_Z_rotated = error_Z*cosd(Dog_Pos(4))-error_X*sind(Dog_Pos(4)); -% -% -% Control_Command(11) = error_yaw_command*K_Y; -% Control_Command(10) = error_Z_rotated*K_P; -% Control_Command(10) = 0; -% -% end + Vector_rotated = Rotation_matrix*Vector_PD_TP'; + + Control_Command(11) = error_yaw_command*K_Y; + Control_Command(10) = Vector_rotated(1)*K_P; %x + Control_Command(9) = Vector_rotated(2)*K_P; %z + + % %% Mode2 + % if Control_Mode == 2 + % Point_Dog = [Dog_Pos(2) Dog_Pos(3)]; + % Vector_PD_TP = Target_Point-Point_Dog; + % Norm_Vector = norm(Vector_PD_TP); + % if(Norm_Vector>0.5) + % angle_r = atan2(-Vector_PD_TP(1),Vector_PD_TP(2)); + % yaw_set = rad2deg(angle_r); + % if yaw_set >= -90 + % yaw_set = yaw_set+90; + % else + % yaw_set = 360+(yaw_set+90); + % end + % end + % + % error_yaw = yaw_set-Dog_Pos(4); + % if abs(error_yaw)>180 + % error_yaw_command=(360+error_yaw); + % else + % error_yaw_command=error_yaw; + % end + % error_Z = Target_Point(1)-Dog_Pos(2); + % + % error_Z_rotated = error_Z*cosd(Dog_Pos(4))-error_X*sind(Dog_Pos(4)); + % + % + % Control_Command(11) = error_yaw_command*K_Y; + % Control_Command(10) = error_Z_rotated*K_P; + % Control_Command(10) = 0; + % + % end else - Control_Command(11) = 0; - Control_Command(9) = 0; - Control_Command(10) = 0; + if length(Way_Points_z)>Way_Point_Number + Way_Point_Number=Way_Point_Number+1; + else + Control_Command(11) = 0; + Control_Command(10) = 0; %x + Control_Command(9) = 0; %z + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + break; + end end disp(Control_Command); Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - %% draw + %% 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); plot(ax,circle_x,circle_y,'b-'); xlabel('X') ylabel('Z') hold on; + plot(ax,0,0,'.'); plot(ax,Target_Point(1),Target_Point(2),'x'); + 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));