add movtion function

Get_Dog_Postion.asv

@@ -0,0 +1,49 @@
+function [result] = Get_Dog_Postion(client,id)
+
+frameData = client.GetLastFrameOfData();
+
+%Get the time stamp
+time_stamp = frameData.fTimestamp;
+
+%Get the marker data
+Dog_Pos = frameData.RigidBodies(id);
+%% Motive coordiate frame
+% wall wall wall wall wall
+%             0
+%             ^ z
+%             |
+%             |
+% pi/2 x <----O      -pi/2
+%
+%
+%             0
+% wall computer wall
+
+%%
+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);
+    if abs(Eul_ang(1)) < pi/2
+        if -Eul_ang(2) < 0
+            yaw = -Eul_ang(2)+2*pi;
+        else 
+            yaw = -Eul_ang(2);
+        end
+    else
+        if -Eul_ang(2) 
+
+    end
+    yaw = -Eul_ang(2)*180/pi;
+
+    result = [time_stamp z x yaw];
+else
+    time_stamp=0;
+    z=0;
+    x=0;
+    yaw=0;
+    result = [time_stamp z x yaw];
+end
+

Get_Dog_Postion.m

@@ -0,0 +1,54 @@
+function [result] = Get_Dog_Postion(client,id)
+
+frameData = client.GetLastFrameOfData();
+
+%Get the time stamp
+time_stamp = frameData.fTimestamp;
+
+%Get the marker data
+Dog_Pos = frameData.RigidBodies(id);
+%% Motive coordiate frame (yaw)
+% wall wall wall wall wall
+%                        0(after) 
+%                        0
+%                        ^ z
+%                        |
+%                        |
+% 90(after)  pi/2 x <----O      -pi/2 270(after)
+%
+%
+%                        0
+%                       180(after)
+% wall computer wall
+
+%%
+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);
+
+    if abs(Eul_ang(1)) > pi/2
+        if -Eul_ang(2) < 0
+            yaw = -Eul_ang(2)+2*pi;
+        else 
+            yaw = -Eul_ang(2);
+        end
+    else
+
+            yaw = pi+Eul_ang(2);
+
+
+    end
+    yaw = yaw*180/pi;
+
+    result = [time_stamp z x yaw];
+else
+    time_stamp=0;
+    z=0;
+    x=0;
+    yaw=0;
+    result = [time_stamp z x yaw];
+end
+

Main.asv

@@ -0,0 +1,58 @@
+%% Clean
+clc; 
+close all;
+
+% this IP is the vm ip
+Robot_Dog_IP = '192.168.12.184';
+Robot_Dog_Port = 1145;
+
+Control_Command = zeros(1,11,'single');
+%% Motive coordiate frame
+% wall wall wall wall wall
+%        ^ z
+%        |
+%        |
+% 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
+% constant on velocity action
+K_h = 1; % Proportional constant on angle action
+
+
+%% Instantiate client object to run Motive API commands
+% 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
+
+fig = figure();
+ax = axes('Parent',fig);
+line_handle = plot(ax,Dog_Pos(2),Dog_Pos(3),'o');
+while true
+    [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID);
+    disp(Dog_Pos);
+    set(line_handle, 'z',Dog_Pos(2),'x',Dog_Pos(3))
+    drawnow;
+    pause(1);
+end
+
+
+

Main.m

@@ -1,5 +1,58 @@
-% Clean
+%% Clean
 clc; 
 close all;
 
-% System Parameters
+% this IP is the vm ip
+Robot_Dog_IP = '192.168.12.184';
+Robot_Dog_Port = 1145;
+
+Control_Command = zeros(1,11,'single');
+%% Motive coordiate frame
+% wall wall wall wall wall
+%        ^ z
+%        |
+%        |
+% 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
+% constant on velocity action
+K_h = 1; % Proportional constant on angle action
+
+
+%% Instantiate client object to run Motive API commands
+% 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
+
+% fig = figure();
+% ax = axes('Parent',fig);
+% line_handle = plot(ax,Dog_Pos(2),Dog_Pos(3),'o');
+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);
+end
+
+
+

Robot_Dog.asv

@@ -0,0 +1,11 @@
+function Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Data)
+% Send command to robot dog
+
+% Create a UDP object
+Robot_Dog_UDP = udpport;
+% Transform single float to uint8
+Control_Data_Uint8 = typecast(Control_Data,'uint8');
+%
+write(Robot_Dog_UDP,Control_Data_Uint8,Robot_Dog_IP,Robot_Dog_Port);
+end
+

Robot_Dog.m

@@ -1,7 +1,11 @@
-function [outputArg1,outputArg2] = Robot_Dog(inputArg1,inputArg2)
-%ROBOT_DOG Summary of this function goes here
-%   Detailed explanation goes here
-outputArg1 = inputArg1;
-outputArg2 = inputArg2;
+function Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Data)
+% Send command to robot dog
+
+% Create a UDP object
+Robot_Dog_UDP = udpport;
+% Transform single float to uint8
+Control_Data_Uint8 = typecast(Control_Data,'uint8');
+% Send Command
+write(Robot_Dog_UDP,Control_Data_Uint8,Robot_Dog_IP,Robot_Dog_Port);
 end