diff --git a/lesson_11/build.gradle b/lesson_11/build.gradle new file mode 100644 index 0000000..2cb961e --- /dev/null +++ b/lesson_11/build.gradle @@ -0,0 +1,233 @@ +/** + * Builds minified merged Python main.py and uploads it to Micro:Bit using 'ufs'. + * All major OSes should be supported. + * + * Tasks: + * clean .. cleans build/ directory + * mini .. minifies all sources into build/mini/ + * buildMini .. builds the merged minified source into build/mini/main.py + * build .. builds the merged (non-minified) source into build/main.py + calls buildMini + * upload .. uploads merged main.py file of choice to Micro:Bit using ufs (see upload options below) + * + * Generic options: + * -Pmain={file.py} .. specifies main.py file (last during merging), optional (autodetected), + * needed just when __main__ block is in multiple source files + * -Pinclude={file1,file2,..} .. adds extra files outside of project dir (all *.py files in project are included automatically) + * -Pexclude={file1,file2,..} .. removes files from the source file list + * + * Upload options: + * -Pmini .. uploads minified version + * -Psudo .. uses 'sudo ufs' instead od 'ufs' for upload (sometimes needed to override port ownership on Linux) + * + * Example build commands: + * + * Basic non-minified build and upload if sudo is not needed, all sources are in local dir + there's just one __main__ block: + * ./gradlew clean build upload + * + * Minified build with sudo upload and added source file from other directory: + * ./gradlew -Pmini -Psudo -Pinclude=../cely_projekt/cely_projekt.py clean build upload + */ +import java.nio.file.Files +import java.util.stream.Collectors +import java.util.stream.Stream + +ext { + useSudo = project.hasProperty('sudo') + useMini = project.hasProperty('mini') + miniDir = "${buildDir}/mini" + includedFiles = propertyCsvToFileList('include') + excludedFiles = propertyCsvToFileList('exclude') + mainFile = getMainFileName(includedFiles, excludedFiles) + // we need all local *.py sources, but we will need to exclude the indicated ones + // we also need to exclude main.py (or whatever was specified as main) and put it at the very end explicitly + sourceFiles = Stream.concat( + Stream.concat( + includedFiles.stream(), + Arrays.stream(projectDir.listFiles()) + .filter { f -> f.name.endsWith('.py') } + ).filter { f -> !f.equals(mainFile) && !excludedFiles.contains(f) }, + Stream.of(mainFile) + ).toList() +} + +task clean { + doLast { + logger.lifecycle("Cleaning ..") + delete "${buildDir}" + } +} + +task minify { + doLast { + mkdir miniDir + sourceFiles.each { file -> + // our main.py is a merged file, we have to avoid name clash + def fileOut = file.name.replaceAll('^main\\.py$', 'main_.py') + exec { + workingDir projectDir + commandLine = [ + 'pyminify', file.getPath(), + '--remove-literal-statements', + '--remove-class-attribute-annotations', '--no-rename-locals', '--no-hoist-literals', + '--no-remove-explicit-return-none', '--no-remove-return-annotations', + '--output', "${miniDir}/${fileOut}" + ] + } + } + } +} + +def mergeSource(BufferedWriter writer, File source, Set imports, Map> importsFrom) { + logger.lifecycle("Merging ${source}") + writer.newLine() + writer.newLine() + try (BufferedReader reader = new BufferedReader(new FileReader(source))) { + boolean header = true; + while (reader.ready()) { + String line = reader.readLine() + if (header) { + if (line.startsWith("class") || line.startsWith("def") || line.startsWith("if")) { + header = false + } else if (line.startsWith("import")) { + line = line.replaceFirst("import\\s+", "") + imports.addAll(Arrays.asList(line.split(",\\s*"))) + } else if (line.startsWith("from")) { + String importSource = line.replaceAll("from (\\S+)\\s?.*\$", "\$1") + String objects = line.replaceFirst("from\\s+(\\S+)\\s+import\\s+", "") + importsFrom.computeIfAbsent(importSource, value -> new LinkedHashSet<>()) + .addAll(Arrays.asList(objects.split(",\\s*"))) + } + } + if (!header) { + writer.write(line) + writer.newLine() + } + } + } +} + +def mergeAll(File target, List sourceFiles) { + def imports = new LinkedHashSet() + def importsFrom = new LinkedHashMap>() + String body + try (StringWriter bodyWriter = new StringWriter()) { + try (BufferedWriter writer = new BufferedWriter((bodyWriter))) { + sourceFiles.each { source -> mergeSource(writer, source, imports, importsFrom) } + } + body = bodyWriter.toString() + } + + List classSources = sourceFiles.stream() + .map { it.name.replaceAll("\\.py", "").replaceAll(".*/", "") } + .collect(Collectors.toList()) + try (BufferedWriter writer = new BufferedWriter((new FileWriter(target, false)))) { + logger.lifecycle("Merging all into ${target}") + String importsCsv = imports.stream() + .filter { !classSources.contains(it) } + .collect(Collectors.joining(", ")) + if (!importsCsv.trim().isBlank()) { + writer.write("import ${importsCsv}") + writer.newLine() + } + importsFrom.entrySet().forEach { entry -> + if (!classSources.contains(entry.getKey())) { + def objects = String.join(", ", entry.getValue()) + writer.write("from ${entry.getKey()} import ${objects}") + writer.newLine() + } + } + writer.write(body) + } +} + +task buildMini { + dependsOn minify + doLast { + List files = sourceFiles.stream() + .map { file -> + // when we were minifying, we had to minify into an alternate name + // to avoid target file name clash, here we need to use it as a new source + new File(miniDir, file.name.replaceAll('^main\\.py$', 'main_.py')) + } + .toList() + mergeAll(new File(miniDir, "main.py"), files) + } +} + +task build { + dependsOn buildMini + doLast { + mergeAll(new File(buildDir, "main.py"), sourceFiles) + } +} + +task upload { + shouldRunAfter build + doLast { + def uploadFile = new File(useMini ? miniDir : buildDir, "main.py") + def cmd = useSudo ? + ['sudo', 'ufs', 'put', "${uploadFile}"] : + ['ufs', 'put', "${uploadFile}"] + logger.lifecycle("Uploading ${useSudo ? "(using sudo) " : ""}merged ${useMini ? "minified " : ""}${uploadFile}") + exec { + workingDir "${projectDir}" + commandLine = cmd + } + } +} + +List propertyCsvToFileList(String property) { + List files = new ArrayList<>() + if (project.hasProperty(property)) { + List patterns = Arrays.asList(project.getProperty(property).split(",")) + files.addAll( + Arrays.stream(projectDir.listFiles()) + .filter { f -> f.name.endsWith('.py') } + .filter { f -> patterns.stream().anyMatch { p -> f.name.matches(p) } } + .toList() + ) + } + return files +} + +/** + * Returns main file name from property (if defined), use main.py (if exists) or scan for content to find __main__. + */ +File getMainFileName(List includedFiles, List excludedFiles) { + if (project.hasProperty('main')) { + def mainFile = new File(projectDir, project.getProperty('main')) + if (!mainFile.exists()) { + throw new Exception("Indicated main file ${mainFile.getPath()} does not exist") + } + // any other main files in the directory are excluded automatically + excludedFiles.addAll( + Arrays. stream(projectDir.listFiles()) + .filter { f -> f.name.endsWith('.py') } + .filter { f -> f.name.startsWith('main_') } + .filter { f -> !f.equals(mainFile) } + .toList() + ) + return mainFile + } else if (new File(projectDir, 'main.py').exists()) { + return new File(projectDir, 'main.py') + } else { + def mainFiles = + Stream.concat( + includedFiles.stream(), + Arrays.stream(projectDir.listFiles()) + ) + .filter { f -> f.name.endsWith('.py') } + .filter { f -> !excludedFiles.contains(f) } + .filter { f -> Files.readAllLines(f.toPath()).contains('if __name__ == "__main__":') } + .toList() + if (mainFiles.isEmpty()) { + throw new Exception("No *.py files found in the project directory looking like a main code block") + } else if (mainFiles.size() > 1) { + excludedFiles.forEach { logger.lifecycle("Excluded: ${it.getAbsolutePath()}") } + logger.lifecycle("Excluded files: ${excludedFiles}") + throw new Exception("Multiple *.py files found in the project directory looking like a main code block: $mainFiles, please specify -Pmain={file}") + } + logger.lifecycle("Detected main code block in file ${mainFiles.getFirst()}") + return mainFiles.getFirst() + } +} diff --git a/lesson_11/build_chaser b/lesson_11/build_chaser new file mode 100755 index 0000000..48d1001 --- /dev/null +++ b/lesson_11/build_chaser @@ -0,0 +1,2 @@ +#!/bin/bash +./gradlew -Pmini clean build upload diff --git a/lesson_11/gradle/wrapper/gradle-wrapper.jar b/lesson_11/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..a4b76b9 Binary files /dev/null and b/lesson_11/gradle/wrapper/gradle-wrapper.jar differ diff --git a/lesson_11/gradle/wrapper/gradle-wrapper.properties b/lesson_11/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..1ed247e --- /dev/null +++ b/lesson_11/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.1-all.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/lesson_11/gradlew b/lesson_11/gradlew new file mode 100755 index 0000000..f5feea6 --- /dev/null +++ b/lesson_11/gradlew @@ -0,0 +1,252 @@ +#!/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. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# 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/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# 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 + +# This is normally unused +# shellcheck disable=SC2034 +APP_BASE_NAME=${0##*/} +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +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 + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + 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 +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +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" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + 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 + # 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 +fi + + +# 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"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# 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/lesson_11/gradlew.bat b/lesson_11/gradlew.bat new file mode 100644 index 0000000..9d21a21 --- /dev/null +++ b/lesson_11/gradlew.bat @@ -0,0 +1,94 @@ +@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 +@rem SPDX-License-Identifier: Apache-2.0 +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +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% equ 0 goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 + +goto fail + +: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 %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/lesson_11/main_chaser.py b/lesson_11/main_chaser.py new file mode 100644 index 0000000..046e69d --- /dev/null +++ b/lesson_11/main_chaser.py @@ -0,0 +1,149 @@ +from microbit import button_a +from utime import ticks_us, ticks_diff + +from sonar import Sonar +from wheel_driver import WheelDriver + +if __name__ == "__main__": + # Tries to maintain the robot around 20 cm in front of an obstacle. + # We use Sonar to measure distance to the obstacle and gradually increase + # the speed of the robot to 50 cm/s. We also support backing off when too close. + # We allow for +/- 3 cm around the target distance to avoid oscillation. + + # The wheels are controlled individually and we are trying to keep the robot + # going straight, so we are aiming at keeping speed in radians constant for both wheels. + DISTANCE_DESIRED = 0.2 + DISTANCE_TOLERANCE = 0.03 + DISTANCE_MAX = 0.6 + # with 30 cm/s the robot was a bit sluggish and was not able to respond well + SPEED_M_MAX = 0.5 + SPEED_M_MIN = 0.25 + SPEED_CHANGE_SLOPE = 1 + + # We use three basic robot modes. + # If the target is lost (distance too high), the robot stops and starts scanning for the target + # in its surrounding area using sonar scanner feature. Once the target is found, the robot + # reorients itself to the right angle and starts following the target again. + ROBOT_MODE_FOLLOW = "follow" + ROBOT_MODE_SCAN = "scan" + ROBOT_MODE_TURN = "turn" + + wheels = WheelDriver(left_pwm_min=60, left_pwm_multiplier=0.08944848, left_pwm_shift=-2.722451, + right_pwm_min=60, right_pwm_multiplier=0.08349663, right_pwm_shift=-2.0864) + sonar = Sonar() + try: + # Calculate the constants for radsec speed dependency on the distance to ensure + # the robot won't go fast when the distance is small. + # When the distance is 50 cm, the speed should be 0.4 m/s. + # When the distance is 20 cm, the speed should be 0.2 m/s. + speed_rad_max = wheels.left.enc.m2rad(SPEED_M_MAX) + speed_rad_min = wheels.left.enc.m2rad(SPEED_M_MIN) + distance_m_to_speed_radsec = (speed_rad_max - speed_rad_min) / (DISTANCE_MAX - DISTANCE_DESIRED) + + info_cycle_length = 1_000_000 + info_cycle_start = ticks_us() + regulation_cycle_length = 50_000 + regulation_cycle_start = ticks_us() + + distance = None + robot_mode = ROBOT_MODE_FOLLOW + print("Switching robot mode to %s" % robot_mode) + while not button_a.is_pressed(): + wheels.update() + sonar.update() + + time_now = ticks_us() + if ticks_diff(time_now, regulation_cycle_start) > regulation_cycle_length: + regulation_cycle_start = time_now + if robot_mode == ROBOT_MODE_TURN: + # We are in the turning mode, we need to wait until the robot reorients itself + # around its center using equal, but opposite left and right wheel speeds (after compensation). + print("Turning, left speed %d, right speed %d" % (wheels.left.speed_pwm, wheels.right.speed_pwm)) + if wheels.left.speed_pwm == 0 and wheels.right.speed_pwm == 0: + robot_mode = ROBOT_MODE_FOLLOW + print("Switching robot mode to %s" % robot_mode) + sonar.set_angle(0) + elif robot_mode == ROBOT_MODE_SCAN: + distance = sonar.get_distance() + if distance < 0: + # the distance is not valid, we might have lost the target, stop the robot + wheels.stop() + if distance < DISTANCE_MAX and sonar.angle != 0: + # the target is found, stop the scanner and reorient the robot + sonar.stop_scan() + print("Found target at distance %.04f at angle %s" % (distance, sonar.angle)) + if sonar.angle == 0: + robot_mode = ROBOT_MODE_FOLLOW + print("Switching robot mode to %s" % robot_mode) + else: + robot_mode = ROBOT_MODE_TURN + print("Switching robot mode to %s" % robot_mode) + # we need to calculate circular distance to travel with each wheel to reorient the robot + # around its center using equal, but opposite left and right wheel speeds (after compensation). + # To do so, we need to know the wheel distance from the center of the robot. + angle_in_rad = sonar.angle * 3.141592653589793 / 180 + rad_to_travel = wheels.left.enc.WHEEL_CENTER_DISTANCE * angle_in_rad + if sonar.angle > 0: + wheels.left.move_radsec_for_distance(speed_rad_min, rad_to_travel) + wheels.right.move_radsec_for_distance(-speed_rad_min, rad_to_travel) + elif sonar.angle < 0: + wheels.left.move_radsec_for_distance(-speed_rad_min, abs(rad_to_travel)) + wheels.right.move_radsec_for_distance(speed_rad_min, abs(rad_to_travel)) + elif robot_mode == ROBOT_MODE_FOLLOW: + distance = sonar.get_distance() + if distance < 0: + # the distance is not valid, we might have lost the target, stop the robot + wheels.stop() + elif distance > DISTANCE_MAX: + # the target is lost, stop the robot, start the scanner + wheels.stop() + if sonar.scan_mode == sonar.SCAN_NONE: + sonar.start_scan() + robot_mode = ROBOT_MODE_SCAN + print("Switching robot mode to %s" % robot_mode) + else: + # the back-off distance is very small, we would normally probably use minimal speed, + # but it might not work due to the wheel calibration, so we still try to use the same error + # correction as for the forward movement. We can use the same mechanism and just indicate + # negative pwm speed to move backwards (thanks to our Wheel implementation). + if (distance > DISTANCE_DESIRED + DISTANCE_TOLERANCE) or \ + (distance < DISTANCE_DESIRED - DISTANCE_TOLERANCE): + distance_error = distance - DISTANCE_DESIRED + desired_speed_radsec = abs(distance_error) * distance_m_to_speed_radsec + speed_rad_min + + current_speed_radsec_left = wheels.left.enc.speed_radsec_avg + speed_error_radsec_left = desired_speed_radsec - current_speed_radsec_left + speed_change_radsec_left = SPEED_CHANGE_SLOPE * speed_error_radsec_left + # print("Current speed %f, Desired speed %f, Speed change left %f" % (current_speed_radsec_left, desired_speed_radsec, speed_change_radsec_left)) + new_speed_radsec_left = min(current_speed_radsec_left + speed_change_radsec_left, + speed_rad_max) + new_speed_pwm_left = wheels.left.radsec2pwm(new_speed_radsec_left) + + current_speed_radsec_right = wheels.right.enc.speed_radsec_avg + speed_error_radsec_right = desired_speed_radsec - current_speed_radsec_right + speed_change_radsec_right = SPEED_CHANGE_SLOPE * speed_error_radsec_right + new_speed_radsec_right = min( + current_speed_radsec_right + speed_change_radsec_right, + speed_rad_max + ) + # print("Current speed %f, Desired speed %f, Speed change right %f" % (current_speed_radsec_right, desired_speed_radsec, speed_change_radsec_right)) + new_speed_pwm_right = wheels.right.radsec2pwm(new_speed_radsec_right) + + if distance_error > 0: + wheels.left.move_pwm_for_distance(new_speed_pwm_left, distance_error) + wheels.right.move_pwm_for_distance(new_speed_pwm_right, distance_error) + else: + wheels.left.move_pwm_for_distance(-new_speed_pwm_left, abs(distance_error)) + wheels.right.move_pwm_for_distance(-new_speed_pwm_right, abs(distance_error)) + else: + wheels.stop() + + if ticks_diff(time_now, info_cycle_start) > info_cycle_length: + info_cycle_start = time_now + speed_msec_left = wheels.left.enc.speed_msec() + speed_msec_right = wheels.right.enc.speed_msec() + print("Distance: %.04fm, speed: L=%.04fm/s, R=%.04fm/s" % (distance, speed_msec_left, speed_msec_right)) + finally: + wheels.stop() + sonar.set_angle(0) + print("Finished") diff --git a/lesson_11/sonar.py b/lesson_11/sonar.py new file mode 100644 index 0000000..4a323a4 --- /dev/null +++ b/lesson_11/sonar.py @@ -0,0 +1,134 @@ +from machine import time_pulse_us +from microbit import pin1, pin8, pin12 +from utime import ticks_us, ticks_diff + + +class Sonar: + """Handles the ultrasonic sensor HC-SR04 to measure distance in m. + Based on https://cdn.sparkfun.com/datasheets/Sensors/Proximity/HCSR04.pdf, + The sensor operates at freq 40Hz and supports ranges from 2cm to 400cm, + with 0.3cm resolution and 3mm precision, providing a viewing angle of 15 degrees. + The sensor uses the speed of sound in the air to calculate the distance. + Trigger Input Signal 10uS TTL pulse is used to start the ranging, + and the module will send out an 8 cycle burst of ultrasound at 40 kHz + and raise its echo. The Echo Output Signal is an input TTL lever signal + and the range in proportion to the duration of the echo signal.""" + SOUND_SPEED = 343 # m/s + SERVO_MIN = 20 # right + SERVO_MAX = 128 # left + SERVO_STEP = (SERVO_MAX - SERVO_MIN) / 180 + + SCAN_NONE = 'none' + SCAN_WIDENING = 'widening' + SCAN_FULL = 'full' + SCAN_STEP = 5 + # We need to find the object at least this many times before we accept it + SCAN_DISTANCE_ACCEPTED_AFTER_FOUND_COUNT = 2 + + def __init__(self, trigger_pin=pin8, echo_pin=pin12, angle_pin=pin1, scan_range_max=0.5, scan_interval=125_000): + self.trigger_pin = trigger_pin + self.trigger_pin.write_digital(0) + self.echo_pin = echo_pin + self.echo_pin.read_digital() + self.angle_pin = angle_pin + self.angle = 0 + self.set_angle(0) + self.scan_mode = self.SCAN_NONE + self.scan_last_time = -1 + self.scan_interval = scan_interval + self.scan_range_max = scan_range_max + self.scan_angle_start = 0 + self.scan_direction = 1 + self.scan_dispersion = 0 + self.scan_distance_found_count = 0 + + def set_angle(self, angle): + """Sets sonar angle from -90 to 90""" + angle = angle if angle >= -91 else -90 + angle = angle if angle <= 90 else 90 + servo_value = self.SERVO_MAX - self.SERVO_STEP * (angle + 90) + print("Setting sonar angle to %d (value %d)" % (angle, servo_value)) + self.angle_pin.write_analog(servo_value) + self.angle = angle + + def get_distance(self): + """Returns the distance in meters measured by the sensor.""" + self.trigger_pin.write_digital(1) + self.trigger_pin.write_digital(0) + + measured_time_us = time_pulse_us(self.echo_pin, 1) + if measured_time_us < 0: + return measured_time_us + + measured_time_sec = measured_time_us / 1_000_000 + return measured_time_sec * self.SOUND_SPEED / 2 + + def start_scan(self): + """Starts the scanning mode.""" + self.scan_mode = self.SCAN_WIDENING + self.scan_angle_start = self.angle + self.scan_direction = 1 + self.scan_dispersion = 0 + self.scan_last_time = -1 + print("Scanning started") + + def stop_scan(self): + """Stops the scanning mode.""" + self.scan_mode = self.SCAN_NONE + print("Scanning stopped") + + def update(self): + """Updates itself based on the current scan mode and elapsed time.""" + if self.scan_mode == self.SCAN_NONE: + return + time_now = ticks_us() + if self.scan_last_time != -1 and ticks_diff(time_now, self.scan_last_time) < self.scan_interval: + return + self.scan_last_time = time_now + + distance = self.get_distance() + if distance < 0: + print("Error %f while getting distance" % distance) + self.scan_distance_found_count = 0 + return + + # we have the object, reorient future scanning to this angle, return to widening mode + if distance < self.scan_range_max: + self.scan_distance_found_count += 1 + if self.scan_distance_found_count < self.SCAN_DISTANCE_ACCEPTED_AFTER_FOUND_COUNT: + return + print("Object detected at %f m" % distance) + self.scan_mode = self.SCAN_WIDENING + self.scan_direction = 1 + self.scan_dispersion = 0 + self.scan_angle_start = self.angle + return + else: + self.scan_distance_found_count = 0 + + # gradually widening scan on both sides until we cover full area, then switch to full scan left-right-left + if self.scan_mode == self.SCAN_WIDENING: + self.scan_dispersion += self.SCAN_STEP + new_angle = self.scan_angle_start + self.scan_dispersion * self.scan_direction + if (abs(new_angle) <= 90): + self.set_angle(new_angle) + self.scan_direction *= -1 + else: + # we reached the end of the scan on one side, we will also finish the other side + self.scan_direction *= -1 + new_angle = self.scan_angle_start + self.scan_dispersion * self.scan_direction + if (abs(new_angle) <= 90): + self.set_angle(new_angle) + else: + # we reached the end of the scan on the other side, we will start scanning the full area + self.scan_mode = self.SCAN_FULL + print("Switching to full scan") + self.set_angle(-90) + self.scan_direction = 1 + + if self.scan_mode == self.SCAN_FULL: + new_angle = self.angle + (self.SCAN_STEP * 2) * self.scan_direction + if (abs(new_angle) <= 90): + self.set_angle(new_angle) + else: + self.scan_direction *= -1 diff --git a/lesson_11/wheel.py b/lesson_11/wheel.py new file mode 100644 index 0000000..921cec9 --- /dev/null +++ b/lesson_11/wheel.py @@ -0,0 +1,140 @@ +from utime import ticks_us, ticks_diff + +from microbit import i2c + +from wheel_encoder import WheelEncoder + + +class Wheel: + """Handles single wheel capable of moving forward or backward + with given (variable) speed and stop immediately or conditionally + based on distance and time.""" + + def __init__(self, name, i2c_address, motor_fwd_cmd, motor_rwd_cmd, sensor_pin, + pwm_min=60, pwm_max=255, pwm_multiplier=0, pwm_shift=0): + """Initializes the wheel.""" + self.name = name + self.i2c_address = i2c_address + self.motor_fwd_cmd = motor_fwd_cmd + self.motor_rwd_cmd = motor_rwd_cmd + self.distance_remain_ticks = -1 + self.distance_req_time_us = -1 + self.distance_start_time_us = 0 + self.speed_pwm = 0 + self.enc = WheelEncoder(sensor_pin=sensor_pin) + self.pwm_min = pwm_min + self.pwm_max = pwm_max + self.pwm_multiplier = pwm_multiplier + self.pwm_shift = pwm_shift + + def move_pwm(self, speed_pwm): + """Moves the wheel using given PWM speed (indefinite ticks, time). + The wheel will continue to move until stop() is called. + The PWM speed is a value between -255 and 255, where 0 means stop.""" + self.set_speed_pwm(speed_pwm) + self.distance_remain_ticks = -1 + self.distance_req_time_us = -1 + + def move_pwm_for_ticks(self, speed_pwm, distance_ticks): + """Moves the wheel forward using given PWM speed for the given distance + in sensor ticks. If the motor is already moving, the asked distance is added + to the remaining distance and the motor continues until no distance remains.""" + self.set_speed_pwm(speed_pwm) + print("Moving %s wheel with speed %d pwm for distance %f ticks" % (self.name, speed_pwm, distance_ticks)) + self.distance_remain_ticks += distance_ticks + + def move_pwm_for_time(self, speed_pwm, distance_time_us): + """Moves the wheel forward using given PWM speed for the given time. + If the motor is already moving, the distance in time is added to the current + distance and the motor continues to move until the total time is reached.""" + self.set_speed_pwm(speed_pwm) + self.distance_req_time_us += distance_time_us + if self.distance_start_time_us == 0: + self.distance_start_time_us = ticks_us() + + def move_pwm_for_distance(self, speed_pwm, distance): + """Moves the wheel forward using given PWM speed for given distance in meters.""" + distance_ticks = int(distance * self.enc.TICKS_PER_M) + self.move_pwm_for_ticks(speed_pwm, distance_ticks) + + def move_radsec_for_distance(self, radsec, distance): + """Moves the wheel using given rad/s speed for given distance in meters.""" + print("Moving %s wheel with speed %f rad/s for distance %f m" % (self.name, radsec, distance)) + speed_pwm = self.radsec2pwm(radsec) + distance_ticks = int(distance * self.enc.TICKS_PER_M) * 2 + self.move_pwm_for_ticks(speed_pwm, distance_ticks) + + def set_speed_pwm(self, speed_pwm): + """Sets the wheel PWM speed (and direction). Does not affect the remaining + distance or time previously set to perform. If the wheel was going + in the other direction, resets the H-bridge other direction first.""" + if speed_pwm == 0: + if self.speed_pwm != 0: + # print("Stopping %s wheel" % self.name) + i2c.write(self.i2c_address, bytes([self.motor_fwd_cmd, 0])) + i2c.write(self.i2c_address, bytes([self.motor_rwd_cmd, 0])) + self.speed_pwm = 0 + return + speed_pwm = int(max(-255, min(255, speed_pwm))) + if self.speed_pwm == speed_pwm: + return + self.enc.reset() + if (self.speed_pwm < 0 < speed_pwm) or (self.speed_pwm > 0 > speed_pwm): + # if we are changing the direction, we need to reset the motor first + motor_reset_cmd = (self.motor_rwd_cmd + if speed_pwm >= 0 else self.motor_fwd_cmd) + # print("Changing %s wheel direction" % self.name) + i2c.write(self.i2c_address, bytes([motor_reset_cmd, 0])) + motor_set_cmd = self.motor_fwd_cmd if speed_pwm > 0 else self.motor_rwd_cmd + print("Setting %s wheel speed_pwm %d" % (self.name, speed_pwm)) + i2c.write(self.i2c_address, bytes([motor_set_cmd, abs(speed_pwm)])) + self.speed_pwm = speed_pwm + + def radsec2pwm(self, radsec): + """Returns the PWM speed for the given rad/s speed. + We use the multiplier and shift values to calculate the PWM speed using formula: + rad_per_sec = pwm * multiplier + shift, for us: pwm = (rad_per_sec - shift) / multiplier.""" + if self.pwm_multiplier == 0: + print("error: wheel %s pwm_multiplier is 0" % self.name) + return 0 + direction = 1 if radsec >= 0 else -1 + return direction * int((abs(radsec) - self.pwm_shift) / self.pwm_multiplier) + + def msec2pwm(self, msec): + """Returns the PWM speed for the given m/s speed.""" + rad_per_sec = self.enc.m2rad(msec) + return self.radsec2pwm(rad_per_sec) + + def stop(self): + """Stops the wheel immediately.""" + self.set_speed_pwm(0) + self.distance_remain_ticks = -1 + self.distance_req_time_us = -1 + self.enc.reset() + + def stop_on_no_work(self): + """Stops the wheel if the remaining distance in ticks or time is reached.""" + stop_due_to_ticks = True + if self.distance_remain_ticks > 0: + stop_due_to_ticks = False + stop_due_to_time = True + if self.distance_req_time_us > 0: + time_delta = ticks_diff(ticks_us(), self.distance_start_time_us) + if time_delta < self.distance_req_time_us: + stop_due_to_time = False + # we stop only if both conditions are met + # otherwise we keep the other condition finish as well + if stop_due_to_ticks and stop_due_to_time: + self.stop() + + def on_tick(self): + """Updates the wheel state based on a new tick, + checks the remaining distance in ticks.""" + if self.distance_remain_ticks > 0: + self.distance_remain_ticks -= 1 + + def update(self): + """Updates the encoder and general wheel state.""" + if self.enc.update() is True: + self.on_tick() + self.stop_on_no_work() diff --git a/lesson_11/wheel_driver.py b/lesson_11/wheel_driver.py new file mode 100644 index 0000000..c36b810 --- /dev/null +++ b/lesson_11/wheel_driver.py @@ -0,0 +1,41 @@ +from microbit import pin14, pin15, i2c + +from wheel import Wheel + + +class WheelDriver: + """Handles the movement of the whole robot + (forward, backward, turning). Activities are either + indefinite or conditional based on ticks, time + or real speed measured by the encoder on wheel level.""" + I2C_ADDRESS = 0x70 + + def __init__(self, + left_pwm_min=50, left_pwm_max=255, left_pwm_multiplier=0, left_pwm_shift=0, + right_pwm_min=50, right_pwm_max=255, right_pwm_multiplier=0, right_pwm_shift=0): + """Initializes the wheel driver.""" + i2c.init(freq=100000) + i2c.write(self.I2C_ADDRESS, b"\x00\x01") + i2c.write(self.I2C_ADDRESS, b"\xE8\xAA") + self.left = Wheel(name="left", i2c_address=self.I2C_ADDRESS, + motor_fwd_cmd=5, motor_rwd_cmd=4, sensor_pin=pin14, + pwm_min=left_pwm_min, pwm_max=left_pwm_max, + pwm_multiplier=left_pwm_multiplier, pwm_shift=left_pwm_shift) + self.right = Wheel(name="right", i2c_address=self.I2C_ADDRESS, + motor_fwd_cmd=3, motor_rwd_cmd=2, sensor_pin=pin15, + pwm_min=right_pwm_min, pwm_max=right_pwm_max, + pwm_multiplier=right_pwm_multiplier, pwm_shift=right_pwm_shift) + self.stop() + + # Please note: normally, we would have aggregate move...() methods here for both wheels, but + # these got removed in favor of smaller code memory footprint + we control each wheel separately anyway. + + def stop(self): + """Stops the robot.""" + self.left.stop() + self.right.stop() + + def update(self): + """Updates the wheel driver, propagating the changes to the hardware.""" + self.left.update() + self.right.update() diff --git a/lesson_11/wheel_encoder.py b/lesson_11/wheel_encoder.py new file mode 100644 index 0000000..f3e36f9 --- /dev/null +++ b/lesson_11/wheel_encoder.py @@ -0,0 +1,103 @@ +from utime import ticks_us, ticks_diff + +class WheelEncoder: + """Encoder is able to monitor the wheel movement precisely + and provide the actual wheel rotation speed over the ticks + measured for X consecutive constant time intervals.""" + USE_BOTH_EDGES = False + TICKS_PER_WHEEL = 40 if USE_BOTH_EDGES else 20 + RAD_PER_WHEEL = 2 * 3.14159265359 + RAD_PER_TICK = RAD_PER_WHEEL / TICKS_PER_WHEEL + WHEEL_CIRCUMFERENCE_M = 0.21 + WHEEL_RADIUS_M = WHEEL_CIRCUMFERENCE_M / RAD_PER_WHEEL + WHEEL_CENTER_DISTANCE = 0.075 + M_PER_WHEEL = RAD_PER_WHEEL * WHEEL_RADIUS_M + TICKS_PER_M = TICKS_PER_WHEEL / M_PER_WHEEL + MIN_TICK_TIME_US = 5_000 # minimum possible tick time (switch instability detection under this value) + MAX_TICK_TIME_US = 200_000 # maximum possible tick time (after which we consider speed to be zero) + AVG_TICK_COUNT = 3 + + def __init__(self, sensor_pin): + """Initializes the wheel encoder.""" + self.sensor_pin = sensor_pin + self.sensor_value = -1 + self.tick_last_time = -1 + self.tick_last_time_avg = -1 + self.update_count = 0 + self.tick_count = 0 + self.speed_radsec = 0 + self.speed_radsec_avg = 0 + self.calc_value = -1 + self.calc_tick = 0 + self.calc_update_count = -1 + + def reset(self): + """Resets the encoder state.""" + self.__init__(self.sensor_pin) + + def update(self): + """Updates the encoder state based on the ongoing command.""" + """Retrieves the sensor value, checks for change and updates the wheel state + based on the ongoing command.""" + self.update_count += 1 + time_now = ticks_us() + last_time_diff = ticks_diff(time_now, self.tick_last_time) + if self.tick_last_time != -1 and last_time_diff < self.MIN_TICK_TIME_US: + return False + sensor_value_now = self.sensor_pin.read_digital() + if sensor_value_now == self.sensor_value: + if last_time_diff >= self.MAX_TICK_TIME_US: + self.speed_radsec = 0 + return False + self.sensor_value = sensor_value_now + if self.tick_last_time == -1: + self.tick_last_time = time_now + self.tick_last_time_avg = time_now + return False + if sensor_value_now == 1: + if self.USE_BOTH_EDGES: + # compensate for much shorter time when the sensor is down + last_time_diff *= 1.8 + else: + # we count just 1->0 change in this mode (to achieve uniformity between 0 and 1) + return False + self.tick_last_time = time_now + self.speed_radsec = self.RAD_PER_TICK / (last_time_diff / 1_000_000) + + # calculate average speed (simplistic, just accumulate last several ticks once in a while) + self.tick_count += 1 + if self.tick_count < self.AVG_TICK_COUNT: + if self.speed_radsec_avg == 0: + self.speed_radsec_avg = self.speed_radsec + else: + last_time_avg_diff = ticks_diff(time_now, self.tick_last_time_avg) + self.speed_radsec_avg = self.RAD_PER_TICK * self.AVG_TICK_COUNT / (last_time_avg_diff / 1_000_000) + self.tick_last_time_avg = time_now + self.tick_count = 0 + + self.calc_value = sensor_value_now + self.calc_tick = self.tick_count + self.calc_update_count = self.update_count + if self.speed_radsec > 0 and self.update_count < (4 if self.USE_BOTH_EDGES else 2): + print("Warning: wheel encoder updating slow, %s counts per change" % self.update_count) + self.update_count = 0 + return True + + def speed_msec(self): + """Returns the current wheel speed in m/s.""" + return self.M_PER_WHEEL * self.speed_radsec / self.RAD_PER_WHEEL + + def speed_msec_avg(self): + """Returns the current wheel speed in m/s.""" + return self.M_PER_WHEEL * self.speed_radsec_avg / self.RAD_PER_WHEEL + + def m2rad(self, m): + """Converts meters to radians.""" + return self.RAD_PER_WHEEL * m / self.M_PER_WHEEL + + def rad2m(self, rad): + """Converts radians to meters.""" + return self.M_PER_WHEEL * rad / self.RAD_PER_WHEEL + + def __str__(self): + return "speed: %f rad/s, %f m/s" % (self.speed_radsec, self.speed_msec())