add P control for Z,X,Yaw

Get_Dog_Postion.m

@@ -9,7 +9,7 @@
 Dog_Pos = frameData.RigidBodies(id);
 %% Motive coordiate frame (yaw)
 % wall wall wall wall wall
-%                        0(after) 
+%                        0(after)
 %                        0
 %                        ^ z
 %                        |
@@ -25,21 +25,18 @@
 if ~isempty(Dog_Pos)
     z=Dog_Pos.z;
     x=Dog_Pos.x;
-    
+
     q = [Dog_Pos.qx, Dog_Pos.qy, Dog_Pos.qz, Dog_Pos.qw];
     Eul_ang = quat2eul(q);
-    
+%% convert yaw
     if abs(Eul_ang(1)) > pi/2
         if -Eul_ang(2) < 0
             yaw = -Eul_ang(2)+2*pi;
-        else 
+        else
             yaw = -Eul_ang(2);
         end
     else
-
-            yaw = pi+Eul_ang(2);
-
-
+        yaw = pi+Eul_ang(2);
     end
     yaw = yaw*180/pi;
 

Main.m

@@ -15,18 +15,14 @@
 % x <----O y(pointing up)
 % wall computer wall
 
-%% Robot dog command
-%
-%     +(11) +(9)  -(11)
-%     +(10)  dog  -(10)
-%           -(9)
-%
+
 %% System Parameters
 
-K_v = 1; % Porportional constant on velocity action
-K_i = 1;  % Integral
+K_P = 0.5; % Porportional constant on velocity action
+K_I = 1;  % Integral
+K_D = 1;
 % constant on velocity action
-K_h = 1; % Proportional constant on angle action
+K_Y = pi/180; % Proportional constant on angle action
 
 
 %% Instantiate client object to run Motive API commands
@@ -43,15 +39,71 @@
 
 Dog_ID = 1; % Rigid body ID of the drone from Motive
 
-% fig = figure();
-% ax = axes('Parent',fig);
-% line_handle = plot(ax,Dog_Pos(2),Dog_Pos(3),'o');
+Target_Point=[0 0];
+yaw_set = 0;
+
+%% figure for movtion track
+fig = figure();
+ax = axes('Parent',fig);
+line_handle = plot(ax,Dog_Pos(3),Dog_Pos(2),'o');
+
+xlabel('X')
+ylabel('Z')
+
+arrow_length=0.2;
+
+
+%%
+% while true
+%     [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID);
+% 
+% end
+
+%velocity walking
+Control_Command(1)=2; 
+%% Robot dog command
+%
+%     +(11) +(9)  -(11)
+%     +(10)  dog  -(10)
+%           -(9)
+%
+%%
 while true
-    [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID);
-    disp(Dog_Pos(4));
-%     set(line_handle, 'XData',Dog_Pos(2),'YData',Dog_Pos(3))
-%     drawnow;
-    pause(0.1);
+    [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, z, x, yaw]
+
+    error_Z = Target_Point(1)-Dog_Pos(2);
+    error_X = -Target_Point(2)-Dog_Pos(3);
+
+    disp(error_X);
+
+    error_yaw = yaw_set-Dog_Pos(4);
+
+    if abs(error_yaw)>180
+        error_yaw=(360+error_yaw);
+    end
+
+    %disp(error_yaw*K_Y);
+    Control_Command(11) = error_yaw*K_Y;
+
+    if abs(error_yaw)<10
+        Control_Command(9) = error_Z*K_P;
+        Control_Command(10) = error_X*K_P;
+    end
+
+
+
+    Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command);
+
+    pause(0.02);
+
+    dy=arrow_length*cosd(Dog_Pos(4));
+    dx=arrow_length*sind(Dog_Pos(4));
+    quiver(Dog_Pos(3),Dog_Pos(2),dx,dy,'r','LineWidth',0.2,'MaxHeadSize',2);
+    set(gca,'XDir','reverse');
+    xlim(ax,[-2,2]);
+    ylim(ax,[-2,2]);
+    %set(line_handle, 'XData',Dog_Pos(3),'YData',Dog_Pos(2))
+    drawnow;
 end