API tools faq
paste
Advertisement
Guest User

Gesture Controlled Robot

a guest
Nov 18th, 2018
116
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 30.96 KB | None | 0 0
raw download clone embed print report
  1. using System;
  2. using System.Collections.Generic;
  3. using System.ComponentModel;
  4. using System.Data;
  5. using System.Drawing;
  6. using System.Linq;
  7. using System.Text;
  8. using System.Threading.Tasks;
  9. using System.Windows.Forms;
  10. using Microsoft.Kinect;
  11. using dynamixel_sdk;
  12. using System.Runtime.InteropServices;
  13. using System.Threading;
  14.  
  15. namespace KinectTest4
  16. {
  17.     public partial class Form1 : Form
  18.     {
  19.         KinectSensor kinectSensor = null;
  20.         BodyFrameReader bodyFrameReader = null;
  21.         Body[] bodies = null;
  22.  
  23.         public static float Wy;
  24.        
  25.         public static float Hy;
  26.         public static float Hx;
  27.         public static float Hz;
  28.         public static float Ey;
  29.         public static float Ex;
  30.         public static float Sx;
  31.         public static float Sy;
  32.  
  33.         public static double Q2;
  34.         public static double angleSE;
  35.         public static double angleEH;
  36.         public static double q1;
  37.         public static double q2;
  38.         public static double anglePitch;
  39.         public static double openclose;
  40.  
  41.         public static int angle1;
  42.         public static int angle23;
  43.         public static int angle45;
  44.  
  45.         public static int d1;
  46.         public static int d23;
  47.         public static int d45;
  48.         public static int d7;
  49.  
  50.  
  51.  
  52.         // Control table address
  53.         public const int ADDR_MX_TORQUE_ENABLE = 24;                  // Control table address is different in Dynamixel model
  54.         public const int ADDR_MX_GOAL_POSITION = 30;
  55.         public const int ADDR_MX_PRESENT_POSITION = 36;
  56.  
  57.         // Data Byte Length
  58.         public const int LEN_MX_GOAL_POSITION = 2;
  59.         public const int LEN_MX_PRESENT_POSITION = 2;
  60.  
  61.         // Protocol version
  62.         public const int PROTOCOL_VERSION = 1;                   // See which protocol version is used in the Dynamixel
  63.  
  64.         // Default setting
  65.         public const int DXL4_ID = 4;                   // Dynamixel ID: 4
  66.         public const int DXL5_ID = 5;                   // Dynamixel ID: 5
  67.         public const int DXL2_ID = 2;                   // Dynamixel ID: 2
  68.         public const int DXL3_ID = 3;                   // Dynamixel ID: 3
  69.         public const int DXL1_ID = 1;                   // Dynamixel ID: 1
  70.         public const int DXL7_ID = 7;                   // Dynamixel ID: 7
  71.         public const int BAUDRATE = 57142;
  72.         public const string DEVICENAME = "COM4";              // Check which port is being used on your controller
  73.                                                               // ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
  74.  
  75.         public const int TORQUE_ENABLE = 1;                   // Value for enabling the torque
  76.         public const int TORQUE_DISABLE = 0;                   // Value for disabling the torque
  77.         public const int DXL_MINIMUM_POSITION_VALUE = 128;                 // Dynamixel will rotate between this value
  78.         public const int DXL_MAXIMUM_POSITION_VALUE = 700;                // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
  79.         public const int DXL_MOVING_STATUS_THRESHOLD = 5;                  // Dynamixel moving status threshold
  80.  
  81.         public const byte ESC_ASCII_VALUE = 0x1b;
  82.  
  83.         public const int COMM_SUCCESS = 0;                   // Communication Success result value
  84.         public const int COMM_TX_FAIL = -1001;               // Communication Tx Failed
  85.  
  86.         public static int POS45 = 128;
  87.         public static int POS23 = 138;
  88.         public static int POS1 = 410;
  89.         public static int POS7 = 700;
  90.  
  91.  
  92.  
  93.         public Form1()
  94.         {
  95.             InitializeComponent();
  96.             initializeKinect();
  97.         }
  98.  
  99.  
  100.         public void initializeKinect()
  101.         {
  102.             kinectSensor = KinectSensor.GetDefault();
  103.  
  104.             if(kinectSensor != null)
  105.             {
  106.                 kinectSensor.Open();
  107.             }
  108.  
  109.             bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();
  110.  
  111.             if(bodyFrameReader != null)
  112.             {
  113.                 bodyFrameReader.FrameArrived += Reader_FrameArrived;
  114.             }
  115.         }
  116.  
  117.         private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
  118.         {
  119.             bool dataReceived = false;
  120.  
  121.             using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
  122.             {
  123.                 if (bodyFrame != null)
  124.                 {
  125.                     if(bodies == null)
  126.                     {
  127.                         bodies = new Body[bodyFrame.BodyCount];
  128.                     }
  129.                     bodyFrame.GetAndRefreshBodyData(bodies);
  130.                     dataReceived = true;
  131.  
  132.                 }
  133.  
  134.                 if (dataReceived)
  135.                 {
  136.                     foreach(Body body in bodies)
  137.                     {
  138.                         if(body.IsTracked)
  139.                         {
  140.                             IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
  141.                             Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
  142.  
  143.                             Joint Rightwrist = joints[JointType.WristRight];
  144.                             Joint Rightthumb = joints[JointType.ThumbRight];
  145.                             Joint LeftHand = joints[JointType.WristLeft];
  146.                             Joint LeftElbow = joints[JointType.ElbowLeft];
  147.                             Joint LeftShoulder = joints[JointType.ShoulderLeft];
  148.  
  149.                             Wy = Rightwrist.Position.Y;
  150.                            
  151.  
  152.                             Hy = LeftHand.Position.Y;
  153.                             Hx = LeftHand.Position.X;
  154.                             Hz = LeftHand.Position.Z;
  155.                             Ex = LeftElbow.Position.X;
  156.                             Ey = LeftElbow.Position.Y;
  157.                             Sx = LeftShoulder.Position.X;
  158.                             Sx = LeftShoulder.Position.X;
  159.  
  160.  
  161.                             if (Hy >= Ey && Hx >= Ex)
  162.                             {
  163.                                 Q2 = Math.Atan((Hy - Ey) / (Hx - Ex));
  164.                             }
  165.                             else if (Hy >= Ey && Hx < Ex)
  166.                             {
  167.                                 Q2 = Math.PI + Math.Atan((Hy - Ey) / (Hx - Ex));
  168.                             }
  169.                             else if (Hy < Ey && Hx < Ex)
  170.                             {
  171.                                 Q2 = Math.PI + Math.Atan((Hy - Ey) / (Hx - Ex));
  172.                             }
  173.                             else if (Hy < Ey && Hx >= Ex)
  174.                             {
  175.                                 Q2 = 2 * Math.PI + Math.Atan((Hy - Ey) / (Hx - Ex));
  176.                             }
  177.  
  178.                             //angle SE and actual EH
  179.  
  180.                             if (Ey >= Sy && Ex >= Sx)
  181.                             {
  182.                                 angleSE = Math.Atan((Ey - Sy) / (Ex - Sx));
  183.  
  184.                                 if (Q2 <= Math.PI)
  185.                                 {
  186.                                     angleEH = Math.PI - angleSE + Q2;
  187.                                 }
  188.                                 else if (Q2 > Math.PI && Q2 <= 3 * Math.PI / 2)
  189.                                 {
  190.                                     angleEH = Q2 - angleSE;
  191.                                 }
  192.                                 else if (Q2 > 3 * Math.PI / 2 && Q2 <= 2 * Math.PI)
  193.                                 {
  194.                                     angleEH = Q2 - angleSE - Math.PI;
  195.                                 }
  196.                             }
  197.                             else if (Ey >= Sy && Ex < Sx)
  198.                             {
  199.                                 angleSE = Math.PI + Math.Atan((Ey - Sy) / (Ex - Sx));
  200.                                 if (Q2 <= 3 * Math.PI / 2)
  201.                                 {
  202.                                     angleEH = Math.PI - angleSE + Q2;
  203.                                 }
  204.                                 else if (Q2 > 2 * Math.PI && ((Ey - Hy) / (Hx - Ex)) < ((Ey - Sy) - (Sx - Ex)))
  205.                                 {
  206.                                     angleEH = Q2 - angleSE - Math.PI;
  207.                                 }
  208.                                 else if (Q2 > 2 * Math.PI && ((Ey - Hy) / (Hx - Ex)) >= ((Ey - Sy) - (Sx - Ex)))
  209.                                 {
  210.                                     angleEH = Math.PI - angleSE + Q2;
  211.                                 }
  212.  
  213.                             }
  214.                             else if (Ey < Sy && Ex < Sx)
  215.                             {
  216.                                 angleSE = Math.PI + Math.Atan((Ey - Sy) / (Ex - Sx));
  217.  
  218.                                 if (Q2 <= Math.PI / 2 && ((Sy - Ey) / (Sx - Ex) >= (Hy - Ey) / (Hx - Ex)))
  219.                                 {
  220.                                     angleEH = 3 * Math.PI - angleSE + Q2;
  221.                                 }
  222.                                 else if (Q2 <= Math.PI / 2 && ((Sy - Ey) / (Sx - Ex) < (Hy - Ey) / (Hx - Ex)))
  223.                                 {
  224.                                     angleEH = Q2 - angleSE + Math.PI;
  225.                                 }
  226.                                 else if (Q2 > Math.PI / 2 && Q2 <= 2 * Math.PI)
  227.                                 {
  228.                                     angleEH = Q2 - angleSE + Math.PI;
  229.                                 }
  230.  
  231.                             }
  232.                             else if (Ey < Sy && Ex >= Sx)
  233.                             {
  234.                                 angleSE = 2 * Math.PI + Math.Atan((Ey - Sy) / (Ex - Sx));
  235.  
  236.                                 if (Q2 <= Math.PI / 2)
  237.                                 {
  238.                                     angleEH = 3 * Math.PI - angleSE + Q2;
  239.                                 }
  240.                                 else if (Q2 > Math.PI / 2 && Q2 <= Math.PI && ((Hy - Ey) / (Ex - Hx) >= (Sy - Ey) / (Ex - Sx)))
  241.                                 {
  242.                                     angleEH = 3 * Math.PI - angleSE + Q2;
  243.                                 }
  244.                                 else if (Q2 > Math.PI / 2 && Q2 <= Math.PI && ((Hy - Ey) / (Ex - Hx) < (Sy - Ey) / (Ex - Sx)))
  245.                                 {
  246.                                     angleEH = Math.PI - angleSE + Q2;
  247.                                 }
  248.                                 else if (Q2 > Math.PI && Q2 <= 3 * Math.PI / 2)
  249.                                 {
  250.                                     angleEH = Q2 - angleSE + Math.PI;
  251.                                 }
  252.                                 else if (Q2 > 3 * Math.PI / 2 && Q2 <= 2 * Math.PI && ((Ey - Hy) / (Hx - Ex) < (Sy - Ey) / (Ex - Sx)))
  253.                                 {
  254.                                     angleEH = angleSE + Math.PI - Q2;
  255.                                 }
  256.                                 else if (Q2 > 3 * Math.PI / 2 && Q2 <= 2 * Math.PI && ((Ey - Hy) / (Hx - Ex) >= (Sy - Ey) / (Ex - Sx)))
  257.                                 {
  258.                                     angleEH = Math.PI - angleSE + Q2;
  259.                                 }
  260.                             }
  261.  
  262.                             angle23 = (int)((180 * angleSE / Math.PI +45-30) * 1023 / 300); ///-45
  263.                             angle45 = (int)((180 * angleEH / Math.PI -30) * 1023 / 300);
  264.                             angle1 = (int)((((Hz * 100 - 90) * 3 / 4) + 120) * 1023 / 300);
  265.  
  266.  
  267.                             if (Wy>= 0)
  268.                             {
  269.                                 d7 = 536;
  270.                             }
  271.                             else
  272.                             {
  273.                                 d7 = 705;
  274.                             }
  275.  
  276.  
  277.                             if (angle23 <= 775 && angle23 >= 128)
  278.                             {
  279.                                 d23 = angle23;
  280.                             }
  281.                             else if (angle23 > 775)
  282.                             {
  283.                                 d23 = 775;
  284.                             }
  285.                             else if (angle23 < 128)
  286.                             {
  287.                                 d23 = 128;
  288.                             }
  289.  
  290.  
  291.                             if (angle45 <= 885 && angle45 >= 128)
  292.                             {
  293.                                 d45 = angle45;
  294.                             }
  295.                             else if (angle45 > 885)
  296.                             {
  297.                                 d23 = 775;
  298.                             }
  299.                             else if (angle45 < 128)
  300.                             {
  301.                                 d23 = 128;
  302.                             }
  303.  
  304.  
  305.                             if (angle1 <= 612 && angle1 >= 410)
  306.                             {
  307.                                 d1 = angle1;
  308.                             }
  309.                             else if (angle1 > 612)
  310.                             {
  311.                                 d1 = 612;
  312.                             }
  313.                             else if (angle1 < 410)
  314.                             {
  315.                                 d1 = 410;
  316.                             }
  317.  
  318.                             q1 = angleSE * 180 / Math.PI;
  319.                             q2 = angleEH * 180 / Math.PI;
  320.  
  321.                             textBox1.Text = d1.ToString("");
  322.                             //textBox2.Text = q1.ToString("");
  323.                             //textBox3.Text = q2.ToString("");
  324.                             textBox2.Text = d23.ToString("");
  325.                             textBox3.Text = d45.ToString("");
  326.                             textBox4.Text = d7.ToString("");
  327.  
  328.  
  329.  
  330.  
  331.                         }
  332.                     }
  333.                 }
  334.  
  335.             }
  336.         }
  337.  
  338.         private void button1_Click(object sender, EventArgs e)
  339.         {
  340.             POS1 = (ushort)d1;
  341.             POS23 = (ushort)d23;
  342.             POS45 = (ushort)d45;
  343.             POS7 = (ushort)d7;
  344.            
  345.             getPos();
  346.             //getPos();
  347.             //getPos();
  348.  
  349.  
  350.         }
  351.  
  352.         private void getPos()
  353.         {
  354.             Console.WriteLine("");
  355.             Console.WriteLine(d1 + " " + d23 + " " + d45 + " " + d7);
  356.             Console.ReadLine();
  357.  
  358.             int port_num = dynamixel.portHandler(DEVICENAME);
  359.  
  360.             // Initialize PacketHandler Structs
  361.             dynamixel.packetHandler();
  362.  
  363.             // Initialize Groupsyncwrite instance
  364.             int group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION);
  365.  
  366.             int index = 0;
  367.             int dxl_comm_result = COMM_TX_FAIL;                                   // Communication result
  368.             bool dxl_addparam_result = false;                                     // AddParam result
  369.             UInt16[] dxl_goal_position45 = new UInt16[2] { (ushort)POS45, (ushort)POS45 };// { DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE }; // Goal position
  370.             UInt16[] dxl_goal_position23 = new UInt16[2] { (ushort)POS23, (ushort)POS23 };
  371.             UInt16[] dxl_goal_position1 = new UInt16[2] { (ushort)POS1, (ushort)POS1 };
  372.             UInt16[] dxl_goal_position7 = new UInt16[2] { (ushort)POS7, (ushort)POS7 };
  373.  
  374.             byte dxl_error = 0;                                                   // Dynamixel error
  375.             UInt16 dxl1_present_position = 0, dxl2_present_position = 0, dxl3_present_position = 0, dxl4_present_position = 0, dxl5_present_position = 0, dxl7_present_position = 0;          // Present position
  376.  
  377.             // Open port
  378.             if (dynamixel.openPort(port_num))
  379.             {
  380.                 //Console.WriteLine("Succeeded to open the port!");
  381.             }
  382.             else
  383.             {
  384.                 //Console.WriteLine("Failed to open the port!");
  385.                 //Console.WriteLine("Press any key to terminate...");
  386.                // Console.ReadKey();
  387.                 return;
  388.             }
  389.  
  390.             // Set port baudrate
  391.             if (dynamixel.setBaudRate(port_num, BAUDRATE))
  392.             {
  393.                // Console.WriteLine("Succeeded to change the baudrate!");
  394.             }
  395.             else
  396.             {
  397.                // Console.WriteLine("Failed to change the baudrate!");
  398.                // Console.WriteLine("Press any key to terminate...");
  399.                // Console.ReadKey();
  400.                 return;
  401.             }
  402.  
  403.             // Enable Dynamixel#1 Torque
  404.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  405.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  406.             {
  407.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  408.             }
  409.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  410.             {
  411.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  412.             }
  413.             else
  414.             {
  415.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL1_ID);
  416.             }
  417.  
  418.             // Enable Dynamixel#2 Torque
  419.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  420.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  421.             {
  422.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  423.             }
  424.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  425.             {
  426.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  427.             }
  428.             else
  429.             {
  430.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL2_ID);
  431.             }
  432.  
  433.             // Enable Dynamixel#3 Torque
  434.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  435.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  436.             {
  437.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  438.             }
  439.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  440.             {
  441.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  442.             }
  443.             else
  444.             {
  445.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL3_ID);
  446.             }
  447.  
  448.             // Enable Dynamixel#4 Torque
  449.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  450.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  451.             {
  452.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  453.             }
  454.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  455.             {
  456.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  457.             }
  458.             else
  459.             {
  460.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL4_ID);
  461.             }
  462.  
  463.             // Enable Dynamixel#5 Torque
  464.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  465.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  466.             {
  467.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  468.             }
  469.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  470.             {
  471.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  472.             }
  473.             else
  474.             {
  475.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL5_ID);
  476.             }
  477.  
  478.             // Enable Dynamixel#7 Torque
  479.             dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
  480.             if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  481.             {
  482.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  483.             }
  484.             else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  485.             {
  486.                 //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  487.             }
  488.             else
  489.             {
  490.                 Console.WriteLine("Dynamixel{0} has been successfully connected ", DXL7_ID);
  491.             }
  492.  
  493.             while (true)
  494.             {
  495.                 //Console.WriteLine("Press any key to continue! (or press ESC to quit!)");
  496.                 //if (Console.ReadKey().KeyChar == ESC_ASCII_VALUE)
  497.                 //break;
  498.  
  499.                 // Add Dynamixel#1 goal position value to the Syncwrite parameter storage
  500.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position1[index], LEN_MX_GOAL_POSITION);
  501.                 if (dxl_addparam_result != true)
  502.                 {
  503.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL1_ID);
  504.                     return;
  505.                 }
  506.  
  507.                 // Add Dynamixel#2 goal position value to the Syncwrite parameter storage
  508.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position23[index], LEN_MX_GOAL_POSITION);
  509.                 if (dxl_addparam_result != true)
  510.                 {
  511.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL2_ID);
  512.                     return;
  513.                 }
  514.  
  515.                 // Add Dynamixel#3 goal position value to the Syncwrite parameter storage
  516.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL3_ID, dxl_goal_position23[index], LEN_MX_GOAL_POSITION);
  517.                 if (dxl_addparam_result != true)
  518.                 {
  519.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL3_ID);
  520.                     return;
  521.                 }
  522.  
  523.                 // Add Dynamixel#4 goal position value to the Syncwrite storage
  524.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL4_ID, dxl_goal_position45[index], LEN_MX_GOAL_POSITION);
  525.                 if (dxl_addparam_result != true)
  526.                 {
  527.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL4_ID);
  528.                     return;
  529.                 }
  530.  
  531.                 // Add Dynamixel#5 goal position value to the Syncwrite parameter storage
  532.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL5_ID, dxl_goal_position45[index], LEN_MX_GOAL_POSITION);
  533.                 if (dxl_addparam_result != true)
  534.                 {
  535.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL5_ID);
  536.                     return;
  537.                 }
  538.  
  539.                 // Add Dynamixel#7 goal position value to the Syncwrite parameter storage
  540.                 dxl_addparam_result = dynamixel.groupSyncWriteAddParam(group_num, DXL7_ID, dxl_goal_position7[index], LEN_MX_GOAL_POSITION);
  541.                 if (dxl_addparam_result != true)
  542.                 {
  543.                     Console.WriteLine("[ID: {0}] groupSyncWrite addparam failed", DXL7_ID);
  544.                     return;
  545.                 }
  546.  
  547.  
  548.  
  549.                 // Syncwrite goal position
  550.                 dynamixel.groupSyncWriteTxPacket(group_num);
  551.                 if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  552.                     //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  553.  
  554.                 // Clear syncwrite parameter storage
  555.                 dynamixel.groupSyncWriteClearParam(group_num);
  556.  
  557.                 do
  558.                 {
  559.                     // Read Dynamixel#1 present position
  560.                     dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION);
  561.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  562.                     {
  563.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  564.                     }
  565.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  566.                     {
  567.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  568.                     }
  569.  
  570.                     // Read Dynamixel#2 present position
  571.                     dxl2_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_PRESENT_POSITION);
  572.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  573.                     {
  574.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  575.                     }
  576.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  577.                     {
  578.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  579.                     }
  580.  
  581.                     // Read Dynamixel#3 present position
  582.                     dxl3_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_MX_PRESENT_POSITION);
  583.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  584.                     {
  585.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  586.                     }
  587.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  588.                     {
  589.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  590.                     }
  591.  
  592.                     // Read Dynamixel#4 present position
  593.                     dxl4_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_MX_PRESENT_POSITION);
  594.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  595.                     {
  596.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  597.                     }
  598.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  599.                     {
  600.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  601.                     }
  602.  
  603.                     // Read Dynamixel#5 present position
  604.                     dxl5_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_MX_PRESENT_POSITION);
  605.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  606.                     {
  607.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  608.                     }
  609.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  610.                     {
  611.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  612.                     }
  613.  
  614.                     // Read Dynamixel#7 present position
  615.                     dxl7_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_MX_PRESENT_POSITION);
  616.                     if ((dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
  617.                     {
  618.                        // Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result)));
  619.                     }
  620.                     else if ((dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
  621.                     {
  622.                         //Console.WriteLine(Marshal.PtrToStringAnsi(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)));
  623.                     }
  624.  
  625.  
  626.                     //Console.WriteLine("[ID: {0}] [ID: {1}] GoalPos: {2}  PresPos: {3}  PresPos: {4}  [ID: {5}] [ID: {6}] GoalPos: {7}  PresPos: {8}  PresPos: {9}",
  627.                     //     DXL2_ID, DXL3_ID, dxl_goal_position23[index], dxl2_present_position, dxl3_present_position, DXL4_ID, DXL5_ID, dxl_goal_position45[index], dxl4_present_position, dxl5_present_position);
  628.  
  629.                     // Console.WriteLine("[ID: {0}] GoalPos: {1}  PresPos: {2}  [ID: {3}] GoalPos: {4}  PresPos: {5}",
  630.                     //      DXL1_ID, dxl_goal_position1[index], dxl1_present_position, DXL7_ID, dxl_goal_position7[index], dxl7_present_position);
  631.  
  632.                     Thread.Sleep(1000);
  633.                     Console.WriteLine(dxl1_present_position + " " + dxl2_present_position + " " + dxl3_present_position + " " + dxl4_present_position + " " + dxl5_present_position + " " + dxl7_present_position);
  634.  
  635.                 }
  636.  
  637.                 while ((Math.Abs(dxl_goal_position45[index] - dxl4_present_position) > DXL_MOVING_STATUS_THRESHOLD)
  638.                 && (Math.Abs(dxl_goal_position45[index] - dxl5_present_position) > DXL_MOVING_STATUS_THRESHOLD)  
  639.                 && (Math.Abs(dxl_goal_position23[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)
  640.                 && (Math.Abs(dxl_goal_position23[index] - dxl3_present_position) > DXL_MOVING_STATUS_THRESHOLD)
  641.                 && (Math.Abs(dxl_goal_position1[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD)
  642.                 && (Math.Abs(dxl_goal_position7[index] - dxl7_present_position) > DXL_MOVING_STATUS_THRESHOLD));
  643.  
  644.                 // Change goal position
  645.                 if (index == 0)
  646.                 {
  647.                     index = 1;
  648.                 }
  649.                 else
  650.                 {
  651.                     index = 0;
  652.                 }
  653.                
  654.                 dynamixel.closePort(port_num);
  655.  
  656.                 return;
  657.  
  658.             }
  659.         }
  660.     }
  661. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!