├── .github └── workflows │ ├── gradle-build.yml │ └── gradle-publish.yml ├── .gitignore ├── README.md ├── build.gradle ├── gradle.properties ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── gradlew ├── gradlew.bat ├── jitpack.yml ├── settings.gradle └── src └── main └── java └── io └── github └── pseudoresonance └── pixy2api ├── Pixy2.java ├── Pixy2CCC.java ├── Pixy2Line.java ├── Pixy2Video.java └── links ├── I2CLink.java ├── Link.java ├── SPILink.java └── UARTLink.java /.github/workflows/gradle-build.yml: -------------------------------------------------------------------------------- 1 | name: Gradle Build/Javadoc 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | workflow_dispatch: 7 | 8 | # Allow only one concurrent deployment, skipping runs queued between the run in-progress and latest queued. 9 | # However, do NOT cancel in-progress runs as we want to allow these production deployments to complete. 10 | concurrency: 11 | group: "pages" 12 | cancel-in-progress: false 13 | 14 | jobs: 15 | build: 16 | runs-on: ubuntu-latest 17 | permissions: 18 | contents: write 19 | pages: write 20 | 21 | steps: 22 | - uses: actions/checkout@v3 23 | - name: Set up JDK 11 24 | uses: actions/setup-java@v3 25 | with: 26 | java-version: '11' 27 | distribution: 'temurin' 28 | server-id: github # Value of the distributionManagement/repository/id field of the pom.xml 29 | settings-path: ${{ github.workspace }} # location for the settings.xml file 30 | 31 | - name: Build with Gradle 32 | uses: gradle/gradle-build-action@bd5760595778326ba7f1441bcf7e88b49de61a25 # v2.6.0 33 | with: 34 | arguments: build javadoc javadocJar sourceJar 35 | 36 | - name: Archive production artifacts 37 | uses: actions/upload-artifact@v3 38 | with: 39 | name: artifacts 40 | path: build/libs/*.jar 41 | 42 | - name: Deploy to GitHub Pages 43 | uses: JamesIves/github-pages-deploy-action@v4.4.3 44 | with: 45 | folder: build/docs/javadoc 46 | -------------------------------------------------------------------------------- /.github/workflows/gradle-publish.yml: -------------------------------------------------------------------------------- 1 | # This workflow uses actions that are not certified by GitHub. 2 | # They are provided by a third-party and are governed by 3 | # separate terms of service, privacy policy, and support 4 | # documentation. 5 | # This workflow will build a package using Gradle and then publish it to GitHub packages when a release is created 6 | # For more information see: https://github.com/actions/setup-java/blob/main/docs/advanced-usage.md#Publishing-using-gradle 7 | 8 | name: Gradle Package 9 | 10 | on: 11 | release: 12 | types: [created] 13 | workflow_dispatch: 14 | 15 | jobs: 16 | build: 17 | 18 | runs-on: ubuntu-latest 19 | permissions: 20 | contents: read 21 | packages: write 22 | 23 | steps: 24 | - uses: actions/checkout@v3 25 | - name: Set up JDK 11 26 | uses: actions/setup-java@v3 27 | with: 28 | java-version: '11' 29 | distribution: 'temurin' 30 | server-id: github # Value of the distributionManagement/repository/id field of the pom.xml 31 | settings-path: ${{ github.workspace }} # location for the settings.xml file 32 | 33 | - name: Build with Gradle 34 | uses: gradle/gradle-build-action@bd5760595778326ba7f1441bcf7e88b49de61a25 # v2.6.0 35 | with: 36 | arguments: build javadoc javadocJar sourceJar 37 | 38 | - name: Archive production artifacts 39 | uses: actions/upload-artifact@v3 40 | with: 41 | name: dist-without-markdown 42 | path: build/libs/*.jar 43 | 44 | # The USERNAME and TOKEN need to correspond to the credentials environment variables used in 45 | # the publishing section of your build.gradle 46 | - name: Publish to GitHub Packages 47 | uses: gradle/gradle-build-action@bd5760595778326ba7f1441bcf7e88b49de61a25 # v2.6.0 48 | with: 49 | arguments: publish 50 | env: 51 | GITHUB_ACTOR: ${{ github.actor }} 52 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 53 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode 2 | 3 | ### C++ ### 4 | # Prerequisites 5 | *.d 6 | 7 | # Compiled Object files 8 | *.slo 9 | *.lo 10 | *.o 11 | *.obj 12 | 13 | # Precompiled Headers 14 | *.gch 15 | *.pch 16 | 17 | # Compiled Dynamic libraries 18 | *.so 19 | *.dylib 20 | *.dll 21 | 22 | # Fortran module files 23 | *.mod 24 | *.smod 25 | 26 | # Compiled Static libraries 27 | *.lai 28 | *.la 29 | *.a 30 | *.lib 31 | 32 | # Executables 33 | *.exe 34 | *.out 35 | *.app 36 | 37 | ### Java ### 38 | # Compiled class file 39 | *.class 40 | 41 | # Log file 42 | *.log 43 | 44 | # BlueJ files 45 | *.ctxt 46 | 47 | # Mobile Tools for Java (J2ME) 48 | .mtj.tmp/ 49 | 50 | # Package Files # 51 | *.jar 52 | *.war 53 | *.nar 54 | *.ear 55 | *.zip 56 | *.tar.gz 57 | *.rar 58 | 59 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 60 | hs_err_pid* 61 | 62 | ### Linux ### 63 | *~ 64 | 65 | # temporary files which can be created if a process still has a handle open of a deleted file 66 | .fuse_hidden* 67 | 68 | # KDE directory preferences 69 | .directory 70 | 71 | # Linux trash folder which might appear on any partition or disk 72 | .Trash-* 73 | 74 | # .nfs files are created when an open file is removed but is still being accessed 75 | .nfs* 76 | 77 | ### macOS ### 78 | # General 79 | .DS_Store 80 | .AppleDouble 81 | .LSOverride 82 | 83 | # Icon must end with two \r 84 | Icon 85 | 86 | # Thumbnails 87 | ._* 88 | 89 | # Files that might appear in the root of a volume 90 | .DocumentRevisions-V100 91 | .fseventsd 92 | .Spotlight-V100 93 | .TemporaryItems 94 | .Trashes 95 | .VolumeIcon.icns 96 | .com.apple.timemachine.donotpresent 97 | 98 | # Directories potentially created on remote AFP share 99 | .AppleDB 100 | .AppleDesktop 101 | Network Trash Folder 102 | Temporary Items 103 | .apdisk 104 | 105 | ### VisualStudioCode ### 106 | .vscode/* 107 | !.vscode/settings.json 108 | !.vscode/tasks.json 109 | !.vscode/launch.json 110 | !.vscode/extensions.json 111 | 112 | ### Windows ### 113 | # Windows thumbnail cache files 114 | Thumbs.db 115 | ehthumbs.db 116 | ehthumbs_vista.db 117 | 118 | # Dump file 119 | *.stackdump 120 | 121 | # Folder config file 122 | [Dd]esktop.ini 123 | 124 | # Recycle Bin used on file shares 125 | $RECYCLE.BIN/ 126 | 127 | # Windows Installer files 128 | *.cab 129 | *.msi 130 | *.msix 131 | *.msm 132 | *.msp 133 | 134 | # Windows shortcuts 135 | *.lnk 136 | 137 | ### Gradle ### 138 | .gradle 139 | /build/ 140 | 141 | # Ignore Gradle GUI config 142 | gradle-app.setting 143 | 144 | # Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) 145 | !gradle-wrapper.jar 146 | 147 | # Cache of project 148 | .gradletasknamecache 149 | 150 | # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 151 | # gradle/wrapper/gradle-wrapper.properties 152 | 153 | # # VS Code Specific Java Settings 154 | .classpath 155 | .project 156 | .settings/ 157 | bin/ 158 | 159 | 160 | # End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode 161 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pixy2JavaAPI 2 | 3 | [![Build Status](https://ci.otake.pw/buildStatus/icon?job=Pixy2JavaAPI&subject=Jenkins)](https://ci.otake.pw/job/Pixy2JavaAPI/) 4 | [Latest Javadocs](https://pseudoresonance.github.io/Pixy2JavaAPI/) 5 | 6 | Pixy2 API ported to Java for FIRST Robotics RoboRIO 7 | 8 | Port by PseudoResonance (Josh Otake) with help from other various contributors. 9 | 10 | Thank you for your support and usage of this API! 11 | 12 | [Original Pixy2 Code for C++ Arduino](https://github.com/charmedlabs/pixy2/tree/master/src/host/arduino/libraries/Pixy2) 13 | 14 | Please read the [wiki](https://github.com/PseudoResonance/Pixy2JavaAPI/wiki) for more detailed information about Pixy2JavaAPI! 15 | 16 | --- 17 | ## Installing the API 18 | To install the API, it can either be downloaded and copied directly into the project, or you can use it with Gradle/Maven. 19 | 20 | The maven repository is located at: https://nexus.otake.pw/repository/maven-public/ The group id is `pw.otake.pseudoresonance` and the artifact name is `pixy2-java-api` For FRC Gradle builds, this can be easily added by adding the following to `build.gradle` in the project's root. 21 | 22 | Add `maven { url 'https://nexus.otake.pw/repository/maven-public/' }` under `repositories` 23 | 24 | Add `implementation 'pw.otake.pseudoresonance:pixy2-java-api:1.4.1'` under `dependencies` Make sure to set the API version to latest available. 25 | 26 | Your `build.gradle` should resemble this: 27 | 28 | ```gradle 29 | repositories { 30 | maven { url 'https://nexus.otake.pw/repository/maven-public/' } 31 | } 32 | 33 | // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. 34 | // Also defines JUnit 4. 35 | dependencies { 36 | implementation 'pw.otake.pseudoresonance:pixy2-java-api:1.4.1' 37 | implementation wpi.deps.wpilib() 38 | nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) 39 | nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) 40 | implementation wpi.deps.vendor.java() 41 | nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) 42 | nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) 43 | testImplementation 'junit:junit:4.12' 44 | simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) 45 | } 46 | ``` 47 | 48 | If there are issues using the repository, or you don't want to use it, the files can also be copied directly into the project and used, however I ask that you leave the file headers intact, so that others can find the project. 49 | 50 | --- 51 | ## Using the API 52 | First create a Pixy2 camera object with `Pixy2 pixy = Pixy2.createInstance(link)` and supply the link type of your choosing. Next, initialize the Pixy2 camera with `pixy.init(arg)`. You can either omit arg, or add a value based on the link type. 53 | 54 | The Pixy2 can now be called on with the various provided methods as outlined in the documentation included in the code and on the Pixy2 website. 55 | 56 | Please read the [wiki](https://github.com/PseudoResonance/Pixy2JavaAPI/wiki/Using-the-API) for more information about how to use the API, including examples. 57 | 58 | --- 59 | ## Supported Links to Communicate with Pixy 60 | SPI, I2C (Untested), UART/Serial (Untested) 61 | 62 | New link types can be easily added to support future hardware, or other Java-based projects by implementing [Link](https://github.com/PseudoResonance/Pixy2JavaAPI/blob/master/src/main/java/io/github/pseudoresonance/pixy2api/links/Link.java) 63 | 64 | --- 65 | ## Wiring Pixy2 to RoboRIO 66 | SPI is the recommended link type due to it's higher data transfer rate as well as better implementation in the WPILib API which helps with efficiency. 67 | 68 | ### SPI 69 | | Pixy2 Port | RoboRIO Port | 70 | | --- | --- | 71 | | 1 | MISO | 72 | | 2 | 5V | 73 | | 3 | SCLK | 74 | | 4 | MOSI | 75 | | 6 | ⏚ Ground | 76 | | 7 | CS0 (Optional) | 77 | 78 | **NOTE**: Pin 7/CS0 pin is the optional SPI Slave Select (SS) pin. It can be connected to any SS pin on the RoboRIO, CS0, CS1, CS2 or CS3. If slave select functionality is not needed, set the Pixy2 to use the data output `Arduino ICSP SPI`. Use `SPI with SS` for slave select support. In the code, the slave pin in use can be selected by using an [initialization argument](#using-the-api) of the corresponding pin. 0 for CS0, 1 for CS1, etc. 79 | 80 | ### I2C 81 | | Pixy2 Port | RoboRIO Port | 82 | | --- | --- | 83 | | 2 | 5V (from VRM) | 84 | | 5 | SCL | 85 | | 6 | ⏚ Ground | 86 | | 9 | SDA | 87 | 88 | **NOTE**: The RoboRIO does not have a 5V output for I2C, and thus, the 5V must be sourced elsewhere, such as from the VRM, or another 5V pin. 89 | 90 | ### UART/Serial/RS-232 91 | | Pixy2 Port | RoboRIO Port | 92 | | --- | --- | 93 | | 1 | RXD | 94 | | 2 | 5V (from VRM) | 95 | | 4 | TXD | 96 | | 6 | ⏚ Ground | 97 | 98 | **WARNING**: The RoboRIO RS-232 port outputs an RS-232 serial signal, which is incompatible with the Pixy2's TTL serial signal and may result in damage to the Pixy2. An RS-232 to TTL converter board can be used, or the Pixy2 can be wired to the TTL serial pins in the RoboRIO's MXP expansion port. 99 | 100 | **NOTE**: The RoboRIO does not have a 5V output for UART/Serial/RS-232, and thus, the 5V must be sourced elsewhere, such as from the VRM, or another 5V pin. 101 | 102 | #### Pixy2 Pinout 103 | ![Pixy2 Pinout](https://docs.pixycam.com/wiki/lib/exe/fetch.php?w=640&tok=f1a03d&media=wiki:v2:image_248_2.jpg "Pixy2 Pinout") 104 | 105 | [Pixy2 Connection Guide](https://docs.pixycam.com/wiki/doku.php?id=wiki:v2:i_don-27t_see_my_controller_supported_what_do_i_do "Pixy2 Connection Guide") 106 | -------------------------------------------------------------------------------- /build.gradle: -------------------------------------------------------------------------------- 1 | plugins { 2 | id "java" 3 | id "edu.wpi.first.GradleRIO" version "${gradle_rio_version}" 4 | id "maven-publish" 5 | } 6 | 7 | sourceCompatibility = JavaVersion.VERSION_11 8 | targetCompatibility = JavaVersion.VERSION_11 9 | 10 | javadoc { 11 | source = sourceSets.main.allJava 12 | classpath = sourceSets.main.runtimeClasspath 13 | } 14 | 15 | // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. 16 | // Also defines JUnit 4. 17 | dependencies { 18 | implementation "edu.wpi.first.wpilibj:wpilibj-java:${wpi.wpilibVersion}" 19 | } 20 | 21 | ext { 22 | jarToPublish = file( "${project.artifact_name}.jar" ) 23 | } 24 | 25 | jar { 26 | baseName "${project.artifact_name}" 27 | } 28 | 29 | task sourceJar(type: Jar, dependsOn: classes) { 30 | baseName "${project.artifact_name}" 31 | classifier 'sources' 32 | from sourceSets.main.allSource 33 | } 34 | 35 | task javadocJar(type: Jar, dependsOn: javadoc) { 36 | baseName "${project.artifact_name}" 37 | classifier = 'javadoc' 38 | from javadoc.destinationDir 39 | } 40 | 41 | task replaceVersionInREADME { 42 | ant.replaceregexp(match:"${project.artifact_group}:${project.artifact_name}:([0-9\\.]+)", replace:"${project.artifact_group}:${project.artifact_name}:${project.artifact_version}", flags:'g', byline:true, encoding: 'UTF-8') { 43 | fileset(dir: '.', includes: 'README.md') 44 | } 45 | } 46 | 47 | publishing { 48 | publications { 49 | mavenJava(MavenPublication) { 50 | from components.java 51 | artifact sourceJar 52 | artifact javadocJar 53 | groupId = "${project.artifact_group}" 54 | artifactId = "${project.artifact_name}" 55 | version = "${project.artifact_version}" 56 | pom { 57 | name = 'Pixy2JavaAPI' 58 | description = 'Java port of Pixy2 API for FIRST Robotics' 59 | url = 'https://github.com/PseudoResonance/Pixy2JavaAPI/' 60 | licenses { 61 | license { 62 | name = 'GNU General Public License, version 2' 63 | url = 'http://www.gnu.org/licenses/gpl-2.0.html' 64 | } 65 | } 66 | developers { 67 | developer { 68 | id = 'PseudoResonance' 69 | name = 'Josh Otake' 70 | email = 'kaio11602@gmail.com' 71 | } 72 | } 73 | } 74 | } 75 | } 76 | repositories { 77 | maven { 78 | name = "GitHubPackages" 79 | url = "https://maven.pkg.github.com/PseudoResonance/Pixy2JavaAPI" 80 | credentials { 81 | username = System.getenv("GITHUB_ACTOR") 82 | password = System.getenv("GITHUB_TOKEN") 83 | } 84 | } 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /gradle.properties: -------------------------------------------------------------------------------- 1 | # API Properties 2 | artifact_group = pw.otake.pseudoresonance 3 | artifact_name = pixy2-java-api 4 | artifact_version = 1.4.1 5 | 6 | # Dependency Versions 7 | gradle_rio_version = 2021.1.1-beta-4 -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PseudoResonance/Pixy2JavaAPI/dd753b74d4f12e353dd95490964db5407e4371ae/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip 4 | zipStoreBase=GRADLE_USER_HOME 5 | zipStorePath=permwrapper/dists 6 | -------------------------------------------------------------------------------- /gradlew: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | 3 | ############################################################################## 4 | ## 5 | ## Gradle start up script for UN*X 6 | ## 7 | ############################################################################## 8 | 9 | # Attempt to set APP_HOME 10 | # Resolve links: $0 may be a link 11 | PRG="$0" 12 | # Need this for relative symlinks. 13 | while [ -h "$PRG" ] ; do 14 | ls=`ls -ld "$PRG"` 15 | link=`expr "$ls" : '.*-> \(.*\)$'` 16 | if expr "$link" : '/.*' > /dev/null; then 17 | PRG="$link" 18 | else 19 | PRG=`dirname "$PRG"`"/$link" 20 | fi 21 | done 22 | SAVED="`pwd`" 23 | cd "`dirname \"$PRG\"`/" >/dev/null 24 | APP_HOME="`pwd -P`" 25 | cd "$SAVED" >/dev/null 26 | 27 | APP_NAME="Gradle" 28 | APP_BASE_NAME=`basename "$0"` 29 | 30 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 31 | DEFAULT_JVM_OPTS='"-Xmx64m"' 32 | 33 | # Use the maximum available, or set MAX_FD != -1 to use that value. 34 | MAX_FD="maximum" 35 | 36 | warn () { 37 | echo "$*" 38 | } 39 | 40 | die () { 41 | echo 42 | echo "$*" 43 | echo 44 | exit 1 45 | } 46 | 47 | # OS specific support (must be 'true' or 'false'). 48 | cygwin=false 49 | msys=false 50 | darwin=false 51 | nonstop=false 52 | case "`uname`" in 53 | CYGWIN* ) 54 | cygwin=true 55 | ;; 56 | Darwin* ) 57 | darwin=true 58 | ;; 59 | MINGW* ) 60 | msys=true 61 | ;; 62 | NONSTOP* ) 63 | nonstop=true 64 | ;; 65 | esac 66 | 67 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar 68 | 69 | # Determine the Java command to use to start the JVM. 70 | if [ -n "$JAVA_HOME" ] ; then 71 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then 72 | # IBM's JDK on AIX uses strange locations for the executables 73 | JAVACMD="$JAVA_HOME/jre/sh/java" 74 | else 75 | JAVACMD="$JAVA_HOME/bin/java" 76 | fi 77 | if [ ! -x "$JAVACMD" ] ; then 78 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME 79 | 80 | Please set the JAVA_HOME variable in your environment to match the 81 | location of your Java installation." 82 | fi 83 | else 84 | JAVACMD="java" 85 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 86 | 87 | Please set the JAVA_HOME variable in your environment to match the 88 | location of your Java installation." 89 | fi 90 | 91 | # Increase the maximum file descriptors if we can. 92 | if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then 93 | MAX_FD_LIMIT=`ulimit -H -n` 94 | if [ $? -eq 0 ] ; then 95 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then 96 | MAX_FD="$MAX_FD_LIMIT" 97 | fi 98 | ulimit -n $MAX_FD 99 | if [ $? -ne 0 ] ; then 100 | warn "Could not set maximum file descriptor limit: $MAX_FD" 101 | fi 102 | else 103 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" 104 | fi 105 | fi 106 | 107 | # For Darwin, add options to specify how the application appears in the dock 108 | if $darwin; then 109 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" 110 | fi 111 | 112 | # For Cygwin, switch paths to Windows format before running java 113 | if $cygwin ; then 114 | APP_HOME=`cygpath --path --mixed "$APP_HOME"` 115 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` 116 | JAVACMD=`cygpath --unix "$JAVACMD"` 117 | 118 | # We build the pattern for arguments to be converted via cygpath 119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` 120 | SEP="" 121 | for dir in $ROOTDIRSRAW ; do 122 | ROOTDIRS="$ROOTDIRS$SEP$dir" 123 | SEP="|" 124 | done 125 | OURCYGPATTERN="(^($ROOTDIRS))" 126 | # Add a user-defined pattern to the cygpath arguments 127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then 128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" 129 | fi 130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh 131 | i=0 132 | for arg in "$@" ; do 133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` 134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option 135 | 136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition 137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` 138 | else 139 | eval `echo args$i`="\"$arg\"" 140 | fi 141 | i=$((i+1)) 142 | done 143 | case $i in 144 | (0) set -- ;; 145 | (1) set -- "$args0" ;; 146 | (2) set -- "$args0" "$args1" ;; 147 | (3) set -- "$args0" "$args1" "$args2" ;; 148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;; 149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; 150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; 151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; 152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; 153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; 154 | esac 155 | fi 156 | 157 | # Escape application args 158 | save () { 159 | for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done 160 | echo " " 161 | } 162 | APP_ARGS=$(save "$@") 163 | 164 | # Collect all arguments for the java command, following the shell quoting and substitution rules 165 | eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" 166 | 167 | # by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong 168 | if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then 169 | cd "$(dirname "$0")" 170 | fi 171 | 172 | exec "$JAVACMD" "$@" 173 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @if "%DEBUG%" == "" @echo off 2 | @rem ########################################################################## 3 | @rem 4 | @rem Gradle startup script for Windows 5 | @rem 6 | @rem ########################################################################## 7 | 8 | @rem Set local scope for the variables with windows NT shell 9 | if "%OS%"=="Windows_NT" setlocal 10 | 11 | set DIRNAME=%~dp0 12 | if "%DIRNAME%" == "" set DIRNAME=. 13 | set APP_BASE_NAME=%~n0 14 | set APP_HOME=%DIRNAME% 15 | 16 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 17 | set DEFAULT_JVM_OPTS="-Xmx64m" 18 | 19 | @rem Find java.exe 20 | if defined JAVA_HOME goto findJavaFromJavaHome 21 | 22 | set JAVA_EXE=java.exe 23 | %JAVA_EXE% -version >NUL 2>&1 24 | if "%ERRORLEVEL%" == "0" goto init 25 | 26 | echo. 27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 28 | echo. 29 | echo Please set the JAVA_HOME variable in your environment to match the 30 | echo location of your Java installation. 31 | 32 | goto fail 33 | 34 | :findJavaFromJavaHome 35 | set JAVA_HOME=%JAVA_HOME:"=% 36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 37 | 38 | if exist "%JAVA_EXE%" goto init 39 | 40 | echo. 41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 42 | echo. 43 | echo Please set the JAVA_HOME variable in your environment to match the 44 | echo location of your Java installation. 45 | 46 | goto fail 47 | 48 | :init 49 | @rem Get command-line arguments, handling Windows variants 50 | 51 | if not "%OS%" == "Windows_NT" goto win9xME_args 52 | 53 | :win9xME_args 54 | @rem Slurp the command line arguments. 55 | set CMD_LINE_ARGS= 56 | set _SKIP=2 57 | 58 | :win9xME_args_slurp 59 | if "x%~1" == "x" goto execute 60 | 61 | set CMD_LINE_ARGS=%* 62 | 63 | :execute 64 | @rem Setup the command line 65 | 66 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 67 | 68 | @rem Execute Gradle 69 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% 70 | 71 | :end 72 | @rem End local scope for the variables with windows NT shell 73 | if "%ERRORLEVEL%"=="0" goto mainEnd 74 | 75 | :fail 76 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 77 | rem the _cmd.exe /c_ return code! 78 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 79 | exit /b 1 80 | 81 | :mainEnd 82 | if "%OS%"=="Windows_NT" endlocal 83 | 84 | :omega 85 | -------------------------------------------------------------------------------- /jitpack.yml: -------------------------------------------------------------------------------- 1 | jdk: 2 | - openjdk11 3 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | pluginManagement { 2 | repositories { 3 | mavenLocal() 4 | gradlePluginPortal() 5 | } 6 | } 7 | -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/Pixy2.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api; 2 | 3 | import java.awt.Color; 4 | import java.util.concurrent.TimeUnit; 5 | 6 | import io.github.pseudoresonance.pixy2api.links.I2CLink; 7 | import io.github.pseudoresonance.pixy2api.links.Link; 8 | import io.github.pseudoresonance.pixy2api.links.SPILink; 9 | import io.github.pseudoresonance.pixy2api.links.UARTLink; 10 | 11 | /** 12 | * Java Port of Pixy2 Arduino Library 13 | * 14 | * Interfaces with the Pixy2 over any provided, compatible link 15 | * 16 | * https://github.com/PseudoResonance/Pixy2JavaAPI 17 | * 18 | * @author PseudoResonance (Josh Otake) 19 | * 20 | * ORIGINAL HEADER - 21 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/TPixy2.h 22 | * ========================================================================================== 23 | * begin license header 24 | * 25 | * This file is part of Pixy CMUcam5 or "Pixy" for short 26 | * 27 | * All Pixy source code is provided under the terms of the GNU General 28 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 29 | * wishing to use Pixy source code, software and/or technologies under 30 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 31 | * Such licensing terms are available for all portions of the Pixy 32 | * codebase presented here. 33 | * 34 | * end license header 35 | * 36 | * Main Pixy template class. This class takes a link class and uses it 37 | * to communicate with Pixy over I2C, SPI, UART or USB using the Pixy 38 | * packet protocol. 39 | */ 40 | 41 | public class Pixy2 { 42 | 43 | public final static int PIXY_BUFFERSIZE = 0x104; 44 | public final static int PIXY_SEND_HEADER_SIZE = 4; 45 | public final static int PIXY_MAX_PROGNAME = 33; 46 | public final static int PIXY_DEFAULT_ARGVAL = 0x80000000; 47 | public final static int PIXY_CHECKSUM_SYNC = 0xc1af; 48 | public final static int PIXY_NO_CHECKSUM_SYNC = 0xc1ae; 49 | 50 | // Packet types 51 | public final static byte PIXY_TYPE_REQUEST_CHANGE_PROG = 0x02; 52 | public final static byte PIXY_TYPE_REQUEST_RESOLUTION = 0x0c; 53 | public final static byte PIXY_TYPE_RESPONSE_RESOLUTION = 0x0d; 54 | public final static byte PIXY_TYPE_REQUEST_VERSION = 0x0e; 55 | public final static byte PIXY_TYPE_RESPONSE_VERSION = 0x0f; 56 | public final static byte PIXY_TYPE_RESPONSE_RESULT = 0x01; 57 | public final static byte PIXY_TYPE_RESPONSE_ERROR = 0x03; 58 | public final static byte PIXY_TYPE_REQUEST_BRIGHTNESS = 0x10; 59 | public final static byte PIXY_TYPE_REQUEST_SERVO = 0x12; 60 | public final static byte PIXY_TYPE_REQUEST_LED = 0x14; 61 | public final static byte PIXY_TYPE_REQUEST_LAMP = 0x16; 62 | public final static byte PIXY_TYPE_REQUEST_FPS = 0x18; 63 | 64 | // Return result values 65 | public final static byte PIXY_RESULT_OK = 0; 66 | public final static byte PIXY_RESULT_ERROR = -1; 67 | public final static byte PIXY_RESULT_BUSY = -2; 68 | public final static byte PIXY_RESULT_CHECKSUM_ERROR = -3; 69 | public final static byte PIXY_RESULT_TIMEOUT = -4; 70 | public final static byte PIXY_RESULT_BUTTON_OVERRIDE = -5; 71 | public final static byte PIXY_RESULT_PROG_CHANGING = -6; 72 | 73 | // RC-servo values 74 | public final static int PIXY_RCS_MIN_POS = 0; 75 | public final static int PIXY_RCS_MAX_POS = 1000; 76 | public final static int PIXY_RCS_CENTER_POS = ((PIXY_RCS_MAX_POS - PIXY_RCS_MIN_POS) / 2); 77 | 78 | public enum LinkType { 79 | SPI, I2C, UART; 80 | } 81 | 82 | private Link link = null; 83 | 84 | protected byte[] buffer = null; 85 | protected int length = 0; 86 | protected int type = 0; 87 | protected byte[] bufferPayload = null; 88 | 89 | protected int frameWidth = -1; 90 | protected int frameHeight = -1; 91 | 92 | protected Version version = null; 93 | 94 | protected Pixy2CCC ccc = null; 95 | protected Pixy2Line line = null; 96 | protected Pixy2Video video = null; 97 | 98 | protected boolean m_cs = false; 99 | 100 | /** 101 | * Constructs Pixy2 object with supplied communication link 102 | * 103 | * @param link {@link Link} to communicate with Pixy2 104 | */ 105 | private Pixy2(Link link) { 106 | this.link = link; 107 | // Initializes send/return buffer and payload buffer 108 | buffer = new byte[PIXY_BUFFERSIZE + PIXY_SEND_HEADER_SIZE]; 109 | bufferPayload = new byte[PIXY_BUFFERSIZE]; 110 | // Initializes tracker objects 111 | this.ccc = new Pixy2CCC(this); 112 | this.line = new Pixy2Line(this); 113 | this.video = new Pixy2Video(this); 114 | } 115 | 116 | /** 117 | * Initializes Pixy2 and waits for startup to complete 118 | * 119 | * @param argument Argument to setup {@link Link} 120 | * 121 | * @return Pixy2 error code 122 | */ 123 | public int init(int argument) { 124 | // Opens link 125 | int ret = link.open(argument); 126 | if (ret >= 0) { 127 | // Tries to connect, times out if unable to communicate after 5 seconds 128 | for (long t = System.currentTimeMillis(); System.currentTimeMillis() - t < 5000;) { 129 | // Gets version and resolution to check if communication is successful and stores for future use 130 | if (getVersion() >= 0) { 131 | getResolution(); 132 | return PIXY_RESULT_OK; 133 | } 134 | try { 135 | TimeUnit.MICROSECONDS.sleep(5000); 136 | } catch (InterruptedException e) { 137 | } 138 | } 139 | return PIXY_RESULT_TIMEOUT; 140 | } 141 | return PIXY_RESULT_ERROR; 142 | } 143 | 144 | /** 145 | * Initializes Pixy2 and waits for startup to complete using default link argument 146 | * value 147 | * 148 | * @return Pixy2 error code 149 | */ 150 | public int init() { 151 | return init(PIXY_DEFAULT_ARGVAL); 152 | } 153 | 154 | /** 155 | * Gets Pixy2 instance with supplied communication link 156 | * 157 | * @param link Communication {@link Link} to Pixy2 158 | * 159 | * @return Pixy2 instance 160 | */ 161 | public static Pixy2 createInstance(Link link) { 162 | return new Pixy2(link); 163 | } 164 | 165 | /** 166 | * Gets Pixy2 instance with supplied communication link type 167 | * 168 | * @param type Communication {@link LinkType} to Pixy2 169 | * 170 | * @return Pixy2 instance 171 | */ 172 | public static Pixy2 createInstance(LinkType type) { 173 | Link link = null; 174 | switch (type) { 175 | case SPI: 176 | link = new SPILink(); 177 | break; 178 | case I2C: 179 | link = new I2CLink(); 180 | break; 181 | case UART: 182 | link = new UARTLink(); 183 | break; 184 | default: 185 | return null; 186 | } 187 | return new Pixy2(link); 188 | } 189 | 190 | /** 191 | * Closes Pixy2 192 | */ 193 | public void close() { 194 | link.close(); 195 | } 196 | 197 | /** 198 | * Get Pixy2 Color Connected Components tracker 199 | * 200 | * @return Pixy2 Color Connected Components tracker 201 | */ 202 | public Pixy2CCC getCCC() { 203 | return this.ccc; 204 | } 205 | 206 | /** 207 | * Get Pixy2 line tracker 208 | * 209 | * @return Pixy2 line tracker 210 | */ 211 | public Pixy2Line getLine() { 212 | return this.line; 213 | } 214 | 215 | /** 216 | * Get Pixy2 video tracker 217 | * 218 | * @return Pixy2 video tracker 219 | */ 220 | public Pixy2Video getVideo() { 221 | return this.video; 222 | } 223 | 224 | public static class Version { 225 | 226 | protected int hardware = 0; 227 | protected int firmwareMajor = 0; 228 | protected int firmwareMinor = 0; 229 | protected int firmwareBuild = 0; 230 | protected char[] firmwareType = new char[10]; 231 | 232 | /** 233 | * Constructs version object with given buffer of version data 234 | * 235 | * @param version Buffer output from Pixy2 containing version data 236 | */ 237 | private Version(byte[] version) { 238 | hardware = ((int) (version[1] & 0xff) << 8) | (int) (version[0] & 0xff); 239 | firmwareMajor = version[2]; 240 | firmwareMinor = version[3]; 241 | firmwareBuild = ((int) (version[5] & 0xff) << 8) | (int) (version[4] & 0xff); 242 | for (int i = 0; i < 10; i++) { 243 | firmwareType[i] = (char) (version[i + 6] & 0xFF); 244 | } 245 | } 246 | 247 | /** 248 | * Prints version data to console 249 | */ 250 | public void print() { 251 | System.out.println(toString()); 252 | } 253 | 254 | /** 255 | * Returns a string of version data 256 | * 257 | * @return String of version data; 258 | */ 259 | public String toString() { 260 | return "hardware ver: 0x" + hardware + " firmware ver: " + firmwareMajor + "." + firmwareMinor + "." 261 | + firmwareBuild + " " + new String(firmwareType); 262 | } 263 | 264 | /** 265 | * Gets Pixy2 Hardware Version 266 | * 267 | * @return Pixy2 Hardware Version 268 | */ 269 | public int getHardware() { 270 | return hardware; 271 | } 272 | 273 | /** 274 | * Gets Pixy2 Firmware Version Major 275 | * 276 | * @return Pixy2 Firmware Version Major 277 | */ 278 | public int getFirmwareMajor() { 279 | return firmwareMajor; 280 | } 281 | 282 | /** 283 | * Gets Pixy2 Firmware Version Minor 284 | * 285 | * @return Pixy2 Firmware Version Minor 286 | */ 287 | public int getFirmwareMinor() { 288 | return firmwareMinor; 289 | } 290 | 291 | /** 292 | * Gets Pixy2 Firmware Version Build 293 | * 294 | * @return Pixy2 Firmware Version Build 295 | */ 296 | public int getFirmwareBuild() { 297 | return firmwareBuild; 298 | } 299 | 300 | /** 301 | * Gets Pixy2 Firmware Type 302 | * 303 | * @return Pixy2 Firmware Type 304 | */ 305 | public char[] getfirmwareType() { 306 | return firmwareType; 307 | } 308 | 309 | /** 310 | * Gets Pixy2 Firmware Type 311 | * 312 | * @return Pixy2 Firmware Type 313 | */ 314 | public String getFirmwareTypeString() { 315 | return new String(firmwareType); 316 | } 317 | } 318 | 319 | /** 320 | * Get width of the pixy's visual frame after initialization 321 | * 322 | * @return Pixy2 Frame Width 323 | */ 324 | public int getFrameWidth() { 325 | return frameWidth; 326 | } 327 | 328 | /** 329 | * Get height of the pixy's visual frame after initialization 330 | * 331 | * @return Pixy2 Frame Height 332 | */ 333 | public int getFrameHeight() { 334 | return frameHeight; 335 | } 336 | 337 | /** 338 | * Looks for Pixy2 communication synchronization bytes to find start of message 339 | * 340 | * @return Pixy2 error code 341 | */ 342 | private byte getSync() { 343 | int i, attempts, cprev, res, start, ret; 344 | byte[] c = new byte[1]; 345 | 346 | // Parse incoming bytes until sync bytes are found 347 | for (i = attempts = cprev = 0; true; i++) { 348 | res = link.receive(c, 1) & 0xff; 349 | if (res >= PIXY_RESULT_OK) { 350 | ret = c[0] & 0xff; 351 | // Since we're using little endian, previous byte is least significant byte 352 | start = cprev; 353 | // Current byte is most significant byte 354 | start |= ret << 8; 355 | cprev = ret; 356 | if (start == PIXY_CHECKSUM_SYNC) { 357 | m_cs = true; 358 | return PIXY_RESULT_OK; 359 | } 360 | if (start == PIXY_NO_CHECKSUM_SYNC) { 361 | m_cs = false; 362 | return PIXY_RESULT_OK; 363 | } 364 | } 365 | // If we've read some bytes and no sync, then wait and try again. 366 | // And do that several more times before we give up. 367 | // Pixy2 guarantees to respond within 100us. 368 | if (i >= 4) { 369 | if (attempts >= 4) 370 | return PIXY_RESULT_ERROR; 371 | try { 372 | TimeUnit.MICROSECONDS.sleep(25); 373 | } catch (InterruptedException e) { 374 | } 375 | attempts++; 376 | i = 0; 377 | } 378 | } 379 | } 380 | 381 | /** 382 | * Gets stored Pixy2 {@link Version} info or retrieves from Pixy2 if not present 383 | * 384 | * @return Pixy2 Version Info 385 | */ 386 | public Version getVersionInfo() { 387 | if (version == null) 388 | getVersion(); 389 | return version; 390 | } 391 | 392 | /** 393 | * Receives packet from Pixy2 and outputs to buffer for further processing 394 | * 395 | * @return Length of bytes received or Pixy2 error code 396 | */ 397 | protected int receivePacket() { 398 | int csSerial, res; 399 | Checksum csCalc = new Checksum(); 400 | 401 | // Waits for sync bytes 402 | res = getSync(); 403 | if (res < 0) 404 | // Sync not found 405 | return res; 406 | if (m_cs) { 407 | // Checksum sync 408 | res = link.receive(buffer, 4); 409 | if (res < 0) 410 | return res; 411 | 412 | type = buffer[0] & 0xff; 413 | length = buffer[1] & 0xff; 414 | 415 | csSerial = ((buffer[3] & 0xff) << 8) | (buffer[2] & 0xff); 416 | 417 | // Receives message from buffer 418 | res = link.receive(buffer, length, csCalc); 419 | 420 | if (res < 0) 421 | return res; 422 | // Checks for accuracy with checksum 423 | if (csSerial != csCalc.getChecksum()) 424 | return PIXY_RESULT_CHECKSUM_ERROR; 425 | } else { 426 | // Non-checksum sync 427 | res = link.receive(buffer, 2); 428 | if (res < 0) 429 | return res; 430 | 431 | type = buffer[0] & 0xff; 432 | length = buffer[1] & 0xff; 433 | 434 | // Receives message from buffer 435 | res = link.receive(buffer, length); 436 | 437 | if (res < 0) 438 | return res; 439 | } 440 | return PIXY_RESULT_OK; 441 | } 442 | 443 | /** 444 | * Sends packet to Pixy2 from buffer 445 | * 446 | * @return Length of bytes sent or Pixy2 error code 447 | */ 448 | protected int sendPacket() { 449 | // Write header info at beginning of buffer 450 | buffer[0] = (byte) (PIXY_NO_CHECKSUM_SYNC & 0xff); 451 | buffer[1] = (byte) ((PIXY_NO_CHECKSUM_SYNC >> 8) & 0xff); 452 | buffer[2] = (byte) type; 453 | buffer[3] = (byte) length; 454 | // Add payload data to buffer 455 | for (int i = 0; i < length; i++) { 456 | buffer[4 + i] = bufferPayload[i]; 457 | } 458 | // Send buffer 459 | return link.send(buffer, (byte) (length + PIXY_SEND_HEADER_SIZE)); 460 | } 461 | 462 | /** 463 | * Sends change program packet to Pixy2 464 | * 465 | * @param prog Program name 466 | * 467 | * @return Pixy2 error code 468 | */ 469 | public byte changeProg(char[] prog) { 470 | int res = 0; 471 | 472 | // Poll for program to change 473 | while (true) { 474 | // Truncates supplied program name, or adds empty characters after to indicate end of string 475 | for (int i = 0; i < PIXY_MAX_PROGNAME; i++) { 476 | if (i < prog.length) 477 | bufferPayload[i] = (byte) prog[i]; 478 | else 479 | bufferPayload[i] = Character.MIN_VALUE; 480 | } 481 | length = PIXY_MAX_PROGNAME; 482 | type = PIXY_TYPE_REQUEST_CHANGE_PROG; 483 | sendPacket(); 484 | if (receivePacket() == 0) { 485 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 486 | | (buffer[0] & 0xff); 487 | if (res > 0) { 488 | getResolution(); // Get resolution for future use 489 | return PIXY_RESULT_OK; // Success 490 | } 491 | } else 492 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 493 | try { 494 | // Timeout to try again 495 | TimeUnit.MICROSECONDS.sleep(1000); 496 | } catch (InterruptedException e) { 497 | } 498 | } 499 | } 500 | 501 | /** 502 | * Gets version info from Pixy2 503 | * 504 | * @return Buffer length or Pixy2 error code 505 | */ 506 | public int getVersion() { 507 | length = 0; 508 | type = PIXY_TYPE_REQUEST_VERSION; 509 | sendPacket(); 510 | if (receivePacket() == 0) { 511 | if (type == PIXY_TYPE_RESPONSE_VERSION) { 512 | version = new Version(buffer); 513 | return length; // Success 514 | } else if (type == PIXY_TYPE_RESPONSE_ERROR) 515 | return PIXY_RESULT_BUSY; 516 | } 517 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 518 | } 519 | 520 | /** 521 | * Gets camera resolution from Pixy2 522 | * 523 | * @return Pixy2 error code 524 | */ 525 | public byte getResolution() { 526 | length = 1; 527 | bufferPayload[0] = 0; // Adds empty byte to payload as placeholder for future queries 528 | type = PIXY_TYPE_REQUEST_RESOLUTION; 529 | sendPacket(); 530 | if (receivePacket() == 0) { 531 | if (type == PIXY_TYPE_RESPONSE_RESOLUTION) { 532 | frameWidth = ((buffer[1] & 0xff) << 8) | (buffer[0] & 0xff); 533 | frameHeight = ((buffer[3] & 0xff) << 8) | (buffer[2] & 0xff); 534 | return PIXY_RESULT_OK; // Success 535 | } else 536 | return PIXY_RESULT_ERROR; 537 | } else 538 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 539 | } 540 | 541 | /** 542 | * Sets Pixy2 camera brightness between 0-255 543 | * 544 | * @param brightness Byte representing camera brightness 545 | * 546 | * @return Pixy2 error code 547 | */ 548 | public byte setCameraBrightness(int brightness) { 549 | int res; 550 | 551 | // Limits brightness between the 0 and 255 552 | brightness = (brightness >= 255 ? 255 : (brightness <= 0 ? 0 : brightness)); 553 | 554 | bufferPayload[0] = (byte) brightness; 555 | length = 1; 556 | type = PIXY_TYPE_REQUEST_BRIGHTNESS; 557 | sendPacket(); 558 | if (receivePacket() == 0 && type == PIXY_TYPE_RESPONSE_RESULT && length == 4) { 559 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 560 | | (buffer[0] & 0xff); 561 | return (byte) res; // Success 562 | } else 563 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 564 | } 565 | 566 | /** 567 | * Sets Pixy2 servo positions between 0-1000 568 | * 569 | * @param pan Pan servo position 570 | * @param tilt Tilt servo position 571 | * 572 | * @return Pixy2 error code 573 | */ 574 | public byte setServos(int pan, int tilt) { 575 | int res; 576 | 577 | // Limits servo values between 0 and 1000 578 | pan = (pan >= PIXY_RCS_MAX_POS ? PIXY_RCS_MAX_POS : (pan <= PIXY_RCS_MIN_POS ? PIXY_RCS_MIN_POS : pan)); 579 | tilt = (tilt >= PIXY_RCS_MAX_POS ? PIXY_RCS_MAX_POS : (tilt <= PIXY_RCS_MIN_POS ? PIXY_RCS_MIN_POS : tilt)); 580 | 581 | bufferPayload[0] = (byte) (pan & 0xff); 582 | bufferPayload[1] = (byte) ((pan >> 8) & 0xff); 583 | bufferPayload[2] = (byte) (tilt & 0xff); 584 | bufferPayload[3] = (byte) ((tilt >> 8) & 0xff); 585 | length = 4; 586 | type = PIXY_TYPE_REQUEST_SERVO; 587 | sendPacket(); 588 | if (receivePacket() == 0 && type == PIXY_TYPE_RESPONSE_RESULT && length == 4) { 589 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 590 | | (buffer[0] & 0xff); 591 | return (byte) res; // Success 592 | } else 593 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 594 | } 595 | 596 | /** 597 | * Sets Pixy2 LED color to specified Color 598 | * 599 | * @param color Color 600 | * 601 | * @return Pixy2 error code 602 | */ 603 | public byte setLED(Color color) { 604 | return setLED(color.getRed(), color.getGreen(), color.getBlue()); 605 | } 606 | 607 | /** 608 | * Sets Pixy2 LED color to specified RGB value 609 | * 610 | * @param rgb RGB value 611 | * 612 | * @return Pixy2 error code 613 | */ 614 | public byte setLED(int rgb) { 615 | return setLED((rgb >> 16) & 0xff, (rgb >> 8) & 0xff, rgb & 0xff); 616 | } 617 | 618 | /** 619 | * Sets Pixy2 LED color to specified RGB values between 0-255 620 | * 621 | * @param r R value 622 | * @param g G value 623 | * @param b B value 624 | * 625 | * @return Pixy2 error code 626 | */ 627 | public byte setLED(int r, int g, int b) { 628 | int res; 629 | 630 | // Limits rgb values between 0 and 255 631 | r = (r >= 255 ? 255 : (r <= 0 ? 0 : r)); 632 | g = (g >= 255 ? 255 : (g <= 0 ? 0 : g)); 633 | b = (b >= 255 ? 255 : (b <= 0 ? 0 : b)); 634 | 635 | bufferPayload[0] = (byte) r; 636 | bufferPayload[1] = (byte) g; 637 | bufferPayload[2] = (byte) b; 638 | length = 3; 639 | type = PIXY_TYPE_REQUEST_LED; 640 | sendPacket(); 641 | if (receivePacket() == 0 && type == PIXY_TYPE_RESPONSE_RESULT && length == 4) { 642 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 643 | | (buffer[0] & 0xff); 644 | return (byte) res; // Success 645 | } else 646 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 647 | } 648 | 649 | /** 650 | * Turns Pixy2 light source on/off 651 | * 652 | * Use 1 to indicate on, 0 to indicate off 653 | * 654 | * @param upper Byte indicating status of white LEDs 655 | * @param lower Byte indicating status of RGB LED 656 | * 657 | * @return Pixy2 error code 658 | */ 659 | public byte setLamp(byte upper, byte lower) { 660 | int res; 661 | 662 | bufferPayload[0] = upper; 663 | bufferPayload[1] = lower; 664 | length = 2; 665 | type = PIXY_TYPE_REQUEST_LAMP; 666 | sendPacket(); 667 | if (receivePacket() == 0 && type == PIXY_TYPE_RESPONSE_RESULT && length == 4) { 668 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 669 | | (buffer[0] & 0xff); 670 | return (byte) res; // Success 671 | } else 672 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 673 | } 674 | 675 | /** 676 | * Gets Pixy2 camera framerate between 2-62fps 677 | * 678 | * @return Framerate or Pixy2 error code 679 | */ 680 | public byte getFPS() { 681 | int res; 682 | 683 | length = 0; // no args 684 | type = PIXY_TYPE_REQUEST_FPS; 685 | sendPacket(); 686 | if (receivePacket() == 0 && type == PIXY_TYPE_RESPONSE_RESULT && length == 4) { 687 | res = ((buffer[3] & 0xff) << 24) | ((buffer[2] & 0xff) << 16) | ((buffer[1] & 0xff) << 8) 688 | | (buffer[0] & 0xff); 689 | return (byte) res; // Success 690 | } else 691 | return PIXY_RESULT_ERROR; // Some kind of bitstream error 692 | } 693 | 694 | // Checksum holder class 695 | public static class Checksum { 696 | 697 | int cs = 0; 698 | 699 | /** 700 | * Adds byte to checksum 701 | * 702 | * @param b Byte to be added 703 | */ 704 | public void updateChecksum(int b) { 705 | cs += b; 706 | } 707 | 708 | /** 709 | * Returns calculated checksum 710 | * 711 | * @return Calculated checksum 712 | */ 713 | public int getChecksum() { 714 | return cs; 715 | } 716 | 717 | /** 718 | * Resets checksum 719 | */ 720 | public void reset() { 721 | cs = 0; 722 | } 723 | 724 | } 725 | 726 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/Pixy2CCC.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api; 2 | 3 | import java.util.ArrayList; 4 | import java.util.Arrays; 5 | import java.util.concurrent.TimeUnit; 6 | 7 | /** 8 | * Java Port of Pixy2 Arduino Library 9 | * 10 | * Defines Color Connected Components tracker for Pixy2 11 | * 12 | * https://github.com/PseudoResonance/Pixy2JavaAPI 13 | * 14 | * @author PseudoResonance (Josh Otake) 15 | * 16 | * ORIGINAL HEADER - 17 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2CCC.h 18 | * ============================================================================================ 19 | * begin license header 20 | * 21 | * This file is part of Pixy CMUcam5 or "Pixy" for short 22 | * 23 | * All Pixy source code is provided under the terms of the GNU General 24 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 25 | * wishing to use Pixy source code, software and/or technologies under 26 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 27 | * Such licensing terms are available for all portions of the Pixy 28 | * codebase presented here. 29 | * 30 | * end license header 31 | * 32 | * This file is for defining the Block struct and the Pixy template 33 | * class version 2. (TPixy2). TPixy takes a communication link as a 34 | * template parameter so that all communication modes (SPI, I2C and 35 | * UART) can share the same code. 36 | */ 37 | 38 | public class Pixy2CCC { 39 | public final static int CCC_MAX_SIGNATURE = 7; 40 | 41 | public final static byte CCC_RESPONSE_BLOCKS = 0x21; 42 | public final static byte CCC_REQUEST_BLOCKS = 0x20; 43 | 44 | // Defines for sigmap: 45 | // You can bitwise "or" these together to make a custom sigmap. 46 | // For example if you're only interested in receiving blocks 47 | // with signatures 1 and 5, you could use a sigmap of 48 | // PIXY_SIG1 | PIXY_SIG5 49 | public final static byte CCC_SIG1 = 0x01; 50 | public final static byte CCC_SIG2 = 0x02; 51 | public final static byte CCC_SIG3 = 0x04; 52 | public final static byte CCC_SIG4 = 0x08; 53 | public final static byte CCC_SIG5 = 0x10; 54 | public final static byte CCC_SIG6 = 0x20; 55 | public final static byte CCC_SIG7 = 0x40; 56 | public final static byte CCC_COLOR_CODES = (byte) 0x80; 57 | 58 | public final static byte CCC_SIG_ALL = (byte) 0xff; // All bits or'ed together 59 | 60 | private final Pixy2 pixy; 61 | 62 | private ArrayList blocks = new ArrayList(); 63 | 64 | /** 65 | * Constructs Pixy2 Color Connected Components tracker 66 | * 67 | * @param pixy Pixy2 instance 68 | */ 69 | protected Pixy2CCC(Pixy2 pixy) { 70 | this.pixy = pixy; 71 | } 72 | 73 | /** 74 | *

