User Tools

Site Tools


lego_2link_arm

This is an old revision of the document!


LEGO 2-Link Robot Arm Tutorial

Author: Norberto Torres-Reyes Email: [email protected]
Date: Last modified on 03/08/19
Keywords: kinematics, planar, robotic arm, 2 link, lego, NXC, mindstorm

Motivation and Audience

This tutorial is for anyone who wishes to create a 2-link robotic arm using LEGO Mindstorm to reinforce theoretical knowledge about 2-link planar mechanisms. Readers of this tutorial are recommended to have the following background and interests:

*Previous experience with LEGO Mindstorm
*Matlab and C-based programming experience
*Knowledge of linear algebra
*Interests in robotics
*Experience with Solidworks

The rest of the tutorial is presented as follows:

Parts List

LEGO NXT parts list is given in full detail below. Most parts can be obtained from the Mindstorm kit, although some extra parts will be needed from LEGO's Technic line of bricks.

Theoretical Background

The theoretical background needed for this tutorial can be fully covered HERE. In short, it covers the kinematics of a 2-link arm using using three different methods: Matrix algebra, Geometry, and computational tools. Additionally,

Build Plans

A few different build plans are available. This includes photographic and 3D model based plans as well as a Solidworks based build plan. The reader may wish recreate the model in both Solidworks and with LEGO parts to familiarize themselves the procedures. The build plans are organized below:





NXT Brick Code

'Bricx Command Center' is the software used to program the NXT Brick. The software is C-based and has many different functions for interfacing with the Brick. The code used to program the robot arm can be downloaded below along with an explanation:


// -------------------------------------------------------------------------//
// Inverse Kinematics for 2R planar manipulator with a closed-form solution //
// Created By: Sangsin Park, Ph. D.                                         //
// Modified By: Norberto Torres-Reyes                                       //
// March 07 2019                                                            //
// -------------------------------------------------------------------------//

// Motor's constants
#define JNT1 OUT_A
#define JNT2 OUT_B
#define FULL_SPEED (100)

// Gear's constants
#define gearRatio1 (2)*(56/12)*(-1)
#define gearRatio2 (3)*(-1)

// Links' constants
#define L1 (12)
#define L2 (10.4)

// Global variables
float theta1_ik = 0.0;
float theta2_ik = 0.0;

// Function declarations
bool IK_2R_Planar_closed(float px, float py);

task main()
{
     // button variables
     bool orangeBtnPushed = FALSE;
     bool l_ArrowBtnPushed = FALSE;
     bool r_ArrowBtnPushed = FALSE;
     bool greyBtnPushed = FALSE;

     int cnt_OrangeBtn = 0;
     int cnt_l_ArrowBtn = 0;
     int cnt_r_ArrowBtn = 0;

     // reference joint angles
     float theta1 = 0;
     float theta2 = 0;

     float px =12.0 ;
     float py = 16.0;
     bool IK_ok = FALSE;

     PlayTone(TONE_B3, 05);
     TextOut(0, LCD_LINE1, "Grey BTN Quits");
     TextOut(0, LCD_LINE2, "Orange BTN Home");

     PosRegEnable(JNT1,PID_1,PID_2,PID_3); // Defines PID control values
     PosRegSetMax(JNT1, 1.0*FULL_SPEED, 10); // Set Port A speed limit and acceleration

     PosRegEnable(JNT2,PID_1,PID_2,PID_3); // Define PID control values
     PosRegSetMax(JNT2, 1.0*FULL_SPEED, 5); // Set Port B speed limit acceleration

     while(greyBtnPushed == FALSE)
     {
         greyBtnPushed = ButtonPressed(BTNEXIT, FALSE);
         orangeBtnPushed = ButtonPressed(BTNCENTER, FALSE);
         l_ArrowBtnPushed = ButtonPressed(BTNLEFT, FALSE);
         r_ArrowBtnPushed = ButtonPressed(BTNRIGHT, FALSE);
         cnt_l_ArrowBtn = ButtonCount(BTNLEFT, FALSE);
         cnt_r_ArrowBtn = ButtonCount(BTNRIGHT, FALSE);

         PosRegSetAngle(JNT1, theta1);
         PosRegSetAngle(JNT2, theta2);
         TextOut(0, LCD_LINE4, FormatNum ("JNT 1: %f" , theta1/gearRatio1));
         TextOut(0, LCD_LINE5, FormatNum ("JNT 2: %f" , theta2/gearRatio2));

         // Code here for going to home postion after orange button is pushed //

         if(orangeBtnPushed == TRUE)
         {
             ClearLine(LCD_LINE4);
             ClearLine(LCD_LINE5);
             orangeBtnPushed = FALSE;
             theta1 = 0.0;
             theta2 = 0.0;
          }

         if(r_ArrowBtnPushed == TRUE)
         {
             r_ArrowBtnPushed = FALSE;
             IK_ok = IK_2R_Planar_closed(px, py);
             if(IK_ok == TRUE)
             {
                 theta1 = theta1_ik*gearRatio1;
                 theta2 = theta2_ik*gearRatio2;
                 TextOut(0, LCD_LINE6, "Solution.");
             }
             else
             {
                 TextOut(0, LCD_LINE6, "No Solution.");
             }
          }
     }
}

bool IK_2R_Planar_closed(float px, float py)
{
     float c2 = (px*px + py*py - L1*L1 - L2*L2)/(2*L1*L2);
     float s2 = -sqrt(1 - c2*c2);
     float c1 = (px*(L1 + L2*c2) + py*L2*s2)/(px*px + py*py);
     float s1 = (py*(L1 + L2*c2) - px*L2*s2)/(px*px + py*py);

     if(c2 >= -1 && c2 <= 1 && c1 >= -1 && c1 <= 1)
     {
          theta1_ik = atan2(s1, c1)*(180/PI);
          theta2_ik = atan2(s2, c2)*(180/PI);

          return TRUE;
     }
     else
     {
          return FALSE;
     }
}




2-Link Lego Code

Running, Testing and Analysis

Conclusions

Final Words

lego_2link_arm.1552024000.txt.gz · Last modified: 2019/03/07 21:46 by ntorresreyes