fix send/get angles&coords

Author: lxw119

Mycobot.csharp/MyCobot.cs

@@ -123,9 +123,10 @@ public void PowerOff()
         /// <param name="jointNo">joint number: 1 ~ 6</param>
         /// <param name="angle">angle value: -180 ~ 180 </param>
         /// <param name="speed">speed value: 0 ~ 100</param>
-        public void SendOneAngle(int jointNo, int angle, int speed)
+        public void SendOneAngle(int jointNo, double angle, int speed)
         {
-            int _angle = angle * 100, idx = 0;
+            int _angle = (int)angle * 100;
+            int idx = 0;
             var command = new byte[9];
             // set header
             command[idx++] = 0xfe;
@@ -147,19 +148,22 @@ public void SendOneAngle(int jointNo, int angle, int speed)
         /// </summary>
         /// <param name="angles">angles[], length: 6</param>
         /// <param name="speed">speed value: 0 ~ 100</param>
-        public void SendAngles(int[] angles, int speed)
+        public void SendAngles(double[] angles, int speed)
         {
             var command = new byte[18];
             var idx = 0;
+            int[] angles1 = {0,0,0,0,0,0};
             command[idx++] = 0xfe;
             command[idx++] = 0xfe;
             command[idx++] = 0x0f;
             command[idx++] = 0x22;
             for (var j = 0; j < angles.Length; ++j)
             {
                 angles[j] *= 100;
+                angles1[j] = (int)(angles[j]);
+                //angles1[j] = (int)(angles[j]*100);
             }
-            var a = Int16ArrToBytes(angles);
+            var a = Int16ArrToBytes(angles1);
             foreach (var t in a)
             {
                 command[idx++] = t;
@@ -174,7 +178,7 @@ public void SendAngles(int[] angles, int speed)
         /// Get all angles
         /// </summary>
         /// <returns>int[], length: 6</returns>
-        public int[] GetAngles()
+        public float[] GetAngles()
         {
             byte[] command = { 0xfe, 0xfe, 0x02, 0x20, 0xfa };
             Write(command, 0, command.Length);
@@ -186,16 +190,16 @@ public int[] GetAngles()
             var m_recvBytes = new byte[_serialPort.BytesToRead];
             var result = _serialPort.Read(m_recvBytes, 0, m_recvBytes.Length);
             if (result <= 0)
-                return Array.Empty<int>();
+                return Array.Empty<float>();
 
             // get valid index
             var idx = getValidIndex(m_recvBytes);
             if (idx == -1)
-                return Array.Empty<int>();
+                return Array.Empty<float>();
 
             // process data
-            var len = (int)m_recvBytes[idx] - 1;
-            var res = new int[6];
+            var len = (float)m_recvBytes[idx] - 1;
+            var res = new float[6];
             var resIdx = 0;
             for (var i = idx + 1; i < idx + len; i += 2)
             {
@@ -211,9 +215,11 @@ public int[] GetAngles()
         /// <param name="coord">coord No: 1 - 6</param>
         /// <param name="value">coord value</param>
         /// <param name="speed">speed: 0 ~ 100</param>
-        public void SendOneCoord(int coord, int value, int speed)
+        public void SendOneCoord(double coord, int value, int speed)
         {
-            int idx = 0, _value = coord < 4 ? coord * 10 : coord * 100;
+            int idx = 0 ;
+            int _value = (int)(coord < 4 ? value * 10 : value * 100);
+
             var command = new byte[9];
             // set header
             command[idx++] = 0xfe;
@@ -238,10 +244,11 @@ public void SendOneCoord(int coord, int value, int speed)
         /// <param name="coords">int[], length: 6</param>
         /// <param name="speed">speed: int, value: 0 ~ 100</param>
         /// <param name="mode">mode:  0 - angular, 1 - linear</param>
-        public void SendCoords(int[] coords, int speed, int mode)
+        public void SendCoords(double[] coords, int speed, int mode)
         {
             var command = new byte[19];
             var idx = 0;
+            int[] coords1= {0,0,0,0,0,0};
             // set header
             command[idx++] = 0xfe;
             command[idx++] = 0xfe;
@@ -251,13 +258,15 @@ public void SendCoords(int[] coords, int speed, int mode)
             for (var i = 0; i < 3; ++i)
             {
                 coords[i] *= 10;
+                coords1[i] = (int)coords[i];
             }
             for (var i = 3; i < 6; ++i)
             {
                 coords[i] *= 100;
+                coords1[i] = (int)coords[i];
             }
             // append to command
-            var a = Int16ArrToBytes(coords);
+            var a = Int16ArrToBytes(coords1);
             foreach (var t in a)
             {
                 command[idx++] = t;

Mycobot.csharp/Program.cs

@@ -7,17 +7,32 @@ class Test
     {
         static void Main(string[] args)
         {
-            MyCobot mc = new MyCobot("COM57");//树莓派机械臂串口名称：/dev/ttyAMA0
+            MyCobot mc = new MyCobot("COM14");//树莓派机械臂串口名称：/dev/ttyAMA0
             mc.Open();
             Thread.Sleep(5000);//windows打开串口后，需要等待5s，Windows打开串口底部basic会重启
             // int[] angles = new[] {100, 100, 100, 100, 100, 100};
             // mc.SendAngles(angles, 50);
-            var recv = mc.GetAngles();
-            foreach (var v in recv)
-            {
-               Console.WriteLine(v);
-            }
+            double[] a = { 0, 0, 90, 0, 0, 0 };
+            mc.SendAngles(a, 70);
 
+                Thread.Sleep(1000);
+                float[] cdd = mc.GetAngles();
+                foreach (var v in cdd)
+                {
+                    Console.WriteLine(v);
+                }
+            mc.SendOneCoord(1, -60, 70);
+            Thread.Sleep(1000);
+            mc.SendOneCoord(1, 20, 70);
+            /*
+                        mc.SendOneCoord(1,20, 70);
+                        Thread.Sleep(1000);
+                        var recv = mc.GetAngles();
+                        foreach (var v in recv)
+                        {
+                           Console.WriteLine(v);
+                        }
+            */
             // int[] coords = new[] {160, 160, 160, 0, 0, 0};
             // mc.SendCoords(coords, 90, 1);
             // Thread.Sleep(5000);
@@ -26,9 +41,13 @@ static void Main(string[] args)
             // {
             // Console.WriteLine(v);
             // }
+            /*           float[] b = { 0,0,0,0,0,0 };
+                       mc.SendAngles(b, 70);
+                       Thread.Sleep(1000);
+                       mc.SendOneAngle(3, 90, 70);
+                       Thread.Sleep(1000);
+            */
 
-            mc.SendOneAngle(1,0, 70);
-            Thread.Sleep(100);
             /*var angle = new int[6];
             angle = mc.GetAngles();
             foreach (var v in angle)