Voice-Controlled Smart Robotic Arm

INTRODUCTION

In the present scenario, robotic arms usually require separate programming for each task, demanding significant time and effort to make modifications for every new operation. Imagine a situation where a robotic arm is equipped with a camera and the ability to understand voice commands and convert it into meaningful instructions, and promptly carry out the assigned tasks.

This approach allows the users to effortlessly communicate with the robotic arm. By adopting this method, users can avoid the complexities of task-specific programming and effortlessly interact with the arm, commanding it to execute tasks or sequences with ease. By incorporating the ability to perform tasks in loops, the arm improves efficiency and minimizes the necessity for continuous human involvement.

WHY SCARA?

The SCARA (Selective Compliance Assembly Robot Arm) robot is ideal for tasks requiring high precision and speed in the horizontal plane. Its unique design allows for selective compliance, making it perfect for assembly, pick-and-place, and packaging applications. SCARA robots offer excellent repeatability and reliability, are cost-effective, and can be easily integrated into various industrial processes, enhancing efficiency and productivity.

DESIGN AND ASSEMBLY OF ROBOTIC ARM

STEP 1 : 3D Parts Printing Process

All the STL files for the 3D parts required to assemble this robotic arm are uploaded to the GitHub link mentioned below. We used the Garuda Thunder Pro V3 3D printer with a print speed of 200mm/sec and a -0.1 horizontal expansion. The material we used is PLA+ and the slicing software is Ultimaker Cura. Any 3D printer with a print size of 300x300x300 mm can be used for this project.

Scara-robot/SCARA Robot Arm - STL FIles at main · Sasi-vadan/Scara-robot (github.com)

Here is a preview of some of the 3D printed parts including robot base and gears:

STEP 2 : Assembly of Mechanical Components

The STEP file of the robotic arm can be accessed through the link below. This file allows users to view all the components of the arm, including their structure and positioning. I suggest opening this STEP file with Autodesk Viewer for better clarity. Although the primary goal of this project is to integrate voice command capabilities with the Intel RealSense camera and AMD Kria KR260, this tutorial provides a comprehensive overview of designing and building a robotic arm from scratch.

Scara-robot/SCARA Robot 3D Model.STEP at main · Sasi-vadan/Scara-robot (github.com)

Here are the bolts sizes required for this project:

STEP 3 : Assembly of Electronic Components

The robot has 4 degrees of freedom and is driven by 4 NEMA 17 stepper motors. Additionally, it features a small servo motor for controlling the end effector, or the robot gripper in this case. Various types of grippers can be used, including pneumatic, magnetic, and angular depending on the use case.

The brain of this SCARA robot is an Arduino UNO board which is paired with a CNC shield and four A4988 stepper drivers for controlling the stepper motors. Commands are sent to the Arduino through the AMD KRIA KR260 Board.

CIRCUIT DIAGRAM :

For powering the robot, we need 12V power supply capable of providing minimum of 4A, but I would suggest 12V 6A power supply. This depends on how the stepper driver’s current limitation is set, and I would suggest to set it at lowest level possible.

MANUEL CONTROL OF ROBOTIC ARM

ARDUINO IDE CODE :

Libraries and Definitions

• Uses AccelStepper and Servo libraries.
• Defines pins for limit switches and stepper motors.

Global Variables

• Variables for positions, angles, steps conversion, input values, and data storage.

Setup Function

• Initializes serial communication, pin modes, and motor settings.
• Performs homing for initial position calibration.
• Attaches and initializes the servo motor for the gripper.

Homing Function

• Moves stepper motors to home positions using limit switches.
• Detaches and reattaches the servo motor during homing.

Gripper Control

• Controls the gripper with a servo motor, storing and recalling positions.

Motion Control

• Converts angles and distances to steps for precise movement control.
• Adjusts speed and acceleration during operation.

Data Handling

• Processes incoming serial data to update motor positions and commands.
• Provides functionality for saving, clearing, and running movement sequences.

#include <AccelStepper.h>
#include <Servo.h>
#include <math.h>

#define limitSwitch1 11
#define limitSwitch2 10
#define limitSwitch3 9
#define limitSwitch4 A3

// Define the stepper motors and the pins the will use

AccelStepper stepper1(1, 2, 5); // (Type:driver, STEP, DIR)
AccelStepper stepper2(1, 3, 6);
AccelStepper stepper3(1, 4, 7);
AccelStepper stepper4(1, 12, 13);

Servo gripperServo;  // create servo object to control a servo

double x = 10.0;
double y = 10.0;
double L1 = 228; // L1 = 228mm
double L2 = 136.5; // L2 = 136.5mm
double theta1, theta2, phi, z;

int stepper1Position, stepper2Position, stepper3Position, stepper4Position;
const float theta1AngleToSteps = 44.444444;
const float theta2AngleToSteps = 35.555555;
const float phiAngleToSteps = 10;
const float zDistanceToSteps = 100;

byte inputValue[5];
int k = 0;
String content = "";
int data[10];
int theta1Array[100];
int theta2Array[100];
int phiArray[100];
int zArray[100];
int gripperArray[100];
int positionsCounter = 0;

void setup() {
Serial.begin(115200);
pinMode(limitSwitch1, INPUT_PULLUP);
pinMode(limitSwitch2, INPUT_PULLUP);
pinMode(limitSwitch3, INPUT_PULLUP);
pinMode(limitSwitch4, INPUT_PULLUP);

// Stepper motors max speed
stepper1.setMaxSpeed(4000);
stepper1.setAcceleration(2000);
stepper2.setMaxSpeed(4000);
stepper2.setAcceleration(2000);
stepper3.setMaxSpeed(4000);
stepper3.setAcceleration(2000);
stepper4.setMaxSpeed(4000);
stepper4.setAcceleration(2000);

homing(); // Perform homing before attaching the servo

gripperServo.attach(A0, 600, 2500);
data[6] = 180;  // Set initial servo position
gripperServo.write(data[6]);
delay(1000);
data[5] = 100;
}

void loop() {
if (Serial.available()) {
content = Serial.readString(); // Read the incoming data from Processing
// Extract the data from the string and put into separate integer variables (data[] array)

for (int i = 0; i < 10; i++) {
int index = content.indexOf(","); // locate the first ","
data[i] = atol(content.substring(0, index).c_str()); // Extract the number from start to the ","
content = content.substring(index + 1); // Remove the number from the string
}

/*
data[0] - SAVE button status
data[1] - RUN button status
data[2] - Joint 1 angle
data[3] - Joint 2 angle
data[4] - Joint 3 angle
data[5] - Z position
data[6] - Gripper value
data[7] - Speed value
data[8] - Acceleration value
*/

// If SAVE button is pressed, store the data into the appropriate arrays

if (data[0] == 1) {
theta1Array[positionsCounter] = data[2] * theta1AngleToSteps; // Store the values in steps = angles * angleToSteps variable
theta2Array[positionsCounter] = data[3] * theta2AngleToSteps;
phiArray[positionsCounter] = data[4] * phiAngleToSteps;
zArray[positionsCounter] = data[5] * zDistanceToSteps;
gripperArray[positionsCounter] = data[6];
positionsCounter++;
}

// Clear data

if (data[0] == 2) {
// Clear the array data to 0

memset(theta1Array, 0, sizeof(theta1Array));
memset(theta2Array, 0, sizeof(theta2Array));
memset(phiArray, 0, sizeof(phiArray));
memset(zArray, 0, sizeof(zArray));
memset(gripperArray, 0, sizeof(gripperArray));
positionsCounter = 0;
}
}

// If RUN button is pressed

while (data[1] == 1) {
stepper1.setSpeed(data[7]);
stepper2.setSpeed(data[7]);
stepper3.setSpeed(data[7]);
stepper4.setSpeed(data[7]);
stepper1.setAcceleration(data[8]);
stepper2.setAcceleration(data[8]);
stepper3.setAcceleration(data[8]);
stepper4.setAcceleration(data[8]);

// Execute the stored steps

for (int i = 0; i <= positionsCounter - 1; i++) {
if (data[1] == 0) {
break;
}

stepper1.moveTo(theta1Array[i]);
stepper2.moveTo(theta2Array[i]);
stepper3.moveTo(phiArray[i]);
stepper4.moveTo(zArray[i]);
while (stepper1.currentPosition() != theta1Array[i] || stepper2.currentPosition() != theta2Array[i] || stepper3.currentPosition() != phiArray[i] || stepper4.currentPosition() != zArray[i]) {
stepper1.run();
stepper2.run();
stepper3.run();
stepper4.run();
}

if (i == 0) {
gripperServo.write(gripperArray[i]);
}

else if (gripperArray[i] != gripperArray[i - 1]) {
gripperServo.write(gripperArray[i]);
delay(800);
// Wait 0.8s for the servo to grab or drop - the servo is slow
}

// Check for change in speed and acceleration or program stop

if (Serial.available()) {
content = Serial.readString(); // Read the incoming data from Processing

// Extract the data from the string and put into separate integer variables (data[] array)

for (int i = 0; i < 10; i++) {
int index = content.indexOf(","); // Locate the first ","
data[i] = atol(content.substring(0, index).c_str()); 
// Extract the number from start to the ","
content = content.substring(index + 1); 
// Remove the number from the string
}

if (data[1] == 0) {
break;
}

// Change speed and acceleration while running the program
stepper1.setSpeed(data[7]);
stepper2.setSpeed(data[7]);
stepper3.setSpeed(data[7]);
stepper4.setSpeed(data[7]);
stepper1.setAcceleration(data[8]);
stepper2.setAcceleration(data[8]);
stepper3.setAcceleration(data[8]);
stepper4.setAcceleration(data[8]);
}
}
}
stepper1Position = data[2] * theta1AngleToSteps;
stepper2Position = data[3] * theta2AngleToSteps;
stepper3Position = data[4] * phiAngleToSteps;
stepper4Position = data[5] * zDistanceToSteps;
stepper1.setSpeed(data[7]);
stepper2.setSpeed(data[7]);
stepper3.setSpeed(data[7]);
stepper4.setSpeed(data[7]);
stepper1.setAcceleration(data[8]);
stepper2.setAcceleration(data[8]);
stepper3.setAcceleration(data[8]);
stepper4.setAcceleration(data[8]);
stepper1.moveTo(stepper1Position);
stepper2.moveTo(stepper2Position);
stepper3.moveTo(stepper3Position);
stepper4.moveTo(stepper4Position);
gripperServo.write(data[6]);

while (stepper1.currentPosition() != stepper1Position || stepper2.currentPosition() != stepper2Position || stepper3.currentPosition() != stepper3Position || stepper4.currentPosition() != stepper4Position) {
stepper1.run();
stepper2.run();
stepper3.run();
stepper4.run();
}
}

