Skip to content

Commit

Permalink
add way points control
Browse files Browse the repository at this point in the history
  • Loading branch information
XI LUO committed Mar 21, 2023
1 parent edb63dd commit 2cdd55f
Showing 1 changed file with 87 additions and 68 deletions.
155 changes: 87 additions & 68 deletions Main.m
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
%% Clean
clc;
clc;
close all;

%% Check List:
Expand All @@ -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
Expand All @@ -18,22 +18,27 @@
%% 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
K_D = 1;
% 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

Expand Down Expand Up @@ -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

Expand All @@ -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()
%
Expand All @@ -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

Expand All @@ -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));
Expand Down

0 comments on commit 2cdd55f

Please sign in to comment.