Smooth Joint Motion

You may have noticed that the servos move very quickly. This can lead to very harsh movements and endanger both the servos and the robot hardware. The next step is therefore to make the movement of the servos slower and more controllable. For this it is necessary to know the current position of the servo. Unfortunately, there is no way to query the current position of a servo, so we have to remember it ourselves.

The following program defines a class "joint" which represents a joint. Each joint remembers the current position and rotates slowly and safely to the new target position when needed. 

  

/* RobotArm
 * Gently moving a joint
 * Stefan Hager 2022
 * 
 * 
 * Note: it is important that the servo is already in 
 * the position provided to the constructor
 */

#ifndef _JOINT_H_
#define _JOINT_H_
 
#include <Servo.h>
#include "Arduino.h"


 
 class Joint
 {
   private:
    int pin, currentPos, speed; 
    Servo servo;
   public:
    Joint(int _pin, int _currentPos, int _speed); // constructor
    void start();
    void moveToTargetPosition(int _angle);
 };

  #endif

    
/* RobotArm
 * Gently moving a joint
 * Stefan Hager 2022
 */

#include "joint.h"

// initialize the joint

Joint::Joint(int _pin, int _currentPos, int _speed)
{ pin = _pin;
  speed = _speed;
  currentPos =  _currentPos;  
}

void Joint::start()
{ 
  servo.attach(pin);
  servo.write(currentPos);
}

void Joint::moveToTargetPosition(int target_angle)
{
  if(target_angle < 0)
    target_angle = 0;
  if(target_angle > 180)
    target_angle = 180;

  if(currentPos <= target_angle)
  {
    while(currentPos <= target_angle)
    {
      currentPos = currentPos + speed;
      servo.write(currentPos);
      delay(20);
    }
  }

  if(currentPos >= target_angle)
  {
    while(currentPos >= target_angle)
    {
      currentPos = currentPos - speed;
      servo.write(currentPos);
      delay(20);
    }
  }
}

  
/* RobotArm
 * Gently moving a joint
 * 
 * Base joint is on pin 9, starts at 90 degrees ans sets speed to 4
 * Stefan Hager 2022
 */

#include "joint.h"
Joint base(9,90,4);

void setup() 
{
  // opens serial port, sets data rate to 9600 bps
  Serial.begin(9600); 
  base.start();
}

void loop() 
{ 
  Serial.println("Insert target value (0 to 180 degrees): "); 
  int value = readnextValue();
  base.moveToTargetPosition(value);
  delay(1000);
}


// 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;
}

Testing The Gripper

Special attention is required for the gripper arm to be able to ensure correct function. First of all you have to make sure that the grippers can be moved easily, second you have ensure that the servo is build in in a 90 degree position.
For the test we will just use two positions: closed (=90°) and open (=180°). Please note that test program also requires the class "Joint". 

  
/* RobotArm
 * Test program for the gripper
 * Please note that test program requires the class "Joint".
 * Stefan Hager 2022
 */

#include <joint.h>

/* 
 *  create all required objects from class Joint
 *  connect your servos to the following pins:
 *  gripper   :   pin 12
 */

Joint gripper(12,90,2);

void setup() 
{
  // opens serial port, sets data rate to 9600 bps
  Serial.begin(9600);  
  gripper.start();  

  // Message to user
  Serial.println("To select position: 1=closed 2=open");
}

void loop() 
{
  Serial.println("Insert command : "); 
  char command = readnextValue();
  if(command == 1)
  { Serial.println("Close gripper..."); 
    gripper.moveToTargetPosition(90);
  }
  if(command == 2)
  {
    Serial.println("Open gripper..."); 
    gripper.moveToTargetPosition(180);
  } 
  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;
}