Android App藍芽連接Lego機械手臂

藍芽的連接程式以前發的另一篇拿來改就好了(比較簡單)：

http://bigbabaychu.blogspot.tw/2012/06/android-nxt.html

１．著個網站是解說藍芽封包控制樂高的型態：

http://www.robotappstore.com/Knowledge-Base/What-Is-a-NXT-Bluetooth-Telegram/24.html

２．可以參考的兩個網站程式碼（比較正式的物件導向跟詳細設定參數的ＵＩ操作）：

https://github.com/Sabissimo/nxt-remote-control

https://github.com/borismus/android-nxt

Ａ　ｐｏｒｔ的封包格式

byte[] data = {0x0c, 0x00, (byte) 0x80, 0x04, 0x00, 0x32, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00};

　　　　　　　　　　　　　　　Ｂ　ｐｏｒｔ的封包格式

byte[] data = {0x0c, 0x00, (byte) 0x80, 0x04, 0x01, 0x32, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00};
　　　　　　　　　　　　　　　Ｃ　ｐｏｒｔ的封包格式
byte[] data = {0x0c, 0x00, (byte) 0x80, 0x04, 0x02, 0x32, 0x07, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00};



data[5]是正轉跟反轉　馬達的格式
data[4]是選Ａ～Ｂ～Ｃ　ｐｏｒｔ
final static byte OUT_A = 0x00;final static byte OUT_B = 0x01;final static byte OUT_C = 0x02;final static byte OUT_AB = 0x03;


ＡＰＰ的按鈕我是採setOnTouchListener觸碰事件MotionEvent.ACTION_DOWN跟MotionEvent.ACTION_UP 跟
        MotionEvent.ACTION_CANCEL參數
===================================================================



2DOF 機械手臂動畫與機械模擬  畫圓












Matlab連接LEGO NXT 元件跟支援網址:




RWTH - Mindstorms NXT Toolbox:
https://www.mathworks.com/matlabcentral/fileexchange/18646-rwth-mindstorms-nxt-toolbox



RWTH-Mindstorms NXT Toolbox:


http://www.mindstorms.rwth-aachen.de/





二維反三角函式公式:







手臂劃過軌跡圖:





程式碼:


l1 = 4; % length of first arm
l2 = 3; % length of second arm

theta1 = 0:0.1:pi/2; % all possible theta1 values
theta2 = 0:0.1:pi; % all possible theta2 values

[THETA1, THETA2] = meshgrid(theta1, theta2); % generate a grid of theta1 and theta2 values

X = l1 * cos(THETA1) + l2 * cos(THETA1 + THETA2); % compute x coordinates
Y = l1 * sin(THETA1) + l2 * sin(THETA1 + THETA2); % compute y coordinates

data1 = [X(:) Y(:) THETA1(:)]; % create x-y-theta1 dataset
data2 = [X(:) Y(:) THETA2(:)]; % create x-y-theta2 dataset

plot(X(:), Y(:), 'r.');
axis equal;
xlabel('X')
ylabel('Y')
title('X-Y co-ordinates generated for all theta1 and theta2 combinations using forward kinematics formulae')























三維模擬:







三維矩陣套用公式:








三維軌跡追蹤:





1.讀取圖片~擷取座標點









2.將座標在2D跟3D繪畫出來









3.手臂驗證畫出星星(覺得慢可以跳到1分鐘後開始看)



















座標軸三維矩陣旋轉:(星星)





4.手臂角度輸出 (q1~q2~q3~q4~q5~q6~q7)






q0=[-3.14159265358979 0.37699 0 1.31 0 1.4451 0];
L1=Link([0 12.4 0 pi/2 0 -pi/2]);
L2=Link([0 0 0 -pi/2 ]);
L3=Link([0 15.43 0 pi/2 ]);
L4=Link([0 0 0 -pi/2 0 0]);
L5=Link([0 15.925 0 pi/2]);
L6=Link([0 0 0 -pi/2 ]);
L7=Link([0 15.0 0 0 0 pi/2]);
Rbt=SerialLink([L1 L2 L3 L4 L5 L6 L7]);




顯示:

  [sqtraj1(i,1)*180/pi sqtraj1(i,2)*180/pi sqtraj1(i,3)*180/pi sqtraj1(i,4)*180/pi sqtraj1(i,5)*180/pi sqtraj1(i,6)*180/pi sqtraj1(i,7)*180/pi]


sqtraj1(i,1)*180/pi


影片:




===================================================================
機器手臂馬達需要具備那些元件:
1.編碼器(ENCODER)
主要目的偵測馬達轉到什麼角度，在好幾千次以上的測試會因為機械的磨損也有些誤差
這時候就編碼器就格外重要的說，編碼器常用的種類為:機械式絕對型編碼器~光學式絕對型編碼器...
2.馬達減速機
3.馬達的種類


====================================================================
如何驗證機器手臂:
1.精準反覆精準定位
2.旋轉延展長度跟角度(齒輪比~運動學)
3.負重測試(力矩)


===================================================================

藍芽部分如果遇到 

No code exists for Action_Found broadcast intent



請加



  ActivityCompat.requestPermissions(this, new String[]{Manifest.permission.ACCESS_FINE_LOCATION}, 200);
===================================================================


DH matrix 齊次座標


Matlab DH matrix 座標圖像:




Matlab DH matrix 座標影片:


 
Matlab DH matrix 圖像:







Matlab DH matrix 影片:





C#  矩陣相乘






