IO: We want motors, they said (implements SoftwareServo)

Some measurements with a logic analyzer and the Raspberry Pi 2
sleep: 0.001500, measured avg: 0.0015357, measured 0.95 perc.: 0.0015573
sleep: 0.0185, measured avg: 0.0186177, measured 0.95 perc.: 0.0186345

servo_pulse_oversleep was set to account for the (expected) overhead of waking up and toggling the pin with help from the numbers above.

java/libraries/io/src/native/Makefile

@@ -2,7 +2,8 @@ TARGET := libprocessing-io.so
 OBJS := impl.o
 CC := gcc
 
-CFLAGS := -std=c99 -fPIC -g
+# prefix with -m32 to compile for linux32
+CFLAGS := -std=gnu99 -fPIC -g
 CFLAGS += -I$(shell dirname $(shell realpath $(shell which javac)))/../include
 CFLAGS += -I$(shell dirname $(shell realpath $(shell which javac)))/../include/linux
 LDFLAGS := -shared

java/libraries/io/src/native/iface.h

@@ -63,6 +63,30 @@ JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_pollDevice
 JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_transferI2c
   (JNIEnv *, jclass, jint, jint, jbyteArray, jbyteArray);
 
+/*
+ * Class:     processing_io_NativeInterface
+ * Method:    servoStartThread
+ * Signature: (III)J
+ */
+JNIEXPORT jlong JNICALL Java_processing_io_NativeInterface_servoStartThread
+  (JNIEnv *, jclass, jint, jint, jint);
+
+/*
+ * Class:     processing_io_NativeInterface
+ * Method:    servoUpdateThread
+ * Signature: (JII)I
+ */
+JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoUpdateThread
+  (JNIEnv *, jclass, jlong, jint, jint);
+
+/*
+ * Class:     processing_io_NativeInterface
+ * Method:    servoStopThread
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoStopThread
+  (JNIEnv *, jclass, jlong);
+
 /*
  * Class:     processing_io_NativeInterface
  * Method:    setSpiSettings

java/libraries/io/src/native/impl.c

@@ -25,15 +25,20 @@
 #include <linux/i2c-dev.h>
 #include <linux/spi/spidev.h>
 #include <poll.h>
+#include <pthread.h>
 #include <stdint.h>
 #include <stdlib.h>
 #include <string.h>
 #include <sys/ioctl.h>
 #include <sys/param.h>
+#include <time.h>
 #include <unistd.h>
 #include "iface.h"
 
 
+static const int servo_pulse_oversleep = 35;  // amount of uS to account for when sleeping
+
+
 JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_openDevice
   (JNIEnv *env, jclass cls, jstring _fn)
 {
@@ -192,6 +197,104 @@ JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_transferI2c
 }
 
 
+typedef struct {
+	int fd;
+	pthread_t thread;
+	int pulse;
+	int period;
+} SERVO_STATE_T;
+
+
+static void* servoThread(void *ptr) {
+	SERVO_STATE_T *state = (SERVO_STATE_T*)ptr;
+	struct timespec on, off;
+	on.tv_sec = 0;
+	off.tv_sec = 0;
+
+	do {
+		write(state->fd, "1", 1);
+
+		on.tv_nsec = state->pulse * 1000;
+		nanosleep(&on, NULL);
+
+		write(state->fd, "0", 1);
+
+		off.tv_nsec = (state->period - state->pulse) * 1000;
+		nanosleep(&off, NULL);
+	} while (1);
+}
+
+
+JNIEXPORT jlong JNICALL Java_processing_io_NativeInterface_servoStartThread
+  (JNIEnv *env, jclass cls, jint gpio, jint pulse, jint period)
+{
+	char path[26 + 19 + 1];
+	int fd;
+	pthread_t thread;
+
+	// setup struct holding our state
+	SERVO_STATE_T *state = malloc(sizeof(SERVO_STATE_T));
+	if (!state) {
+		return -ENOMEM;
+	}
+	memset(state, 0, sizeof(*state));
+	state->pulse = (pulse - servo_pulse_oversleep > 0) ? pulse - servo_pulse_oversleep : 0;
+	// we're obviously also oversleeping in the general period case
+	// but other than the pulse, this doesn't seem to be crucial with servos
+	state->period = period;
+
+	// open gpio
+	sprintf(path, "/sys/class/gpio/gpio%d/value", gpio);
+	state->fd = open(path, O_WRONLY);
+	if (state->fd < 0) {
+		free(state);
+		return -errno;
+	}
+
+	// start thread
+	int ret = pthread_create(&state->thread, NULL, servoThread, state);
+	if (ret != 0) {
+		free(state);
+		return -ret;
+	}
+
+	// set scheduling policy and priority
+	struct sched_param param;
+	param.sched_priority = 75;
+	ret = pthread_setschedparam(state->thread, SCHED_FIFO, &param);
+	if (ret != 0) {
+		fprintf(stderr, "Error setting thread policy: %s\n", strerror(ret));
+	}
+
+	return (intptr_t)state;
+}
+
+
+JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoUpdateThread
+  (JNIEnv *env, jclass cls, jlong handle, jint pulse, jint period)
+{
+	SERVO_STATE_T *state = (SERVO_STATE_T*)(intptr_t)handle;
+	state->pulse = (pulse - servo_pulse_oversleep > 0) ? pulse - servo_pulse_oversleep : 0;
+	state->period = period;
+	return 0;
+}
+
+
+JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoStopThread
+  (JNIEnv *env, jclass cls, jlong handle)
+{
+	SERVO_STATE_T *state = (SERVO_STATE_T*)(intptr_t)handle;
+
+	// signal thread to stop
+	pthread_cancel(state->thread);
+	pthread_join(state->thread, NULL);
+
+	close(state->fd);
+	free(state);
+	return 0;
+}
+
+
 JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_setSpiSettings
   (JNIEnv *env, jclass cls, jint handle, jint _maxSpeed, jint dataOrder, jint mode)
 {

java/libraries/io/src/processing/io/NativeInterface.java

@@ -65,6 +65,10 @@ public class NativeInterface {
   public static native int pollDevice(String fn, int timeout);
   /* I2C */
   public static native int transferI2c(int handle, int slave, byte[] out, byte[] in);
