/*
* #%L
* **********************************************************************
* ORGANIZATION : Pi4J
* PROJECT : Pi4J :: Java Examples
* FILENAME : PCA9685GpioServoExample.java
*
* This file is part of the Pi4J project. More information about
* this project can be found here: http://www.pi4j.com/
* **********************************************************************
* %%
* Copyright (C) 2012 - 2013 Pi4J
* %%
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
* #L%
*/
import java.io.IOException;
import java.math.BigDecimal;
import java.util.Scanner;
import com.pi4j.component.servo.Servo;
import com.pi4j.component.servo.impl.GenericServo;
import com.pi4j.component.servo.impl.GenericServo.Orientation;
import com.pi4j.component.servo.impl.PCA9685GpioServoProvider;
import com.pi4j.gpio.extension.pca.PCA9685GpioProvider;
import com.pi4j.gpio.extension.pca.PCA9685Pin;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinPwmOutput;
import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CFactory;
/**
* Simple servo tester application demonstrating Pi4J's Servo component.
*
* @author Christian Wehrli
* @see Servo
* @see com.pi4j.gpio.extension.pca.PCA9685GpioProvider
*/
public class PCA9685GpioServoExample {
//------------------------------------------------------------------------------------------------------------------
// main
//------------------------------------------------------------------------------------------------------------------
public static void main(String args[]) throws Exception {
System.out.println("<--Pi4J--> PCA9685 Servo Tester Example ... started.");
PCA9685GpioServoExample example = new PCA9685GpioServoExample();
Scanner scanner = new Scanner(System.in);
char command = ' ';
while (command != 'x') {
printUsage();
command = readCommand(scanner);
switch (command) {
case 'c' : // Choose Channel
example.chooseChannel(scanner);
break;
case 'n' : // Neutral Position
example.approachNeutralPosition();
break;
case 'm' : // Move
example.move(scanner);
break;
case 's' : // Sub Trim
example.subtrim(scanner);
break;
case 'r' : // Reverse
example.reverse();
break;
case 't' : // Travel (adjust endpoints)
example.travel(scanner);
break;
case 'p' : // Sweep
example.sweep(scanner);
break;
case 'i' : // Info
example.info();
break;
case 'x' : // Exit
System.out.println("Servo Example - END.");
break;
case ' ' :
System.err.println("Invalid input.");
break;
default :
System.err.println("Unknown command [" + command + "].");
break;
}
}
}
private static char readCommand(Scanner scanner) {
char result = ' ';
String input = scanner.nextLine();
if (input.trim().isEmpty() == false) {
result = input.trim().toLowerCase().charAt(0);
}
return result;
}
private static void printUsage() {
System.out.println("");
System.out.println("|- Commands ---------------------------------------------------------------------");
System.out.println("| c : choose active servo channel ");
System.out.println("| n : neutral - approach neutral position ");
System.out.println("| m : move servo position ");
System.out.println("| s : subtrim ");
System.out.println("| r : reverse servo direction ");
System.out.println("| t : travel - adjust endpoints ");
System.out.println("| p : sweep - continuously move between max left and max right position) ");
System.out.println("| i : info - provide info for all servo channels ");
System.out.println("| x : exit ");
System.out.println("|--------------------------------------------------------------------------------");
}
//------------------------------------------------------------------------------------------------------------------
// PCA9685GpioProvider
//------------------------------------------------------------------------------------------------------------------
private final PCA9685GpioProvider gpioProvider;
private final PCA9685GpioServoProvider gpioServoProvider;
private final Servo[] servos;
private int activeServo;
public PCA9685GpioServoExample() throws Exception {
gpioProvider = createProvider();
// Define outputs in use for this example
provisionPwmOutputs(gpioProvider);
gpioServoProvider = new PCA9685GpioServoProvider(gpioProvider);
servos = new Servo[16];
// Provide servo on channel 0
servos[0] = new GenericServo(gpioServoProvider.getServoDriver(PCA9685Pin.PWM_00), "Servo_1 (default settings)");
// Provide servo on channel 1
servos[1] = new GenericServo(gpioServoProvider.getServoDriver(PCA9685Pin.PWM_01), "Servo_2 (max. endpoints)");
servos[1].setProperty(Servo.PROP_END_POINT_LEFT, Float.toString(Servo.END_POINT_MAX));
servos[1].setProperty(Servo.PROP_END_POINT_RIGHT, Float.toString(Servo.END_POINT_MAX));
// Provide servo on channel 2
servos[2] = new GenericServo(gpioServoProvider.getServoDriver(PCA9685Pin.PWM_02), "Servo_3 (subtrim)");
servos[2].setProperty(Servo.PROP_SUBTRIM, Float.toString(Servo.SUBTRIM_MAX_LEFT));
// Provide servo on channel 3
servos[3] = new GenericServo(gpioServoProvider.getServoDriver(PCA9685Pin.PWM_03), "Servo_4 (reverse)");
servos[3].setProperty(Servo.PROP_IS_REVERSE, Boolean.toString(true));
// Set active servo
activeServo = 0;
}
public void chooseChannel(Scanner scanner) {
System.out.println("");
System.out.println("|- Choose channel ---------------------------------------------------------------");
System.out.println("| Choose active servo channel [0..15] ");
System.out.println("| Example: 0<Enter> ");
System.out.println("|--------------------------------------------------------------------------------");
int channel = -1;
boolean isValidChannel = false;
while (isValidChannel == false) {
String input = null;
try {
input = scanner.nextLine();
channel = Integer.parseInt(input);
if (channel >= 0 && channel <= 15) {
isValidChannel = true;
} else {
System.err.println("Unsupported servo channel [" + channel + "], provide number between 0 and 15.");
}
} catch (NumberFormatException e) {
System.err.println("Invalid input [" + input + "], provide number between 0 and 15.");
}
}
activeServo = channel;
System.out.println("Active servo channel: " + activeServo);
}
public void approachNeutralPosition() {
System.out.println("Approach neutral position");
servos[activeServo].setPosition(0);
}
public void move(Scanner scanner) {
System.out.println("");
System.out.println("|- Move Position ----------------------------------------------------------------");
System.out.println("| Move servo position to the left or to the right. ");
System.out.println("| Example: l10<Enter> this would move the servo from its current position to the ");
System.out.println("| left by 10% ");
System.out.println("| Example: r<Enter> this would move the servo from its current position to the ");
System.out.println("| right by 1% ");
System.out.println("| -> subsequent single <Enter> will repeat the previous command ");
System.out.println("| -> max travel to either side is 100% ");
System.out.println("| Exit command: x<Enter> ");
System.out.println("|--------------------------------------------------------------------------------");
String command = null;
while ("x".equals(command) == false) {
float currentPosition = servos[activeServo].getPosition();
System.out.println("Current servo position: " + currentPosition);
String input = scanner.nextLine();
if (input.trim().isEmpty() == false) {
command = input.trim().toLowerCase();
}
if (command != null) {
int sign;
if (command.startsWith("l")) {
sign = -1;
} else if (command.startsWith("r")) {
sign = 1;
} else if (command.equals("x")) {
continue;
} else {
System.err.println("Unknown command [" + command + "].");
command = null;
continue;
}
int moveAmount = 1;
try {
moveAmount = Integer.parseInt(command.substring(1));
if (moveAmount < 0 || moveAmount > 100) {
moveAmount = 1;
System.out.println("Move amount is out of range - defaulted to [1].");
}
System.out.println("Move amount is [" + moveAmount + "].");
} catch (Exception e) {
System.out.println("Move amount defaulted to [1].");
}
float newPosition = currentPosition + moveAmount * sign;
if (newPosition < Servo.POS_MAX_LEFT) {
newPosition = Servo.POS_MAX_LEFT;
System.out.println("Max left position exceeded - set position to " + Servo.POS_MAX_LEFT + "%");
} else if (newPosition > Servo.POS_MAX_RIGHT) {
newPosition = Servo.POS_MAX_RIGHT;
System.out.println("Max right position exceeded - set position to " + Servo.POS_MAX_RIGHT + "%");
}
servos[activeServo].setPosition(newPosition);
command = (sign == 1 ? "r" : "l") + moveAmount;
}
}
}
public void subtrim(Scanner scanner) {
System.out.println("");
System.out.println("|- Subtrim, adjust servo neutral position ---------------------------------------");
System.out.println("| Example: r<Enter> this would move the servos neutral position by one step to ");
System.out.println("| the right ");
System.out.println("| Example: l<Enter> this would move the servos neutral position by one step to ");
System.out.println("| the left ");
System.out.println("| -> subsequent single <Enter> will repeat the previous command ");
System.out.println("| -> max adjustment to either side is 200 steps ");
System.out.println("| Exit command: x<Enter> ");
System.out.println("|--------------------------------------------------------------------------------");
System.out.println("| Current Servo position: " + servos[activeServo].getPosition() + "] ");
System.out.println("|--------------------------------------------------------------------------------");
String command = null;
while ("x".equals(command) == false) {
String propertySubtrim = servos[activeServo].getProperty(Servo.PROP_SUBTRIM, Servo.PROP_SUBTRIM_DEFAULT);
int currentSubtrim = Integer.parseInt(propertySubtrim);
System.out.println("Current subtrim: " + currentSubtrim);
String input = scanner.nextLine();
if (input.trim().isEmpty() == false) {
command = input.trim().toLowerCase();
}
if (command != null) {
int moveAmount;
if (command.startsWith("l")) {
moveAmount = -1;
} else if (command.startsWith("r")) {
moveAmount = 1;
} else if (command.equals("x")) {
continue;
} else {
System.err.println("Unknown command [" + command + "].");
command = null;
continue;
}
float newSubtrim = currentSubtrim + moveAmount;
if (newSubtrim < Servo.SUBTRIM_MAX_LEFT) {
newSubtrim = Servo.SUBTRIM_MAX_LEFT;
System.out.println("Max left subtrim exceeded - set value to " + Servo.SUBTRIM_MAX_LEFT);
} else if (newSubtrim > Servo.SUBTRIM_MAX_RIGHT) {
newSubtrim = Servo.SUBTRIM_MAX_RIGHT;
System.out.println("Max right subtrim exceeded - set value to " + Servo.SUBTRIM_MAX_RIGHT);
}
servos[activeServo].setProperty(Servo.PROP_SUBTRIM, Float.toString(newSubtrim));
}
}
}
public void reverse() {
boolean isReverse = Boolean.parseBoolean(servos[activeServo].getProperty(Servo.PROP_IS_REVERSE));
Boolean newValue = isReverse ? Boolean.FALSE : Boolean.TRUE;
servos[activeServo].setProperty(Servo.PROP_IS_REVERSE, newValue.toString());
System.out.println("is reverse: " + newValue);
}
public void travel(Scanner scanner) {
System.out.println("");
System.out.println("|- Travel -----------------------------------------------------------------------");
System.out.println("| Adjust endpoints. ");
System.out.println("| Example: r125<Enter> adjust RIGHT endpoint to 125 ");
System.out.println("| -> min: 0, max: 150, default 100 ");
System.out.println("| Exit command: x<Enter> ");
System.out.println("|--------------------------------------------------------------------------------");
String command = null;
while ("x".equals(command) == false) {
String propertyEndpointLeft = servos[activeServo].getProperty(Servo.PROP_END_POINT_LEFT, Servo.PROP_END_POINT_DEFAULT);
String propertyEndpointRight = servos[activeServo].getProperty(Servo.PROP_END_POINT_RIGHT, Servo.PROP_END_POINT_DEFAULT);
System.out.println("Current endpoints: LEFT [" + propertyEndpointLeft + "], RIGHT [" + propertyEndpointRight + "]");
String input = scanner.nextLine();
if (input.trim().isEmpty() == false) {
command = input.trim().toLowerCase();
}
if (command != null) {
String propertyToAdjust;
if (command.startsWith("l")) {
propertyToAdjust = Servo.PROP_END_POINT_LEFT;
} else if (command.startsWith("r")) {
propertyToAdjust = Servo.PROP_END_POINT_RIGHT;
} else if (command.equals("x")) {
continue;
} else {
System.err.println("Unknown command [" + command + "].");
command = null;
continue;
}
int newEndpointValue;
try {
newEndpointValue = Integer.parseInt(command.substring(1));
if (newEndpointValue < Servo.END_POINT_MIN || newEndpointValue > Servo.END_POINT_MAX) {
System.out.println("Endpoint value is out of range - defaulted to [" + Servo.PROP_END_POINT_DEFAULT + "].");
newEndpointValue = Integer.parseInt(Servo.PROP_END_POINT_DEFAULT);
}
System.out.println("New value for property [" + propertyToAdjust + "]: " + newEndpointValue + "");
} catch (Exception e) {
System.out.println("Endpoint value for property [" + propertyToAdjust + "] defaulted to [" + Servo.PROP_END_POINT_DEFAULT + "].");
newEndpointValue = Integer.parseInt(Servo.PROP_END_POINT_DEFAULT);
}
servos[activeServo].setProperty(propertyToAdjust, Integer.toString(newEndpointValue));
}
}
}
public void sweep(Scanner scanner) throws Exception {
System.out.println("");
System.out.println("|- Sweep ------------------------------------------------------------------------");
System.out.println("| Continuously moves the servo between POS_MAX_LEFT and POS_MAX_RIGHT. ");
System.out.println("| To change speed provide value between 1 and 10 (10 for max speed) ");
System.out.println("| Example: 7<Enter> ");
System.out.println("| Default speed: 5 ");
System.out.println("| Exit command: x<Enter> ");
System.out.println("|--------------------------------------------------------------------------------");
// create and start sweeper thread
Sweeper sweeper = new Sweeper();
sweeper.start();
// handle user commands
String command = null;
while ("x".equals(command) == false) {
String input = scanner.nextLine();
if (input.trim().isEmpty() == false) {
command = input.trim().toLowerCase();
}
if (command != null) {
if (command.equals("x")) {
continue;
}
try {
int speed = Integer.parseInt(command);
sweeper.setSpeed(speed);
} catch (NumberFormatException e) {
System.err.println("Invalid speed value [" + command + "]. Allowed values [1..10] ");
}
}
}
sweeper.interrupt();
servos[activeServo].setPosition(Servo.POS_NEUTRAL);
}
public void info() {
for (int i = 0; i < servos.length; i++) {
Servo servo = servos[i];
System.out.println("Channel " + (i < 10 ? " " : "") + i + ": " + (servo != null ? servo.toString() : "N.A."));
}
}
//------------------------------------------------------------------------------------------------------------------
// Helpers
//------------------------------------------------------------------------------------------------------------------
private PCA9685GpioProvider createProvider() throws IOException {
BigDecimal frequency = PCA9685GpioProvider.ANALOG_SERVO_FREQUENCY;
BigDecimal frequencyCorrectionFactor = new BigDecimal("1.0578");
I2CBus bus = I2CFactory.getInstance(I2CBus.BUS_1);
return new PCA9685GpioProvider(bus, 0x40, frequency, frequencyCorrectionFactor);
}
private GpioPinPwmOutput[] provisionPwmOutputs(final PCA9685GpioProvider gpioProvider) {
GpioController gpio = GpioFactory.getInstance();
GpioPinPwmOutput myOutputs[] = {
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_00, "Servo 00"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_01, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_02, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_03, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_04, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_05, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_06, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_07, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_08, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_09, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_10, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_11, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_12, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_13, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_14, "not used"),
gpio.provisionPwmOutputPin(gpioProvider, PCA9685Pin.PWM_15, "not used")};
return myOutputs;
}
private class Sweeper extends Thread {
private int speed = 5;
private final int step = 1; // make sure this is always true: 100 % step = 0
private final int maxSleepBetweenSteps = 100;
@Override
public void run() {
int position = 0;
Orientation orientation = Orientation.RIGHT;
while (!Thread.currentThread().isInterrupted()) {
try {
if (orientation == Orientation.RIGHT) {
if (position < Servo.POS_MAX_RIGHT) {
position += step;
} else {
orientation = Orientation.LEFT;
position -= step;
}
} else if (orientation == Orientation.LEFT) {
if (position > Servo.POS_MAX_LEFT) {
position -= step;
} else {
orientation = Orientation.RIGHT;
position += step;
}
} else {
System.err.println("Unsupported value for enum <ServoBase.Orientation>: [" + orientation + "].");
}
servos[activeServo].setPosition(position);
Thread.currentThread();
if (position % 10 == 0) {
System.out.println("Position: " + position);
}
Thread.sleep(maxSleepBetweenSteps / speed);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
}
public void setSpeed(int speed) {
if (speed < 1) {
this.speed = 1;
} else if (speed > 10) {
this.speed = 10;
} else {
this.speed = speed;
}
}
}
}