diff --git a/build.gradle b/build.gradle index 2deb66d..c0e9e69 100644 --- a/build.gradle +++ b/build.gradle @@ -7,7 +7,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:4.0.1' + classpath 'com.android.tools.build:gradle:7.2.0' // NOTE: Do not place your application dependencies here; they belong diff --git a/doc/HubTest.java b/doc/HubTest.java new file mode 100644 index 0000000..ecd7c5d --- /dev/null +++ b/doc/HubTest.java @@ -0,0 +1,96 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.openftc.i2cdrivers.SRSHub; + +@TeleOp(name = "HubTest") +public class HubTest extends LinearOpMode { + public void runOpMode() throws InterruptedException { + // All ports default to NONE, buses default to empty + SRSHub.Config config = new SRSHub.Config(); + + config.setEncoder( + 1, + SRSHub.Encoder.PWM + ); + + config.setEncoder( + 2, + SRSHub.Encoder.QUADRATURE + ); + + config.addI2CDevice( + 1, + new SRSHub.GoBildaPinpoint( + -50, + -75, + 19.89f, + SRSHub.GoBildaPinpoint.EncoderDirection.FORWARD, + SRSHub.GoBildaPinpoint.EncoderDirection.FORWARD + ) + ); + + RobotLog.clearGlobalWarningMsg(); + + SRSHub hub = hardwareMap.get( + SRSHub.class, + "srshub" + ); + + hub.init(config); + + while (!hub.ready()); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + hub.update(); + + if (hub.disconnected()) { + telemetry.addLine("srshub disconnected"); + } else { + telemetry.addData( + "encoder 1 position", + hub.readEncoder(1).position + ); + + telemetry.addData( + "encoder 2 position", + hub.readEncoder(2).position + ); + + telemetry.addData( + "encoder 2 velocity", + hub.readEncoder(2).velocity + ); + + SRSHub.GoBildaPinpoint pinpoint = hub.getI2CDevice( + 1, + SRSHub.GoBildaPinpoint.class + ); + + if (!pinpoint.disconnected) { + telemetry.addData( + "pose x (mm)", + pinpoint.xPosition + ); + + telemetry.addData( + "pose y (mm)", + pinpoint.yPosition + ); + + telemetry.addData( + "pose heading (rad)", + pinpoint.hOrientation + ); + } + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/doc/images/build-output.png b/doc/images/build-output.png new file mode 100644 index 0000000..f35e8fa Binary files /dev/null and b/doc/images/build-output.png differ diff --git a/doc/images/onbot-external.png b/doc/images/onbot-external.png new file mode 100644 index 0000000..f4bab22 Binary files /dev/null and b/doc/images/onbot-external.png differ diff --git a/doc/images/onbot-upload.png b/doc/images/onbot-upload.png new file mode 100644 index 0000000..f9f1a1b Binary files /dev/null and b/doc/images/onbot-upload.png differ diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 13372ae..7454180 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 9ad7d5f..04d577a 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ -#Fri Sep 18 10:38:36 EDT 2020 +#Sun Sep 14 17:54:27 EDT 2025 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-7.3.3-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-6.1.1-all.zip diff --git a/gradlew b/gradlew index 9d82f78..1b6c787 100755 --- a/gradlew +++ b/gradlew @@ -1,74 +1,129 @@ -#!/usr/bin/env bash +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ############################################################################## -## -## Gradle start up script for UN*X -## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# ############################################################################## -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS="" +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit APP_NAME="Gradle" -APP_BASE_NAME=`basename "$0"` +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' # Use the maximum available, or set MAX_FD != -1 to use that value. -MAX_FD="maximum" +MAX_FD=maximum -warn ( ) { +warn () { echo "$*" -} +} >&2 -die ( ) { +die () { echo echo "$*" echo exit 1 -} +} >&2 # OS specific support (must be 'true' or 'false'). cygwin=false msys=false darwin=false -case "`uname`" in - CYGWIN* ) - cygwin=true - ;; - Darwin* ) - darwin=true - ;; - MINGW* ) - msys=true - ;; +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; esac -# Attempt to set APP_HOME -# Resolve links: $0 may be a link -PRG="$0" -# Need this for relative symlinks. -while [ -h "$PRG" ] ; do - ls=`ls -ld "$PRG"` - link=`expr "$ls" : '.*-> \(.*\)$'` - if expr "$link" : '/.*' > /dev/null; then - PRG="$link" - else - PRG=`dirname "$PRG"`"/$link" - fi -done -SAVED="`pwd`" -cd "`dirname \"$PRG\"`/" >/dev/null -APP_HOME="`pwd -P`" -cd "$SAVED" >/dev/null - CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + # Determine the Java command to use to start the JVM. if [ -n "$JAVA_HOME" ] ; then if [ -x "$JAVA_HOME/jre/sh/java" ] ; then # IBM's JDK on AIX uses strange locations for the executables - JAVACMD="$JAVA_HOME/jre/sh/java" + JAVACMD=$JAVA_HOME/jre/sh/java else - JAVACMD="$JAVA_HOME/bin/java" + JAVACMD=$JAVA_HOME/bin/java fi if [ ! -x "$JAVACMD" ] ; then die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME @@ -77,7 +132,7 @@ Please set the JAVA_HOME variable in your environment to match the location of your Java installation." fi else - JAVACMD="java" + JAVACMD=java which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the @@ -85,76 +140,95 @@ location of your Java installation." fi # Increase the maximum file descriptors if we can. -if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then - MAX_FD_LIMIT=`ulimit -H -n` - if [ $? -eq 0 ] ; then - if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then - MAX_FD="$MAX_FD_LIMIT" - fi - ulimit -n $MAX_FD - if [ $? -ne 0 ] ; then - warn "Could not set maximum file descriptor limit: $MAX_FD" - fi - else - warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" - fi +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac fi -# For Darwin, add options to specify how the application appears in the dock -if $darwin; then - GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" -fi +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) -# For Cygwin, switch paths to Windows format before running java -if $cygwin ; then - APP_HOME=`cygpath --path --mixed "$APP_HOME"` - CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` - JAVACMD=`cygpath --unix "$JAVACMD"` - - # We build the pattern for arguments to be converted via cygpath - ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` - SEP="" - for dir in $ROOTDIRSRAW ; do - ROOTDIRS="$ROOTDIRS$SEP$dir" - SEP="|" - done - OURCYGPATTERN="(^($ROOTDIRS))" - # Add a user-defined pattern to the cygpath arguments - if [ "$GRADLE_CYGPATTERN" != "" ] ; then - OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" - fi # Now convert the arguments - kludge to limit ourselves to /bin/sh - i=0 - for arg in "$@" ; do - CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` - CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option - - if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition - eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` - else - eval `echo args$i`="\"$arg\"" + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) fi - i=$((i+1)) + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg done - case $i in - (0) set -- ;; - (1) set -- "$args0" ;; - (2) set -- "$args0" "$args1" ;; - (3) set -- "$args0" "$args1" "$args2" ;; - (4) set -- "$args0" "$args1" "$args2" "$args3" ;; - (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; - (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; - (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; - (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; - (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; - esac fi -# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules -function splitJvmOpts() { - JVM_OPTS=("$@") -} -eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS -JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME" - -exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@" +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat index 8a0b282..107acd3 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -1,3 +1,19 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + @if "%DEBUG%" == "" @echo off @rem ########################################################################## @rem @@ -8,20 +24,23 @@ @rem Set local scope for the variables with windows NT shell if "%OS%"=="Windows_NT" setlocal -@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -set DEFAULT_JVM_OPTS= - set DIRNAME=%~dp0 if "%DIRNAME%" == "" set DIRNAME=. set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + @rem Find java.exe if defined JAVA_HOME goto findJavaFromJavaHome set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 -if "%ERRORLEVEL%" == "0" goto init +if "%ERRORLEVEL%" == "0" goto execute echo. echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. @@ -35,7 +54,7 @@ goto fail set JAVA_HOME=%JAVA_HOME:"=% set JAVA_EXE=%JAVA_HOME%/bin/java.exe -if exist "%JAVA_EXE%" goto init +if exist "%JAVA_EXE%" goto execute echo. echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% @@ -45,34 +64,14 @@ echo location of your Java installation. goto fail -:init -@rem Get command-line arguments, handling Windowz variants - -if not "%OS%" == "Windows_NT" goto win9xME_args -if "%@eval[2+2]" == "4" goto 4NT_args - -:win9xME_args -@rem Slurp the command line arguments. -set CMD_LINE_ARGS= -set _SKIP=2 - -:win9xME_args_slurp -if "x%~1" == "x" goto execute - -set CMD_LINE_ARGS=%* -goto execute - -:4NT_args -@rem Get arguments from the 4NT Shell from JP Software -set CMD_LINE_ARGS=%$ - :execute @rem Setup the command line set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + @rem Execute Gradle -"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* :end @rem End local scope for the variables with windows NT shell diff --git a/i2cdrivers/build.gradle b/i2cdrivers/build.gradle index 4fdfd60..54d5e08 100644 --- a/i2cdrivers/build.gradle +++ b/i2cdrivers/build.gradle @@ -3,7 +3,7 @@ apply plugin: 'com.android.library' ext { PUBLISH_GROUP_ID = 'org.openftc' PUBLISH_ARTIFACT_ID = 'i2cdrivers' - PUBLISH_VERSION = '1.0.0' + PUBLISH_VERSION = '1.0.1' PUBLISH_DESC = 'OpenFTC I2C Drivers Library' PUBLISH_URL = 'https://github.com/OpenFTC/I2C-Drivers' @@ -12,13 +12,13 @@ ext { } android { - compileSdkVersion 28 + compileSdkVersion 30 defaultConfig { - minSdkVersion 23 + minSdkVersion 24 targetSdkVersion 28 versionCode 1 - versionName "0.1" + versionName "0.2" } buildTypes { @@ -32,14 +32,15 @@ android { abortOnError false } compileOptions { - sourceCompatibility JavaVersion.VERSION_1_7 - targetCompatibility JavaVersion.VERSION_1_7 + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 } } dependencies { //compileOnly fileTree(include: ['*.aar'], dir: '../libs') - compileOnly 'org.firstinspires.ftc:RobotCore:7.0.0' + compileOnly 'org.firstinspires.ftc:RobotCore:11.0.0' + compileOnly 'org.firstinspires.ftc:Hardware:11.0.0' compileOnly 'androidx.appcompat:appcompat:1.2.0' } diff --git a/i2cdrivers/src/main/java/org/openftc/i2cdrivers/QwiicLEDStick.java b/i2cdrivers/src/main/java/org/openftc/i2cdrivers/QwiicLEDStick.java deleted file mode 100644 index a24d10c..0000000 --- a/i2cdrivers/src/main/java/org/openftc/i2cdrivers/QwiicLEDStick.java +++ /dev/null @@ -1,190 +0,0 @@ -package org.openftc.i2cdrivers; - -import android.graphics.Color; -import androidx.annotation.ColorInt; - -import com.qualcomm.robotcore.hardware.I2cAddr; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; -import com.qualcomm.robotcore.hardware.I2cWaitControl; -import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; -import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; - -@I2cDeviceType() -@DeviceProperties(name = "QWIIC LED Stick", description = "Sparkfun QWIIC LED Stick", xmlTag = "QWIIC_LED_STICK") -public class QwiicLEDStick extends I2cDeviceSynchDevice { - - private enum Commands { - CHANGE_LED_LENGTH(0x70), - WRITE_SINGLE_LED_COLOR(0x71), - WRITE_ALL_LED_COLOR(0x72), - WRITE_RED_ARRAY(0x73), - WRITE_GREEN_ARRAY(0x74), - WRITE_BLUE_ARRAY(0x75), - WRITE_SINGLE_LED_BRIGHTNESS(0x76), - WRITE_ALL_LED_BRIGHTNESS(0x77), - WRITE_ALL_LED_OFF(0x78); - int bVal; - - Commands(int bVal) { - this.bVal = bVal; - } - } - - /** - * Change the color of a specific LED - * - * @param position which LED to change (1 - 255) - * @param color what color to set it to - */ - public void setColor(int position, @ColorInt int color) { - byte[] data = new byte[4]; - data[0] = (byte) position; - data[1] = (byte) Color.red(color); - data[2] = (byte) Color.green(color); - data[3] = (byte) Color.blue(color); - writeI2C(Commands.WRITE_SINGLE_LED_COLOR, data); - } - - /** - * Change the color of all LEDs to a single color - * - * @param color what the color should be - */ - public void setColor(@ColorInt int color) { - byte[] data = new byte[3]; - data[0] = (byte) Color.red(color); - data[1] = (byte) Color.green(color); - data[2] = (byte) Color.blue(color); - writeI2C(Commands.WRITE_ALL_LED_COLOR, data); - } - - /** - * Send a segment of the LED array - * - * @param cmd command to send - * @param array the values (limited from 0..255) - * @param offset the starting value (LED only, array starts at 0) - * @param length the length to send - */ - private void sendSegment(Commands cmd, int[] array, int offset, int length) { - byte[] data = new byte[length + 2]; - data[0] = (byte) length; - data[1] = (byte) offset; - - for (int i = 0; i < length; i++) { - data[2 + i] = (byte) array[i]; - } - writeI2C(cmd, data); - } - - /** - * Change the color of an LED color segment - * - * @param colors what the colors should be - * @param offset where in the array to start - * @param length length to send (limited to 12) - */ - private void setLEDColorSegment(@ColorInt int[] colors, int offset, int length) { - int[] redArray = new int[length]; - int[] greenArray = new int[length]; - int[] blueArray = new int[length]; - - for (int i = 0; i < colors.length; i++) { - redArray[i] = Color.red(colors[i + offset]); - greenArray[i] = Color.green(colors[i + offset]); - blueArray[i] = Color.blue(colors[i + offset]); - } - sendSegment(Commands.WRITE_RED_ARRAY, redArray, offset, length); - sendSegment(Commands.WRITE_GREEN_ARRAY, greenArray, offset, length); - sendSegment(Commands.WRITE_BLUE_ARRAY, blueArray, offset, length); - } - - /** - * Change the color of all LEDs using arrays - * - * @param colors array of colors to set lights to - */ - public void setColors(@ColorInt int[] colors) { - int length = colors.length; - - int numInLastSegment = length % 12; - int numSegments = length / 12; - for (int i = 0; i < numSegments; i++) { - setLEDColorSegment(colors, i * 12, 12); - } - setLEDColorSegment(colors, numSegments * 12, numInLastSegment); - } - - /** - * Set the brightness of an individual LED - * - * @param number which LED to change (1-255) - * @param brightness brightness level (0-31) - */ - public void setBrightness(int number, int brightness) { - byte[] data = new byte[2]; - data[0] = (byte) number; - data[1] = (byte) brightness; - writeI2C(Commands.WRITE_SINGLE_LED_BRIGHTNESS, data); - } - - /** - * Set the brightness of all LEDs - * - * @param brightness brightness level (0-31) - */ - public void setBrightness(int brightness) { - byte[] data = new byte[1]; - data[0] = (byte) brightness; - writeI2C(Commands.WRITE_ALL_LED_BRIGHTNESS, data); - } - - /** - * Turn all LEDS off... - */ - public void turnAllOff() { - setColor(0); - } - - /** - * Change the length of the LED strip - * - * @param newLength 1 to 100 (longer than 100 not supported) - */ - public void changeLength(int newLength) { - byte[] data = new byte[1]; - data[0] = (byte) newLength; - writeI2C(Commands.CHANGE_LED_LENGTH, data); - } - - private void writeI2C(Commands cmd, byte[] data) { - deviceClient.write(cmd.bVal, data, I2cWaitControl.WRITTEN); - } - - - @Override - public Manufacturer getManufacturer() { - return Manufacturer.Other; - } - - @Override - protected synchronized boolean doInitialize() { - return true; - } - - @Override - public String getDeviceName() { - return "Qwiic LED Strip"; - } - - private final static I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x23); - - public QwiicLEDStick(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { - super(deviceClient, deviceClientIsOwned); - - this.deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT); - super.registerArmingStateCallback(false); - } - -} diff --git a/i2cdrivers/src/main/java/org/openftc/i2cdrivers/SRSHub.java b/i2cdrivers/src/main/java/org/openftc/i2cdrivers/SRSHub.java new file mode 100644 index 0000000..aaef70f --- /dev/null +++ b/i2cdrivers/src/main/java/org/openftc/i2cdrivers/SRSHub.java @@ -0,0 +1,1410 @@ +package org.openftc.i2cdrivers; + +import static java.lang.Thread.sleep; + +import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch; +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.qualcomm.robotcore.util.RobotLog; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.BitSet; +import java.util.Set; + +@I2cDeviceType +@DeviceProperties(xmlTag = "SRSHub", name = "SRSHub") +public class SRSHub extends I2cDeviceSynchDevice { + private static final int I2C_ADDRESS = 0x57; + + private static final int DEVICE_ID = 0x61; + + private static final int DEVICE_MAJOR_VERSION = 1; + private static final int DEVICE_MINOR_VERSION = 1; + private static final int DEVICE_PATCH_VERSION = 1; + + private static final int BITS_PER_ANALOG_DIGITAL_DEVICE = 2; + private static final int BITS_PER_ENCODER = 2; + private static final int MIN_BITS_PER_I2C_BUS = 16; + private static final int BITS_PER_I2C_DEVICE = 4; + + private static final ByteOrder BYTE_ORDER = ByteOrder.LITTLE_ENDIAN; + + private Config config; + + private int updateLength = 8; + + private boolean ready = false; + private boolean disconnected = false; + + private final double[] analogDigitalValues = + new double[12]; + + private final PosVel[] encoderValues = new PosVel[6]; + + public enum AnalogDigitalDevice { + ANALOG(0), + DIGITAL(1), + NONE(2); + + final int value; + + AnalogDigitalDevice(int value) { + this.value = value; + } + } + + public enum Encoder { + QUADRATURE(0), + PWM(1), + NONE(2); + + final int value; + + Encoder(int value) { + this.value = value; + } + } + + public static class PosVel { + public int position = 0; + public int velocity = 0; + } + + public static abstract class I2CDevice { + protected abstract int getValue(); + + protected abstract int getInitLength(); + + protected abstract int getUpdateLength(); + + protected abstract int getAddress(); + + protected abstract BitSet getConfig(); + + protected abstract void parseUpdate(BitSet data, int start); + } + + public static class APDS9151 extends I2CDevice { + private final BitSet config = new BitSet(0); + + public boolean disconnected = false; + + public short proximity; + public int infrared; + public int red; + public int green; + public int blue; + + protected int getValue() { + return 0; + } + + protected int getInitLength() { + return 0; + } + + protected int getUpdateLength() { + return 92; + } + + protected int getAddress() { + return 0x52; + } + + protected BitSet getConfig() { + return config; + } + + protected void parseUpdate(BitSet data, int start) { + int index = start; + + disconnected = data.get(index++); + + byte[] proximityChunk = data + .get( + index, + index + 11 + ) + .toByteArray(); + + byte[] paddedProximityChunk = new byte[2]; + + System.arraycopy( + proximityChunk, + 0, + paddedProximityChunk, + 0, + proximityChunk.length + ); + + proximity = ByteBuffer + .wrap(paddedProximityChunk) + .order(BYTE_ORDER) + .getShort(); + + index += 11; + + byte[] infraredChunk = data + .get( + index, + index + 20 + ) + .toByteArray(); + + byte[] paddedInfraredChunk = new byte[4]; + + System.arraycopy( + infraredChunk, + 0, + paddedInfraredChunk, + 0, + infraredChunk.length + ); + + infrared = ByteBuffer + .wrap(paddedInfraredChunk) + .order(BYTE_ORDER) + .getInt(); + + index += 20; + + byte[] redChunk = data + .get( + index, + index + 20 + ) + .toByteArray(); + + byte[] paddedRedChunk = new byte[4]; + + System.arraycopy( + redChunk, + 0, + paddedRedChunk, + 0, + redChunk.length + ); + + red = ByteBuffer + .wrap(paddedRedChunk) + .order(BYTE_ORDER) + .getInt(); + + index += 20; + + byte[] greenChunk = data + .get( + index, + index + 20 + ) + .toByteArray(); + + byte[] paddedGreenChunk = new byte[4]; + + System.arraycopy( + greenChunk, + 0, + paddedGreenChunk, + 0, + greenChunk.length + ); + + green = ByteBuffer + .wrap(paddedGreenChunk) + .order(BYTE_ORDER) + .getInt(); + + index += 20; + + byte[] blueChunk = data + .get( + index, + index + 20 + ) + .toByteArray(); + + byte[] paddedBlueChunk = new byte[4]; + + System.arraycopy( + blueChunk, + 0, + paddedBlueChunk, + 0, + blueChunk.length + ); + + blue = ByteBuffer + .wrap(paddedBlueChunk) + .order(BYTE_ORDER) + .getInt(); + } + } + + public static class VL53L5CX extends I2CDevice { + public enum Resolution { + GRID_4x4(0), + GRID_8x8(1); + + final byte value; + + Resolution(int value) { + this.value = (byte) value; + } + } + + private final BitSet config = new BitSet(1); + + public boolean disconnected = false; + + public final short[] distances; + + public VL53L5CX(Resolution resolution) { + config.set( + 0, + resolution.value == 1 + ); + + distances = resolution.value == 0 ? new short[16] : new short[64]; + } + + protected int getValue() { + return 1; + } + + protected int getInitLength() { + return 1; + } + + protected int getUpdateLength() { + return config.get(0) ? 705 : 177; + } + + protected int getAddress() { + return 0x29; + } + + protected BitSet getConfig() { + return config; + } + + protected void parseUpdate(BitSet data, int start) { + int index = start; + + disconnected = data.get(index++); + + for (int k = 0; k < (getUpdateLength() - 1) / 11; k++) { + byte[] chunk = data + .get( + index, + index + 11 + ) + .toByteArray(); + + index += 11; + + byte[] paddedChunk = new byte[2]; + + System.arraycopy( + chunk, + 0, + paddedChunk, + 0, + chunk.length + ); + + distances[k] = ByteBuffer + .wrap(paddedChunk) + .order(BYTE_ORDER) + .getShort(); + } + } + } + + public static class VL53L0X extends I2CDevice { + private final BitSet config = new BitSet(0); + + public boolean disconnected = false; + + public float distance; + + protected int getValue() { + return 2; + } + + protected int getInitLength() { + return 0; + } + + protected int getUpdateLength() { + return 17; + } + + protected int getAddress() { + return 0x29; + } + + protected BitSet getConfig() { + return config; + } + + protected void parseUpdate(BitSet data, int start) { + int index = start; + + disconnected = data.get(index++); + + byte[] chunk = data + .get( + index, + index + 16 + ) + .toByteArray(); + + byte[] paddedChunk = new byte[4]; + + System.arraycopy( + chunk, + 0, + paddedChunk, + 0, + chunk.length + ); + + distance = ByteBuffer + .wrap(paddedChunk) + .order(BYTE_ORDER) + .getShort() & 0xFFFF; + } + } + + public static class GoBildaPinpoint extends I2CDevice { + public enum EncoderDirection { + FORWARD, + REVERSED + } + + private final BitSet config; + + public boolean disconnected = false; + + public short deviceStatus; + + public float xPosition; + public float yPosition; + public float hOrientation; + + public float xVelocity; + public float yVelocity; + public float hVelocity; + + private void packConfigFloat(int start, float data) { + int bits = Float.floatToIntBits(data); + + for (int i = 0; i < 32; i++) { + config.set(start + i, ((bits >> (31 - i)) & 1) == 1); + } + } + + /** + * @param xPodOffset the offset of your forward tracking pod from the tracking center in millimeters + * @param yPodOffset the offset of your strafe tracking pod from the tracking center in millimeters + * @param encoderResolution the millimeters traveled per encoder tick + * @param xEncoderDirection the direction of the forward encoder + * @param yEncoderDirection the direction of the strafe encoder + */ + public GoBildaPinpoint( + float xPodOffset, + float yPodOffset, + float encoderResolution, + EncoderDirection xEncoderDirection, + EncoderDirection yEncoderDirection + ) { + ByteBuffer buffer = ByteBuffer.allocate(13); + + buffer.putFloat(xPodOffset); + buffer.putFloat(yPodOffset); + buffer.putFloat(encoderResolution); + + int directionBits = 0; + + if (xEncoderDirection == EncoderDirection.REVERSED) { + directionBits |= 1; + } + + if (yEncoderDirection == EncoderDirection.REVERSED) { + directionBits |= 2; + } + + buffer.put((byte) directionBits); + + config = BitSet.valueOf(buffer.array()); + } + + protected int getValue() { + return 3; + } + + protected int getInitLength() { + return 98; + } + + protected int getUpdateLength() { + return 201; + } + + protected int getAddress() { + return 0x31; + } + + protected BitSet getConfig() { + return config; + } + + protected void parseUpdate(BitSet data, int start) { + int index = start; + + disconnected = data.get(index++); + + byte[] deviceStatusChunk = data + .get( + index, + index + 8 + ) + .toByteArray(); + + byte[] paddedDeviceStatusChunk = new byte[2]; + + System.arraycopy( + deviceStatusChunk, + 0, + paddedDeviceStatusChunk, + 0, + deviceStatusChunk.length + ); + + deviceStatus = ByteBuffer + .wrap(paddedDeviceStatusChunk) + .order(BYTE_ORDER) + .getShort(); + + index += 8; + + byte[] xPositionChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedXPositionChunk = new byte[4]; + + System.arraycopy( + xPositionChunk, + 0, + paddedXPositionChunk, + 0, + xPositionChunk.length + ); + + xPosition = ByteBuffer + .wrap(paddedXPositionChunk) + .order(BYTE_ORDER) + .getFloat(); + + index += 32; + + byte[] yPositionChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedYPositionChunk = new byte[4]; + + System.arraycopy( + yPositionChunk, + 0, + paddedYPositionChunk, + 0, + yPositionChunk.length + ); + + yPosition = ByteBuffer + .wrap(paddedYPositionChunk) + .order(BYTE_ORDER) + .getFloat(); + + index += 32; + + byte[] hOrientationChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedHOrientationChunk = new byte[4]; + + System.arraycopy( + hOrientationChunk, + 0, + paddedHOrientationChunk, + 0, + hOrientationChunk.length + ); + + hOrientation = ByteBuffer + .wrap(paddedHOrientationChunk) + .order(BYTE_ORDER) + .getFloat(); + + index += 32; + + byte[] xVelocityChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedXVelocityChunk = new byte[4]; + + System.arraycopy( + xVelocityChunk, + 0, + paddedXVelocityChunk, + 0, + xVelocityChunk.length + ); + + xVelocity = ByteBuffer + .wrap(paddedXVelocityChunk) + .order(BYTE_ORDER) + .getFloat(); + + index += 32; + + byte[] yVelocityChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedYVelocityChunk = new byte[4]; + + System.arraycopy( + yVelocityChunk, + 0, + paddedYVelocityChunk, + 0, + yVelocityChunk.length + ); + + yVelocity = ByteBuffer + .wrap(paddedYVelocityChunk) + .order(BYTE_ORDER) + .getFloat(); + + index += 32; + + byte[] hVelocityChunk = data + .get( + index, + index + 32 + ) + .toByteArray(); + + byte[] paddedHVelocityChunk = new byte[4]; + + System.arraycopy( + hVelocityChunk, + 0, + paddedHVelocityChunk, + 0, + hVelocityChunk.length + ); + + hVelocity = ByteBuffer + .wrap(paddedHVelocityChunk) + .order(BYTE_ORDER) + .getFloat(); + } + } + + public static class Config { + private boolean locked = false; + + protected final AnalogDigitalDevice[] analogDigitalDevices = + new AnalogDigitalDevice[12]; + + protected final Encoder[] encoders = new Encoder[6]; + + protected final ArrayList[] i2cBuses = new ArrayList[]{ + new ArrayList(), + new ArrayList(), + new ArrayList() + }; + + public Config() { + Arrays.fill( + analogDigitalDevices, + AnalogDigitalDevice.NONE + ); + + Arrays.fill( + encoders, + Encoder.NONE + ); + } + + /** + * configures an analog-digital pin to be analog, digital, or none + * + * @param pin the pin being configured, from 1 to 12 + * @param device the type of device on the pin + * + * @throws IndexOutOfBoundsException if the pin is not between 1 and 12, inclusive + * @throws IllegalStateException if init has already been called + */ + public void setAnalogDigitalDevice( + int pin, + AnalogDigitalDevice device + ) { + if (pin < 1 || pin > 12) { + throwException( + IndexOutOfBoundsException.class, + "AnalogDigitalDevice pin " + + "must be from 1 to 12" + ); + } + + if (locked) { + throwException( + IllegalStateException.class, + "Config has already been " + + "passed to the SRSHub; changes cannot be made" + ); + } + + analogDigitalDevices[pin - 1] = device; + } + + /** + * configures an encoder port to be quadrature, pwm, or none + * + * @param port the port being configured, from 1 to 6 + * @param device the type of device on the port + * + * @throws IndexOutOfBoundsException if the port is not between 1 and 6, inclusive + * @throws IllegalStateException if init has already been called + */ + public void setEncoder(int port, Encoder device) { + if (port < 1 || port > 6) { + throwException( + IndexOutOfBoundsException.class, + "Encoder port must " + + "be from 1 to 6" + ); + } + + if (locked) { + throwException( + IllegalStateException.class, + "Config has already been " + + "passed to the SRSHub; changes cannot be made" + ); + } + + encoders[port - 1] = device; + } + + /** + * adds a device to an I2C bus + * + * @param bus the bus to which the device is being added, from 1 to 3 + * @param device the (unique) type of the device on the bus + * + * @throws IndexOutOfBoundsException if the bus is not between 1 and 3, inclusive + * @throws IllegalStateException if init has already been called or if a device of the same I2C address has been configured on the bus + */ + public void addI2CDevice(int bus, I2CDevice device) { + if (bus < 1 || bus > 3) { + throwException( + IndexOutOfBoundsException.class, + "I2C bus must be from 1 to" + + " 3" + ); + } + + if (locked) { + throwException( + IllegalStateException.class, + "Config has already been " + + "passed to the SRSHub; changes cannot be made" + ); + } + + for (I2CDevice i2cDevice : i2cBuses[bus - 1]) { + if (i2cDevice.getClass() == device.getClass()) { + throwException( + IllegalStateException.class, + "I2C Bus #" + bus + " " + + "already has a device of type " + device + .getClass() + .getName() + ); + } + + if (i2cDevice.getAddress() == device.getAddress()) { + throwException( + IllegalStateException.class, + "I2C Bus #" + bus + " " + + "already has a bus of type " + i2cDevice + .getClass() + .getName() + " which has an I2C address conflicting " + + "with the " + device + .getClass() + .getName() + ); + } + } + + i2cBuses[bus - 1].add(device); + } + + protected void lock() { + locked = true; + } + } + + public SRSHub( + I2cDeviceSynchSimple deviceClient, + boolean deviceClientIsOwned + ) { + super( + deviceClient, + deviceClientIsOwned + ); + + this.deviceClient.setI2cAddress(I2cAddr.create7bit(I2C_ADDRESS)); + super.registerArmingStateCallback(false); + } + + protected boolean doInitialize() { + ((LynxI2cDeviceSynch) this.deviceClient).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K); + + isInitialized = false; + + verifyInitialization(); + + return true; + } + + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + public String getDeviceName() { + return "SRSHub"; + } + + enum Register { + DEVICE_INFO( + 0x00, + 4 + ), + + RESTART( + 0x01, + 1 + ), + + INIT( + 0x02, + -1 + ), + + READ( + 0x03, + -1 + ); + + public final byte address; + public final int length; + + Register(int address, int length) { + this.address = (byte) address; + this.length = length; + } + } + + private void verifyInitialization() { + if (!isInitialized) { + byte[] deviceInfo = ByteBuffer.wrap(deviceClient.read( + Register.DEVICE_INFO.address, + Register.DEVICE_INFO.length + )).order(BYTE_ORDER).array(); + + int deviceId = deviceInfo[0]; + + if (deviceId != DEVICE_ID) { + RobotLog.addGlobalWarningMessage( + "SRSHub initialization failed" + ); + + disconnected = true; + + return; + } + + int deviceMajorVersion = deviceInfo[1]; + int deviceMinorVersion = deviceInfo[2]; + int devicePatchVersion = deviceInfo[3]; + + if (deviceMajorVersion != DEVICE_MAJOR_VERSION || + deviceMinorVersion != DEVICE_MINOR_VERSION || + devicePatchVersion != DEVICE_PATCH_VERSION) { + throwException( + RuntimeException.class, + "SRSHub does not report correct firmware version; " + + "received v" + deviceMajorVersion + "." + + deviceMinorVersion + "." + + devicePatchVersion + ", expected v" + + DEVICE_MAJOR_VERSION + "." + + DEVICE_MINOR_VERSION + "." + + DEVICE_PATCH_VERSION + ); + } + + isInitialized = true; + } + } + + private static void throwException(Class exception, String message) { + RobotLog.setGlobalErrorMsg(message); + + try { + throw (exception.getConstructor(String.class).newInstance(message)); + } + catch (Exception e) { + throw new RuntimeException(e); + } + } + + private int computeCRC16(byte[] data) { + int crc = 0x0000; + + for (int i = 0; i < data.length - 2; i++) { + crc ^= (data[i] & 0xFF) << 8; + + for (int j = 0; j < 8; j++) { + if ((crc & 0x8000) != 0) { + crc = (crc << 1) ^ 0x1021; + } + else { + crc <<= 1; + } + + crc &= 0xFFFF; + } + } + + return crc & 0xFFFF; + } + + /** + * passes the configuration to the SRSHub + * + * @param config the configuration details that will be passed to the SRSHub + */ + public void init( + Config config + ) { + config.lock(); + this.config = config; + + ready = false; + disconnected = false; + + updateLength = 8; + + deviceClient.write( + Register.RESTART.address, + new byte[Register.RESTART.length] + ); + + isInitialized = false; + + Arrays.fill( + analogDigitalValues, + 0 + ); + + for (int i = 0; i < encoderValues.length; i++) { + encoderValues[i] = new PosVel(); + } + + int initLength = + config.analogDigitalDevices.length * BITS_PER_ANALOG_DIGITAL_DEVICE + config.encoders.length * BITS_PER_ENCODER + config.i2cBuses.length * MIN_BITS_PER_I2C_BUS; + + int[] busLengths = new int[config.i2cBuses.length]; + + for (int i = 0; i < config.i2cBuses.length; i++) { + for (int j = 0; j < config.i2cBuses[i].size(); j++) { + busLengths[i] += BITS_PER_I2C_DEVICE + config.i2cBuses[i] + .get(j) + .getInitLength(); + } + + initLength += busLengths[i]; + } + + BitSet init = new BitSet(initLength); + + int index = 0; + + for (int i = 0; i < config.analogDigitalDevices.length; i++) { + switch (config.analogDigitalDevices[i]) { + case ANALOG: + updateLength += 12; + + break; + case DIGITAL: + updateLength += 1; + + break; + case NONE: + break; + } + + for (int j = 0; j < BITS_PER_ANALOG_DIGITAL_DEVICE; j++) { + init.set( + index++, + (config.analogDigitalDevices[i].value >> j & 1) == 1 + ); + } + } + + for (int i = 0; i < config.encoders.length; i++) { + if (config.encoders[i] != Encoder.NONE) { + updateLength += 48; + } + + for (int j = 0; j < BITS_PER_ENCODER; j++) { + init.set( + index++, + (config.encoders[i].value >> j & 1) == 1 + ); + } + } + + for (int i = 0; i < config.i2cBuses.length; i++) { + for (int j = 0; j < MIN_BITS_PER_I2C_BUS; j++) { + init.set( + index++, + (busLengths[i] >> j & 1) == 1 + ); + } + + for (int j = 0; j < config.i2cBuses[i].size(); j++) { + I2CDevice device = config.i2cBuses[i].get(j); + + updateLength += device.getUpdateLength(); + + for (int k = 0; k < BITS_PER_I2C_DEVICE; k++) { + init.set( + index++, + (device.getValue() >> k & 1) == 1 + ); + } + + for (int k = 0; k < device.getInitLength(); k++) { + init.set( + index++, + device.getConfig().get(k) + ); + } + } + } + + updateLength = 2 + (updateLength + 7) / 8; + + if (updateLength > 100) { + throwException( + IllegalStateException.class, + "Maximum bulk-read length of 100 bytes exceeded" + ); + } + + byte[] data = new byte[(initLength + 7) / 8]; + + System.arraycopy( + ByteBuffer + .wrap(init.toByteArray()) + .array(), + 0, + data, + 0, + init.toByteArray().length + ); + + try { + sleep(2500); + } + catch (InterruptedException e) { + throw new RuntimeException(e); + } + + verifyInitialization(); + + deviceClient.write( + Register.INIT.address, + data + ); + + update(); + + ready = true; + } + + /** + * bulk-reads data from the SRSHub as specified in the configuration + * + * @throws IllegalStateException if the SRSHub has not yet been initialized + * @throws RuntimeException if the SRSHub is unable to update according to the provided configuration + */ + public void update() { + if (config == null) { + throwException( + IllegalStateException.class, + "The SRSHub must be initialized " + + "before updating" + ); + } + + byte[] rawData = deviceClient.read( + Register.READ.address, + updateLength + ); + + if (rawData == null || rawData.length == 0 || rawData[0] != DEVICE_ID) { + disconnected = true; + + return; + } + + int receivedCRC = ((rawData[rawData.length - 2] & 0xFF) << 8) | (rawData[rawData.length - 1] & 0xFF); + + int computedCRC = computeCRC16( + rawData + ); + + if (receivedCRC != computedCRC) { + return; + } + + disconnected = false; + + BitSet data = BitSet.valueOf(rawData); + + int index = 8; + + for (int i = 0; i < config.analogDigitalDevices.length; i++) { + switch (config.analogDigitalDevices[i]) { + case ANALOG: + byte[] chunk = data + .get( + index, + index + 12 + ) + .toByteArray(); + + byte[] paddedChunk = new byte[2]; + + System.arraycopy( + chunk, + 0, + paddedChunk, + 0, + chunk.length + ); + + analogDigitalValues[i] = ByteBuffer + .wrap(paddedChunk) + .order(BYTE_ORDER) + .getShort() / (double) 4095; + + index += 12; + + break; + case DIGITAL: + analogDigitalValues[i] = data.get(index++) ? 1 : 0; + + break; + case NONE: + break; + } + } + + for (int i = 0; i < config.encoders.length; i++) { + switch (config.encoders[i]) { + case QUADRATURE: + int lastPosition = encoderValues[i].position; + + encoderValues[i] = new PosVel(); + + byte[] quadratureChunk = data + .get( + index, + index + 16 + ).toByteArray(); + + byte[] paddedQuadratureChunk = new byte[2]; + + System.arraycopy( + quadratureChunk, + 0, + paddedQuadratureChunk, + 0, + quadratureChunk.length + ); + + encoderValues[i].position = lastPosition + ByteBuffer + .wrap(paddedQuadratureChunk) + .order(BYTE_ORDER) + .getShort(); + + index += 16; + + byte[] quadratureVelocityChunk = data + .get( + index, + index + 32 + ).toByteArray(); + + byte[] paddedQuadratureVelocityChunk = new byte[4]; + + System.arraycopy( + quadratureVelocityChunk, + 0, + paddedQuadratureVelocityChunk, + 0, + quadratureVelocityChunk.length + ); + + encoderValues[i].velocity = ByteBuffer + .wrap(paddedQuadratureVelocityChunk) + .order(BYTE_ORDER) + .getInt(); + + index += 32; + + break; + case PWM: + encoderValues[i] = new PosVel(); + + byte[] pwmChunk = data + .get( + index, + index + 16 + ).toByteArray(); + + byte[] paddedPWMChunk = new byte[2]; + + System.arraycopy( + pwmChunk, + 0, + paddedPWMChunk, + 0, + pwmChunk.length + ); + + encoderValues[i].position = ByteBuffer + .wrap(paddedPWMChunk) + .order(BYTE_ORDER) + .getShort(); + + index += 16; + + byte[] pwmVelocityChunk = data + .get( + index, + index + 32 + ).toByteArray(); + + byte[] paddedPWMVelocityChunk = new byte[4]; + + System.arraycopy( + pwmVelocityChunk, + 0, + paddedPWMVelocityChunk, + 0, + pwmVelocityChunk.length + ); + + encoderValues[i].velocity = ByteBuffer + .wrap(paddedPWMVelocityChunk) + .order(BYTE_ORDER) + .getInt(); + + index += 32; + + break; + case NONE: + break; + } + } + + for (int i = 0; i < config.i2cBuses.length; i++) { + for (int j = 0; j < config.i2cBuses[i].size(); j++) { + I2CDevice device = config.i2cBuses[i] + .get(j); + + device.parseUpdate(data, index); + index += device.getUpdateLength(); + } + } + } + + /** + * @return whether the SRSHub is done initializing + */ + public boolean ready() { + return ready; + } + + /** + * @return whether the most recent update failed + */ + public boolean disconnected() { + return disconnected; + } + + /** + * gets the current value of the AnalogDigitalDevice at the specified pin + * + * @param pin the pin being read, from 1 to 12 + * + * @return the current value read from the AnalogDigitalDevice; from 0 to 1 for analog devices and 0 or 1 for digital devices + * + * @throws IndexOutOfBoundsException if the pin is not between 1 and 12, inclusive + * @throws IllegalStateException if the SRSHub has not yet been initialized + * @throws IllegalStateException if the pin was not configured + */ + public double readAnalogDigitalDevice(int pin) { + if (pin < 1 || pin > 12) { + throwException( + IndexOutOfBoundsException.class, + "AnalogDigitalDevice pin " + + "must be from 1 to 12" + ); + } + + if (config == null) { + throwException( + IllegalStateException.class, + "The SRSHub must be initialized " + + "before reading" + ); + } + + if (config.analogDigitalDevices[pin - 1] == AnalogDigitalDevice.NONE) { + throwException( + IllegalStateException.class, + "AnalogDigitalDevice pin #" + pin + + " was not configured" + ); + } + + return analogDigitalValues[pin - 1]; + } + + /** + * gets the current position and velocity of the encoder at the specified port + * + * @param port the port being read, from 1 to 6 + * + * @return the current position and velocity of the encoder; for quadrature encoders this is in ticks/ticks per second; for PWM encoders this is in pulse + * width (microseconds/microseconds per second) + * + * @throws IndexOutOfBoundsException if the port is not between 1 and 6, inclusive + * @throws IllegalStateException if the SRSHub has not yet been initialized + * @throws IllegalStateException if the port was not configured + */ + public PosVel readEncoder(int port) { + if (port < 1 || port > 6) { + throwException( + IndexOutOfBoundsException.class, + "Encoder port " + + "must be from 1 to 6" + ); + } + + if (config == null) { + throwException( + IllegalStateException.class, + "The SRSHub must be initialized " + + "before reading" + ); + } + + if (config.encoders[port - 1] == Encoder.NONE) { + throwException( + IllegalStateException.class, + "Encoder port #" + port + + " was not configured" + ); + } + + return encoderValues[port - 1]; + } + + /** + * gets the current value(s) read from the specified I2C device at the specified bus + * + * @param bus the bus from which the device is being read, from 1 to 3 + * @param deviceClass the type of device being read + * + * @return a wrapper for the current value(s) returned by the I2C device + * + * @throws IndexOutOfBoundsException if the bus is not between 1 and 3, inclusive + * @throws IllegalStateException if the SRSHub has not yet been initialized + * @throws IllegalStateException if the device was not configured on the bus + */ + public T getI2CDevice(int bus, Class deviceClass) { + if (bus < 1 || bus > 3) { + throwException( + IndexOutOfBoundsException.class, + "I2C bus must be from 1 to 3" + ); + } + + if (config == null) { + throwException( + IllegalStateException.class, + "The SRSHub must be initialized before reading" + ); + } + + for (I2CDevice device : config.i2cBuses[bus - 1]) { + if (deviceClass.isInstance(device)) { + return deviceClass.cast(device); + } + } + + throwException( + IllegalStateException.class, + "I2C device " + deviceClass.getName() + + " was not configured on bus #" + bus + ); + + return null; + } +} \ No newline at end of file diff --git a/readme.md b/readme.md index f2f5c88..f00d746 100644 --- a/readme.md +++ b/readme.md @@ -1,29 +1,78 @@ # I2C-Drivers -A collection of I2C drivers for devices not supported out of the box in the FTC SDK +A collection of I2C drivers for devices not supported out of the box in the FTC SDK. -## Installation instructions: +## Installation instructions for Android Studio Teams 1. Open your FTC SDK Android Studio project 2. Open the `build.gradle` file for the TeamCode module: - ![img-here](doc/images/teamcode-gradle.png) + ![img-here](doc/images/teamcode-gradle.png) 3. At the bottom, add this: dependencies { - implementation 'org.openftc:i2cdrivers:1.0.0' + implementation 'org.openftc:i2cdrivers:1.0.1' } 4. Now perform a Gradle Sync: - ![img-here](doc/images/gradle-sync.png) + ![img-here](doc/images/gradle-sync.png) 5. You're ready to go :) +## Installation instructions for OnBot Java Teams + +For those teams using OnBot Java for their season or for prototyping, you may know that when you add I2C drivers to OnBot Java, you then need to reboot the robot every time you change your Java code or it won't find the device. + +We didn't like this and you probably don't, either. The solution is to build this project and upload it as an external libary. + +### Option 1: Download the pre-built release file (easy) + +1. Just download the .aar file from the repository's [releases page](https://github.com/sms-robotics/I2C-Drivers/releases) + +### Option 2: Build this project yourself (harder) + +1. Open this project in Android Studio + +2. Allow it to Gradle sync, etc. + +3. You want to run the `I2C-Drivers [build]` configuration. That will produce files in the `build/outputs/aar/` directory as such: + + ![img-here](doc/images/build-output.png) + +### Remaining Steps + +1. Now go to OnBot Java. + +2. Click the Upload Files button. + + ![img-here](doc/images/onbot-upload.png) + +3. Give it the `i2cdrivers-release.aar` file and wait for it to be ok with what you've done. It will give you a "Close" button. + +4. You will now see the library in the `ExternalLibraries` directory: + + ![img-here](doc/images/onbot-external.png) + +5. Now you can use the [Sample OpMode](doc/HubTest.java) + +Notice that we can now pull from `org.openftc.i2cdrivers.SRSHub`. + +```java +import org.openftc.i2cdrivers.SRSHub; +``` + ## Changelog: ### v1.0.0 - Initial release, with a driver for the Qwiic LED stick donated by FTC16072 - Quantum Quacks + +### v1.0.1 + + - Removal of Qwiic LED stick because it's now officially supported in the SDK. + - Added support for SRS Hub (https://github.com/Simple-Robotics-System) + - Updated instructions to support OnBot Java + - Bump tooling versions to work with Android Studio N