Skip to content

Commit 9843d1c

Browse files
Olivier LeDiourisgohai
Olivier LeDiouris
authored and
gohai
committed
IO: Add PCA9685 servo/PWM controller example
Donated by @OlivierLD, minor cleanups by @gohai
1 parent 5db3ec9 commit 9843d1c

File tree

2 files changed

+178
-0
lines changed

2 files changed

+178
-0
lines changed

‎java/libraries/io/examples/Servo_I2C_PCA9685/PCA9685.pde‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
import processing.io.I2C;
2+
3+
// PCA9685 is a 16-channel servo/PWM driver
4+
// datasheet: https://cdn-shop.adafruit.com/datasheets/PCA9685.pdf
5+
// code contributed by @OlivierLD
6+
7+
public class PCA9685 extends I2C {
8+
public final static int PCA9685_ADDRESS = 0x40;
9+
10+
// registers used
11+
public final static int MODE1 = 0x00;
12+
public final static int PRESCALE = 0xFE;
13+
public final static int LED0_ON_L = 0x06;
14+
public final static int LED0_ON_H = 0x07;
15+
public final static int LED0_OFF_L = 0x08;
16+
public final static int LED0_OFF_H = 0x09;
17+
18+
private int address;
19+
private int freq = 200; // 200 Hz default frequency (after power-up)
20+
private boolean hasFreqSet = false; // whether a different frequency has been set
21+
private int minPulses[] = new int[16];
22+
private int maxPulses[] = new int[16];
23+
24+
25+
public PCA9685(String dev) {
26+
this(dev, PCA9685_ADDRESS);
27+
}
28+
public PCA9685(String dev, int address) {
29+
super(dev);
30+
this.address = address;
31+
// reset device
32+
command(MODE1, (byte) 0x00);
33+
}
34+
35+
36+
public void attach(int channel) {
37+
// same as on Arduino
38+
attach(channel, 544, 2400);
39+
}
40+
41+
public void attach(int channel, int minPulse, int maxPulse) {
42+
if (channel < 0 || 15 < channel) {
43+
throw new IllegalArgumentException("Channel must be between 0 and 15");
44+
}
45+
minPulses[channel] = minPulse;
46+
maxPulses[channel] = maxPulse;
47+
48+
// set the PWM frequency to be the same as on Arduino
49+
if (!hasFreqSet) {
50+
frequency(50);
51+
}
52+
}
53+
54+
public void write(int channel, float angle) {
55+
if (channel < 0 || 15 < channel) {
56+
throw new IllegalArgumentException("Channel must be between 0 and 15");
57+
}
58+
if (angle < 0 || 180 < angle) {
59+
throw new IllegalArgumentException("Angle must be between 0 and 180");
60+
}
61+
int us = (int)(minPulses[channel] + (angle/180.0) * (maxPulses[channel]-minPulses[channel]));
62+
63+
double pulseLength = 1000000; // 1s = 1,000,000 us per pulse
64+
pulseLength /= freq; // 40..1000 Hz
65+
pulseLength /= 4096; // 12 bits of resolution
66+
int pulse = us;
67+
pulse /= pulseLength;
68+
// println(pulseLength + " us per bit, pulse:" + pulse);
69+
pwm(channel, 0, pulse);
70+
}
71+
72+
public boolean attached(int channel) {
73+
if (channel < 0 || 15 < channel) {
74+
return false;
75+
}
76+
return (maxPulses[channel] != 0) ? true : false;
77+
}
78+
79+
public void detach(int channel) {
80+
pwm(channel, 0, 0);
81+
minPulses[channel] = 0;
82+
maxPulses[channel] = 0;
83+
}
84+
85+
86+
/**
87+
* @param freq 40..1000 Hz
88+
*/
89+
public void frequency(int freq) {
90+
this.freq = freq;
91+
float preScaleVal = 25000000.0f; // 25MHz
92+
preScaleVal /= 4096.0; // 4096: 12-bit
93+
preScaleVal /= freq;
94+
preScaleVal -= 1.0;
95+
// println("Setting PWM frequency to " + freq + " Hz");
96+
// println("Estimated pre-scale: " + preScaleVal);
97+
double preScale = Math.floor(preScaleVal + 0.5);
98+
// println("Final pre-scale: " + preScale);
99+
byte oldmode = (byte) readU8(MODE1);
100+
byte newmode = (byte) ((oldmode & 0x7F) | 0x10); // sleep
101+
command(MODE1, newmode); // go to sleep
102+
command(PRESCALE, (byte) (Math.floor(preScale)));
103+
command(MODE1, oldmode);
104+
delay(5);
105+
command(MODE1, (byte) (oldmode | 0x80));
106+
hasFreqSet = true;
107+
}
108+
109+
/**
110+
* @param channel 0..15
111+
* @param on cycle offset to turn output on (0..4095)
112+
* @param off cycle offset to turn output off again (0..4095)
113+
*/
114+
public void pwm(int channel, int on, int off) {
115+
if (channel < 0 || 15 < channel) {
116+
throw new IllegalArgumentException("Channel must be between 0 and 15");
117+
}
118+
if (on < 0 || 4095 < on) {
119+
throw new IllegalArgumentException("On must be between 0 and 4095");
120+
}
121+
if (off < 0 || 4095 < off) {
122+
throw new IllegalArgumentException("Off must be between 0 and 4095");
123+
}
124+
if (off < on) {
125+
throw new IllegalArgumentException("Off must be greater than On");
126+
}
127+
command(LED0_ON_L + 4 * channel, (byte) (on & 0xFF));
128+
command(LED0_ON_H + 4 * channel, (byte) (on >> 8));
129+
command(LED0_OFF_L + 4 * channel, (byte) (off & 0xFF));
130+
command(LED0_OFF_H + 4 * channel, (byte) (off >> 8));
131+
}
132+
133+
134+
private void command(int register, byte value) {
135+
beginTransmission(address);
136+
write(register);
137+
write(value);
138+
endTransmission();
139+
}
140+
141+
private byte readU8(int register) {
142+
beginTransmission(address);
143+
write(register);
144+
byte[] ba = read(1);
145+
endTransmission();
146+
return (byte)(ba[0] & 0xFF);
147+
}
148+
}

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

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
import processing.io.*;
2+
PCA9685 servos;
3+
4+
// see setup.png in the sketch folder for wiring details
5+
6+
void setup() {
7+
size(400, 300);
8+
//printArray(I2C.list());
9+
servos = new PCA9685("i2c-1", 0x40);
10+
servos.attach(0);
11+
servos.attach(1);
12+
}
13+
14+
void draw() {
15+
background(0);
16+
stroke(255);
17+
strokeWeight(3);
18+
19+
// we don't go right to the edge to prevent
20+
// making the servo unhappy
21+
float angle = 90 + sin(frameCount / 100.0)*85;
22+
servos.write(0, angle);
23+
float y = map(angle, 0, 180, 0, height);
24+
line(0, y, width/2, y);
25+
26+
angle = 90 + cos(frameCount / 100.0)*85;
27+
servos.write(1, 90 + cos(frameCount / 100.0)*85);
28+
y = map(angle, 0, 180, 0, height);
29+
line(width/2, y, width, y);
30+
}

0 commit comments

Comments
 (0)