fixed mode1 bug

Get_Dog_Postion.m

@@ -40,12 +40,12 @@
     end
     yaw = yaw*180/pi;
 
-    result = [time_stamp z x yaw];
+    result = [time_stamp x z yaw];
 else
     time_stamp=0;
     z=0;
     x=0;
     yaw=0;
-    result = [time_stamp z x yaw];
+    result = [time_stamp x z yaw];
 end
 

Main.m

@@ -31,7 +31,7 @@
 % Mode 2: Dog will turn to target point and walk to it.
 
 Control_Mode=1;
-Target_Point=[0 0];
+Target_Point=[0.3 -0.2]; %[x,z]
 yaw_set = -1; 
 
 % [0,360)
@@ -69,8 +69,7 @@
 %% figure for movtion track
 fig = figure();
 ax = axes('Parent',fig);
-xlabel('X')
-ylabel('Z')
+
 arrow_length=0.2;
 
 %% Robot dog command
@@ -89,60 +88,103 @@
 %        |
 % x <----O y(pointing up)
 % wall computer wall
+
 %% Loop 
 while true
     % get position from camera
     [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, z, x, yaw]
 
-    %% feedback control
-    if Control_Mode == 1
-        if yaw_set == -1
-            error_yaw = yaw_set-Dog_Pos(4);
-            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;
+    %% Feedback control
+
+    % Calculate Distance
+    Point_Dog = [Dog_Pos(2) Dog_Pos(3)];  %[x,z]
+    Vector_PD_TP = Target_Point-Point_Dog;
+    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 
+        %% 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
             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
-
-        error_Z = Target_Point(1)-Dog_Pos(2);
-        error_X = -Target_Point(2)-Dog_Pos(3);
-
-        error_Z_rotated = error_Z*cosd(error_yaw)-error_X*sind(error_yaw);
-        error_X_rotated = error_Z*sind(error_yaw)+error_X*cosd(error_yaw);
-
-        Control_Command(11) = error_yaw_command*K_Y;
-        Control_Command(9) = error_Z_rotated*K_P;
-        Control_Command(10) = error_X_rotated*K_P;
-
+%         %% 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;
     end
     disp(Control_Command);
     Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command);
 
-    %50Hz
-    pause(0.02);
+
     %% 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,'x');
+    plot(ax,Target_Point(1),Target_Point(2),'x');
     ax.DataAspectRatio=[1 1 1];
     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);
+    quiver(Dog_Pos(2),Dog_Pos(3),dx,dy,'r','LineWidth',0.2,'MaxHeadSize',2);
     set(gca,'XDir','reverse');
     xlim(ax,[-3,3]);
     ylim(ax,[-2,2]);
     hold off;
     drawnow;
+    %% 50Hz
+    pause(0.02);
 end