Skip to content

Commit ad19238

Browse files
gohaigohai
committed
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.
1 parent 4ae8946 commit ad19238

File tree

11 files changed

+330
-1
lines changed

11 files changed

+330
-1
lines changed

‎java/libraries/io/examples/ServoSweep/ServoSweep.pde‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
import processing.io.*;
2+
3+
// see setup.png in the sketch folder for wiring details
4+
// for more reliable operation it is recommended to power
5+
// the servo from an external power source, see setup_better.png
6+
7+
SoftwareServo servo1;
8+
SoftwareServo servo2;
9+
10+
void setup() {
11+
size(400, 300);
12+
servo1 = new SoftwareServo(this);
13+
servo1.attach(17);
14+
servo2 = new SoftwareServo(this);
15+
servo2.attach(4);
16+
}
17+
18+
void draw() {
19+
background(0);
20+
stroke(255);
21+
strokeWeight(3);
22+
23+
// we don't go right to the edge to prevent
24+
// making the servo unhappy
25+
float angle = 90 + sin(frameCount / 100.0)*85;
26+
servo1.write(angle);
27+
float y = map(angle, 0, 180, 0, height);
28+
line(0, y, width/2, y);
29+
30+
angle = 90 + cos(frameCount / 100.0)*85;
31+
servo2.write(90 + cos(frameCount / 100.0)*85);
32+
y = map(angle, 0, 180, 0, height);
33+
line(width/2, y, width, y);
34+
}

‎java/libraries/io/examples/ServoSweep/setup.png‎

62.2 KB
Loading

‎java/libraries/io/examples/ServoSweep/setup_better.png‎

71.1 KB
Loading

‎java/libraries/io/library/linux-armv6hf/libprocessing-io.so‎

4.82 KB
Binary file not shown.

‎java/libraries/io/library/linux32/libprocessing-io.so‎

3.11 KB
Binary file not shown.

‎java/libraries/io/library/linux64/libprocessing-io.so‎

3.33 KB
Binary file not shown.

‎java/libraries/io/src/native/Makefile‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@ TARGET := libprocessing-io.so
22
OBJS := impl.o
33
CC := gcc
44

5-
CFLAGS := -std=c99 -fPIC -g
5+
# prefix with -m32 to compile for linux32
6+
CFLAGS := -std=gnu99 -fPIC -g
67
CFLAGS += -I$(shell dirname $(shell realpath $(shell which javac)))/../include
78
CFLAGS += -I$(shell dirname $(shell realpath $(shell which javac)))/../include/linux
89
LDFLAGS := -shared

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

Lines changed: 24 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

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

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,15 +25,20 @@
2525
#include <linux/i2c-dev.h>
2626
#include <linux/spi/spidev.h>
2727
#include <poll.h>
28+
#include <pthread.h>
2829
#include <stdint.h>
2930
#include <stdlib.h>
3031
#include <string.h>
3132
#include <sys/ioctl.h>
3233
#include <sys/param.h>
34+
#include <time.h>
3335
#include <unistd.h>
3436
#include "iface.h"
3537

3638

39+
static const int servo_pulse_oversleep = 35; // amount of uS to account for when sleeping
40+
41+
3742
JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_openDevice
3843
(JNIEnv *env, jclass cls, jstring _fn)
3944
{
@@ -192,6 +197,104 @@ JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_transferI2c
192197
}
193198

194199

200+
typedef struct {
201+
int fd;
202+
pthread_t thread;
203+
int pulse;
204+
int period;
205+
} SERVO_STATE_T;
206+
207+
208+
static void* servoThread(void *ptr) {
209+
SERVO_STATE_T *state = (SERVO_STATE_T*)ptr;
210+
struct timespec on, off;
211+
on.tv_sec = 0;
212+
off.tv_sec = 0;
213+
214+
do {
215+
write(state->fd, "1", 1);
216+
217+
on.tv_nsec = state->pulse * 1000;
218+
nanosleep(&on, NULL);
219+
220+
write(state->fd, "0", 1);
221+
222+
off.tv_nsec = (state->period - state->pulse) * 1000;
223+
nanosleep(&off, NULL);
224+
} while (1);
225+
}
226+
227+
228+
JNIEXPORT jlong JNICALL Java_processing_io_NativeInterface_servoStartThread
229+
(JNIEnv *env, jclass cls, jint gpio, jint pulse, jint period)
230+
{
231+
char path[26 + 19 + 1];
232+
int fd;
233+
pthread_t thread;
234+
235+
// setup struct holding our state
236+
SERVO_STATE_T *state = malloc(sizeof(SERVO_STATE_T));
237+
if (!state) {
238+
return -ENOMEM;
239+
}
240+
memset(state, 0, sizeof(*state));
241+
state->pulse = (pulse - servo_pulse_oversleep > 0) ? pulse - servo_pulse_oversleep : 0;
242+
// we're obviously also oversleeping in the general period case
243+
// but other than the pulse, this doesn't seem to be crucial with servos
244+
state->period = period;
245+
246+
// open gpio
247+
sprintf(path, "/sys/class/gpio/gpio%d/value", gpio);
248+
state->fd = open(path, O_WRONLY);
249+
if (state->fd < 0) {
250+
free(state);
251+
return -errno;
252+
}
253+
254+
// start thread
255+
int ret = pthread_create(&state->thread, NULL, servoThread, state);
256+
if (ret != 0) {
257+
free(state);
258+
return -ret;
259+
}
260+
261+
// set scheduling policy and priority
262+
struct sched_param param;
263+
param.sched_priority = 75;
264+
ret = pthread_setschedparam(state->thread, SCHED_FIFO, &param);
265+
if (ret != 0) {
266+
fprintf(stderr, "Error setting thread policy: %s\n", strerror(ret));
267+
}
268+
269+
return (intptr_t)state;
270+
}
271+
272+
273+
JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoUpdateThread
274+
(JNIEnv *env, jclass cls, jlong handle, jint pulse, jint period)
275+
{
276+
SERVO_STATE_T *state = (SERVO_STATE_T*)(intptr_t)handle;
277+
state->pulse = (pulse - servo_pulse_oversleep > 0) ? pulse - servo_pulse_oversleep : 0;
278+
state->period = period;
279+
return 0;
280+
}
281+
282+
283+
JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_servoStopThread
284+
(JNIEnv *env, jclass cls, jlong handle)
285+
{
286+
SERVO_STATE_T *state = (SERVO_STATE_T*)(intptr_t)handle;
287+
288+
// signal thread to stop
289+
pthread_cancel(state->thread);
290+
pthread_join(state->thread, NULL);
291+
292+
close(state->fd);
293+
free(state);
294+
return 0;
295+
}
296+
297+
195298
JNIEXPORT jint JNICALL Java_processing_io_NativeInterface_setSpiSettings
196299
(JNIEnv *env, jclass cls, jint handle, jint _maxSpeed, jint dataOrder, jint mode)
197300
{

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,10 @@ public static int writeFile(String fn, String out) {
6565
public static native int pollDevice(String fn, int timeout);
6666
/* I2C */
6767
public static native int transferI2c(int handle, int slave, byte[] out, byte[] in);
68+
/* SoftwareServo */
69+
public static native long servoStartThread(int gpio, int pulse, int period);
70+
public static native int servoUpdateThread(long handle, int pulse, int period);
71+
public static native int servoStopThread(long handle);
6872
/* SPI */
6973
public static native int setSpiSettings(int handle, int maxSpeed, int dataOrder, int mode);
7074
public static native int transferSpi(int handle, byte[] out, byte[] in);

0 commit comments

Comments
 (0)