From 29a48d2d4094d3bdc62a886fdbf9f114d2f2225e Mon Sep 17 00:00:00 2001 From: Your Name Date: Sat, 8 Jul 2023 15:43:54 +0200 Subject: [PATCH] a --- PCA9685GpioExample.java | 581 +++++++++++++++++++++++++++ __pycache__/threading.cpython-39.pyc | Bin 0 -> 603 bytes simpletest.py | 55 +++ simpletest2.py | 53 +++ simpletest3.py | 46 +++ testwalk.py | 49 +++ testwalk2.py | 20 + testwalk3.py | 11 + testwalk4.py | 14 + testwalk4.py.save | 7 + thr.py | 77 ++++ 11 files changed, 913 insertions(+) create mode 100644 PCA9685GpioExample.java create mode 100644 __pycache__/threading.cpython-39.pyc create mode 100755 simpletest.py create mode 100755 simpletest2.py create mode 100755 simpletest3.py create mode 100755 testwalk.py create mode 100644 testwalk2.py create mode 100644 testwalk3.py create mode 100644 testwalk4.py create mode 100644 testwalk4.py.save create mode 100644 thr.py diff --git a/PCA9685GpioExample.java b/PCA9685GpioExample.java new file mode 100644 index 0000000..00765c7 --- /dev/null +++ b/PCA9685GpioExample.java @@ -0,0 +1,581 @@ + +/* + * #%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 "); + 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 this would move the servo from its current position to the "); + System.out.println("| left by 10% "); + System.out.println("| Example: r this would move the servo from its current position to the "); + System.out.println("| right by 1% "); + System.out.println("| -> subsequent single will repeat the previous command "); + System.out.println("| -> max travel to either side is 100% "); + System.out.println("| Exit command: x "); + 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 this would move the servos neutral position by one step to "); + System.out.println("| the right "); + System.out.println("| Example: l this would move the servos neutral position by one step to "); + System.out.println("| the left "); + System.out.println("| -> subsequent single will repeat the previous command "); + System.out.println("| -> max adjustment to either side is 200 steps "); + System.out.println("| Exit command: x "); + 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 adjust RIGHT endpoint to 125 "); + System.out.println("| -> min: 0, max: 150, default 100 "); + System.out.println("| Exit command: x "); + 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 "); + System.out.println("| Default speed: 5 "); + System.out.println("| Exit command: x "); + 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 : [" + 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; + } + } + } + } + + + + + + + + + + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } + } diff --git a/__pycache__/threading.cpython-39.pyc b/__pycache__/threading.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..89b0d150fc8c5671026cd21c50a03a560e834971 GIT binary patch literal 603 zcmZuty>8nu5WXWNg;JF?nTl>*wbU$ zGz29@I}sWDnLNkLj81OjS{O? zt#qvsSd*sY8ta5p&M3XW3#|7jqrFS04Z|{LLx%EeRTo^`ytxGwwiBp|tuVuJg~6P# z``Lf9etRgHn3ye^uH$wHy5;#S_U&5K_EpYsCP3(6iC(>jsebWEy@MG0cvTkk59>oZMBOfxR7jg{-f~F1U9l8e=Jg%811XL0 z@L{s(S3Ftvj_hQZIlK15#4j8-WV`vt_3u#l>G`IcdE0kJ#)dcxSmGfT&*