void serialFlush() {
while (Serial.available() > 0) {  //while there are characters in the serial buffer, because Serial.available is >0
Serial.read();         // get one character
}
}

void homing() {
// Detach the servo to avoid any unintended movements
gripperServo.detach();
// Homing Stepper4
while (digitalRead(limitSwitch4) != 1) {
stepper4.setSpeed(1500);
stepper4.runSpeed();
stepper4.setCurrentPosition(17000); // When limit switch pressed set position to 0 steps
}

delay(20);
stepper4.moveTo(10000);
while (stepper4.currentPosition() != 10000) {
stepper4.run();
}

// Homing Stepper3
while (digitalRead(limitSwitch3) != 1) {
stepper3.setSpeed(-1100);
stepper3.runSpeed();
stepper3.setCurrentPosition(-1662); // When limit switch pressed set position to 0 steps
}
delay(20);
stepper3.moveTo(0);
while (stepper3.currentPosition() != 0) {
stepper3.run();
}

// Homing Stepper2
while (digitalRead(limitSwitch2) != 1) {
stepper2.setSpeed(-1300);
stepper2.runSpeed();
stepper2.setCurrentPosition(-5420); // When limit switch pressed set position to -5440 steps
}

delay(20);
stepper2.moveTo(0);
while (stepper2.currentPosition() != 0) {
stepper2.run();
}

// Homing Stepper1
while (digitalRead(limitSwitch1) != 1) {
stepper1.setSpeed(-1200);
stepper1.runSpeed();
stepper1.setCurrentPosition(-3955); // When limit switch pressed set position to 0 steps
}

delay(20);
stepper1.moveTo(0);
while (stepper1.currentPosition() != 0) {
stepper1.run();
}

// Reattach the servo after homing is complete
gripperServo.attach(A0, 600, 2500);

// Restore the servo to its last known position
gripperServo.write(data[6]);
}

Comments are present at every section of the code to explain its working. After powering the robot with a 12V, 6A supply connected to the CNC shield, upload the code to the Arduino. Once the code is uploaded, the arm calibrates itself and returns to the home position.

here is a video of the caliberation process :

PROCESSING IDE :

For controlling the robotic arm manually, we are using the Processing IDE to create a GUI for the servo motors. The Graphic User Interface is made using the ControlP5 library for the Processing IDE, allowing us to easily create buttons, sliders, text fields, and more.

There are two methods for controlling robots in terms of positioning and orientation: forward kinematics and inverse kinematics.

• Forward Kinematics: This method is used to find the position and orientation of the end-effector from the given joint angles.
• Inverse Kinematics: This method is used to find the joint angles for a given position of the end-effector. In robotics, this is more practical as we often need the robot to position its tool to specific X, Y, and Z coordinates. Using inverse kinematics, we can calculate the joint angles required for any given coordinates.

CODE FOR PROCESSING IDE :

import processing.serial.*;
import controlP5.*; 
import static processing.core.PApplet.*;

Serial myPort;
ControlP5 cp5; // controlP5 object

int j1Slider = 0;
int j2Slider = 0;
int j3Slider = 0;
int zSlider = 100;
int j1JogValue = 0;
int j2JogValue = 0;
int j3JogValue = 0;
int zJogValue = 0;
int speedSlider = 500;
int accelerationSlider = 500;
int gripperValue = 180;
int gripperAdd=180;
int positionsCounter = 0;


int saveStatus = 0;
int runStatus = 0;

int slider1Previous = 0;
int slider2Previous = 0;
int slider3Previous = 0;
int sliderzPrevious = 100;
int speedSliderPrevious = 500;
int accelerationSliderPrevious = 500;
int gripperValuePrevious = 100;

boolean activeIK = false;

