From dbdeb9cf660c1d1846098b6ebab829831647d6db Mon Sep 17 00:00:00 2001 From: gohai Date: Sat, 21 May 2016 21:54:35 +0200 Subject: [PATCH] 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. --- .../io/examples/ServoSweep/ServoSweep.pde | 34 ++++ java/libraries/io/src/native/Makefile | 3 +- java/libraries/io/src/native/iface.h | 24 +++ java/libraries/io/src/native/impl.c | 103 +++++++++++ .../io/src/processing/io/NativeInterface.java | 4 + .../io/src/processing/io/SoftwareServo.java | 163 ++++++++++++++++++ 6 files changed, 330 insertions(+), 1 deletion(-) create mode 100644 java/libraries/io/examples/ServoSweep/ServoSweep.pde create mode 100644 java/libraries/io/src/processing/io/SoftwareServo.java diff --git a/java/libraries/io/examples/ServoSweep/ServoSweep.pde b/java/libraries/io/examples/ServoSweep/ServoSweep.pde new file mode 100644 index 000000000..a09d52ab1 --- /dev/null +++ b/java/libraries/io/examples/ServoSweep/ServoSweep.pde @@ -0,0 +1,34 @@ +import processing.io.*; + +// see setup.png in the sketch folder for wiring details +// for more reliable operation it is recommended to power +// the servo from an external power source, see setup_better.png + +SoftwareServo servo1; +SoftwareServo servo2; + +void setup() { + size(400, 300); + servo1 = new SoftwareServo(this); + servo1.attach(17); + servo2 = new SoftwareServo(this); + servo2.attach(4); +} + +void draw() { + background(0); + stroke(255); + strokeWeight(3); + + // we don't go right to the edge to prevent + // making the servo unhappy + float angle = 90 + sin(frameCount / 100.0)*85; + servo1.write(angle); + float y = map(angle, 0, 180, 0, height); + line(0, y, width/2, y); + + angle = 90 + cos(frameCount / 100.0)*85; + servo2.write(90 + cos(frameCount / 100.0)*85); + y = map(angle, 0, 180, 0, height); + line(width/2, y, width, y); +} diff --git a/java/libraries/io/src/native/Makefile b/java/libraries/io/src/native/Makefile index 101a20ee5..f33218a6c 100644 --- a/java/libraries/io/src/native/Makefile +++ b/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 diff --git a/java/libraries/io/src/native/iface.h b/java/libraries/io/src/native/iface.h index 0cda66685..32e235e1e 100644 --- a/java/libraries/io/src/native/iface.h +++ b/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 diff --git a/java/libraries/io/src/native/impl.c b/java/libraries/io/src/native/impl.c index b2748f7d1..0f9402ab9 100644 --- a/java/libraries/io/src/native/impl.c +++ b/java/libraries/io/src/native/impl.c @@ -25,15 +25,20 @@ #include #include #include +#include #include #include #include #include #include +#include #include #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, ¶m); + 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) { diff --git a/java/libraries/io/src/processing/io/NativeInterface.java b/java/libraries/io/src/processing/io/NativeInterface.java index ea32292c0..3b1b3e0c3 100644 --- a/java/libraries/io/src/processing/io/NativeInterface.java +++ b/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); diff --git a/java/libraries/io/src/processing/io/SoftwareServo.java b/java/libraries/io/src/processing/io/SoftwareServo.java new file mode 100644 index 000000000..dc516a775 --- /dev/null +++ b/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)); + } + } + } +}