add filter

Dog_connection_test.m

@@ -0,0 +1,61 @@
+%% Clean
+clc;
+close all;
+
+%% Robot Dog Network Parameters
+% this IP is the vm ip
+Robot_Dog_IP = '192.168.123.161';
+Robot_Dog_Port = 1145;
+
+
+%% parameters
+run_time = 5;
+
+x_speed = 1.5; %(0.11,1]
+z_speed = 0;
+%% Robot Dog Command Initialized
+Control_Command = zeros(1,11,'single');
+%velocity walking
+% 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
+
+%% 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
+
+Control_Command(1)=12;
+
+Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command);
+
+
+

Follow.m

@@ -21,7 +21,7 @@
 % Porportional constant on velocity action
 K_P_x = 0.6;
 K_P_z = 0.6;
-K_P_yaw = 0.07;
+K_P_yaw = 0.3;
 
 % Integral
 K_I_x = 1.8;
@@ -48,9 +48,9 @@
 
 %% Control Setting
 
-Control_Speed=0.8;
+Control_Speed=0.6;
 
-Switch_Distance=0.1;
+Switch_Distance=0.8;
 
 % Target Point
 %[x,z]
@@ -72,7 +72,7 @@
 %          180
 %
 % wall computer wall
-yaw_set = 0;
+yaw_set = -2;
 
 
 %% Instantiate client object to run Motive API commands
@@ -92,7 +92,7 @@
 theClient.Initialize(HostIP, HostIP);
 
 Dog_ID = 1; % Rigid body ID of the drone from Motive
-Drone_ID = 2;
+Car_ID = 2;
 % Robot dog command
 %     Control_Command()
 %
@@ -113,6 +113,7 @@
 %
 % wall computer wall
 
+
 %% Init Parameters
 
 integral_x = 0;
@@ -139,12 +140,10 @@
     % 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];
+    [Car_time, Car_x, Car_z, Car_yaw] = Get_Dog_Postion(theClient, Car_ID);
+    if abs(Car_x)<2.5 || abs(Car_z)<1.6
+        Target_Point = [Car_x,Car_z];
     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;
@@ -153,6 +152,7 @@
 
         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';
 
@@ -187,14 +187,19 @@
         else
             Control_Command(10) = 0;   %x
             Control_Command(9)  = 0;   %z
-            Control_Command(11) = 0; %yaw
+            Control_Command(11) = Control_yaw; %yaw
+            if Error_Yaw < 5
+                Control_Command(11) = 0;
+            end
             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

Real_Time_Location_Figure.m

@@ -0,0 +1,38 @@
+function Real_Time_Location_Figure(x,z,yaw,t_x,t_z,arrow_length,r,r_x,r_z)
+%REAL_TIME_LOCATION_FIGURE Summary of this function goes here
+%   Detailed explanation goes here
+circle_center =[r_x,r_z];
+circle_radius =r;
+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);
+
+fig = findall(0,'Type','Figure');
+if isempty(fig)
+    fig = figure();
+end
+
+ax = axes('Parent',fig);
+cla(ax); % Clears the current axes
+hold(ax, 'on');
+
+plot(ax,circle_x,circle_y,'b-');
+xlabel('X')
+ylabel('Z')
+
+plot(ax,0,0,'.');
+plot(ax,t_x,t_z,'.','Color','r','MarkerSize',20);
+
+ax.DataAspectRatio=[1 1 1];
+dy=arrow_length*cosd(yaw);
+dx=arrow_length*sind(yaw);
+quiver(x,z,dx,dy,'r','LineWidth',0.2,'MaxHeadSize',2);
+
+plot(x,z,'Color','r');
+set(gca,'XDir','reverse');
+xlim(ax,[-3,3]);
+ylim(ax,[-2,2]);
+
+hold(ax, 'off');
+drawnow;
+end

Yaw_Controllor.m

@@ -1,4 +1,4 @@
-function [error_yaw_command,yaw_set_mode2] = Yaw_Controllor(yaw_set,Dog_yaw,Vector)
+function [error_yaw_command,yaw_ref] = Yaw_Controllor(yaw_set,Dog_yaw,Vector)
 %YAW_CONTROLLOR Summary of this function goes here
 %   Detailed explanation goes here
 % Yaw
@@ -12,7 +12,6 @@
 %          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
@@ -22,7 +21,7 @@
         yaw_set_mode2 = yaw_set_mode2+360;
     end
     error_yaw = yaw_set_mode2-Dog_yaw;
-
+    yaw_ref=yaw_set_mode2;
     if error_yaw<-180
         error_yaw_command=(360+error_yaw);
     elseif error_yaw > 180
@@ -39,6 +38,7 @@
     else
         error_yaw_command=error_yaw;
     end
+    yaw_ref = yaw_set;
 end
 
 end