程式碼:
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ConsoleApplication3
{

 

    class Program
    {
        static void Main(string[] args)
        {
            double theta1 = 0, d1 = 12.4, a1 = 0,alpha1 = 1.571 ;
            double theta2 = 0, d2 = 12.4, a2 = 0,alpha2 = 1.571;

            // double theta3,alpha3,a3,d3;
            // double theta4,alpha4,a4,d4;
            // double theta5,alpha5,a5,d5;
            // double theta6,alpha6,a6,d6; 
            int x, y, z;
            x = 4;
            y = 4;
            z = 4;
            double[,] X = { { cosd(theta1), -sind(theta1) * cosd(alpha1), sind(theta1) * sind(alpha1), a1 * cosd(theta1) }, 
                            { sind(theta1),cosd(theta1)*cosd(alpha1),-cosd(theta1)*sind(alpha1),a1* sind(theta1)}, 
                            { 0,sind(alpha1),cosd(alpha1),d1 }, 
                            {0,0,0,1 } };
            double[,] Y = { { cosd(theta2), -sind(theta2) * cosd(alpha2), sind(theta2) * sind(alpha2), a2 * cosd(theta2) }, 
                            { sind(theta2),cosd(theta2)*cosd(alpha2),-cosd(theta2)*sind(alpha2),a2* sind(theta2)}, 
                            { 0,sind(alpha2),cosd(alpha2),d2 }, 
                            {0,0,0,1 } };
            double[,] Z = new double[4, 4];
            double s1 = 1;
           

            Console.Write("X矩陣\n");
            for (int i = 0; i < x; i++)
            {
                for (int j = 0; j < y; j++)
                {
                    Console.Write(X[i, j] + " ");
                }
                Console.Write('\n');
            }

            Console.Write("Y矩陣\n");
            for (int i = 0; i < y; i++)
            {
                for (int j = 0; j < z; j++)
                {
                    //印出原本
                    Console.Write(Y[i, j] + " ");
                }
                Console.Write('\n');
            }

            Console.Write("X*Y矩陣\n");
            for (int i = 0; i < x; i++)
            {
                for (int j = 0; j < z; j++)
                {
                    Z[i, j] = 0;//初始化
                    for (int k = 0; k < y; k++)
                    {
                        Z[i, j] += X[i, k] * Y[k, j];
                    }
                    Console.Write(Math.Round(Z[i, j],4) + " ");
                }
                Console.Write('\n');
            }
            Console.Read();
        }


        public static double sind(double s1)
        {   
            return  Math.Round(Math.Sin(s1 * Math.PI / 180),4);
        }

        public static double cosd(double c1)
        {
            return  Math.Round(Math.Cos(c1 * Math.PI / 180),4);
        }
    }
}

========================================================================


程式碼:
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ConsoleApplication4
{
    class Program
    {
        static void Main(string[] args)
        {
             double theta1 = 0, d1 = 12.4, a1 = 0,alpha1 = 1.571 ;
            double theta2 = 0, d2 = 12.4, a2 = 0,alpha2 = 1.571;

            // double theta3,alpha3,a3,d3;
            // double theta4,alpha4,a4,d4;
            // double theta5,alpha5,a5,d5;
            // double theta6,alpha6,a6,d6; 
            int x, y, z;
            x = 4;
            y = 4;
            z = 4;
            double[,] X = { { Math.Cos(theta1), -Math.Sin(theta1) * Math.Cos(alpha1), Math.Sin(theta1) * Math.Sin(alpha1), a1 * Math.Cos(theta1) }, 
                            { Math.Sin(theta1),Math.Cos(theta1)*Math.Cos(alpha1),-Math.Cos(theta1)*Math.Sin(alpha1),a1* Math.Sin(theta1)}, 
                            { 0,Math.Sin(alpha1),Math.Cos(alpha1),d1 }, 
                            {0,0,0,1 } };
            double[,] Y = { { Math.Cos(theta2), -Math.Sin(theta2) * Math.Cos(alpha2), Math.Sin(theta2) * Math.Sin(alpha2), a2 * Math.Cos(theta2) }, 
                            { Math.Sin(theta2),Math.Cos(theta2)*Math.Cos(alpha2),-Math.Cos(theta2)*Math.Sin(alpha2),a2* Math.Sin(theta2)}, 
                            { 0,Math.Sin(alpha2),Math.Cos(alpha2),d2 }, 
                            {0,0,0,1 } };
            double[,] Z = new double[4, 4];
          
           

            Console.Write("X矩陣\n");
            for (int i = 0; i < x; i++)
            {
                for (int j = 0; j < y; j++)
                {
                    Console.Write(X[i, j] + " ");
                }
                Console.Write('\n');
            }

            Console.Write("Y矩陣\n");
            for (int i = 0; i < y; i++)
            {
                for (int j = 0; j < z; j++)
                {
                    //印出原本
                    Console.Write(Y[i, j] + " ");
                }
                Console.Write('\n');
            }

            Console.Write("X*Y矩陣\n");
            for (int i = 0; i < x; i++)
            {
                for (int j = 0; j < z; j++)
                {
                    Z[i, j] = 0;//初始化
                    for (int k = 0; k < y; k++)
                    {
                        Z[i, j] += X[i, k] * Y[k, j];
                    }
                    Console.Write(Math.Round(Z[i, j],4) + " ");
                }
                Console.Write('\n');
            }
            Console.Read();
        
        }
    }
}
重點:




軸控卡
=================














(後續有時間再一一說明各個的部分~跟簡易的樣本~-而有錯誤歡迎更正跟指教)