Controlling a Delta-X Robot Arm with G-Code

Delta-X Robot

Delta robots are often used for factory "pick and place" operations.

Delta robot arms are not as precise as some other robot arm geometries, but they can move fast and are easy to program. The millimeter-level accuracy is good enough for my project to move an end effector over weeds or crop plants.

I bought the delta robot arm from Delta-X, a Vietnamese company. They shipped the robot from Da Nang, Vietnam to Hawaii. When it arrived, I connect it to the bottom of the robot frame.

Delta robot kinematics, © Wikimedia

Delta robot kinematics (green arms are fixed length, at 90° to the blue axis that they rotate about)

The delta robot arm is programmed with G-code, a simple language used for the control of many machine tools.

I wrote a program to run on the Raspberry Pi computer and communicate with the delta robot arm by sending G-Code commands over a serial port. The program is listed below.

//
//  delta.cpp
//
//  Serial interface for Delta-Robot.
//

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include <errno.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include "utils.cpp"
#include "serial.cpp"

static const char RobotPort[] = "/dev/cu.usbmodem144101";
//static const char RobotPort[] = "/dev/tty.DeltaX-SerialPort";

static SInt fd; // Device file descriptor

static void
sendLine(const char * const fmt, ...)
	    __attribute__ ((format(printf, 1, 2)));

static void
sendLine(const char *fmt, ...)  // Send a formated string to the robot
{
  char s[0x1000];
  va_list args;
  va_start(args, fmt);
  vsprintf(s, fmt, args);
  Write(fd, s, strlen(s));
  Write(fd, "\n", 1);
}

static void
clearSerial(void)  // Clear buffered data.
{
  while (true) {
    SInt c = serialReadOneByte(fd, 1000000);    
    if (c == -1) {
      printf("serial empty\n");
      break;
    } else {
      printf("%c", c);
    }
  }
}

static void
gotoCommand(double x, double y, double z)
{
  double thirtyDegrees = 30*(M_PI/180.0);
  double oneTwentyDegrees = 120*(M_PI/180.0);
  double realX = x*cos(thirtyDegrees) + y*cos(oneTwentyDegrees);
  double realY = x*sin(thirtyDegrees) + y*sin(oneTwentyDegrees);
  sendLine("G01 X%f Y%f Z%f", realX, realY, z);
  usleep(1000000);
}

static void
arcCommand(double x, double y, double i, double j)
{
  double thirtyDegrees = 30*(M_PI/180.0);
  double oneTwentyDegrees = 120*(M_PI/180.0);
  double realX = x*cos(thirtyDegrees) + y*cos(oneTwentyDegrees);
  double realY = x*sin(thirtyDegrees) + y*sin(oneTwentyDegrees);
  double realI = i*cos(thirtyDegrees) + j*cos(oneTwentyDegrees);
  double realJ = i*sin(thirtyDegrees) + j*sin(oneTwentyDegrees);
  sendLine("G02 X%f Y%f I%f J%f", realX, realY, realI, realJ);
}

int
main(void)
{
  fd = serialPortOpen(RobotPort);
  clearSerial();
  sendLine("IsDelta");
  sendLine("IsDelta");
  clearSerial();
  sendLine("G28");
  usleep(2000000);
  double upH = -360;
  double downH = -385;
  gotoCommand(-20.0, 50.0, upH);
  gotoCommand(-20.0, 50.0, downH);
  gotoCommand(30.0, 50.0, downH);
  gotoCommand(30.0, 50.0, upH);
  gotoCommand(-20.0, -50.0, upH);
  gotoCommand(-20.0, -50.0, downH);
  gotoCommand(30.0, -50.0, downH);
  gotoCommand(30.0, -50.0, upH);
  gotoCommand(60.0, -70.0, downH);
  arcCommand(60.0, 70.0, 0.0, 70.0);
  for (;;) {
    SInt c = serialReadOneByte(fd, 1000000);
    if (c == -1) {
      printf("serial empty\n");
    } else {
      printf("%d = %c\n", c, c);
    }
  }
}

//
//  serial.cpp
//

static SInt
serialPortOpen(const char *devicePath)
{
  int fd;
  struct termios options;
  fd = open(devicePath, O_RDWR | O_NOCTTY | O_NDELAY);
  if (fd < 0) {
    fatal("Device \"%s\" cannot be opened: %s", devicePath, syserr());
  }
  fcntl(fd, F_SETFL, FNDELAY);       // Open the device in nonblocking mode
  if (tcgetattr(fd, &options) < 0) { // Get the current options of the port
    fatal("tcgetattr failed: %s", syserr());
  }
  bzero(&options, sizeof(options));  // Clear all the options
  cfsetispeed(&options, B115200);    // Set the baud rate at 115200 bauds
  cfsetospeed(&options, B115200);
	    
  options.c_cflag |= ( CLOCAL | // Ignore modem control lines
		       CREAD  | // Enable receive
		       CS8    | // 8 data, 1 stop, no parity, no flow control
		       PARMRK); // Mark framing errors
	    
  options.c_iflag |= ( IGNPAR | IGNBRK );
  options.c_oflag = 0;
  options.c_cc[VTIME]=0;    // Timer unused
  options.c_cc[VMIN]=0;     // At least on character before satisfy reading
  cfmakeraw(&options);
  if (tcsetattr(fd, TCSANOW, &options) < 0) {  // Activate the settings
    fatal("tcsetattr failed, %s: %s", devicePath, syserr());
  }
  return fd;
}

static SInt
serialReadOneByte(SInt fd, UInt usecs)
{
  if (! select1(usecs, fd)) {
    return -1;  // Timeout
  }
  UInt8 c;
  Read(fd, &c, 1);
  return (SInt) c;
}