int xP=365;
int yP=0;
int zP=100;
float L1 = 228; // L1 = 228mm
float L2 = 136.5; // L2 = 136.5mm
float theta1, theta2, phi, z;

String[] positions = new String[100];

String data;

void setup() {

  size(960, 800);
  myPort = new Serial(this, "COM3", 115200);
  
  cp5 = new ControlP5(this);

  PFont pfont = createFont("Arial", 25, true); // use true/false for smooth/no-smooth
  ControlFont font = new ControlFont(pfont, 22);
  ControlFont font2 = new ControlFont(pfont, 25);

  //J1 controls
  cp5.addSlider("j1Slider")
    .setPosition(110, 190)
    .setSize(270, 30)
    .setRange(-90, 266) // Slider range, corresponds to Joint 1 or theta1 angle that the robot can move to
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
  cp5.addButton("j1JogMinus")
    .setPosition(110, 238)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG-")
    ;
  cp5.addButton("j1JogPlus")
    .setPosition(290, 238)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG+")
    ;
  cp5.addNumberbox("j1JogValue")
    .setPosition(220, 243)
    .setSize(50, 30)
    .setRange(0, 20)
    .setFont(font)
    .setMultiplier(0.1)
    .setValue(1)
    .setDirection(Controller.HORIZONTAL) // change the control direction to left/right
    .setCaptionLabel("")
    ;

  //J2 controls
  cp5.addSlider("j2Slider")
    .setPosition(110, 315)
    .setSize(270, 30)
    .setRange(-150, 150)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
  cp5.addButton("j2JogMinus")
    .setPosition(110, 363)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG-")
    ;
  cp5.addButton("j2JogPlus")
    .setPosition(290, 363)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG+")
    ;
  cp5.addNumberbox("j2JogValue")
    .setPosition(220, 368)
    .setSize(50, 30)
    .setRange(0, 20)
    .setFont(font)
    .setMultiplier(0.1)
    .setValue(1)
    .setDirection(Controller.HORIZONTAL) // change the control direction to left/right
    .setCaptionLabel("")
    ;
  //J3 controls
  cp5.addSlider("j3Slider")
    .setPosition(110, 440)
    .setSize(270, 30)
    .setRange(-162, 162)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
  cp5.addButton("j3JogMinus")
    .setPosition(110, 493)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG-")
    ;
  cp5.addButton("j3JogPlus")
    .setPosition(290, 493)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG+")
    ;
  cp5.addNumberbox("j3JogValue")
    .setPosition(220, 493)
    .setSize(50, 30)
    .setRange(0, 20)
    .setFont(font)
    .setMultiplier(0.1)
    .setValue(1)
    .setDirection(Controller.HORIZONTAL) // change the control direction to left/right
    .setCaptionLabel("")
    ;

  //Z controls
  cp5.addSlider("zSlider")
    .setPosition(110, 565)
    .setSize(270, 30)
    .setRange(0, 150)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
  cp5.addButton("zJogMinus")
    .setPosition(110, 618)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG-")
    ;
  cp5.addButton("zJogPlus")
    .setPosition(290, 618)
    .setSize(90, 40)
    .setFont(font)
    .setCaptionLabel("JOG+")
    ;
  cp5.addNumberbox("zJogValue")
    .setPosition(220, 618)
    .setSize(50, 30)
    .setRange(0, 20)
    .setFont(font)
    .setMultiplier(0.1)
    .setValue(1)
    .setDirection(Controller.HORIZONTAL) // change the control direction to left/right
    .setCaptionLabel("")
    ;


  cp5.addTextfield("xTextfield")
    .setPosition(530, 205)
    .setSize(70, 40)
    .setFont(font)
    .setColor(255)
    .setCaptionLabel("")
    ;
  cp5.addTextfield("yTextfield")
    .setPosition(680, 205)
    .setSize(70, 40)
    .setFont(font)
    .setColor(255)
    .setCaptionLabel("")
    ;
  cp5.addTextfield("zTextfield")
    .setPosition(830, 205)
    .setSize(70, 40)
    .setFont(font)
    .setColor(255)
    .setCaptionLabel("")
    ;

  cp5.addButton("move")
    .setPosition(590, 315)
    .setSize(240, 45)
    .setFont(font)
    .setCaptionLabel("MOVE TO POSITION")
    ;

  cp5.addButton("savePosition")
    .setPosition(470, 520)
    .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("SAVE POSITION")
    ;

  cp5.addButton("run")
    .setPosition(725, 520)
    .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("RUN PROGRAM")
    ;

  cp5.addButton("updateSA")
    .setPosition(760, 590)
    .setSize(150, 40)
    .setFont(font)
    .setCaptionLabel("(Update)")
    ;

  cp5.addButton("clearSteps")
    .setPosition(490, 650)
    .setSize(135, 40)
    .setFont(font)
    .setCaptionLabel("(CLEAR)")
    ;

  //Z controls
  cp5.addSlider("speedSlider")
    .setPosition(490, 740)
    .setSize(180, 30)
    .setRange(500, 4000)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;

  cp5.addSlider("accelerationSlider")
    .setPosition(720, 740)
    .setSize(180, 30)
    .setRange(500, 4000)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
  cp5.addSlider("gripperValue")
    .setPosition(605, 445)
    .setSize(190, 30)
    .setRange(0, 100)
    .setValue(50)
    .setColorLabel(#3269c2)
    .setFont(font)
    .setCaptionLabel("")
    ;
}

void draw() { 
  background(#F2F2F2); // background black
  textSize(26);
  fill(33);
  text("Forward Kinematics", 120, 135); 
  text("Inverse Kinematics", 590, 135); 
  textSize(40);
  text("SCARA Robot Control", 260, 60); 
  textSize(45);
  text("J1", 35, 250); 
  text("J2", 35, 375);
  text("J3", 35, 500);
  text("Z", 35, 625);
  textSize(22);
  text("Speed", 545, 730);
  text("Acceleration", 745, 730);

  //println("PREV: "+accelerationSlider);
  fill(speedSlider);
  fill(accelerationSlider);
  fill(j1Slider);
  fill(j2Slider);
  fill(j3Slider);
  fill(zSlider);
  fill(j1JogValue);
  fill(j2JogValue);
  fill(j3JogValue);
  fill(zJogValue);
  fill(gripperValue);


  updateData();
  println(data);

  saveStatus=0; // keep savePosition variable 0 or false. See, when button SAVE pressed it makes the value 1, which indicates to store the value in the arduino code

  // If slider moved, calculate new position of X,Y and Z with forward kinematics
  if (slider1Previous != j1Slider) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      theta1 = round(cp5.getController("j1Slider").getValue()); // get the value from the slider1
      theta2 = round(cp5.getController("j2Slider").getValue());
      forwardKinematics();
      myPort.write(data);
    }
  }
  slider1Previous = j1Slider;

  if (slider2Previous != j2Slider) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      theta1 = round(cp5.getController("j1Slider").getValue()); // get the value from the slider1
      theta2 = round(cp5.getController("j2Slider").getValue());
      forwardKinematics();
      myPort.write(data);
    }
  }
  slider2Previous = j2Slider;

  if (slider3Previous != j3Slider) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      theta1 = round(cp5.getController("j1Slider").getValue()); // get the value from the slider1
      theta2 = round(cp5.getController("j2Slider").getValue());
      forwardKinematics();
      myPort.write(data);
    }
  }
  slider3Previous = j3Slider;

  if (sliderzPrevious != zSlider) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      zP = round(cp5.getController("zSlider").getValue());
      myPort.write(data);
    }
  }
  sliderzPrevious = zSlider;

  if (gripperValuePrevious != gripperValue) {
    if (activeIK == false) {     // Check whether the inverseKinematics mode is active, Executre Forward kinematics only if inverseKinematics mode is off or false
      gripperAdd = round(cp5.getController("gripperValue").getValue());
      gripperValue=gripperAdd+50;
      updateData();
      println(data);
      myPort.write(data);
    }
  }
  gripperValuePrevious = gripperValue;
  activeIK = false; // deactivate inverseKinematics so the above if statements can be executed the next interation

  fill(33);
  textSize(32);
  text("X: ", 500, 290);
  text(xP, 533, 290);
  text("Y: ", 650, 290);
  text(yP, 685, 290);
  text("Z: ", 800, 290);
  text(zP, 835, 290);
  textSize(26);
  text("Gripper", 650, 420);
  text("CLOSE", 510, 470);
  text("OPEN", 810, 470);
  textSize(18);

  if (positionsCounter >0 ) {
    text(positions[positionsCounter-1], 460, 630);
    text("Last saved position: No."+(positionsCounter-1), 460, 600);
  } else {
    text("Last saved position:", 460, 600);
    text("None", 460, 630);
  }
}

 // FORWARD KINEMATICS