+  /* SoftwareServo */
+  public static native long servoStartThread(int gpio, int pulse, int period);
+  public static native int servoUpdateThread(long handle, int pulse, int period);
+  public static native int servoStopThread(long handle);
   /* SPI */
   public static native int setSpiSettings(int handle, int maxSpeed, int dataOrder, int mode);
   public static native int transferSpi(int handle, byte[] out, byte[] in);

java/libraries/io/src/processing/io/SoftwareServo.java

@@ -0,0 +1,163 @@
+/* -*- mode: java; c-basic-offset: 2; indent-tabs-mode: nil -*- */
+
+/*
+  Copyright (c) The Processing Foundation 2015
+  Hardware I/O library developed by Gottfried Haider
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General
+  Public License along with this library; if not, write to the
+  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
+  Boston, MA  02111-1307  USA
+*/
+
+package processing.io;
+
+import processing.core.*;
+
+
+/**
+ *  @webref
+ */
+public class SoftwareServo {
+
+  public static final int DEFAULT_MIN_PULSE = 544;
+  public static final int DEFAULT_MAX_PULSE = 2400;
+
+  protected int pin = -1;           // gpio number (-1 .. not attached)
+  protected int handle = -1;        // native thread id (<0 .. not started)
+  protected int period = 20000;     // 20 ms (50 Hz)
+  protected int minPulse = 0;       // minimum pulse width in microseconds
+  protected int maxPulse = 0;       // maximum pulse width in microseconds
+  protected int pulse = 0;          // current pulse in microseconds
+
+
+  /**
+   *  Opens a servo motor
+   *  @param parent typically use "this"
+   *  @webref
+   */
+  public SoftwareServo(PApplet parent) {
+    NativeInterface.loadLibrary();
+  }
+
+
+  /**
+   *  Closes a servo motor
+   *  @webref
+   */
+  public void close() {
+    detach();
+  }
+
+
+  protected void finalize() throws Throwable {
+    try {
+      close();
+    } finally {
+      super.finalize();
+    }
+  }
+
+
+  /**
+   *  Attaches a servo motor to a GPIO pin
+   *  @param pin GPIO pin
+   *  @webref
+   */
+  public void attach(int pin) {
+    detach();
+    this.pin = pin;
+    this.minPulse = DEFAULT_MIN_PULSE;
+    this.maxPulse = DEFAULT_MAX_PULSE;
+  }
+
+
+  /**
+   *  Attaches a servo motor to a GPIO pin
+   *  @param pin GPIO pin
+   *  @param minPulse minimum pulse width in microseconds (default: 544, same as on Arduino)
+   *  @param maxPulse maximum pulse width in microseconds (default: 2400, same as on Arduino)
+   *  @webref
+   */
+  public void attach(int pin, int minPulse, int maxPulse) {
+    detach();
+    this.pin = pin;
+    this.minPulse = minPulse;
+    this.maxPulse = maxPulse;
+  }
+
+
+  /**
+   *  Move a servo motor to a given orientation
+   *  @param angle angle in degrees (controls speed and direction on continuous-rotation servos)
+   *  @webref
+   */
+  public void write(float angle) {
+    if (attached() == false) {
+      System.err.println("You need to call attach(pin) before write(angle).");
+      throw new RuntimeException("Servo is not attached");
+    }
+
+    if (angle < 0 || 180 < angle) {
+      System.err.println("Only degree values between 0 and 180 can be used.");
+      throw new IllegalArgumentException("Illegal value");
+    }
+    pulse = (int)(minPulse + (angle/180.0) * (maxPulse-minPulse));
+
+    if (handle < 0) {
+      // start a new thread
+      GPIO.pinMode(pin, GPIO.OUTPUT);
+      if (NativeInterface.isSimulated()) {
+        return;
+      }
+      handle = NativeInterface.servoStartThread(pin, pulse, period);
+      if (handle < 0) {
+        throw new RuntimeException(NativeInterface.getError(handle));
+      }
+    } else {
+      // thread already running
+      int ret = NativeInterface.servoUpdateThread(handle, pulse, period);
+      if (ret < 0) {
+        throw new RuntimeException(NativeInterface.getError(ret));
+      }
+    }
+  }
+
+
+  /**
+   *  Returns whether a servo motor is attached to a pin
+   *  @return true if attached, false is not
+   *  @webref
+   */
+  public boolean attached() {
+    return (pin != -1);
+  }
+
+
+  /**
+   *  Detatches a servo motor from a GPIO pin
+   *  @webref
+   */
+  public void detach() {
+    if (0 <= handle) {
+      // stop thread
+      int ret = NativeInterface.servoStopThread(handle);
+      GPIO.pinMode(pin, GPIO.INPUT);
+      handle = -1;
+      pin = -1;
+      if (ret < 0) {
+        throw new RuntimeException(NativeInterface.getError(ret));
+      }
+    }
+  }
+}