Control Software - A Prototype

In the next section we will build a complete prototype for the control of the robot. The goal is to be able to select and control all axes individually via the serial monitor.

Joint Designation

To do this, we must first determine a designation of the joints.

In our program we will use the following abbreviations to select the joint:

1  :   selects the base joint
2  :   selects the shoulder joint
3  :   selects the elbow joint
4  :   selects the gripper

Complete Setup and Pinout

Before we can start programming, we first have to build the complete circuit with all 4 servos. It is important to use an external voltage source for the servos, because the current demand of 4 servos at the same time could overload the Arduino. It is also important to connect GND of the power supply to a GND pin on the Arduino baord. 

base      : PIN 9
shoulder  : PIN 10
elbow     : PIN 11
gripper   : PIN 12

Program

With this program you can first select a single axis and then set the servo to the desired value.

  
/* Robotarm
 * Robotarm Control Prototype
 * Stefan Hager 2022
 */

#include <Servo.h>

Servo base;  
Servo shoulder;
Servo elbow;
Servo gripper;

// create all required objects from class Servo


void setup() 
{
  // opens serial port, sets data rate to 9600 bps
  Serial.begin(9600); 
  base.attach(9);  
  shoulder.attach(10);  
  elbow.attach(11);  
  gripper.attach(12);  

  // Message to user
  Serial.println("To select joint: 1=base 2=shoulder 3=elbow 4=gripper");
  Serial.println("Insert target angle between 0 and 180 degrees");
}

void loop() 
{
     
  Serial.println("Select joint : "); 
  char command = readnextValue();
  Serial.println("Select value : "); 
  int value = readnextValue();
  if(command == 1)
  {
    Serial.print("Base moving to position "); 
    Serial.println(value);
    if(value >= 0 && value <= 180)
    {
      base.write(value);
    }
  }
  if(command == 2)
  {
    Serial.print("Shoulder moving to position ");
    Serial.println(value); 
    if(value >= 0 && value <= 180)
    {
      shoulder.write(value);
    } 
  } 
  if(command == 3)
  {
    Serial.print("Elbow moving to position ");
    Serial.println(value);
    if(value >= 0 && value <= 180)
    {
      elbow.write(value);
    } 
  }
  if(command == 4)
  {
    Serial.print("Gripper moving to position ");
    Serial.println(value); 
    if(value >= 0 && value <= 180)
    {
      gripper.write(value);
    } 
  }
  delay(1000);
}

// reads a character from serial 

// reads one number from serial and deletes 
// all misleading characters
  
int readnextValue()
{

  while (Serial.available() == 0) 
  {
    // empty loop - we are still waiting for a real value
  } 
  // read the incoming value
  int value = Serial.parseInt();
  // clears the serial buffer
  while(Serial.available() > 0) {
    char t = Serial.read();
  }
  return value;
}