void forwardKinematics() {
  float theta1F = theta1 * PI / 180;   // degrees to radians
  float theta2F = theta2 * PI / 180;
  xP = round(L1 * cos(theta1F) + L2 * cos(theta1F + theta2F));
  yP = round(L1 * sin(theta1F) + L2 * sin(theta1F + theta2F));
}

 // INVERSE KINEMATICS
void inverseKinematics(float x, float y) {
  theta2 = acos((sq(x) + sq(y) - sq(L1) - sq(L2)) / (2 * L1 * L2));
  if (x < 0 & y < 0) {
    theta2 = (-1) * theta2;
  }
  
  theta1 = atan(x / y) - atan((L2 * sin(theta2)) / (L1 + L2 * cos(theta2)));
  
  theta2 = (-1) * theta2 * 180 / PI;
  theta1 = theta1 * 180 / PI;

 // Angles adjustment depending in which quadrant the final tool coordinate x,y is
  if (x >= 0 & y >= 0) {       // 1st quadrant
    theta1 = 90 - theta1;
  }
  if (x < 0 & y > 0) {       // 2nd quadrant
    theta1 = 90 - theta1;
  }
  if (x < 0 & y < 0) {       // 3d quadrant
    theta1 = 270 - theta1;
    phi = 270 - theta1 - theta2;
    phi = (-1) * phi;
  }
  if (x > 0 & y < 0) {       // 4th quadrant
    theta1 = -90 - theta1;
  }
  if (x < 0 & y == 0) {
    theta1 = 270 + theta1;
  }
  
  // Calculate "phi" angle so gripper is parallel to the X axis
  phi = 90 + theta1 + theta2;
  phi = (-1) * phi;

  // Angle adjustment depending in which quadrant the final tool coordinate x,y is
  if (x < 0 & y < 0) {       // 3d quadrant
    phi = 270 - theta1 - theta2;
  }
  if (abs(phi) > 165) {
    phi = 180 + phi;
  }

  theta1=round(theta1);
  theta2=round(theta2);
  phi=round(phi);
  
  cp5.getController("j1Slider").setValue(theta1);
  cp5.getController("j2Slider").setValue(theta2);
  cp5.getController("j3Slider").setValue(phi);
  cp5.getController("zSlider").setValue(zP);
}

void controlEvent(ControlEvent theEvent) {  

  if (theEvent.isController()) { 
    println(theEvent.getController().getName());
  }
}

public void xTextfield(String theText) {
  //If we enter a value into the Textfield, read the value, convert to integer, set the inverseKinematics mode active
  xP=Integer.parseInt(theText);
  activeIK = true;
  inverseKinematics(xP, yP); // Use inverse kinematics to calculate the J1(theta1), J2(theta2), and J3(phi) positions
  //activeIK = false;
  println("Test; theta1: "+theta1+" theta2: "+theta2);
}
public void yTextfield(String theText) {
  yP=Integer.parseInt(theText);
  activeIK = true;
  inverseKinematics(xP, yP);
  //activeIK = false;
}
public void zTextfield(String theText) {
  zP=Integer.parseInt(theText);
  activeIK = true;
  inverseKinematics(xP, yP);
}

public void j1JogMinus() {
  int a = round(cp5.getController("j1Slider").getValue());
  a=a-j1JogValue;
  cp5.getController("j1Slider").setValue(a);
}
//J1 control
public void j1JogPlus() {
  int a = round(cp5.getController("j1Slider").getValue());
  a=a+j1JogValue;
  cp5.getController("j1Slider").setValue(a);
}
//J2 control
public void j2JogMinus() {
  int a = round(cp5.getController("j2Slider").getValue());
  a=a-j2JogValue;
  cp5.getController("j2Slider").setValue(a);
}
public void j2JogPlus() {
  int a = round(cp5.getController("j2Slider").getValue());
  a=a+j2JogValue;
  cp5.getController("j2Slider").setValue(a);
}
//J3 control
public void j3JogMinus() {
  int a = round(cp5.getController("j3Slider").getValue());
  a=a-j3JogValue;
  cp5.getController("j3Slider").setValue(a);
}
public void j3JogPlus() {
  int a = round(cp5.getController("j3Slider").getValue());
  a=a+j3JogValue;
  cp5.getController("j3Slider").setValue(a);
}
//J3 control
public void zJogMinus() {
  int a = round(cp5.getController("zSlider").getValue());
  a=a-zJogValue;
  cp5.getController("zSlider").setValue(a);
}
public void zJogPlus() {
  int a = round(cp5.getController("zSlider").getValue());
  a=a+zJogValue;
  ;
  cp5.getController("zSlider").setValue(a);
}

public void move() {

  myPort.write(data);
  println(data);
}

public void savePosition() {
  // Save the J1, J2, J3 and Z position in the array 
  positions[positionsCounter]="J1="+str(round(cp5.getController("j1Slider").getValue()))
    +"; J2=" + str(round(cp5.getController("j2Slider").getValue()))
    +"; J3="+str(round(cp5.getController("j3Slider").getValue()))
    +"; Z="+str(round(cp5.getController("zSlider").getValue()))  
    +"; Gripper="+str(round(cp5.getController("gripperValue").getValue())); // Add Gripper value
  positionsCounter++;
  saveStatus = 1;
  updateData();
  myPort.write(data);
  saveStatus=0;
}

public void run() {

  if (runStatus == 0) {
    cp5.getController("run").setCaptionLabel("STOP");
    cp5.getController("run").setColorLabel(#e74c3c);

    runStatus = 1;
  } else if (runStatus == 1) {
    runStatus = 0;
    cp5.getController("run").setCaptionLabel("RUN PROGRAM");
    cp5.getController("run").setColorLabel(255);
  }
  updateData();
  myPort.write(data);
}
public void updateSA() {
  myPort.write(data);
}
public void clearSteps() {
  saveStatus = 2; // clear all steps / program
  updateData();
  myPort.write(data);
  println("Clear: "+data);
  positionsCounter=0;
  saveStatus = 0;
}

public void updateData() {
  data = str(saveStatus)
    +","+str(runStatus)
    +","+str(round(cp5.getController("j1Slider").getValue())) 
    +","+str(round(cp5.getController("j2Slider").getValue()))
    +","+str(round(cp5.getController("j3Slider").getValue()))
    +","+str(round(cp5.getController("zSlider").getValue()))
    +","+str(round(cp5.getController("gripperValue").getValue()))
    +","+str(speedSlider)
    +","+str(accelerationSlider);
}

After uploading and running the code, the GUI should appear as shown below:

AUTOMATIC CONTROL OF ROBOTIC ARM

INTEL REAL SENSE CAMERA :

The 3D camera which we are using for our project is the Intel RealSense, a depth-sensing camera equipped with four lenses, each serving a distinct purpose:

• Infrared Laser Projector: Projects a grid of infrared light points into the scene. The infrared cameras capture these points to determine the depth of each point by calculating the displacement between the projected and captured points, thus providing depth information.
• Infrared Cameras: Capture the infrared spectrum, which is useful for imaging in low-light conditions. These cameras can detect infrared light emitted, reflected, or transmitted by objects, enabling vision even without visible light.
• Color Camera: Captures standard visual information, complementing the depth data from the infrared cameras. This allows for enhanced functionalities such as facial recognition, augmented reality, and 3D modeling.

Combining the data from these lenses, the RealSense camera can generate detailed three-dimensional information about objects, supporting a range of applications including gesture recognition, object recognition, and precise depth measurement.

OBJECT DETECTION USING YOLO AND REALSENSE IN PYTHON :

These are four objects which we are using to train the model :

CODE :

from typing import Tuple, Optional
import pyrealsense2 as rs
import numpy as np
import cv2
import time
import torch 

class RealSenseCamera:
    def _init_(self):
        super()._init_()
        # Configure depth and color streams
        self.pipeline = rs.pipeline()
        self.config = rs.config()
        self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        self.align = rs.align(rs.stream.color)
        
        # Initialize YOLOv8 model
        self.model = torch.hub.load('ultralytics/yolov5', 'yolov5s', pretrained=True)
        self.model.eval()

    def capture(self):
        # Start streaming
        self.pipeline.start(self.config)
        # Warm up
        for _ in range(60):
            frames = self.pipeline.wait_for_frames()

    def release(self):
        self.pipeline.stop()

    def update_frame(self) -> None:
        frames = self.pipeline.wait_for_frames()
        aligned_frames = self.align.process(frames)
        self.curr_frame = aligned_frames
        self.curr_frame_time = time.time_ns()

    def color_frame(self) -> Optional[np.ndarray]:
        frame = self.curr_frame.get_color_frame()
        if not frame:
            return None
        frame = np.asanyarray(frame.get_data())
        return frame

    def detect_objects(self, frame: np.ndarray) -> np.ndarray:
        results = self.model(frame)
        return results

def main():
    camera = RealSenseCamera()
    camera.capture()
    
    while True:
        camera.update_frame()
        frame = camera.color_frame()
        if frame is not None:
            results = camera.detect_objects(frame)
            # Display results
            frame_with_boxes = results.render()[0]  
#Render bounding boxes on the frame
            cv2.imshow("Detection", frame_with_boxes)
            
            if cv2.waitKey(1) & 0xFF == ord('q'):
                break

    camera.release()
    cv2.destroyAllWindows()

if _name_ == "_main_":
    main()

Both color recognition and shape recognition are based on algorithms provided by OpenCV to identify and grasp objects. A simple algorithm for HSV detection is all that's needed to detect colors.

# init color
def __init__(self) -> None:
self.area_low_threshold = 15000
self.detected_name = None
self.hsv_range = {
"green": ((40, 50, 50), (90, 256, 256)),
# "blueA": ((91, 100, 100), (105, 256, 256)),
# "yellow": ((20, 240, 170), (30, 256, 256)),
"yellow": ((15, 46, 43), (30, 256, 256)),
"redA": ((0, 100, 100), (6, 256, 256)),
"redB": ((170, 100, 100), (179, 256, 256)),
# "orange": ((8, 100, 100), (15, 256, 256)),
"blue": ((100, 43, 46), (124, 256, 256)),
}
# process of image
result = []
for color, (hsv_low, hsv_high) in self.hsv_range.items():
hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
in_range = cv2.inRange(hsv_frame, hsv_low, hsv_high)
# Dilate and corrode color areas
kernel = np.ones((5, 5), np.uint8)
in_range = cv2.morphologyEx(in_range, cv2.MORPH_CLOSE, kernel)
in_range = cv2.morphologyEx(in_range, cv2.MORPH_OPEN, kernel)
contours, hierarchy = cv2.findContours(
in_range, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
)
contours = list(
filter(lambda x: cv2.contourArea(x) > self.area_low_threshold, contours)
)
rects = list(map(cv2.minAreaRect, contours))
boxes = list(map(cv2.boxPoints, rects))
boxes = list(map(np.int32, boxes))
if len(boxes) != 0:
if color.startswith("red"):
color = "red"
for box in boxes:
result.append(ColorDetector.DetectResult(color, box))
# self.detected_name = result
self.detected_name = result[0].color
return result

VOICE COMMANDS

We trained the DFRobot voice recognition sensor to recognize four commands: "cube," "cuboid," "cylinder," and "hexagonal prism." The Intel RealSense camera is used to identify these objects. When a command is detected, the robotic arm is activated to place the corresponding object in a predefined home position.

from typing import Tuple, Optional
import pyrealsense2 as rs
import numpy as np
import cv2
import time
import torch
import serial  # For reading from the DFRobot voice recognition sensor

class RealSenseCamera:
    def __init__(self):
        super().__init__()
        # Configure depth and color streams
        self.pipeline = rs.pipeline()
        self.config = rs.config()
        self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        self.align = rs.align(rs.stream.color)
        
        # Initialize YOLOv8 model
        self.model = torch.hub.load('ultralytics/yolov5', 'yolov5s', pretrained=True)
        self.model.eval()
        
        # Initialize voice recognition sensor (assuming connected via serial)
        self.ser = serial.Serial('COM3', 9600)  # Adjust COM port and baud rate as necessary

    def capture(self):
        # Start streaming
        self.pipeline.start(self.config)
        # Warm up
        for _ in range(60):
            frames = self.pipeline.wait_for_frames()

    def release(self):
        self.pipeline.stop()

    def update_frame(self) -> None:
        frames = self.pipeline.wait_for_frames()
        aligned_frames = self.align.process(frames)
        self.curr_frame = aligned_frames
        self.curr_frame_time = time.time_ns()

    def color_frame(self) -> Optional[np.ndarray]:
        frame = self.curr_frame.get_color_frame()
        if not frame:
            return None
        frame = np.asanyarray(frame.get_data())
        return frame

    def detect_objects(self, frame: np.ndarray) -> np.ndarray:
        results = self.model(frame)
        return results

    def read_voice_command(self) -> str:
        if self.ser.in_waiting > 0:
            command = self.ser.readline().decode().strip()
            return command
        return ""

def main():
    camera = RealSenseCamera()
    camera.capture()
    
    while True:
        camera.update_frame()
        frame = camera.color_frame()
        if frame is not None:
            results = camera.detect_objects(frame)
            frame_with_boxes = results.render()[0]
            cv2.imshow("Detection", frame_with_boxes)
        
        # Read voice command
        voice_command = camera.read_voice_command()
        
        if voice_command:
            print(f"Voice command received: {voice_command}")
            # Process the command and take action
            if voice_command == "cube":
                # Handle the cube command (e.g., move the robotic arm to interact with a cube)
                pass
            elif voice_command == "cuboid":
                # Handle the cuboid command
                pass
            elif voice_command == "cylinder":
                # Handle the cylinder command
                pass
            elif voice_command == "hexagonal prism":
                # Handle the hexagonal prism command
                pass

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    camera.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

INTERFACING THE PROJECT WITH AMD KRIA KR260

Hardware Setup: Connect the Kria KR260 board to your robotic arm, Intel RealSense camera, and DFRobot voice recognition sensor. Ensure all components are properly wired and powered.

• Install Vitis AI: Begin by installing Vitis AI on your development machine. This involves downloading the software and setting up any necessary dependencies.
• Train and Convert Model: Train your object detection model, like YOLO, to identify objects such as cubes, cuboids, cylinders, and hexagonal prisms. Once trained, convert the model to ONNX format if needed.
• Optimize and Compile: Use Vitis AI's tools to optimize your ONNX model for the Kria KR260. After optimization, compile the model with the Vitis AI Compiler.
• Deploy and Develop: Deploy the compiled model onto the Kria KR260 board. Next, develop an application in Python or C++ that leverages Vitis AI’s API. This application will process the RealSense camera’s images through the AI model and handle the voice commands from the DFRobot sensor.
• Control the Arm: Configure the application to control the robotic arm based on the detected objects and recognized voice commands. For instance, when the system recognizes an object and hears a command, it should direct the robotic arm to place the object in a specific position.
• Testing and Tuning: Finally, test the integrated system to ensure everything works smoothly and make any necessary adjustments to improve performance.

FINAL OUTPUT :

When you issue the "cube" command, the system activates the robotic arm and uses the Intel RealSense camera to detect the cube's position. Once the cube is located, the system searches for the predefined placement position. The robotic arm then picks up the cube and places it in the designated spot.