Gets signature {@link Block}s from Pixy2

75 | * 76 | *

Defaults to waiting for a response, getting blocks from all signatures and a maximum of all 256 blocks

77 | * 78 | *

Returned data should be retrieved from the cache with {@link #getBlockCache()}

79 | * 80 | * @return Pixy2 error code 81 | */ 82 | public int getBlocks() { 83 | return getBlocks(true, CCC_SIG_ALL, 0xff); 84 | } 85 | 86 | /** 87 | *

Gets signature {@link Block}s from Pixy2

88 | * 89 | *

Defaults to getting blocks from all signatures and a maximum of all 256 blocks

90 | * 91 | *

Returned data should be retrieved from the cache with {@link #getBlockCache()}

92 | * 93 | * @param wait Whether to wait for Pixy2 if data is not available 94 | * 95 | * @return Pixy2 error code 96 | */ 97 | public int getBlocks(boolean wait) { 98 | return getBlocks(wait, CCC_SIG_ALL, 0xff); 99 | } 100 | 101 | /** 102 | *

Gets signature {@link Block}s from Pixy2

103 | * 104 | *

Defaults to getting a maximum of all 256 blocks

105 | * 106 | *

Returned data should be retrieved from the cache with {@link #getBlockCache()}

107 | * 108 | * @param wait Whether to wait for Pixy2 if data is not available 109 | * @param sigmap Sigmap to look for 110 | * 111 | * @return Pixy2 error code 112 | */ 113 | public int getBlocks(boolean wait, int sigmap) { 114 | return getBlocks(wait, sigmap, 0xff); 115 | } 116 | 117 | /** 118 | *

Gets signature {@link Block}s from Pixy2

119 | * 120 | *

Returned data should be retrieved from the cache with {@link #getBlockCache()}

121 | * 122 | * @param wait Whether to wait for Pixy2 if data is not available 123 | * @param sigmap Sigmap to look for 124 | * @param maxBlocks Maximum blocks to look for 125 | * 126 | * @return Pixy2 error code 127 | */ 128 | public int getBlocks(boolean wait, int sigmap, int maxBlocks) { 129 | long start = System.currentTimeMillis(); 130 | 131 | while (true) { 132 | // Fill in request data 133 | pixy.bufferPayload[0] = (byte) sigmap; 134 | pixy.bufferPayload[1] = (byte) maxBlocks; 135 | pixy.length = 2; 136 | pixy.type = CCC_REQUEST_BLOCKS; 137 | 138 | // Send request 139 | pixy.sendPacket(); 140 | if (pixy.receivePacket() == 0) { 141 | if (pixy.type == CCC_RESPONSE_BLOCKS) { 142 | // Clears current cache of blocks 143 | blocks.clear(); 144 | // Iterates through and creates block objects from buffer 145 | for (int i = 0; i + 13 < pixy.length; i += 14) { 146 | Block b = new Block(((pixy.buffer[i + 1] & 0xff) << 8) | (pixy.buffer[i] & 0xff), 147 | ((pixy.buffer[i + 3] & 0xff) << 8) | (pixy.buffer[i + 2] & 0xff), 148 | ((pixy.buffer[i + 5] & 0xff) << 8) | (pixy.buffer[i + 4] & 0xff), 149 | ((pixy.buffer[i + 7] & 0xff) << 8) | (pixy.buffer[i + 6] & 0xff), 150 | ((pixy.buffer[i + 9] & 0xff) << 8) | (pixy.buffer[i + 8] & 0xff), 151 | ((pixy.buffer[i + 11] & 0xff) << 8) | (pixy.buffer[i + 10] & 0xff), 152 | (pixy.buffer[i + 12] & 0xff), (pixy.buffer[i + 13] & 0xff)); 153 | blocks.add(b); 154 | } 155 | return blocks.size(); // Success 156 | } else if (pixy.type == Pixy2.PIXY_TYPE_RESPONSE_ERROR) { 157 | // Deal with busy and program changing states from Pixy2 (we'll wait) 158 | if (pixy.buffer[0] == Pixy2.PIXY_RESULT_BUSY) { 159 | if (!wait) 160 | return Pixy2.PIXY_RESULT_BUSY; // New data not available yet 161 | } else if (pixy.buffer[0] == Pixy2.PIXY_RESULT_PROG_CHANGING) { 162 | return pixy.buffer[0]; 163 | } 164 | 165 | } 166 | } else { 167 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 168 | } 169 | if (System.currentTimeMillis() - start > 500) { 170 | return Pixy2.PIXY_RESULT_ERROR; // Timeout to prevent lockup 171 | } 172 | // If we're waiting for frame data, don't thrash Pixy with requests. 173 | // We can give up half a millisecond of latency (worst case) 174 | try { 175 | TimeUnit.MICROSECONDS.sleep(500); 176 | } catch (InterruptedException e) { 177 | } 178 | } 179 | } 180 | 181 | /** 182 | *

Gets ArrayList of signature blocks from cache

183 | * 184 | *

{@link #getBlocks(boolean, int, int)} must be executed first to get the data actual from Pixy2

185 | * 186 | * @return Pixy2 signature Blocks 187 | */ 188 | public ArrayList getBlockCache() { 189 | return blocks; 190 | } 191 | 192 | public static class Block { 193 | 194 | private int signature, x, y, width, height, angle, index, age; 195 | 196 | /** 197 | * Constructs signature block instance 198 | * 199 | * @param signature Block signature 200 | * @param x X value 201 | * @param y Y value 202 | * @param width Block width 203 | * @param height Block height 204 | * @param angle Angle from camera 205 | * @param index Block index 206 | * @param age Block age 207 | */ 208 | public Block(int signature, int x, int y, int width, int height, int angle, int index, int age) { 209 | this.signature = signature; 210 | this.x = x; 211 | this.y = y; 212 | this.width = width; 213 | this.height = height; 214 | this.angle = angle; 215 | this.index = index; 216 | this.age = age; 217 | } 218 | 219 | /** 220 | * Prints signature block data to console 221 | */ 222 | public void print() { 223 | System.out.println(toString()); 224 | } 225 | 226 | /** 227 | * Returns a string of signature block data 228 | * 229 | * @return String of signature block data 230 | */ 231 | public String toString() { 232 | int i, j; 233 | int[] sig = new int[6]; 234 | int d; 235 | boolean flag; 236 | String out = ""; 237 | if (signature > CCC_MAX_SIGNATURE) { 238 | // Color code! (CC) 239 | // Convert signature number to an octal string 240 | for (i = 12, j = 0, flag = false; i >= 0; i -= 3) { 241 | d = (signature >> i) & 0x07; 242 | if (d > 0 && !flag) 243 | flag = true; 244 | if (flag) 245 | sig[j++] = d + '0'; 246 | } 247 | sig[j] = '\0'; 248 | out = "CC block sig: " + Arrays.toString(sig) + " (" + signature + " decimal) x: " + x + " y: " + y 249 | + " width: " + width + " height: " + height + " angle: " + angle + " index: " + index + " age: " 250 | + age; 251 | 252 | } else // Regular block. Note, angle is always zero, so no need to print 253 | out = "sig: " + signature + " x: " + x + " y: " + y + " width: " + width + " height: " + height 254 | + " index: " + index + " age: " + age; 255 | return out; 256 | } 257 | 258 | /** 259 | * @return Block signature 260 | */ 261 | public int getSignature() { 262 | return signature; 263 | } 264 | 265 | /** 266 | * @return Block X value 267 | */ 268 | public int getX() { 269 | return x; 270 | } 271 | 272 | /** 273 | * @return Block Y value 274 | */ 275 | public int getY() { 276 | return y; 277 | } 278 | 279 | /** 280 | * @return Block width 281 | */ 282 | public int getWidth() { 283 | return width; 284 | } 285 | 286 | /** 287 | * @return Block height 288 | */ 289 | public int getHeight() { 290 | return height; 291 | } 292 | 293 | /** 294 | * @return Angle from camera 295 | */ 296 | public int getAngle() { 297 | return angle; 298 | } 299 | 300 | /** 301 | * @return Block index 302 | */ 303 | public int getIndex() { 304 | return index; 305 | } 306 | 307 | /** 308 | * @return Block age 309 | */ 310 | public int getAge() { 311 | return age; 312 | } 313 | 314 | } 315 | 316 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/Pixy2Line.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api; 2 | 3 | import java.util.Arrays; 4 | import java.util.concurrent.TimeUnit; 5 | 6 | /** 7 | * Java Port of Pixy2 Arduino Library 8 | * 9 | * Defines line tracker for Pixy2 10 | * 11 | * https://github.com/PseudoResonance/Pixy2JavaAPI 12 | * 13 | * @author PseudoResonance (Josh Otake) 14 | * 15 | * ORIGINAL HEADER - 16 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2Line.h 17 | * ============================================================================================= 18 | * begin license header 19 | * 20 | * This file is part of Pixy CMUcam5 or "Pixy" for short 21 | * 22 | * All Pixy source code is provided under the terms of the GNU General 23 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 24 | * wishing to use Pixy source code, software and/or technologies under 25 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 26 | * Such licensing terms are available for all portions of the Pixy 27 | * codebase presented here. 28 | * 29 | * end license header 30 | * 31 | * This file is for defining the Block struct and the Pixy template 32 | * class version 2. (TPixy2). TPixy takes a communication link as a 33 | * template parameter so that all communication modes (SPI, I2C and 34 | * UART) can share the same code. 35 | */ 36 | 37 | public class Pixy2Line { 38 | public final static byte LINE_REQUEST_GET_FEATURES = 0x30; 39 | public final static byte LINE_RESPONSE_GET_FEATURES = 0x31; 40 | public final static byte LINE_REQUEST_SET_MODE = 0x36; 41 | public final static byte LINE_REQUEST_SET_VECTOR = 0x38; 42 | public final static byte LINE_REQUEST_SET_NEXT_TURN_ANGLE = 0x3a; 43 | public final static byte LINE_REQUEST_SET_DEFAULT_TURN_ANGLE = 0x3c; 44 | public final static byte LINE_REQUEST_REVERSE_VECTOR = 0x3e; 45 | 46 | public final static byte LINE_GET_MAIN_FEATURES = 0x00; 47 | public final static byte LINE_GET_ALL_FEATURES = 0x01; 48 | 49 | public final static byte LINE_MODE_TURN_DELAYED = 0x01; 50 | public final static byte LINE_MODE_MANUAL_SELECT_VECTOR = 0x02; 51 | public final static byte LINE_MODE_WHITE_LINE = (byte) 0x80; 52 | 53 | // Features 54 | public final static byte LINE_VECTOR = 0x01; 55 | public final static byte LINE_INTERSECTION = 0x02; 56 | public final static byte LINE_BARCODE = 0x04; 57 | public final static byte LINE_ALL_FEATURES = (LINE_VECTOR | LINE_INTERSECTION | LINE_BARCODE); 58 | 59 | public final static byte LINE_FLAG_INVALID = 0x02; 60 | public final static byte LINE_FLAG_INTERSECTION_PRESENT = 0x04; 61 | 62 | public final static byte LINE_MAX_INTERSECTION_LINES = 6; 63 | 64 | private final Pixy2 pixy; 65 | 66 | private Vector[] vectors = null; 67 | 68 | private Intersection[] intersections = null; 69 | 70 | private Barcode[] barcodes = null; 71 | 72 | /** 73 | * Constructs Pixy2 Line Tracker 74 | * 75 | * @param pixy Pixy2 instance 76 | */ 77 | protected Pixy2Line(Pixy2 pixy) { 78 | this.pixy = pixy; 79 | } 80 | 81 | /** 82 | *

Gets all features from Pixy2

83 | * 84 | *

Defaults to getting all available features and waiting for a response

85 | * 86 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

87 | * 88 | * @return Pixy2 error code 89 | */ 90 | public byte getAllFeatures() { 91 | return getFeatures(LINE_GET_ALL_FEATURES, LINE_ALL_FEATURES, true); 92 | } 93 | 94 | /** 95 | *

Gets the main features from the Pixy2. This is a more constrained line 96 | * tracking algorithm.

97 | * 98 | *

Defaults to getting all available features and waiting for a response

99 | * 100 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

101 | * 102 | * @return Pixy2 error code 103 | */ 104 | public byte getMainFeatures() { 105 | return getFeatures(LINE_GET_MAIN_FEATURES, LINE_ALL_FEATURES, true); 106 | } 107 | 108 | /** 109 | *

Gets all features from Pixy2

110 | * 111 | *

Defaults to waiting for a response

112 | * 113 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

114 | * 115 | * @param features Features to get 116 | * 117 | * @return Pixy2 error code 118 | */ 119 | public byte getAllFeatures(byte features) { 120 | return getFeatures(LINE_GET_ALL_FEATURES, features, true); 121 | } 122 | 123 | /** 124 | *

Gets the main features from the Pixy2. This is a more constrained line 125 | * tracking algorithm.

126 | * 127 | *

Defaults to waiting for a response

128 | * 129 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

130 | * 131 | * @param features Features to get 132 | * 133 | * @return Pixy2 error code 134 | */ 135 | public byte getMainFeatures(byte features) { 136 | return getFeatures(LINE_GET_MAIN_FEATURES, features, true); 137 | } 138 | 139 | /** 140 | *

Gets all features from Pixy2

141 | * 142 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

143 | * 144 | * @param features Features to get 145 | * @param wait Wait for response 146 | * 147 | * @return Pixy2 error code 148 | */ 149 | public byte getAllFeatures(byte features, boolean wait) { 150 | return getFeatures(LINE_GET_ALL_FEATURES, features, wait); 151 | } 152 | 153 | /** 154 | *

Gets the main features from the Pixy2. This is a more constrained line 155 | * tracking algorithm.

156 | * 157 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

158 | * 159 | * @param features Features to get 160 | * @param wait Wait for response 161 | * 162 | * @return Pixy2 error code 163 | */ 164 | public byte getMainFeatures(byte features, boolean wait) { 165 | return getFeatures(LINE_GET_MAIN_FEATURES, features, wait); 166 | } 167 | 168 | /** 169 | *

Gets specified features from Pixy2

170 | * 171 | *

Returned data should be retrieved from the cache with {@link #getVectorCache()}, {@link #getIntersectionCache()} or {@link #getBarcodeCache()}

172 | * 173 | * @param type Type of features to get 174 | * @param features Features to get 175 | * @param wait Wait for response 176 | * 177 | * @return Pixy2 error code 178 | */ 179 | private byte getFeatures(byte type, byte features, boolean wait) { 180 | byte res; 181 | int offset, fsize, ftype; 182 | byte[] fdata; 183 | 184 | vectors = null; 185 | intersections = null; 186 | barcodes = null; 187 | 188 | long start = System.currentTimeMillis(); 189 | 190 | while (true) { 191 | // Fill in request data 192 | pixy.length = 2; 193 | pixy.type = LINE_REQUEST_GET_FEATURES; 194 | pixy.bufferPayload[0] = type; 195 | pixy.bufferPayload[1] = features; 196 | 197 | // Send request 198 | pixy.sendPacket(); 199 | if (pixy.receivePacket() == 0) { 200 | if (pixy.type == LINE_RESPONSE_GET_FEATURES) { 201 | // Parse line response 202 | for (offset = 0, res = 0; pixy.length > offset; offset += fsize + 2) { 203 | ftype = pixy.buffer[offset]; 204 | fsize = pixy.buffer[offset + 1]; 205 | fdata = Arrays.copyOfRange(pixy.buffer, offset + 2, pixy.length); 206 | if (ftype == LINE_VECTOR) { 207 | // Parse line data 208 | vectors = new Vector[(int) Math.floor(fdata.length / 6)]; 209 | for (int i = 0; (i + 1) * 6 <= fdata.length; i++) { 210 | vectors[i] = new Vector(fdata[(6 * i)] & 0xFF, fdata[(6 * i) + 1] & 0xFF, 211 | fdata[(6 * i) + 2] & 0xFF, fdata[(6 * i) + 3] & 0xFF, fdata[(6 * i) + 4] & 0xFF, 212 | fdata[(6 * i) + 5] & 0xFF); 213 | } 214 | res |= LINE_VECTOR; 215 | } else if (ftype == LINE_INTERSECTION) { 216 | // Parse intersection data 217 | int size = 4 + (4 * LINE_MAX_INTERSECTION_LINES); 218 | intersections = new Intersection[(int) Math 219 | .floor(fdata.length / (4 + (4 * LINE_MAX_INTERSECTION_LINES)))]; 220 | for (int i = 0; (i + 1) * size < fdata.length; i++) { 221 | IntersectionLine[] lines = new IntersectionLine[LINE_MAX_INTERSECTION_LINES]; 222 | for (int l = 0; l < LINE_MAX_INTERSECTION_LINES; l++) { 223 | int arr = ((size * i) + 4); 224 | int index = fdata[arr + (4 * l)]; 225 | int reserved = fdata[arr + (4 * l) + 1]; 226 | short angle = (short) (((fdata[arr + (4 * l) + 3] & 0xff) << 8) 227 | | (fdata[arr + (4 * l) + 2] & 0xff)); 228 | IntersectionLine intLine = new IntersectionLine(index, reserved, angle); 229 | lines[l] = intLine; 230 | } 231 | intersections[i] = new Intersection(fdata[size * i] & 0xFF, 232 | fdata[(size * i) + 1] & 0xFF, fdata[(size * i) + 2] & 0xFF, 233 | fdata[(size * i) + 3] & 0xFF, lines); 234 | } 235 | res |= LINE_INTERSECTION; 236 | } else if (ftype == LINE_BARCODE) { 237 | // Parse barcode data 238 | barcodes = new Barcode[(int) Math.floor(fdata.length / 4)]; 239 | for (int i = 0; (i + 1) * 4 <= fdata.length; i++) { 240 | barcodes[i] = new Barcode(fdata[(4 * i)] & 0xFF, fdata[(4 * i) + 1] & 0xFF, 241 | fdata[(4 * i) + 2] & 0xFF, fdata[(4 * i) + 3] & 0xFF); 242 | } 243 | res |= LINE_BARCODE; 244 | } else 245 | break; // Parse error 246 | } 247 | return res; // Success 248 | } else if (pixy.type == Pixy2.PIXY_TYPE_RESPONSE_ERROR) { 249 | // If it's not a busy response, return the error 250 | if (pixy.buffer[0] != Pixy2.PIXY_RESULT_BUSY) 251 | return pixy.buffer[0]; 252 | else if (!wait) // We're busy 253 | return Pixy2.PIXY_RESULT_BUSY; // New data not available yet 254 | } 255 | } else 256 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 257 | 258 | if (System.currentTimeMillis() - start > 500) { 259 | return Pixy2.PIXY_RESULT_ERROR; // Timeout to prevent lockup 260 | } 261 | // If we're waiting for frame data, don't thrash Pixy with requests. 262 | // We can give up half a millisecond of latency (worst case) 263 | try { 264 | TimeUnit.MICROSECONDS.sleep(500); 265 | } catch (InterruptedException e) { 266 | } 267 | } 268 | } 269 | 270 | /** 271 | *

Gets detected lines from cache

272 | * 273 | *

{@link #getFeatures(byte, byte, boolean)} must be executed first to get the data actual from Pixy2

274 | * 275 | * @return Pixy2 Lines 276 | */ 277 | public Vector[] getVectorCache() { 278 | return vectors; 279 | } 280 | 281 | /** 282 | *

Gets detected intersections from cache

283 | * 284 | *

{@link #getFeatures(byte, byte, boolean)} must be executed first to get the data actual from Pixy2

285 | * 286 | * @return Pixy2 Intersectionss 287 | */ 288 | public Intersection[] getIntersectionCache() { 289 | return intersections; 290 | } 291 | 292 | /** 293 | *

Gets detected barcodes from cache

294 | * 295 | *

{@link #getFeatures(byte, byte, boolean)} must be executed first to get the data actual from Pixy2

296 | * 297 | * @return Pixy2 Barcodes 298 | */ 299 | public Barcode[] getBarcodeCache() { 300 | return barcodes; 301 | } 302 | 303 | /** 304 | * Sets Pixy2 line tracking mode 305 | * 306 | * @param mode Pixy2 line tracking mode 307 | * 308 | * @return Pixy2 error code 309 | */ 310 | public byte setMode(int mode) { 311 | int res; 312 | 313 | pixy.bufferPayload[0] = (byte) (mode & 0xff); 314 | pixy.bufferPayload[1] = (byte) ((mode >> 8) & 0xff); 315 | pixy.bufferPayload[2] = (byte) ((mode >> 16) & 0xff); 316 | pixy.bufferPayload[3] = (byte) ((mode >> 24) & 0xff); 317 | pixy.length = 1; 318 | pixy.type = LINE_REQUEST_SET_MODE; 319 | pixy.sendPacket(); 320 | if (pixy.receivePacket() == 0 && pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 321 | res = ((pixy.buffer[3] & 0xff) << 24) | ((pixy.buffer[2] & 0xff) << 16) | ((pixy.buffer[1] & 0xff) << 8) 322 | | (pixy.buffer[0] & 0xff); 323 | return (byte) res; // Success 324 | } else 325 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 326 | } 327 | 328 | /** 329 | * Sets turn angle to use for next intersection 330 | * 331 | * @param angle Turn angle 332 | * 333 | * @return Pixy2 error code 334 | */ 335 | public byte setNextTurn(short angle) { 336 | int res; 337 | 338 | pixy.bufferPayload[0] = (byte) (angle & 0xff); 339 | pixy.bufferPayload[1] = (byte) ((angle >> 8) & 0xff); 340 | pixy.length = 2; 341 | pixy.type = LINE_REQUEST_SET_NEXT_TURN_ANGLE; 342 | pixy.sendPacket(); 343 | if (pixy.receivePacket() == 0 && pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 344 | res = ((pixy.buffer[3] & 0xff) << 24) | ((pixy.buffer[2] & 0xff) << 16) | ((pixy.buffer[1] & 0xff) << 8) 345 | | (pixy.buffer[0] & 0xff); 346 | return (byte) res; // Success 347 | } else 348 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 349 | } 350 | 351 | /** 352 | * Sets default angle to turn to at an intersection 353 | * 354 | * @param angle Turn angle 355 | * 356 | * @return Pixy2 error code 357 | */ 358 | public byte setDefaultTurn(short angle) { 359 | int res; 360 | 361 | pixy.bufferPayload[0] = (byte) (angle & 0xff); 362 | pixy.bufferPayload[1] = (byte) ((angle >> 8) & 0xff); 363 | pixy.length = 2; 364 | pixy.type = LINE_REQUEST_SET_DEFAULT_TURN_ANGLE; 365 | pixy.sendPacket(); 366 | if (pixy.receivePacket() == 0 && pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 367 | res = ((pixy.buffer[3] & 0xff) << 24) | ((pixy.buffer[2] & 0xff) << 16) | ((pixy.buffer[1] & 0xff) << 8) 368 | | (pixy.buffer[0] & 0xff); 369 | return (byte) res; // Success 370 | } else 371 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 372 | } 373 | 374 | /** 375 | * Choose vector to track manually 376 | * 377 | * @param index Index of vector 378 | * 379 | * @return Pixy2 error code 380 | */ 381 | public byte setVector(int index) { 382 | int res; 383 | 384 | pixy.bufferPayload[0] = (byte) index; 385 | pixy.length = 1; 386 | pixy.type = LINE_REQUEST_SET_VECTOR; 387 | pixy.sendPacket(); 388 | if (pixy.receivePacket() == 0 && pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 389 | res = ((pixy.buffer[3] & 0xff) << 24) | ((pixy.buffer[2] & 0xff) << 16) | ((pixy.buffer[1] & 0xff) << 8) 390 | | (pixy.buffer[0] & 0xff); 391 | return (byte) res; // Success 392 | } else 393 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 394 | } 395 | 396 | /** 397 | * Requests to invert vector 398 | * 399 | * @return Pixy2 error code 400 | */ 401 | public byte reverseVector() { 402 | int res; 403 | 404 | pixy.length = 0; 405 | pixy.type = LINE_REQUEST_REVERSE_VECTOR; 406 | pixy.sendPacket(); 407 | if (pixy.receivePacket() == 0 && pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 408 | res = ((pixy.buffer[3] & 0xff) << 24) | ((pixy.buffer[2] & 0xff) << 16) | ((pixy.buffer[1] & 0xff) << 8) 409 | | (pixy.buffer[0] & 0xff); 410 | return (byte) res; // Success 411 | } else 412 | return Pixy2.PIXY_RESULT_ERROR; // Some kind of bitstream error 413 | } 414 | 415 | public static class Vector { 416 | 417 | private int x0, y0, x1, y1, index, flags; 418 | 419 | /** 420 | * Constructs Vector instance 421 | * 422 | * @param x0 X0 value 423 | * @param y0 Y0 value 424 | * @param x1 X1 value 425 | * @param y1 Y1 value 426 | * @param index Vector index 427 | * @param flags Vector flags 428 | */ 429 | public Vector(int x0, int y0, int x1, int y1, int index, int flags) { 430 | this.x0 = x0; 431 | this.y0 = y0; 432 | this.x1 = x1; 433 | this.y1 = y1; 434 | this.index = index; 435 | this.flags = flags; 436 | } 437 | 438 | /** 439 | * Prints vector data to console 440 | */ 441 | public void print() { 442 | System.out.println(toString()); 443 | } 444 | 445 | /** 446 | * Returns a string of vector data 447 | * 448 | * @return String of vector data 449 | */ 450 | public String toString() { 451 | return "vector: (" + x0 + " " + y0 + ") (" + x1 + " " + y1 + ") index: " + index + " flags: " + flags; 452 | } 453 | 454 | /** 455 | * @return X0 value 456 | */ 457 | public int getX0() { 458 | return x0; 459 | } 460 | 461 | /** 462 | * @return Y0 value 463 | */ 464 | public int getY0() { 465 | return y0; 466 | } 467 | 468 | /** 469 | * @return X1 value 470 | */ 471 | public int getX1() { 472 | return x1; 473 | } 474 | 475 | /** 476 | * @return Y1 value 477 | */ 478 | public int getY1() { 479 | return y1; 480 | } 481 | 482 | /** 483 | * @return Vector index 484 | */ 485 | public int getIndex() { 486 | return index; 487 | } 488 | 489 | /** 490 | * @return Vector flags 491 | */ 492 | public int getFlags() { 493 | return flags; 494 | } 495 | 496 | } 497 | 498 | public static class IntersectionLine { 499 | 500 | private int index, reserved; 501 | private short angle; 502 | 503 | /** 504 | * Constructs IntersectionLine object 505 | * 506 | * @param index IntersectionLine index 507 | * @param reserved Reserved 508 | * @param angle Line angle 509 | */ 510 | public IntersectionLine(int index, int reserved, short angle) { 511 | this.index = index; 512 | this.reserved = reserved; 513 | this.angle = angle; 514 | } 515 | 516 | /** 517 | * Prints intersection line data to console 518 | */ 519 | public void print() { 520 | System.out.println(toString()); 521 | } 522 | 523 | /** 524 | * Returns a string of intersection line data 525 | * 526 | * @return String of intersection line data 527 | */ 528 | public String toString() { 529 | return "intersection line: index: " + index + " reserved: " + reserved + " angle: " + angle; 530 | } 531 | 532 | /** 533 | * @return IntersectionLine index 534 | */ 535 | public int getIndex() { 536 | return index; 537 | } 538 | 539 | /** 540 | * @return Reserved 541 | */ 542 | public int getReserved() { 543 | return reserved; 544 | } 545 | 546 | /** 547 | * @return Line angle 548 | */ 549 | public short getAngle() { 550 | return angle; 551 | } 552 | 553 | } 554 | 555 | public static class Intersection { 556 | 557 | private int x, y, number, reserved; 558 | private IntersectionLine[] lines = new IntersectionLine[LINE_MAX_INTERSECTION_LINES]; 559 | 560 | /** 561 | * Constructs Intersection object 562 | * 563 | * @param x X value 564 | * @param y Y value 565 | * @param number Number of lines 566 | * @param reserved Reserved 567 | * @param lines Array of lines 568 | */ 569 | public Intersection(int x, int y, int number, int reserved, IntersectionLine[] lines) { 570 | this.x = x; 571 | this.y = y; 572 | this.number = number; 573 | this.reserved = reserved; 574 | this.lines = lines; 575 | } 576 | 577 | /** 578 | * Prints intersection data to console 579 | */ 580 | public void print() { 581 | System.out.println("intersection: (" + x + " " + y + ")"); 582 | for (int i = 0; i < lines.length; i++) { 583 | IntersectionLine line = lines[i]; 584 | System.out.println(" " + i + " index: " + line.getIndex() + " angle: " + line.getAngle()); 585 | } 586 | } 587 | 588 | /** 589 | * Returns a string of intersection data 590 | * 591 | * @return String of intersection data 592 | */ 593 | public String toString() { 594 | String ret = "intersection: (" + x + " " + y + ")"; 595 | for (int i = 0; i < lines.length; i++) { 596 | IntersectionLine line = lines[i]; 597 | ret += " line: " + i + " index: " + line.getIndex() + " angle: " + line.getAngle(); 598 | } 599 | return ret; 600 | } 601 | 602 | /** 603 | * @return X value 604 | */ 605 | public int getX() { 606 | return x; 607 | } 608 | 609 | /** 610 | * @return Y value 611 | */ 612 | public int getY() { 613 | return y; 614 | } 615 | 616 | /** 617 | * @return Number of lines 618 | */ 619 | public int getNumber() { 620 | return number; 621 | } 622 | 623 | /** 624 | * @return Reserved 625 | */ 626 | public int getReserved() { 627 | return reserved; 628 | } 629 | 630 | /** 631 | * @return Array of lines 632 | */ 633 | public IntersectionLine[] getLines() { 634 | return lines; 635 | } 636 | 637 | } 638 | 639 | public static class Barcode { 640 | 641 | private int x, y, flags, code; 642 | 643 | /** 644 | * Constructs barcode object 645 | * 646 | * @param x X value 647 | * @param y Y value 648 | * @param flags Barcode flags 649 | * @param code Code 650 | */ 651 | public Barcode(int x, int y, int flags, int code) { 652 | this.x = x; 653 | this.y = y; 654 | this.flags = flags; 655 | this.code = code; 656 | } 657 | 658 | /** 659 | * Prints barcode data to console 660 | */ 661 | public void print() { 662 | System.out.println(toString()); 663 | } 664 | 665 | /** 666 | * Returns a string of barcode data 667 | * 668 | * @return String of barcode data 669 | */ 670 | public String toString() { 671 | return "barcode: (" + x + " " + y + ") value: " + code + " flags: " + flags; 672 | } 673 | 674 | /** 675 | * @return X value 676 | */ 677 | public int getX() { 678 | return x; 679 | } 680 | 681 | /** 682 | * @return Y value 683 | */ 684 | public int getY() { 685 | return y; 686 | } 687 | 688 | /** 689 | * @return Barcode flags 690 | */ 691 | public int getFlags() { 692 | return flags; 693 | } 694 | 695 | /** 696 | * @return Code 697 | */ 698 | public int getCode() { 699 | return code; 700 | } 701 | 702 | } 703 | 704 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/Pixy2Video.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api; 2 | 3 | import java.awt.Color; 4 | import java.util.concurrent.TimeUnit; 5 | 6 | /** 7 | * Java Port of Pixy2 Arduino Library 8 | * 9 | * Defines video getter for Pixy2 10 | * 11 | * https://github.com/PseudoResonance/Pixy2JavaAPI 12 | * 13 | * @author PseudoResonance (Josh Otake) 14 | * 15 | * ORIGINAL HEADER - 16 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2Video.h 17 | * ============================================================================================== 18 | * begin license header 19 | * 20 | * This file is part of Pixy CMUcam5 or "Pixy" for short 21 | * 22 | * All Pixy source code is provided under the terms of the GNU General 23 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 24 | * wishing to use Pixy source code, software and/or technologies under 25 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 26 | * Such licensing terms are available for all portions of the Pixy 27 | * codebase presented here. 28 | * 29 | * end license header 30 | * 31 | * This file is for defining the Block struct and the Pixy template 32 | * class version 2. (TPixy2). TPixy takes a communication link as a 33 | * template parameter so that all communication modes (SPI, I2C and 34 | * UART) can share the same code. 35 | */ 36 | 37 | public class Pixy2Video { 38 | public final static byte VIDEO_REQUEST_GET_RGB = 0x70; 39 | 40 | private final Pixy2 pixy; 41 | 42 | /** 43 | * Constructs Pixy2 video getter 44 | * 45 | * @param pixy Pixy2 instance 46 | */ 47 | protected Pixy2Video(Pixy2 pixy) { 48 | this.pixy = pixy; 49 | } 50 | 51 | /** 52 | * Gets average RGB value at 5x5 area around specified coordinates in the image 53 | * 54 | *

Defaults to saturating response color

55 | * 56 | * @param x X value 57 | * @param y Y value 58 | * @param rgb RGB container to return values in 59 | * 60 | * @return Pixy2 error code 61 | */ 62 | public int getRGB(int x, int y, RGB rgb) { 63 | return getRGB(x, y, rgb, true); 64 | } 65 | 66 | /** 67 | * Gets average RGB value at 5x5 area around specified coordinates in the image 68 | * 69 | * @param x X value 70 | * @param y Y value 71 | * @param rgb RGB container to return values in 72 | * @param saturate Whether or not to scale all RGB values to maximize the 73 | * greatest value at 255 74 | * 75 | * @return Pixy2 error code 76 | */ 77 | public int getRGB(int x, int y, RGB rgb, boolean saturate) { 78 | long start = System.currentTimeMillis(); 79 | 80 | while (true) { 81 | pixy.bufferPayload[0] = (byte) (x & 0xff); 82 | pixy.bufferPayload[1] = (byte) ((x >> 8) & 0xff); 83 | pixy.bufferPayload[2] = (byte) (y & 0xff); 84 | pixy.bufferPayload[3] = (byte) ((y >> 8) & 0xff); 85 | pixy.bufferPayload[4] = (byte) (saturate == true ? 1 : 0); 86 | pixy.length = 5; 87 | pixy.type = VIDEO_REQUEST_GET_RGB; 88 | pixy.sendPacket(); 89 | if (pixy.receivePacket() == 0) { 90 | if (pixy.type == Pixy2.PIXY_TYPE_RESPONSE_RESULT && pixy.length == 4) { 91 | rgb.setRGB(pixy.buffer[0], pixy.buffer[1], pixy.buffer[2]); 92 | return 0; // Success 93 | } else if (pixy.type == Pixy2.PIXY_TYPE_RESPONSE_ERROR 94 | && pixy.buffer[0] == Pixy2.PIXY_RESULT_PROG_CHANGING) { 95 | // Deal with program changing by waiting 96 | try { 97 | TimeUnit.MICROSECONDS.sleep(500); 98 | } catch (InterruptedException e) { 99 | } 100 | continue; 101 | } 102 | } 103 | if (System.currentTimeMillis() - start > 500) { 104 | return Pixy2.PIXY_RESULT_ERROR; // Timeout to prevent lockup 105 | } 106 | return Pixy2.PIXY_RESULT_ERROR; 107 | } 108 | } 109 | 110 | public static class RGB { 111 | 112 | int r, g, b; 113 | 114 | /** 115 | * Constructs RGB container 116 | * 117 | * @param r R value 118 | * @param g G value 119 | * @param b B value 120 | */ 121 | public RGB(int r, int g, int b) { 122 | // Limits rgb values between the min and max 123 | this.r = (r >= 255 ? 255 : (r <= 0 ? 0 : r)); 124 | this.g = (g >= 255 ? 255 : (g <= 0 ? 0 : g)); 125 | this.b = (b >= 255 ? 255 : (b <= 0 ? 0 : b)); 126 | } 127 | 128 | /** 129 | * @return Color object containing RGB 130 | */ 131 | public Color getColor() { 132 | return new Color(r, g, b); 133 | } 134 | 135 | /** 136 | * @return R value 137 | */ 138 | public byte getR() { 139 | return (byte) r; 140 | } 141 | 142 | /** 143 | * @return G value 144 | */ 145 | public byte getG() { 146 | return (byte) g; 147 | } 148 | 149 | /** 150 | * @return B value 151 | */ 152 | public byte getB() { 153 | return (byte) b; 154 | } 155 | 156 | /** 157 | * Sets R value between 0-255 158 | * 159 | * @param r R value 160 | */ 161 | public void setR(int r) { 162 | // Limits r value between the min and max 163 | this.r = (r >= 255 ? 255 : (r <= 0 ? 0 : r)); 164 | } 165 | 166 | /** 167 | * Sets G value between 0-255 168 | * 169 | * @param g G value 170 | */ 171 | public void setG(int g) { 172 | // Limits g value between the min and max 173 | this.g = (g >= 255 ? 255 : (g <= 0 ? 0 : g)); 174 | } 175 | 176 | /** 177 | * Sets B value between 0-255 178 | * 179 | * @param b B value 180 | */ 181 | public void setB(int b) { 182 | // Limits b value between the min and max 183 | this.b = (b >= 255 ? 255 : (b <= 0 ? 0 : b)); 184 | } 185 | 186 | /** 187 | * Sets RGB value from Color 188 | * 189 | * @param color Color 190 | */ 191 | public void setRGB(Color color) { 192 | this.r = color.getRed(); 193 | this.g = color.getGreen(); 194 | this.b = color.getBlue(); 195 | } 196 | 197 | /** 198 | * Sets RGB value 199 | * 200 | * @param rgb RGB value 201 | */ 202 | public void setRGB(int rgb) { 203 | this.r = (rgb >> 16) & 0xff; 204 | this.g = (rgb >> 8) & 0xff; 205 | this.b = rgb & 0xff; 206 | } 207 | 208 | /** 209 | * Sets RGB values between 0-255 210 | * 211 | * @param r R value 212 | * @param g G value 213 | * @param b B value 214 | */ 215 | public void setRGB(int r, int g, int b) { 216 | // Limits r value between the min and max 217 | this.r = (r >= 255 ? 255 : (r <= 0 ? 0 : r)); 218 | this.g = (g >= 255 ? 255 : (g <= 0 ? 0 : g)); 219 | this.b = (b >= 255 ? 255 : (b <= 0 ? 0 : b)); 220 | } 221 | 222 | } 223 | 224 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/links/I2CLink.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api.links; 2 | 3 | import java.util.Arrays; 4 | 5 | import edu.wpi.first.wpilibj.I2C; 6 | import io.github.pseudoresonance.pixy2api.Pixy2; 7 | import io.github.pseudoresonance.pixy2api.Pixy2.Checksum; 8 | 9 | /** 10 | * Java Port of Pixy2 Arduino Library 11 | * 12 | * FIRST Robotics WPI API I2C Link to Pixy2 13 | * 14 | * CURRENTLY UNTESTED - Use SPI if possible! 15 | * 16 | * https://github.com/PseudoResonance/Pixy2JavaAPI 17 | * 18 | * @author PseudoResonance (Josh Otake) 19 | * 20 | * ORIGINAL HEADER - 21 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2I2C.h 22 | * ============================================================================================ 23 | * begin license header 24 | * 25 | * This file is part of Pixy CMUcam5 or "Pixy" for short 26 | * 27 | * All Pixy source code is provided under the terms of the GNU General 28 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 29 | * wishing to use Pixy source code, software and/or technologies under 30 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 31 | * Such licensing terms are available for all portions of the Pixy 32 | * codebase presented here. 33 | * 34 | * end license header 35 | * 36 | * Arduino I2C link class 37 | */ 38 | 39 | public class I2CLink implements Link { 40 | private final static int PIXY_I2C_DEFAULT_ADDR = 0x54; 41 | private final static int PIXY_I2C_MAX_SEND = 16; // don't send any more than 16 bytes at a time 42 | 43 | private I2C i2c = null; 44 | 45 | /** 46 | * Opens I2C port 47 | * 48 | * @param arg I2C port 49 | * 50 | * @return Returns 0 51 | */ 52 | public int open(int arg) { 53 | I2C.Port port; 54 | switch (arg) { 55 | case 1: 56 | port = I2C.Port.kMXP; 57 | break; 58 | case Pixy2.PIXY_DEFAULT_ARGVAL: 59 | default: 60 | port = I2C.Port.kOnboard; 61 | } 62 | i2c = new I2C(port, PIXY_I2C_DEFAULT_ADDR); 63 | return 0; 64 | } 65 | 66 | /** 67 | * Closes I2C port 68 | */ 69 | public void close() { 70 | i2c.close(); 71 | } 72 | 73 | /** 74 | * Receives and reads specified length of bytes from I2C 75 | * 76 | * @param buffer Byte buffer to return value 77 | * @param length Length of value to read 78 | * @param cs Checksum 79 | * 80 | * @return Length of value read 81 | */ 82 | public int receive(byte[] buffer, int length, Checksum cs) { 83 | int i, n; 84 | if (cs != null) 85 | cs.reset(); 86 | for (i = 0; i < length; i += n) { 87 | // n is the number read -- it most likely won't be equal to length 88 | n = 0; 89 | byte[] read = new byte[length - i]; 90 | i2c.readOnly(read, (length - i)); 91 | for (int k = 0; k < read.length; k++) { 92 | n++; 93 | byte b = read[k]; 94 | if (cs != null) { 95 | int csb = b & 0xff; 96 | cs.updateChecksum(csb); 97 | } 98 | buffer[k + i] = b; 99 | } 100 | } 101 | return length; 102 | } 103 | 104 | /** 105 | * Receives and reads specified length of bytes from I2C 106 | * 107 | * @param buffer Byte buffer to return value 108 | * @param length Length of value to read 109 | * 110 | * @return Length of value read 111 | */ 112 | public int receive(byte[] buffer, int length) { 113 | return receive(buffer, length, null); 114 | } 115 | 116 | /** 117 | * Writes and sends buffer over I2C 118 | * 119 | * @param buffer Byte buffer to send 120 | * @param length Length of value to send 121 | * 122 | * @return Length of value sent 123 | */ 124 | public int send(byte[] buffer, int length) { 125 | int i, packet; 126 | for (i = 0; i < length; i += PIXY_I2C_MAX_SEND) { 127 | if (length - i < PIXY_I2C_MAX_SEND) 128 | packet = (length - i); 129 | else 130 | packet = PIXY_I2C_MAX_SEND; 131 | byte[] send = Arrays.copyOfRange(buffer, i, packet); 132 | i2c.writeBulk(send, packet); 133 | } 134 | return length; 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/links/Link.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api.links; 2 | 3 | import io.github.pseudoresonance.pixy2api.Pixy2.Checksum; 4 | 5 | /** 6 | * Java Port of Pixy2 Arduino Library 7 | * 8 | * Link interface for connecting to Pixy2 9 | * 10 | * https://github.com/PseudoResonance/Pixy2JavaAPI 11 | * 12 | * @author PseudoResonance (Josh Otake) 13 | */ 14 | 15 | public interface Link { 16 | 17 | /** 18 | * Opens link 19 | * 20 | * @param arg Link argument 21 | * 22 | * @return Returns state 23 | */ 24 | public int open(int arg); 25 | 26 | /** 27 | * Closes link 28 | */ 29 | public void close(); 30 | 31 | /** 32 | * Receives and reads specified length of bytes over link 33 | * 34 | * @param buffer Byte buffer to return value 35 | * @param length Length of value to read 36 | * @param cs Checksum 37 | * 38 | * @return Length of value read 39 | */ 40 | public int receive(byte[] buffer, int length, Checksum cs); 41 | 42 | /** 43 | * Receives and reads specified length of bytes over link 44 | * 45 | * @param buffer Byte buffer to return value 46 | * @param length Length of value to read 47 | * 48 | * @return Length of value read 49 | */ 50 | public int receive(byte[] buffer, int length); 51 | 52 | /** 53 | * Writes and sends buffer over link 54 | * 55 | * @param buffer Byte buffer to send 56 | * @param length Length of value to send 57 | * 58 | * @return Length of value sent 59 | */ 60 | public int send(byte[] buffer, int length); 61 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/links/SPILink.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api.links; 2 | 3 | import edu.wpi.first.wpilibj.SPI; 4 | import io.github.pseudoresonance.pixy2api.Pixy2.Checksum; 5 | 6 | /** 7 | * Java Port of Pixy2 Arduino Library 8 | * 9 | * FIRST Robotics WPI API SPI Link to Pixy2 10 | * 11 | * https://github.com/PseudoResonance/Pixy2JavaAPI 12 | * 13 | * @author PseudoResonance (Josh Otake) 14 | * 15 | * ORIGINAL HEADER - 16 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2.h 17 | * ========================================================================================= 18 | * begin license header 19 | * 20 | * This file is part of Pixy CMUcam5 or "Pixy" for short 21 | * 22 | * All Pixy source code is provided under the terms of the GNU General 23 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 24 | * wishing to use Pixy source code, software and/or technologies under 25 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 26 | * Such licensing terms are available for all portions of the Pixy 27 | * codebase presented here. 28 | * 29 | * end license header 30 | * 31 | * Arduino ICSP SPI link class 32 | */ 33 | 34 | public class SPILink implements Link { 35 | private final static int PIXY_SPI_CLOCKRATE = 2000000; 36 | 37 | private SPI spi = null; 38 | 39 | /** 40 | * Opens SPI port 41 | * 42 | * @param arg SPI port 43 | * 44 | * @return Returns 0 45 | */ 46 | public int open(int arg) { 47 | SPI.Port port; 48 | switch (arg) { 49 | case 1: 50 | port = SPI.Port.kOnboardCS1; 51 | break; 52 | case 2: 53 | port = SPI.Port.kOnboardCS2; 54 | break; 55 | case 3: 56 | port = SPI.Port.kOnboardCS3; 57 | break; 58 | case 4: 59 | port = SPI.Port.kMXP; 60 | break; 61 | case 0: 62 | default: 63 | port = SPI.Port.kOnboardCS0; 64 | } 65 | spi = new SPI(port); 66 | spi.setClockRate(PIXY_SPI_CLOCKRATE); 67 | spi.setMSBFirst(); 68 | spi.setSampleDataOnTrailingEdge(); 69 | spi.setClockActiveLow(); 70 | spi.setChipSelectActiveLow(); 71 | return 0; 72 | } 73 | 74 | /** 75 | * Closes SPI port 76 | */ 77 | public void close() { 78 | spi.close(); 79 | } 80 | 81 | /** 82 | * Receives and reads specified length of bytes from SPI 83 | * 84 | * @param buffer Byte buffer to return value 85 | * @param length Length of value to read 86 | * @param cs Checksum 87 | * 88 | * @return Length of value read 89 | */ 90 | public int receive(byte[] buffer, int length, Checksum cs) { 91 | if (cs != null) 92 | cs.reset(); 93 | spi.read(false, buffer, length); 94 | if (cs != null) 95 | for (int i = 0; i < length; i++) { 96 | int csb = buffer[i] & 0xff; 97 | cs.updateChecksum(csb); 98 | } 99 | return length; 100 | } 101 | 102 | /** 103 | * Receives and reads specified length of bytes from SPI 104 | * 105 | * @param buffer Byte buffer to return value 106 | * @param length Length of value to read 107 | * 108 | * @return Length of value read 109 | */ 110 | public int receive(byte[] buffer, int length) { 111 | return receive(buffer, length, null); 112 | } 113 | 114 | /** 115 | * Writes and sends buffer over SPI 116 | * 117 | * @param buffer Byte buffer to send 118 | * @param length Length of value to send 119 | * 120 | * @return Length of value sent 121 | */ 122 | public int send(byte[] buffer, int length) { 123 | spi.write(buffer, length); 124 | return length; 125 | } 126 | } -------------------------------------------------------------------------------- /src/main/java/io/github/pseudoresonance/pixy2api/links/UARTLink.java: -------------------------------------------------------------------------------- 1 | package io.github.pseudoresonance.pixy2api.links; 2 | 3 | import java.util.concurrent.TimeUnit; 4 | 5 | import edu.wpi.first.wpilibj.SerialPort; 6 | import io.github.pseudoresonance.pixy2api.Pixy2; 7 | import io.github.pseudoresonance.pixy2api.Pixy2.Checksum; 8 | 9 | /** 10 | * Java Port of Pixy2 Arduino Library 11 | * 12 | * FIRST Robotics WPI API UART/Serial Link to Pixy2 13 | * 14 | * CURRENTLY UNTESTED - Use SPI if possible! 15 | * 16 | * https://github.com/PseudoResonance/Pixy2JavaAPI 17 | * 18 | * @author PseudoResonance (Josh Otake) 19 | * 20 | * ORIGINAL HEADER - 21 | * https://github.com/charmedlabs/pixy2/blob/master/src/host/arduino/libraries/Pixy2/Pixy2UART.h 22 | * ============================================================================================= 23 | * begin license header 24 | * 25 | * This file is part of Pixy CMUcam5 or "Pixy" for short 26 | * 27 | * All Pixy source code is provided under the terms of the GNU General 28 | * Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html). Those 29 | * wishing to use Pixy source code, software and/or technologies under 30 | * different licensing terms should contact us at cmucam@cs.cmu.edu. 31 | * Such licensing terms are available for all portions of the Pixy 32 | * codebase presented here. 33 | * 34 | * end license header 35 | * 36 | * Arduino UART link class, intended to be used with an Arduino with 37 | * more than 1 UART, like the Arduino MEGA 2560. 38 | */ 39 | 40 | public class UARTLink implements Link { 41 | private final static int PIXY_UART_BAUDRATE = 19200; 42 | 43 | private SerialPort serial = null; 44 | 45 | /** 46 | * Opens UART/Serial port 47 | * 48 | * @param arg UART/Serial port 49 | * 50 | * @return Returns 0 51 | */ 52 | public int open(int arg) { 53 | SerialPort.Port port; 54 | switch (arg) { 55 | case 1: 56 | port = SerialPort.Port.kUSB; 57 | break; 58 | case 2: 59 | port = SerialPort.Port.kUSB1; 60 | break; 61 | case 3: 62 | port = SerialPort.Port.kUSB2; 63 | break; 64 | case 4: 65 | port = SerialPort.Port.kMXP; 66 | break; 67 | case Pixy2.PIXY_DEFAULT_ARGVAL: 68 | default: 69 | port = SerialPort.Port.kOnboard; 70 | } 71 | serial = new SerialPort(PIXY_UART_BAUDRATE, port); 72 | return 0; 73 | } 74 | 75 | /** 76 | * Closes UART/Serial port 77 | */ 78 | public void close() { 79 | serial.close(); 80 | } 81 | 82 | /** 83 | * Receives and reads specified length of bytes from UART/Serial 84 | * 85 | * @param buffer Byte buffer to return value 86 | * @param length Length of value to read 87 | * @param cs Checksum 88 | * 89 | * @return Length of value read 90 | */ 91 | public int receive(byte[] buffer, int length, Checksum cs) { 92 | int i, j, c; 93 | if (cs != null) 94 | cs.reset(); 95 | for (i = 0; i < length; i++) { 96 | // wait for byte, timeout after 2ms 97 | // note for a baudrate of 19.2K, each byte takes about 500us 98 | for (j = 0; true; j++) { 99 | if (j == 200) 100 | return -1; 101 | c = serial.read(1)[0]; 102 | if (c >= 0) 103 | break; 104 | try { 105 | TimeUnit.MICROSECONDS.sleep(10); 106 | } catch (InterruptedException e) { 107 | } 108 | } 109 | buffer[i] = (byte) c; 110 | if (cs != null) { 111 | byte b = buffer[i]; 112 | int csb = b & 0xff; 113 | cs.updateChecksum(csb); 114 | } 115 | } 116 | return length; 117 | } 118 | 119 | /** 120 | * Receives and reads specified length of bytes from UART/Serial 121 | * 122 | * @param buffer Byte buffer to return value 123 | * @param length Length of value to read 124 | * 125 | * @return Length of value read 126 | */ 127 | public int receive(byte[] buffer, int length) { 128 | return receive(buffer, length, null); 129 | } 130 | 131 | /** 132 | * Writes and sends buffer over UART/Serial 133 | * 134 | * @param buffer Byte buffer to send 135 | * @param length Length of value to send 136 | * 137 | * @return Length of value sent 138 | */ 139 | public int send(byte[] buffer, int length) { 140 | return serial.write(buffer, length); 141 | } 142 | } 143 | --------------------------------------------------------------------------------