2016年4月25日 星期一

繼續 AMIGO Arm 開發


同時間重拾 AMIGO Arm 及 AMIGO Cubebot 的開發,真的很花時間。不過,如果能完成兩部機體,相信會十分滿足。

早上畫了 AMIGO Cubebot 平台部件,成功輸出;晚間轉到 AMIGO Arm 程式開發。重讀一年前計算好的「Inverse Kinematics」,把它填入各關節的尺寸。先編寫 Javascript 程式按輸入的座標來計算四個伺服馬達的角度。用 Javascript 是方便快捷,待測試成功後便在 Arduino 端重寫一次。以是下 Javascript 代碼:
<html>
    <head>
        <script>
            var _pi = 3.141592654;

            //  All distance are in mm
            var _screenWidthMM = 148;
            var _screenHeightMM = 196;

            var _aArmDistanceMM = 143;                        // a
            var _bArmDistanceMM = 143;                        // b
            var _penAndJointDistanceMM = 42;                // c
            var _baseAndJointDistanceMM = 88;                // Z1
            var _penAndSurfaceDistanceMM = 40;                // Z2

            function radians2Degree(radians)  {
                var degrees = radians*(180/_pi);
                return degrees;
            }

            function calculate()  {
                var x = document.getElementById("x").value;
                var y = document.getElementById("y").value;

                //  X = 0 = Horizontal center
                x -= (768/2);

                var gamma = radians2Degree(Math.atan2(x, y));

                var verticalDistance = ((y*_screenHeightMM)/1024)-_penAndJointDistanceMM;
                var penJointAndBaseJointHeightMM = (_baseAndJointDistanceMM-_penAndSurfaceDistanceMM);

                //  L
                var baseJointAndTouchPointSlopeMM = Math.sqrt(Math.pow(verticalDistance, 2)+Math.pow(penJointAndBaseJointHeightMM, 2));

                var aSquare = Math.pow(_aArmDistanceMM, 2);
                var bSquare = Math.pow(_bArmDistanceMM, 2);
                var lSquare = Math.pow(baseJointAndTouchPointSlopeMM, 2);

                var alpha1 = Math.acos((lSquare+aSquare-bSquare)/(2*baseJointAndTouchPointSlopeMM*_aArmDistanceMM));
                var alpha2 = Math.acos((_baseAndJointDistanceMM-_penAndSurfaceDistanceMM)/baseJointAndTouchPointSlopeMM);
                var alpha = radians2Degree(alpha1+alpha2);

                var beta = radians2Degree(Math.acos((aSquare+bSquare-lSquare)/(2*_aArmDistanceMM*_bArmDistanceMM)));

                var delta = (alpha+beta)-90;

                document.getElementById("output").innerHTML = 
                    "Vertical distance: "+verticalDistance+" mm"+
                    "<br>L: "+baseJointAndTouchPointSlopeMM+" mm"+
                    "<br>Alpha: "+alpha+" degrees"+
                    "<br>Beta: "+beta+" degrees"+
                    "<br>Delta: "+delta+" degrees"+
                    "<br>Gamma: "+gamma+" degrees";
            }
        </script>
    </head>
    <body>
        Please enter a coordinate within 768x1024
         <br>X:<input id="x" value="768">
         <br>Y:<input id="y" value="1024">
         <br><button onclick="calculate();">Calculate</button>
         <br><div id="output" style="margin-top:10px;"></div>

        <script>calculate();</script>
    </body>
</html>

試過不同座標,角度值都看似正確;於是把程序轉換成 Arduino Nano 的 C 語言。以相同座標輸入,得出了不同的答案。原因是 Arduino Nano 這些小板,在處理變數類型時,數值範圍會來得細小。把一些出現較大數值的地方改為 double 或 long 後,再在某些運算中標明使用 double 後,最終得出跟 Javascript 端相同的結果。現在連 Arduino 端也準備好,下一步就是測試及調校了。希望能趕及 5 月 5 日展出!

沒有留言: