├── documents ├── turn-designer.xlsx ├── cli.md ├── encoders.md ├── profiles.md ├── motors.md ├── maze.md ├── steering.md ├── systick.md ├── sensors.md └── reporting.md ├── cppcheck.md ├── .gitignore ├── mazerunner-core ├── .gitignore ├── battery.h ├── queue.h ├── README.MD ├── systick.h ├── switches.h ├── mazerunner-core.ino ├── config.h ├── config-ukmarsbot.h ├── motion.h ├── profile.h ├── adc.h ├── encoders.h ├── sensors.h ├── motors.h ├── config-robot-osmium.h ├── cli.h └── reporting.h ├── test └── README ├── LICENSE ├── lib └── README ├── include └── README ├── platformio.ini ├── .travis.yml ├── post-build-script.py ├── .clang-format └── README.md /documents/turn-designer.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ukmars/mazerunner-core/HEAD/documents/turn-designer.xlsx -------------------------------------------------------------------------------- /cppcheck.md: -------------------------------------------------------------------------------- 1 | CPPCHECK the code with 2 | 3 | cppcheck --enable=all --force -q --language=c++ *.cpp *.h *.ino 2>&1 | less 4 | 5 | 6 | run in the src folder -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/ 3 | *.lst 4 | *.map 5 | *.code-workspace 6 | data/ 7 | sheets/ 8 | *.zip 9 | html/ 10 | .idea/ 11 | # Ignore Mac files 12 | .DS_Store 13 | cmake-*/ 14 | firmware.* 15 | -------------------------------------------------------------------------------- /mazerunner-core/.gitignore: -------------------------------------------------------------------------------- 1 | /Release/ 2 | /sloeber.ino.cpp 3 | 4 | # Sloeber projects are defined by sloeber.cfg not the Eclipse files 5 | .cproject 6 | .project 7 | .settings/ 8 | .sproject 9 | spec.d 10 | 11 | 12 | # Mac files we don't care about 13 | .DS_Store 14 | 15 | # VIM files we don't care about 16 | *.swp 17 | 18 | -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Peter Harrison 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | ;PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [platformio] 12 | src_dir = mazerunner-core 13 | default_envs = nano 14 | 15 | ; shared by all the development environments 16 | [env] 17 | monitor_speed = 115200 18 | build_flags = -Wl,-Map,firmware.map 19 | -Wl,-u,vfprintf -lprintf_flt -lm 20 | ; items defined here will be available to the preprocessor in your code 21 | ; -D ROBOT_NAME=ROBOT_ORION 22 | ; -D BUILD_ENV_NAME=$PIOENV 23 | ; -D BUILD_PLATFORM=$PIOPLATFORM 24 | ; -D BUILD_TIME=$UNIX_TIME 25 | ; If you do not have a Python install, comment out the extra_scripts line 26 | extra_scripts = post:post-build-script.py 27 | check_flags = -DCPPCHECK 28 | ; if they are not auto-detected, here are some examples for defining serial ports 29 | ; linux ports 30 | ; upload_port = /dev/ttyUSB0 31 | ; monitor_port = /dev/ttyUSB0 32 | ; windows ports 33 | ; upload_port = COM3 34 | ; monitor_port = COM5 35 | ; mac ports 36 | ; upload_port = /dev/cu.wchusbserial* 37 | ; monitor_port = /dev/cu.wchusbserial* 38 | 39 | [env:nano] 40 | platform = atmelavr 41 | board = nanoatmega328 42 | framework = arduino 43 | 44 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Continuous Integration (CI) is the practice, in software 2 | # engineering, of merging all developer working copies with a shared mainline 3 | # several times a day < https://docs.platformio.org/page/ci/index.html > 4 | # 5 | # Documentation: 6 | # 7 | # * Travis CI Embedded Builds with PlatformIO 8 | # < https://docs.travis-ci.com/user/integration/platformio/ > 9 | # 10 | # * PlatformIO integration with Travis CI 11 | # < https://docs.platformio.org/page/ci/travis.html > 12 | # 13 | # * User Guide for `platformio ci` command 14 | # < https://docs.platformio.org/page/userguide/cmd_ci.html > 15 | # 16 | # 17 | # Please choose one of the following templates (proposed below) and uncomment 18 | # it (remove "# " before each line) or use own configuration according to the 19 | # Travis CI documentation (see above). 20 | # 21 | 22 | 23 | # 24 | # Template #1: General project. Test it using existing `platformio.ini`. 25 | # 26 | 27 | # language: python 28 | # python: 29 | # - "2.7" 30 | # 31 | # sudo: false 32 | # cache: 33 | # directories: 34 | # - "~/.platformio" 35 | # 36 | # install: 37 | # - pip install -U platformio 38 | # - platformio update 39 | # 40 | # script: 41 | # - platformio run 42 | 43 | 44 | # 45 | # Template #2: The project is intended to be used as a library with examples. 46 | # 47 | 48 | # language: python 49 | # python: 50 | # - "2.7" 51 | # 52 | # sudo: false 53 | # cache: 54 | # directories: 55 | # - "~/.platformio" 56 | # 57 | # env: 58 | # - PLATFORMIO_CI_SRC=path/to/test/file.c 59 | # - PLATFORMIO_CI_SRC=examples/file.ino 60 | # - PLATFORMIO_CI_SRC=path/to/test/directory 61 | # 62 | # install: 63 | # - pip install -U platformio 64 | # - platformio update 65 | # 66 | # script: 67 | # - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N 68 | -------------------------------------------------------------------------------- /mazerunner-core/battery.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include "adc.h" 16 | #include "config.h" 17 | 18 | class Battery; 19 | 20 | extern Battery battery; 21 | 22 | /*** 23 | * The Battery class monitors the battery voltage and provides a 24 | * correction factor that is used by the motors to ensure that 25 | * the actual voltage supplied to the motors will be correct even 26 | * if the battery is over or under its nominal voltage. 27 | * 28 | * The robot config file calculates the value of the constant 29 | * BATTERY_MULTIPLIER based on the values of the potential divider 30 | * used in the battery monitor circuit. This is stored as a constant 31 | * in the config because it saves storage and/or a calculation step. 32 | * 33 | */ 34 | class Battery { 35 | public: 36 | explicit Battery(uint8_t channel) : m_adc_channel(channel){}; 37 | 38 | void update() { 39 | m_adc_value = adc.get_dark(m_adc_channel); 40 | m_battery_volts = BATTERY_MULTIPLIER * m_adc_value; 41 | } 42 | 43 | float voltage() { 44 | return m_battery_volts; 45 | } 46 | 47 | private: 48 | Battery(); // no instantiation without an adc channel 49 | int m_adc_value = 0; 50 | int m_adc_channel; 51 | float m_battery_volts = 0; 52 | }; 53 | -------------------------------------------------------------------------------- /mazerunner-core/queue.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #pragma once 13 | 14 | /** 15 | * The Queue class is used to speed up flooding of the maze. It is very 16 | * simple and so not very flexible but shouldbe enough for the flooding. 17 | * 18 | * On a larger processor, you might create a Queue of operations that 19 | * constitute the required moves in a fast run. there is not much room on 20 | * the Arduino Nano. 21 | * 22 | * In the code, queues are created dynamically on the stack so you should 23 | * have some confidence that there is always enough space. Do not create 24 | * large queues if there in any doubt. The results can be unpredictable and 25 | * without obvious warnings. The default size is 64 elements. It is not clear 26 | * what the minimum queue size should be to guarantee no errors. 27 | * 28 | * By only using queues as local variables in this way, there can be no memory 29 | * leaks or heap fragmentation. 30 | * 31 | * To define a Queue of 64 bytes use something like: 32 | * Queue q_bytes; 33 | * 34 | * To define a Queue of 100 integers use something like: 35 | * Queue g_ints; 36 | */ 37 | template 38 | class Queue { 39 | public: 40 | Queue() { 41 | clear(); 42 | } 43 | 44 | int size() { 45 | return mItemCount; 46 | } 47 | 48 | void clear() { 49 | mHead = 0; 50 | mTail = 0; 51 | mItemCount = 0; 52 | } 53 | 54 | void add(item_t item) { 55 | if (mItemCount >= num_items) { 56 | // Optionally: handle overflow (e.g., ignore, overwrite, assert) 57 | return; // but drop the item for now 58 | } 59 | mData[mTail] = item; 60 | ++mTail; 61 | ++mItemCount; 62 | if (mTail >= num_items + 1) { 63 | mTail = 0; 64 | } 65 | } 66 | 67 | item_t head() { 68 | if (mItemCount == 0) { 69 | // Optionally: handle underflow (e.g., return default, assert) 70 | return item_t(); // default-constructed item 71 | } 72 | item_t result = mData[mHead]; 73 | ++mHead; 74 | if (mHead >= num_items + 1) { 75 | mHead = 0; 76 | } 77 | --mItemCount; 78 | return result; 79 | } 80 | 81 | protected: 82 | item_t mData[num_items + 1]; 83 | int mHead = 0; 84 | int mTail = 0; 85 | int mItemCount = 0; 86 | 87 | private: 88 | // while this is probably correct, prevent use of the copy constructor 89 | Queue(const Queue &rhs) { 90 | } 91 | }; 92 | -------------------------------------------------------------------------------- /mazerunner-core/README.MD: -------------------------------------------------------------------------------- 1 | # Mazerunner-Core 2 | 3 | The original ukmarsbot-mazerunner software is a broad, multi-purpose set of routines that lets you set up your robot and run it in a maze-based contest. As such, it was full of useful but possibly misleading code fragments as it tried to do everything in one place. 4 | 5 | This **mazerunner-core** repository does just the contest part of the job along with a small number of test routines. To characterise and configure many of the options like controller constants and so on, you will need other software. 6 | 7 | 8 | ## Functions 9 | 10 | The available options are relatively limited but will be enough to let you calibrate the wall sensors and perform basic turn configuration as well as run the robot as a maze solver or wall follower. 11 | 12 | ### Maze Solving 13 | The maze solver code is very basic and will just search back and forth between the start and the goal. On each pass, the path taken will improve so long as a better path has been discovered. The code is not optimal and makes no attempt to do things like run faster through known sections or run subsequent passes more quickly. That is left as an exercise for the user. 14 | 15 | ### Wall Following 16 | Wall following is also very simple and will follow a left wall until the robot is physically stopped. Because the same techniques for motion and turning are used here and the maze solver, you can use the wall following function to help you tune the turns. Simply make a rectangular 'racetrack' maze and have the robot run around it either clockwise or anticlockwise so that is does repeated turns. This makes it easy to see the effect of changing the turn parameters. 17 | 18 | ## Bluetooth 19 | 20 | The Serial port on the side of the robot can accept a HC-05 or HC-06 Bluetooth Serial adaptor. From board revision V1.1 onwards you should be able to plug the adaptor directly into the port but you will need to check the actual pin order on your HC05/6. V1.0 motherboards will need an adaptor and some modification to the port. 21 | 22 | The software includes a Serial command line interface that can be used with the normal USB connector and the BT adaptor simultaneously. A BT adaptoor is by far the better option as it is always ready and will not reset the processor when used. Also, the USB cable is a serious impediment when trying to do things like calibrate sensors. 23 | 24 | ## Operation 25 | 26 | When the code is loaded onto the robot, operation is primarily governed by the function selection switches and the user button. 27 | 28 | - select a function on the switches 29 | - reset the processor 30 | - activate the function by pressing the button 31 | 32 | Some builtin functions have a secondary start that is triggered by briefly occluding the front sensors. For example, in maze running mode, the user button starts the function. The bult-in LED blinks to indicate that it is ready and you can place the robot in the maze. The robot will not actually start a run until triggered by occluding the front sensors. This is my preference for a start that does not disturb the robot. You may choose whatever technique suits you. 33 | 34 | ***NOTE*** the maze map is stored in EEPROM. To clear the stored maze, hold down the user button and reset the processor. Hold down the button until the user LED fashes. -------------------------------------------------------------------------------- /mazerunner-core/systick.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef SYSTICK_H 13 | #define SYSTICK_H 14 | #include "Arduino.h" 15 | #include "adc.h" 16 | #include "config.h" 17 | #include "motion.h" 18 | #include "motors.h" 19 | #include "sensors.h" 20 | #include "switches.h" 21 | class Systick { 22 | public: 23 | // don't let this start firing up before we are ready. 24 | // call the begin method explicitly. 25 | void begin() { 26 | // set 27 | bitClear(TCCR2B, WGM22); 28 | bitClear(TCCR2A, WGM20); 29 | bitSet(TCCR2A, WGM21); 30 | // set divisor to 128 => 125kHz 31 | bitSet(TCCR2B, CS22); 32 | bitClear(TCCR2B, CS21); 33 | bitSet(TCCR2B, CS20); 34 | OCR2A = 249; // (16000000/128/500)-1 => 500Hz 35 | bitSet(TIMSK2, OCIE2A); 36 | delay(40); // make sure it runs for a few cycles before we continue 37 | } 38 | /*** 39 | * This is the SYSTICK ISR. It runs at 500Hz by default and is 40 | * called from the TIMER 2 interrupt (vector 7). 41 | * 42 | * All the time-critical control functions happen in here. 43 | * 44 | * interrupts are enabled at the start of the ISR so that encoder 45 | * counts are not lost. 46 | * 47 | * The last thing it does is to start the sensor reads so that they 48 | * will be ready to use next time around. 49 | * 50 | * Timing tests indicate that, with the robot at rest, the systick ISR 51 | * consumes about 10% of the available system bandwidth. 52 | * 53 | * With just a single profile active and moving, that increases to nearly 30%. 54 | * Two such active profiles increases it to about 35-40%. 55 | * 56 | * The reason that two profiles does not take up twice as much time is that 57 | * an active profile has a processing overhead even if there is no motion. 58 | * 59 | * Most of the load is due to that overhead. While the profile generates actual 60 | * motion, there is an additional load. 61 | * 62 | * 63 | */ 64 | void update() { 65 | // digitalWriteFast(LED_BUILTIN, 1); 66 | // NOTE - the code here seems to get inlined and so the function is 2800 bytes! 67 | // grab the encoder values first because they will continue to change 68 | encoders.update(); 69 | motion.update(); 70 | sensors.update(); 71 | battery.update(); 72 | 73 | motors.update_controllers(motion.velocity(), motion.omega(), sensors.get_steering_feedback()); 74 | adc.start_conversion_cycle(); 75 | // NOTE: no code should follow this line; 76 | } 77 | }; 78 | 79 | extern Systick systick; 80 | 81 | #endif -------------------------------------------------------------------------------- /documents/cli.md: -------------------------------------------------------------------------------- 1 | # Command Line Interface 2 | 3 | If you have a USB lead connected to your UKMARSBOT, or a Serial Bluetooth device connected, you can interact with it via a terminal program. 4 | 5 | At present, a small number of commands are implemented that will let you print out the maze details, view and set some of the configuration settings. These are mostly single letter commands and the robot will respond immediately. For example, with a terminal connected, just press the 's' key, followed by return, and a single line of sensor configuration data will be sent. 6 | 7 | Note that the commands are case insensitive and anything you type is converted to upper case by the robot. 8 | 9 | ## Simple commands 10 | 11 | So far the following single-character commands are interpreted. Where a command is followed by 'n', that is an indication that a numeric argument must be supplied. 12 | 13 | 14 | | cmd | Function | 15 | |:----------|-------------------------------------------------| 16 | | W | 'Walls' - display the current maze map | 17 | | R | 'Route' - display the current best route | 18 | | S | 'Sensors' - one line of sensor data | 19 | | T n | 'Test' - Run Test number n | 20 | | U n | 'User' - Run User function n | 21 | | $ | Settings commands - see below | 22 | 23 | --- 24 | 25 | ## Settings commands 26 | 27 | Many of the constants needed fortuning and calibrating the robot are stored ins a settings structure. These values are populated either from the `config.h` file or read from EEPROM. At present, the values are all read from the `config.h` defaults. Future releases will use the EEPROM based values. Meanwhile, it is possible to view and edit these settings without re-programming the robot completely. Values you enter will not be saved after a reset unless you do so explicitly so take notes while experimenting and edit the code later if necessary. 28 | 29 | The following settings commands are implemented: 30 | 31 | | cmd | Function | 32 | |:----:|-----------------------------------------------------------| 33 | | $ | Display all setting by index number | 34 | | $n | Display a single setting for index number n | 35 | | $$ | display all settings as C declarations with name and type | 36 | | $@ | fetch all settings from EEPROM, overwrite current values | 37 | | $! | store all current settings values to EEPROM | 38 | | $# | reset all settings to defaults from config.h | 39 | 40 | The reason there are so ways to see the settings values - with and without a full declaration - is to make it easier to use a simple short-hand manual method for changing settings and for later use with a host-based manager. It is likely that the short-form version will have the setting name appended as a comment at some time. 41 | 42 | ### View and change a setting 43 | 44 | To view a single setting use the form $n where n is the index number of the settings as shown in the list that you get from the $ settings list command. For example, the command $2 will display the current value for the forward controller KP setting 45 | 46 | To change a setting use the form $n=vvvvv, where n is the setting number and vvvvv is the new value. For example, to change the forward controller KP setting to 2.13, you can enter `$2=2.13`. the new value will be echoed to the screen for confirmation. 47 | 48 | There is no undo so double check before writing to EEPROM. You can always get back to the compiled in defaults with `$#`. 49 | -------------------------------------------------------------------------------- /documents/encoders.md: -------------------------------------------------------------------------------- 1 | # Encoders 2 | Attached to each motor is an encoder which generates a series of pulses as the motor shaft turns. Each encoder generates two sets of pulses that are in quadrature. That is, the pulses on one of the channels is delayed compared to the other channel - they do not change state at the same time: 3 | ``` 4 | ___ ___ ___ __ 5 | channel A | |___| |___| |___| 6 | ___ ___ ___ _ 7 | channel B __| |___| |___| |___| 8 | 9 | ``` 10 | 11 | The electronics of UKMARSBOT modifies these signals slightly so that they are suitable for generating interrupts on two of the pins of the ATmega328p in the Arduino nano. 12 | 13 | Each encoder generates interrupts using one channel and feeds the other channel to another pin. Thus there are two interrupt sources and two additional inputs used by the encoders in total. 14 | 15 | Each interrupt keeps track of one encoder. By comparing the interrupting signal with the signal from the other channel, the service routine can tell if the motor has moved forward or backwards. The movement is always one step per interrupt. 16 | 17 | Every change in state of the two encoder channels will generate an interrupt. That means there will be four changes per magnet pole per revolution of the motor shaft. 18 | 19 | The stock encoders from Pololu have just three magnet poles and so they will give 12 counts per rotation of the motor shaft. 20 | 21 | ## Encoder resolution 22 | 23 | Consider a motor with one of these encoders connected to a 22:1 gearbox. Now a single turn of the output shaft will give 264 counts. If the drive wheel has a diameter of 28mm then one turn of the wheel is approximately 88mm of linear travel (28 * PI, or about 28*22/7). 24 | 25 | So, for 88mm of travel, the wheel will give 264 counts and the basic resolution of the encoder is going to be 264/88 = 3 counts per mm of travel. The robot has two drive wheels so, for forward motion, both encoders can be combined to give an overall resolution of 6 counts per mm. 26 | 27 | In the `config.h` file, you will be able to enter values that describe the encoders and drive train in _your_ robot. Because real robots have some small variations in things like actual wheel diameter, you can use the calibration test code to fine tune those values for the best possible accuracy. 28 | 29 | Once the encoder resolution is known, the software will have little difficulty in recording distances and speeds in real world units (mm and mm/s) rather than some abstract number of counts and counts per tick. 30 | 31 | ## Encoder pulse frequency 32 | 33 | Each encoder count generates an interrupt. Suppose the robot in this example is travelling at 2000mm/s. With a combined resolution of 6 counts per mm, you can see that there will be about 6 * 2000 = 12000 interrupts per second from the encoders. That is quite a high rate and, even if the processor had nothing else to do, each interrupt would have a maximum of 80 microseconds available for its work. 34 | 35 | Of course,the processor is always busy with several tasks including the systick event that must run 500 times per second. 36 | 37 | In this code, the systick interrupt uses the option to enable other interrupts while it runs. That is not normally the case. Each encoder interrupt is carefully written to take as little time as possible - about 4 microseconds typically. 38 | 39 | It should be clear that there is a lot going on behind the scenes in the processor. You will see quite a lot of examples in the code where special precautions are taken to make sure that critical operations are not held up and that variables modified by interrupt routines are not corrupted when used by other operations. Look for `volatile` declarations and `ATOMIC_BLOCK` statement blocks. 40 | -------------------------------------------------------------------------------- /mazerunner-core/switches.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef SWITCHES_H 13 | #define SWITCHES_H 14 | 15 | #include 16 | #include 17 | #include "adc.h" 18 | #include "config.h" 19 | 20 | /*** 21 | * The Switches class looks after the multifunction analogue input on UKMARSBOT. 22 | * 23 | * A single analogue channel lets you examine four dip switches and a pushbutton 24 | * using a single ADC line. 25 | * 26 | * The dip switches short out combinations of resistors in a potential divider chain 27 | * and thus cause a different voltage to be presented to the ADC input pin. The 28 | * maximum voltage is applied when all switches are open. That voltage will be 29 | * about 66% of the full range of the analogue channel with the resistors chosen. 30 | * 31 | * The top resistor in this chain has a pushbutton in parallel so that pressing 32 | * the button pulls the input all the way up to the positive supply giving a 33 | * full scale adc reading. 34 | * 35 | * There is no debounce on this circuit or in the software. None has yet proven 36 | * necessary. The simplest option would be to place a small capacitor between 37 | * the ADC input and ground. 38 | * 39 | * NOTE: The switches class relies upon the ADC being updated regularly in the 40 | * systick event. 41 | */ 42 | 43 | // we need a forward declaration... 44 | class Switches; 45 | // so that we can declare the instance 46 | extern Switches switches; 47 | class Switches { 48 | public: 49 | explicit Switches(uint8_t channel) : m_channel(channel){}; 50 | 51 | void update() { 52 | m_switches_adc = adc.get_dark(m_channel); 53 | } 54 | 55 | /** 56 | * 57 | * @brief Convert the switch ADC reading into a switch reading. 58 | * @return integer in range 0..16 or -1 if there is an error 59 | */ 60 | int read() { 61 | update(); 62 | 63 | if (m_switches_adc > 800) { 64 | return 16; 65 | } 66 | for (int i = 0; i < 16; i++) { 67 | int low = pgm_read_word_near(adc_thesholds + i); 68 | int high = pgm_read_word_near(adc_thesholds + i + 1); 69 | if (m_switches_adc > (low + high) / 2) { 70 | return i; 71 | } 72 | } 73 | return -1; 74 | } 75 | 76 | inline bool button_pressed() { 77 | return read() == 16; 78 | } 79 | 80 | void wait_for_button_press() { 81 | while (not(button_pressed())) { 82 | delay(10); 83 | }; 84 | } 85 | 86 | void wait_for_button_release() { 87 | while (button_pressed()) { 88 | delay(10); 89 | }; 90 | } 91 | 92 | void wait_for_button_click() { 93 | wait_for_button_press(); 94 | wait_for_button_release(); 95 | delay(250); 96 | } 97 | 98 | // for testing 99 | int adc_reading() { 100 | update(); 101 | return m_switches_adc; 102 | } 103 | 104 | private: 105 | uint8_t m_channel = 255; 106 | int m_switches_adc = 0; 107 | }; 108 | 109 | #endif -------------------------------------------------------------------------------- /post-build-script.py: -------------------------------------------------------------------------------- 1 | # For more information look here 2 | # https://docs.platformio.org/en/latest/projectconf/advanced_scripting.html 3 | # 4 | 5 | 6 | Import("env", "projenv") 7 | 8 | 9 | # access to global build environment 10 | # print(env.Dump()) 11 | 12 | # access to project build environment (is used source files in "src" folder) 13 | # print(projenv.Dump()) 14 | 15 | # Show sizes 16 | env.AddPostAction( 17 | "$BUILD_DIR/${PROGNAME}.elf", 18 | env.VerboseAction(" ".join([ 19 | "avr-size", "$BUILD_DIR/${PROGNAME}.elf" 20 | ]), "Building $BUILD_DIR/${PROGNAME}.hex") 21 | ) 22 | # Make listing 23 | env.AddPostAction( 24 | "$BUILD_DIR/${PROGNAME}.elf", 25 | env.VerboseAction(" ".join([ 26 | "avr-objdump -d -S --disassemble-zeroes", "$BUILD_DIR/${PROGNAME}.elf", 27 | " > ", "${PROGNAME}.lst" 28 | ]), "Generate listing file ${PROGNAME}.lst") 29 | ) 30 | # Make ram list 31 | env.AddPostAction( 32 | "$BUILD_DIR/${PROGNAME}.elf", 33 | env.VerboseAction(" ".join([ 34 | 'avr-nm -Crtd --size-sort', 35 | '$BUILD_DIR/${PROGNAME}.elf', 36 | '| grep -i " [dbv] "', 37 | '> ${PROGNAME}.ram.txt' 38 | ]), "Generate RAM file ${PROGNAME}.ram.txt") 39 | ) 40 | 41 | # Make flash list 42 | env.AddPostAction( 43 | "$BUILD_DIR/${PROGNAME}.elf", 44 | env.VerboseAction(" ".join([ 45 | ' avr-nm -Crtd --size-sort', 46 | '$BUILD_DIR/${PROGNAME}.elf', 47 | ' | grep -i " [t] "', 48 | ' > ${PROGNAME}.flash.txt' 49 | ]), "Generate FLASH file ${PROGNAME}.flash.txt") 50 | ) 51 | 52 | 53 | # format sources 54 | env.AddPostAction( 55 | "$BUILD_DIR/${PROGNAME}.elf", 56 | env.VerboseAction(" ".join([ 57 | "clang-format -i -style=file", 58 | # " --verbose ", 59 | "${PROJECT_SRC_DIR}/*.cpp", 60 | "${PROJECT_SRC_DIR}/*.h", 61 | "${PROJECT_SRC_DIR}/*.ino", 62 | ]), "Format sources in ${PROJECT_SRC_DIR}") 63 | ) 64 | 65 | 66 | # 67 | # Dump build environment (for debug purpose) 68 | # print(env.Dump()) 69 | # 70 | 71 | # 72 | # Change build flags in runtime 73 | # 74 | # env.ProcessUnFlags("-DVECT_TAB_ADDR") 75 | # env.Append(CPPDEFINES=("VECT_TAB_ADDR", 0x123456789)) 76 | 77 | # 78 | # Upload actions 79 | # 80 | 81 | # def before_upload(source, target, env): 82 | # print("before_upload") 83 | # # do some actions 84 | 85 | # # call Node.JS or other script 86 | # env.Execute("node --version") 87 | 88 | 89 | # def after_upload(source, target, env): 90 | # print("after_upload") 91 | # # do some actions 92 | 93 | # print("Current build targets", map(str, BUILD_TARGETS)) 94 | 95 | # env.AddPreAction("upload", before_upload) 96 | # env.AddPostAction("upload", after_upload) 97 | 98 | # 99 | # Custom actions when building program/firmware 100 | # 101 | 102 | # env.AddPreAction("buildprog", callback...) 103 | # env.AddPostAction("buildprog", callback...) 104 | 105 | # 106 | # Custom actions for specific files/objects 107 | # 108 | 109 | # env.AddPreAction("$BUILD_DIR/${PROGNAME}.elf", [callback1, callback2,...]) 110 | # env.AddPostAction("$BUILD_DIR/${PROGNAME}.hex", callback...) 111 | 112 | # custom action before building SPIFFS image. For example, compress HTML, etc. 113 | # env.AddPreAction("$BUILD_DIR/spiffs.bin", callback...) 114 | 115 | # custom action for project's main.cpp 116 | # env.AddPostAction("$BUILD_DIR/src/main.cpp.o", callback...) 117 | 118 | # Custom HEX from ELF 119 | # env.AddPostAction( 120 | # "$BUILD_DIR/${PROGNAME}.elf", 121 | # env.VerboseAction(" ".join([ 122 | # "$OBJCOPY", "-O", "ihex", "-R", ".eeprom", 123 | # "$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/${PROGNAME}.hex" 124 | # ]), "Building $BUILD_DIR/${PROGNAME}.hex") 125 | # ) 126 | 127 | -------------------------------------------------------------------------------- /documents/profiles.md: -------------------------------------------------------------------------------- 1 | # Profiles 2 | 3 | Movement of UKMARSBOT is managed by the velocity profiles. Code for the Profile class is found in profile.h. 4 | 5 | A Profile is a computer generates set of velocities that varies over time. Both forward and rotary motion have profiles and both are treated exactly the same by the code. In fact, the forward and rotation profiles are both instances of the same C++ class. The only difference is that one has units of mm, mm/s and mm/s/s and the other has units of deg, deg/s and deg/s/s. Nothing in the Profile code cares about what the units represent - they are just numbers - and so the same code can be used for both. 6 | 7 | ## Profile features 8 | 9 | Profiles have three phases. 10 | 11 | - **Accelerating** where the speed is changing from the start speed to the running speed. 12 | - **Constant velocity** where the speed is not changing. 13 | - **Braking** where the speed is changing to the final speed. 14 | 15 | The easiest type of profile to understand is one where the speed is zero, increases to a maximum value, remains steady until braking is needed and then reduces the speed steadily until at rest again. 16 | 17 | ``` 18 | --------------------- 19 | ^ / \ 20 | V | / \ 21 | | / \ 22 | ------------- ---------- 23 | time -> 24 | 25 | ``` 26 | 27 | To describe such a profile, a number of parameters are needed. These are: 28 | 29 | - **distance** is the complete distance (or angle) over which the movement occurs 30 | - **maximum speed** is the speed of the central, constance velocity phase 31 | - **end speed** is the final speed that the profile must reach when distance is complete 32 | - **acceleration** is the permitted rate of change of speed 33 | 34 | A profile is always started with these parameters and it runs until it is finished. In this context, 'finished' just means that the given distance has been reached.. User code can wait and do nothing until the profile finishes or it can perform other tasks while it waits. 35 | 36 | Speeds and accelerations are always positive but the distance can be negative so that the robot can move forwards or backwards and can turn left (positive) or right (negative). 37 | 38 | Profiles are very flexible. For example, if the distance is very small, the profile may not be able to reach the maximum speed but will still perform whatever acceleration and braking it can. The starting speed and ending speed do not have to be zero. If the profiler is already running at some speed, the accelerating phase will simply try to match the given maximum speed, even if it is smaller than the current speed. The end speed need not be zero in which case the profiler will continue to run at the specified speed even after it has finished. 39 | 40 | All of the robot's movements are created by starting and manipulating profiles. 41 | 42 | Because the two profiles are independent, either can be started, stopped or modified at any time. For example, to make the robot perform a smooth, continuous turn, you could start a forward profile so that it finishes at a constant speed and then begin a rotation profile that turns by just 90 degrees. The result will be a forward movement of the robot followed by a smooth right-angle turn nd then the robot will continue in a straight line. A final forward profile can be started to bring it to a halt after some distance. The radius of the turn will be determined by the combination of the robot's forward speed and maximum angular velocity during the turn. 43 | 44 | ## Profile updates 45 | 46 | Once started by user code, both the forward and rotation profiles are updates automatically by the systick service which normally runs 500 times per second. Thus, once started, a profile will continue to generate speeds and so update the controllers. A profile can be disabled by setting it into an IDLE state. The update still runs but the output does not drive the motors. 47 | -------------------------------------------------------------------------------- /mazerunner-core/mazerunner-core.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #include 13 | #include "adc.h" 14 | #include "battery.h" 15 | #include "cli.h" 16 | #include "config.h" 17 | #include "encoders.h" 18 | #include "maze.h" 19 | #include "motion.h" 20 | #include "motors.h" 21 | #include "mouse.h" 22 | #include "reporting.h" 23 | #include "sensors.h" 24 | #include "switches.h" 25 | #include "systick.h" 26 | 27 | /******************************************************************************/ 28 | 29 | // Global objects are all defined here. The declarations are in 30 | // the corresponding header files 31 | Systick systick; // the main system control loop 32 | AnalogueConverter adc; // controls all analogue conversions 33 | Battery battery(BATTERY_ADC_CHANNEL); // monitors battery voltage 34 | Switches switches(SWITCHES_ADC_CHANNEL); // monitors the button and switches 35 | Encoders encoders; // tracks the wheel encoder counts 36 | Sensors sensors; // make sensor alues from adc vdata 37 | Motion motion; // high level motion operations 38 | Motors motors; // low level control for drive motors 39 | Profile forward; // speed profiles for forward motion 40 | Profile rotation; // speed profiles for rotary motion 41 | Maze maze PERSISTENT; // holds maze map (even after a reset) 42 | Mouse mouse; // all the main robot logic is here 43 | CommandLineInterface cli; // user interaction on the serial port 44 | Reporter reporter; // formatted reporting of robot state 45 | 46 | /******************************************************************************/ 47 | 48 | // The encoder ISR routines do not need to be explicitly defined 49 | // because the callbacks are assigned though the Arduino API 50 | // Other interrupt service routines are defined here for want of 51 | // a better place to put them 52 | 53 | ISR(ADC_vect) { 54 | adc.callback_adc_isr(); 55 | } 56 | 57 | ISR(TIMER2_COMPA_vect, ISR_NOBLOCK) { 58 | systick.update(); 59 | } 60 | 61 | /******************************************************************************/ 62 | void setup() { 63 | Serial.begin(BAUDRATE); 64 | // redirectPrintf(); // send printf output to Serial (uses 20 bytes RAM) 65 | pinMode(LED_USER, OUTPUT); 66 | digitalWrite(LED_USER, 0); 67 | pinMode(LED_BUILTIN, OUTPUT); 68 | digitalWrite(LED_BUILTIN, 0); 69 | adc.begin(); 70 | motors.begin(); 71 | motors.disable_controllers(); 72 | encoders.begin(); 73 | /// do not begin systick until the hardware is setup 74 | systick.begin(); 75 | /// keep the button held down after a reset to clear the maze 76 | /// otherwise you will use the last-saved map. 77 | if (switches.button_pressed()) { 78 | maze.initialise(); 79 | mouse.blink(2); 80 | Serial.println(F("Maze cleared")); 81 | switches.wait_for_button_release(); 82 | } 83 | /// leave the emitters off unless we are actually using the sensors 84 | /// less power, less risk 85 | sensors.disable(); 86 | maze.set_goal(GOAL); 87 | reporter.set_printer(Serial); 88 | Serial.println(); 89 | Serial.println(F(CODE)); 90 | Serial.println(F(NAME)); 91 | Serial.println(F("RDY")); 92 | cli.prompt(); 93 | } 94 | 95 | /// the main loop exists only to initiate tasks either as a result 96 | /// of pressing the button or sending a command through the serial port 97 | void loop() { 98 | if (switches.button_pressed()) { 99 | switches.wait_for_button_release(); 100 | int function = switches.read(); 101 | cli.run_function(function); 102 | } 103 | cli.process_serial_data(); 104 | } 105 | -------------------------------------------------------------------------------- /documents/motors.md: -------------------------------------------------------------------------------- 1 | # Motors 2 | 3 | UKMARSBOT uses a pair of metal gearmotors compatible with the Pololu mini metal gearmotor. These have a 6 Volt N20-sized motor attached to a small gearbox. Gear ratios vary from 5:1 upwards. A common choice is the 20:1 gearbox. Note that the actual gear ratio is never exactly that stated for the motor because of the choice of internal gears. On my example the actual gear ratio is 19.54:1. Any compatible motor with gearbox can be fitted. 4 | 5 | For feedback of speed and position, the motors must have an extended rear shaft with an encoder fitted that generates electrical pulses as the motor shaft turns. The encoders are dealt with in detail in their own section. 6 | 7 | ## PWM 8 | 9 | The default PWM frequency for the Arduino nano is only 490Hz. For these motors, that is not a good choice and the code configures the PWM generator to run at 31.25kHz. The higher frequency give a smoother response and is out of the range of human hearing so there will be no whining from the motor drive. 10 | 11 | DC electric motors will run at a constant speed for a given input voltage and load. Since the PWM duty cycle is just a percentage of the available drive voltage, arrangements are made in the motor control code to compensate for changes in the battery Voltage. Your code should set a drive Voltage for the motors rather than a PWM duty cycle and the driver code will make the necessary calculations. 12 | 13 | ## Wheel control 14 | 15 | It might seem natural for robot control software to work by setting the speeds of the individual motors. Set the motors to the same speed and the robot will drive straight. Set one motor faster than the other and the robot will turn to one side. In the UKMARSBOT software, things are done a little differently. 16 | 17 | Speed control managed in two components - _forward_ motion and _rotation_. Each is controlled separately and the two components are combined to generate commands for each of the wheels. Without going into the maths to much, the individual speeds are calculated from 18 | 19 | left = forward - rotation 20 | right = forward + rotation 21 | 22 | If rotation is zero, both wheels get the same speed and the robot moves in a straight line controlled by the forward component. If the forward component is zero, the robot will turn on the spot with an angular velocity controlled by the rotation component. Notice that a positive rotation will turn the robot left. 23 | 24 | Any combination for the forward and rotation components can be used to move the robot in linear motion, smooth curved turns or in-place spin turns. You don't have to do any work trying to calculate individual wheel speeds - that is already done for you. 25 | 26 | ## PD Controllers 27 | 28 | Each of the motion types has its own Proportional-Differential (PD) controller that ensures that the wheels are doing what they need to for that motion type. You will have heard of PID controllers and may wonder what happened to the I term. For the type of control employed in UKMARSBOT, the I term is not needed and a simplified controller can be used. This is because the controllers used are not controlling the speed directly. Instead, they control the _position_ and PD control is more than adequate for position control in this configuration. At every systick cycle, the control software works out what the new position should be for the current forward and rotation speeds and compares that with the position of the robot as reported by the encoders. the controllers then generate commands to try and reduce that error to zero. For continuous motion, the set points change at every tick and the controllers move the robot to catch up. 29 | 30 | This kind of control scheme is reliable and robust in this application but only useful for the small distance changes between control loop cycles. It is not a good scheme for trying to tell the robot it needs to be at a new location 180mm away. 31 | 32 | The new positions are generated in small increments every cycle by the motion profilers. These are described in their own section. 33 | 34 | Each of the motion PD controllers has a pair of constants that determine its behaviour. These are the following constant in `config.h` 35 | 36 | ``` 37 | // forward motion controller constants 38 | const float FWD_KP = 2.0; 39 | const float FWD_KD = 1.1; 40 | 41 | // rotation motion controller constants 42 | const float ROT_KP = 2.1; 43 | const float ROT_KD = 1.2; 44 | ``` 45 | 46 | The values shown are probably acceptable for a standard UKMARSBOT using 6 Volt motors with 12 pulse encoders and 20:1 gearboxes. If your robot has a different drivetrain, you may want to tune these values somewhat. The system is not overly sensitive to the controller gains. A separate section will look at how to tune the controllers to get a better response. 47 | -------------------------------------------------------------------------------- /documents/maze.md: -------------------------------------------------------------------------------- 1 | # The Maze 2 | 3 | Since this is a Maze running robot, you will need some code for storing and manipulating a map of the maze as well as the ability to decide on the best route to the goal. The `maze.cpp` file contains all this. 4 | 5 | ## Maze mapping 6 | 7 | For simplicity, the code assumes a 16x16 cell classic maze and the wall data is stored in an array of bytes. Each byte represents the wall data for one cell and each wall is represented by a single bit in that byte. 8 | 9 | Because the North wall of one cell is also the South wall of an adjacent cell, wall information is actually stored twice and care must be taken to update neighbour cells when the wall information is changed. 10 | 11 | In this implementation a one-dimensional array is used to store the two-dimensional map. The first cell in the array (cell 0) is the start cell in the South West corner of the maze. The next cell in the array is the cell immediately to the North of that and so-on. After the first column, the array continues with the cell to the East of the start cell. this is cell 16. 12 | 13 | ## Directions 14 | 15 | So that it is clear everywhere, the four cardinal directions are given numeric values: 16 | 17 | #define NORTH 0 18 | #define EAST 1 19 | #define SOUTH 2 20 | #define WEST 3 21 | 22 | 23 | ## Wall representation 24 | 25 | In each byte, a single bit is used to represent the presence of a wall: 26 | 27 | 00000000 28 | |||| 29 | |||` NORTH 30 | ||`- EAST 31 | |`-- SOUTH 32 | `-- WEST 33 | 34 | Thus the value store in the start cell, which always has walls to the West, South and East is 00001110 = 0x0e = 14 35 | 36 | Functions are provided that let you set and clear wall data as well as loading, printing and saving the maze data. 37 | 38 | ## Surviving reset 39 | 40 | It is really frustrating if your robot explores a lot of the maze and then crashes. Without care, pressing the reset button will erase the processors memory and you have to start all over again. Even more annoying is the observation that connecting a serial lead to the Arduino causes a processor reset and memory gets wiped again. You can, and probably should, arrange to save the maze to the processor's on-board EEPROM. Not all processors have that option though. 41 | 42 | In this code, the maze map is stored in a special section of RAM that will not be wiped after a reset. Note that a power-down _will_ clear even that memory though. You can now press the reset button - or connect a serial lead - and the maze data will be preserved. 43 | 44 | ## Maze solving 45 | 46 | A lot of new builders get hung up on the business of 'solving' the maze. Practically speaking it is not too hard and, in any case, is almost literally the last thing you need to do for your robot. After exploring and mapping the maze walls, the robot needs to be able to find the shortest, or best, route from the start to the goal. This is done by a process called 'flooding'. This is not the place for a long description of the flooding algorithm - there are many resources online that describe how it is done. in essence, the aim is to produce a map of costs that let the robot choose the least-cost neighbour so that it can plan its next move accordingly. That map is another array of 256 bytes organized in the same way as the maze wall data. The cost for cell 0 is in the first element of the array, ad the cost for the cell to the North is in the second element and so on. 47 | 48 | In this code, there is a simple and efficient flooding function that should be able to fully flood the maze in about 7 milliseconds. That is fast enough that you can afford to flood the maze at every cell when exploring so that your robot can perform an intelligent search, always trying to find the best route as it searches for the goal. 49 | 50 | ## The goal 51 | 52 | In a full-sized, classic maze, there are 256 cells in a 16x16 square. The goal is one of the four cells in the centre. That is not practical at home so you will probably have a smaller maze and will want to have a goal somewhere that you can reach. in the file `maze.h` you will find a definition for the goal cell location that you can change. just don't forget to set it back to one of the contest cell locations when you run a full contest. More than one contestant has been surprised to find their robot searches for and runs quickly to some place other than the actual goal. 53 | 54 | The goal cell location is given in hexadecimal just to help visualise where it is. A practice goal at 0x22 would be in the third column and third row. For the idle, you could set the practice goal to 0x10 which is the cell to the East of the start cell. then you don't even need to stretch out to collect the robot. 55 | 56 | Contest goal cells are any one of 0x77, 0x78, 0x87, 0x88. 57 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: false 15 | AllowShortIfStatementsOnASingleLine: false 16 | AllowShortLoopsOnASingleLine: false 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: Yes 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: true 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakInheritanceList: BeforeColon 43 | BreakBeforeTernaryOperators: true 44 | BreakConstructorInitializersBeforeComma: false 45 | BreakConstructorInitializers: BeforeColon 46 | BreakAfterJavaFieldAnnotations: false 47 | BreakStringLiterals: true 48 | ColumnLimit: 160 49 | CommentPragmas: '^ IWYU pragma:' 50 | CompactNamespaces: false 51 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 52 | ConstructorInitializerIndentWidth: 4 53 | ContinuationIndentWidth: 4 54 | Cpp11BracedListStyle: true 55 | DerivePointerAlignment: true 56 | DisableFormat: false 57 | ExperimentalAutoDetectBinPacking: false 58 | FixNamespaceComments: true 59 | ForEachMacros: 60 | - foreach 61 | - Q_FOREACH 62 | - BOOST_FOREACH 63 | IncludeBlocks: Preserve 64 | IncludeCategories: 65 | - Regex: '^' 66 | Priority: 2 67 | - Regex: '^<.*\.h>' 68 | Priority: 1 69 | - Regex: '^<.*' 70 | Priority: 2 71 | - Regex: '.*' 72 | Priority: 3 73 | IncludeIsMainRegex: '([-_](test|unittest))?$' 74 | IndentCaseLabels: true 75 | IndentPPDirectives: None 76 | IndentWidth: 2 77 | IndentWrappedFunctionNames: false 78 | JavaScriptQuotes: Leave 79 | JavaScriptWrapImports: true 80 | KeepEmptyLinesAtTheStartOfBlocks: false 81 | MacroBlockBegin: '' 82 | MacroBlockEnd: '' 83 | MaxEmptyLinesToKeep: 1 84 | NamespaceIndentation: None 85 | ObjCBinPackProtocolList: Never 86 | ObjCBlockIndentWidth: 2 87 | ObjCSpaceAfterProperty: false 88 | ObjCSpaceBeforeProtocolList: true 89 | PenaltyBreakAssignment: 2 90 | PenaltyBreakBeforeFirstCallParameter: 1 91 | PenaltyBreakComment: 300 92 | PenaltyBreakFirstLessLess: 120 93 | PenaltyBreakString: 1000 94 | PenaltyBreakTemplateDeclaration: 10 95 | PenaltyExcessCharacter: 1000000 96 | PenaltyReturnTypeOnItsOwnLine: 200 97 | PointerAlignment: Left 98 | RawStringFormats: 99 | - Language: Cpp 100 | Delimiters: 101 | - cc 102 | - CC 103 | - cpp 104 | - Cpp 105 | - CPP 106 | - 'c++' 107 | - 'C++' 108 | CanonicalDelimiter: '' 109 | BasedOnStyle: google 110 | - Language: TextProto 111 | Delimiters: 112 | - pb 113 | - PB 114 | - proto 115 | - PROTO 116 | EnclosingFunctions: 117 | - EqualsProto 118 | - EquivToProto 119 | - PARSE_PARTIAL_TEXT_PROTO 120 | - PARSE_TEST_PROTO 121 | - PARSE_TEXT_PROTO 122 | - ParseTextOrDie 123 | - ParseTextProtoOrDie 124 | CanonicalDelimiter: '' 125 | BasedOnStyle: google 126 | ReflowComments: true 127 | SortIncludes: true 128 | SortUsingDeclarations: true 129 | SpaceAfterCStyleCast: false 130 | SpaceAfterTemplateKeyword: true 131 | SpaceBeforeAssignmentOperators: true 132 | SpaceBeforeCpp11BracedList: false 133 | SpaceBeforeCtorInitializerColon: true 134 | SpaceBeforeInheritanceColon: true 135 | SpaceBeforeParens: ControlStatements 136 | SpaceBeforeRangeBasedForLoopColon: true 137 | SpaceInEmptyParentheses: false 138 | SpacesBeforeTrailingComments: 2 139 | SpacesInAngles: false 140 | SpacesInContainerLiterals: true 141 | SpacesInCStyleCastParentheses: false 142 | SpacesInParentheses: false 143 | SpacesInSquareBrackets: false 144 | Standard: Auto 145 | StatementMacros: 146 | - Q_UNUSED 147 | - QT_REQUIRE_VERSION 148 | TabWidth: 8 149 | UseTab: Never 150 | ... 151 | -------------------------------------------------------------------------------- /documents/steering.md: -------------------------------------------------------------------------------- 1 | # Steering 2 | 3 | However well tuned and reliable your robot might be, it will need some way to use the maze walls and posts to ensure that it is travelling in a nice straight line, properly centered between the walls. Without this, it will be difficult to make sure that turns are accurate an repeatable and, of course, a crash will spoil your day. 4 | 5 | In case you have a line following setup as well as a maze based robot, you should know that the principles described here work just as well for line following. 6 | 7 | UKMARSBOT has two sensors that look at the walls to either side. Either or both those walls may not be present but there will always be at least a post every 180mm in a classic maze. These sensors do not measure the distance to the walls directly. Instead, they measure the amount of light reflected from the walls and use that as an indicator of distance. Why should you care about the distinction? Well, the sensor response if very non-linear. That is, halving the distance does not just double the reflected light, it increases it by much more. That can be a benefit in some ways but, if you wanted to do a full analysis and design of a steering control system, it is a bit of a problem. 8 | 9 | Luckily, the steering problem can e easily solved, in large part, by a very simple approach. 10 | 11 | ## Normalisation and calibration 12 | 13 | Sensor responses are always normalised by calibrating for a given maze. That means that the steering control can always expect to use sensor readings in a known range even if the particular robot has more or less sensitive sensors. The UKMARSBOT code assumes that the sensors have been correctly calibrated and normalised so that the side sensor give a normalised reading of 100 when the robot is correctly positioned between two walls. 14 | 15 | ## Error calculation 16 | 17 | There are two kinds of error that indicate the robot is off course in the maze. An _offset_ error is where the robot is too far to one side or another. A _heading_ error is where the robot is not pointing parallel to the walls. Both these errors give similar responses from the sensors and it is difficult to distinguish one form the other. Fortunately, they are both corrected in the same way - by turning away from the wall that is too close. 18 | 19 | If there is a wall on either side of the robot, all that is needed then is to calculate the difference between the two sensor readings and that will give us our error. In the code, this is referred to as the cross-track-error (CTE). This term is also used for line following and it is just easier to have one simple term to describe these things. 20 | 21 | The calculation is done such that a negative reading indicates that the robot is too close to a wall on the left. When using the normalised sensor readings, the error can be just 22 | 23 | cte = right_sensor - left_sensor; 24 | 25 | If one of the walls is missing then it you jut calculate the error seen by the sensor that has a wall but it must be doubled because only one wall is contributing instead of two. 26 | 27 | In the code, the error for each wall is first calculated and then the overall error is calculated based on which walls are present. 28 | 29 | ## Edges 30 | 31 | As the robot sensors sweep across a wall edge, the response will drop off briefly and the sensors will give the impression that the robot is drifting away from a wall. there is a risk that the controller might then try to 'follow the edge' and deviate from its proper path a little. This is a real problem sometimes and careful tuning of the steering controller may be needed to make sure it is not too disruptive. 32 | 33 | ## Steering control 34 | 35 | Once the robot has a measure of the error, it must have a way to correct its heading to try and get that error to zero. It may be tempting to come up with an y number of elaborate schemes but, for most purposes, the simplest is the best. 36 | 37 | Earlier it was noted that the sensor error can look exactly like an angular error in the robot. That suggests that you can take the sensor error and just add it to the encoder rotation error. The robot controller will see that as a rotation error and attempt to correct it as a part of its normal control mechanism. For proper correction, you will need to decide how much of the sensor error is used for this feedback. Too much and the robot will be twitchy and try to follow every small change in sensor reading. Too little and the robot will be very slow to correct. This is just Proportional control - the P in PID. It may be improved by also adding a Derivative term but you may find it is not needed and that proportional control is enough. Both KP and KD constants are provided in config.h so that you can tune your robot for your needs. 38 | 39 | When tuning the steering constants, try to aim for a robot that will correct modest errors within 1 to 2 cells of travel. Don't make it too aggressive or you can end up with large corrections still under way as you approach a turn and that rarely ends well. 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Release 0.2.0 2 | 3 | This ran successfully on a stock UKMARSBOT in competition on October 7th 2023. It may still contain bugs and you will need to setup and calibrate your own robot before you can expect it to run well. 4 | 5 | It is intended to be a complete, fairly bare-bones micromouse software suite. It should contain all the basic functionality needed to allow a UKMARSBOT or close derivative to explore a classic micromouse maze and search to the center and back to the start. 6 | 7 | There are a couple of calibation routines by way of an example of how you might do the basic sensor calibration. There is also a command line interpreter for those who have a radio serial link such as a bluetooth module. 8 | 9 | 10 | 11 | --- 12 | 13 | # Mazerunner Core 14 | 15 | This repository holds a version of the mazerunner code for a version of UKMARSBOT that can run in a classic micromouse maze. 16 | 17 | The code assumes you have a standard UKMARSBOT assembled, using an Arduino nano with a basic or advanced wall sensor connected. 18 | 19 | All the project code is in the directory `mazerunner-core` 20 | 21 | ### Arduino IDE users 22 | 23 | For the impatient, if you are wanting to run this code in the Arduino IDE then you can download or clone the complete repository and simply open the `mazerunner-core.ino` file. The rduino IDE will be able to build and flash the prject. It will also open all the other files in the `mazerunner-core` directory. That may be surprising to you but it is just trying to be helpful. 24 | 25 | 26 | ### Platform IO users 27 | 28 | The project is maintained and developed using PlatformIO with Visual Studio Code. If you have not used this as a development platform, I highly recommend it as a huge improvement over the very dated Arduino IDE. You can find instructions for installing VSCode and PlatformIO at https://ukmars.org/resources/platformio-vscode-windows/ 29 | 30 | ## Initial Configuration 31 | 32 | Before flashing your robot, look in the config.h file. In here you can place entries that let you maintain your own robot-specific config file containing a number of default settings describing things like the motor gear ratio, wheel diameter and wheel spacing. 33 | 34 | Each physical robot has its own config file. In the main config.h, add an entry for your robot name and create a corresponding config file in the same directory. You can start by copying one of the existing robot config files. Check that these are at least close to the values required for your robot. 35 | 36 | Because it is not possible to know how you wired your motors and encoders, you will also find defaults for the motor and encoder polarity. You will need to first check that moving the wheels forwards by hand increases the corresponding encoder count. If not, change the encoder polarity settings as appropriate. If the robot then moves in the wrong direction or just turns instead of moving forward, adjust the relevant motor polarity settings. 37 | 38 | The config-ukmarsbot file contains things like pin definitions. These will be correct for the UKMARSBOT using a four-emitter sensor board. If you have a custom derivative, this is where you get to assign pins ot functions. 39 | 40 | ## Getting started 41 | 42 | To characterise your robot, you will need a separate test suite. Use code from the main mazerunner repository or the ukmarsbot-test repository or, of course, write your own. At the very least you will need to configure the encoder resolution and do some sensor calibration. 43 | 44 | **The first time you start the robot, make sure that switches are set to zero**. That is, all the switches should be 'down' towards the rear of the robot. Now connect the serial monitor at 115200 baud. When the robot boots, you should see a prompt written to the monitor. 45 | 46 | Enter a qustion mark followed by return to see a summary of the available commands. 47 | 48 | Most of the functions are activated by first pressing the user button and then bringing your hand close to the front sensor and away again. This is a convenient way to start an action without disturbing the robot. Before that will work, you must calibrate the sensor response. The instructions for that, and the other tests are in the README.MD file found in the code folder. 49 | 50 | ## Bluetooth 51 | 52 | You will gain a lot of flexibility by connecting a Bluetooth adaptor such as the HC-05 or HC-06 to the serial port on the front left of the robot. On Versions 1.1 and later, you can simply plug that straight in. The V1.0 boards will need some minor modification. 53 | 54 | Check the default baud rate for your BT adaptor as these can vary. 55 | 56 | 57 | ## Extending the code 58 | 59 | At the time of writing, this code contains an absolute minimal set of code needed to search a classic micromouse maze and then run it using the best route found. There is also a wall-following mode since it is a trivial change to the maze-solver. 60 | 61 | ## Updates 62 | 63 | Be aware that, if you download a newer copy of the code, and simply unpack it into the same folder, you will overwrite your code and your changes will be lost. Don't be like Julian, don't do that. 64 | 65 | ## Contributing 66 | 67 | If you have any thoughts about the code, suggestions for changes or improvements, please use the github issues mechanism so that other users can benefit from your observations. Those other users may also be able to offer assistance if the author(s) are not available. 68 | -------------------------------------------------------------------------------- /documents/systick.md: -------------------------------------------------------------------------------- 1 | # Systick 2 | 3 | The systick function is the beating heart of the robot control software. During operation many tasks must be performed at regular, fixed intervals. These tasks are time-sensitive and they must run frequently for good control of the robot. 4 | 5 | While some tasks could be performed at a lower rate and with reduced priority compared to others, the Arduino nano used in UKMARSBOT is not running a real-time operating system so it is most convenient to run all the periodic tasks at the same rate. 6 | 7 | There is a balance to be struck between wanting to run the tasks as frequently as possible and the risk of using up all the system resources just running regular systick updates. 8 | 9 | In UKMARSBOT, the systick task runs at 500Hz - every 2 milliseconds. 10 | 11 | ## Tasks 12 | 13 | Systick looks after the following: 14 | 15 | * collect encoder counts 16 | * monitor battery voltage 17 | * update motion profiles 18 | * correct steering errors 19 | * update the motor controllers 20 | * provide drive signals to the motors 21 | * update the sensors 22 | 23 | ### Encoder counts 24 | 25 | The motors each have an encoder attached that generates pulses at what can be quite high frequencies when running fast. These pulses generate interrupts that are very simple and execute quickly. All those interrupts do is increment or decrement counters. There is not time for anything more sophisticated. In systick, those counters - one for each wheel - are checked and the values used to work out how far the each wheel has moved since the last systick event. The counts for each wheel are converted into forward motion, in mm, and rotary motion, in degrees. These converted encoder values are used as the feedback inputs to the motor controllers. 26 | 27 | ### Battery voltage 28 | 29 | There are two primary reasons for checking the battery voltage. First, it is important that the battery not be discharged too much. Not only might this damage the battery but the robot control may become unreliable and unpredictable. The other reason is to make it possible to ensure that the motor drive is able to take into account changes in battery voltage. Freshly charged batteries will have a higher voltage that soon drops to a more steady value. Heavy demand on the batteries, such as when accelerating the robot, can also reduce the voltage temporarily. By monitoring the available voltage every systick cycle, the motor drive can be adjusted to compensate for supply changes. 30 | 31 | ### Motion profiles 32 | 33 | Robot movement is governed by generating velocity profiles for forward and rotational motion. These profiles keep track of the robot's commanded speed and position. The profiler software takes into account the acceleration and speed limits and ensures that the robot will reach a set point at exactly the right speed. The current output of the profilers is used as the set point input for the motor controllers. 34 | 35 | ### Steering correction 36 | 37 | Whenever the robot is traveling in a straight line, it is possible to use the values read from the sensors to calculate any errors caused by the robot being offset to one side or caused by the robot having a heading error. Both kinds of error will cause the robot to crash and both look very similar in terms of the values obtained form the sensors. The calculated steering error is called the Cross Track Error (CTE). It is always treated as if it is a heading error. To give a suitable correction to the robot, a separate steering controller uses the CTE to calculate a correction value that is added to the measured encoder angle. From the motor controller's point of view, this steering correction looks like the robot is turning unexpectedly. 38 | 39 | ### Motor controllers 40 | 41 | Once the robot set points, from the profilers, and the actual positions, from the encoders and steering, are known, they can be used as inputs to the motor controllers. There is one controller each for forward and rotational motion. Both these types of motion are profiled and controlled independently so that any combination of the two can be used to generate complex motion. When the controllers have made their calculations, the resulting correction outputs are combined into drive components for each of the wheels and sent to the motors as left and right drive voltages. The battery voltage compensation adjusts the actual PWM duty cycles so that the motors see the commanded voltage even if the battery voltage changes. 42 | 43 | ### Sensor control 44 | 45 | It may seem odd to be testing the sensors at the end of the systick cycle rather than the beginning. The reason is that the ADC conversion times on the ATmega328 chip are particularly slow and if systick had to wait around for all eight channels to convert, twice, is would waste a lot of processor time. Instead, the sensors are sampled using a separate sequence of interrupts. The last thing that happens in systick is that the first ADC conversion is triggered. Each conversion generates an interrupt which lets the code collect the relevant value and start another conversion. In this way, processing time is only used in collecting results, not waiting for conversions to finish. By the time the next systick cycle occurs, all the sensor results have been collected and are ready to use. At most, they are likely to be 1-2ms out of date. For the performance levels of the system, this delay is of no real consequence. 46 | No code must follow the sensor cycle start in systick or it will be interrupted by the sensor conversion interrupts. 47 | -------------------------------------------------------------------------------- /mazerunner-core/config.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef CONFIG_H 13 | #define CONFIG_H 14 | 15 | #include 16 | #define CODE "MAZERUNNER-CORE" 17 | /*** 18 | * The config.h file determines the actual robot variant that 19 | * will be the target for the build. 20 | * 21 | * This files lets you pick a specific robot that has its unique 22 | * configuration stored in that file. In this way, you can write 23 | * generic code that will work with a variety of different actual 24 | * robots. There are a number of example robot files in the project. 25 | * You should pick one that is closest to your setup, copy it and 26 | * then amend the details in that copy. Finally, add or modify 27 | * the selection settings below to use your new robot configuration. 28 | * 29 | */ 30 | 31 | /*************************************************************************/ 32 | #define STRING2(x) #x 33 | #define STRING(x) STRING2(x) 34 | 35 | const float RADIANS_PER_DEGREE = 2 * PI / 360.0; 36 | const float DEGREES_PER_RADIAN = 360.0 / 2 * PI; 37 | 38 | /*** 39 | * Structure definitions used in the software. Declared here for lack of a 40 | * better place because the structure is needed by the robot configs. 41 | * 42 | * Robot specific instances and values are in the robot config file. 43 | * 44 | * TODO: redefine these in terms of offsets from cell centre 45 | * TODO: replace trigger value with sensor identifier 46 | */ 47 | struct TurnParameters { 48 | int speed; // mm/s - constant forward speed during turn 49 | int entry_offset; // mm - distance from turn pivot to turn start 50 | int exit_offset; // mm - distance from turn pivot to turn end 51 | float angle; // deg - total turn angle 52 | float omega; // deg/s - maximum angular velocity 53 | float alpha; // deg/s/s - angular acceleration 54 | int trigger; // - front sensor value at start of turn 55 | }; 56 | 57 | /*************************************************************************/ 58 | /*** 59 | * You may use a slightly different hardware platform than UKMARSBOT 60 | * Here you can include a suitable hardware configuration to define 61 | * things like IO pins, ADC channels and so on 62 | */ 63 | 64 | /// list the hardware platforms you can suport 65 | #define HARDWARE_UNKNOWN 0 66 | #define HARDWARE_UKMARSBOT_1_3A 1 67 | 68 | /// define the choice for this build 69 | #define HARDWARE HARDWARE_UKMARSBOT_1_3A 70 | 71 | /// include the relevant config file 72 | #if HARDWARE == HARDWARE_UKMARSBOT_1_3A 73 | #include "config-ukmarsbot.h" 74 | #else 75 | #error "NO HARDWARE DEFINED" 76 | #endif 77 | 78 | /*************************************************************************/ 79 | /*** 80 | * It is possible that you might want to run the robot in a number of 81 | * different mazes with different calibration values. The config file 82 | * can have different sensor defaults for each of these environments 83 | * so here you can define which set will be used. 84 | */ 85 | /// these are the events that require individual settings 86 | #define EVENT_HOME 1 87 | #define EVENT_UK 2 88 | #define EVENT_PORTUGAL 3 89 | #define EVENT_APEC 4 90 | 91 | // choose the one you will be using BEFORE selecting the robot below 92 | #define EVENT EVENT_HOME 93 | #if EVENT == EVENT_HOME 94 | #define GOAL Location(1, 0) 95 | #else 96 | #define GOAL Location(7, 7) 97 | #endif 98 | // This is the size, in mm, for each cell in the maze. 99 | const float FULL_CELL = 180.0f; 100 | const float HALF_CELL = FULL_CELL / 2.0; 101 | 102 | /*************************************************************************/ 103 | /*** 104 | * Even with the same basic hardware, you may build robots with different 105 | * characteristics such as the motor gear ratio or the wheel size or sensor 106 | * arrangement. 107 | * 108 | * These characteristics are kept in their own config files. Add you robot 109 | * to the list and create a corresponding config file with its custom values. 110 | * 111 | * If you have only one robot then you can reduce this section to a single 112 | * include line. 113 | */ 114 | /// these are the variants that you can run 115 | #define ROBOT_NOT_DEFINED 0 116 | #define ROBOT_CORE_OSMIUM 1 117 | #define ROBOT_ORION 2 118 | 119 | /// this is the variant you are building for. 120 | #define ROBOT ROBOT_ORION 121 | 122 | #if ROBOT == ROBOT_CORE_OSMIUM 123 | #include "config-robot-osmium.h" 124 | #elif ROBOT == ROBOT_ORION 125 | #include "config-robot-orion.h" 126 | #else 127 | #error "NO ROBOT DEFINED" 128 | #endif 129 | 130 | /*************************************************************************/ 131 | /*** 132 | * This piece of magic lets you define a variable, such as the maze, that can 133 | * survive a processor reset. The downside is that you MUST take care to 134 | * properly initialise it when necessary. If you just turn on the robot for 135 | * example, the maze will have random data in it. 136 | * 137 | * CLEAR THE MAZE BEFORE EVERY CONTEST 138 | * 139 | * The mazerunner code clears the maze if the user button is held down during 140 | * a reset. 141 | */ 142 | #define PERSISTENT __attribute__((section(".noinit"))) 143 | 144 | #endif 145 | -------------------------------------------------------------------------------- /documents/sensors.md: -------------------------------------------------------------------------------- 1 | # Sensors 2 | 3 | UKMARSBOT is a multi-purpose robot and is designed so that you can plug in a variety of sensors. For the mazerunner variant, all the code and documentation assumes that the basic wall sensor board is fitted. This has three reflective sensors that use visible or infra-red light emitters and matching phototransistors. One reflective sensor points directly forwards and can measure reflections from a wall ahead. Two more sensor point out and forwards at about 45 degrees to sense walls on the left and right of the robot. The sensors are normally aligned so that they point at the posts when the robot is positioned centrally in a maze cell. 4 | 5 | ## Wall Sensors 6 | 7 | The wall sensors are used to measure the distance to a wall. They are not able to measure the actual distance though. Instead, they measure the amount of light reflected from the wall when the emitter is illuminating the wall. The amount of reflected light is a function of both the distance to the wall and the angle between the wall and the sensor. This relationship is quite complex. Fortunately, for most practical purposes, the software only needs to know that close walls appear brighter and that the response is always bigger as the wall gets closer. 8 | 9 | There are four issues to be aware of that spoil this simple interpretation. 10 | 11 | - First, when the sensor is very close to a wall, the circuit board shields the detector and prevents it from seeing all the reflected light. Thus the apparent brightness will increase sharply when very close to a wall. This is not normally a problem but you should take care when approaching a wall ahead that you do not get too close. 12 | - Second, walls can be shiny and reflect light much more strongly when the illumination is close to perpendicular to a wall. This mostly affects the front sensor which nearly always points directly at right angles to the wall ahead. The consequence of this is that the value seen by the front sensor can fall off much more than you might expect for quite small alignment errors. 13 | - Third, a corner in the maze can confuse the side sensors. The corners form a box reflector and reflect the light strongly and may cause the robot to see readings that indicate that it is closer to a wall than it actually is. 14 | - Finally, and related to the previous issue, because the side sensors are not pointing straight out to the sides, they will get a return from a wall ahead if they are close enough. Not only can that look like there is a wall where none exists, it can cause a serious error in the steering. Returns from a wall ahead will reverse the sense of the steering control and can cause positive feedback. This reveals itself as a robot that approaches a wall ahead and then suddenly veers off to one side - possibly losing control completely. The front sensor can be used to provide a warning that this is about to happen. 15 | 16 | ## Pulsed operation 17 | 18 | The environment in which the robot runs is not always friendly. That is, there may be variable amounts of ambient illumination and, worse, it may be strong _and_ directional. Think sun coming in from a window. If the sensors just measured the light coming from a wall illuminated by the emitter, that reading would change along with the ambient illumination. For this reason, the sensors are always run in pulsed, differential mode. First a reading is taken with the emitter off - that is the 'dark' value and serves to indicate the level of ambient illumination. Then the emitter is turned on and another reading is taken - the 'lit' value. The difference between these two reading (lit-dark) is the reflection causes only by the emitter and is a much more reliable measure of the wall distance. Note that the scheme can still fail if there is so much background light that the detector is nearly saturated even with the emitter off. 19 | 20 | ## Wall presence. 21 | 22 | As well as giving a measure of the distance to a wall, the sensors must indicate whether or not a wall is present. For the side sensors this is reasonably simple. A typical method is to take note of the sensor reading when the robot is correctly positioned and can clearly illuminate a wall on either side. From that, the detection threshold can just be set to 50% of that nominal value. That corresponds to the emitter illumination spot falling half on and half off a wall. If you want to be a little more sophisticated, you can add some hysteresis and/or sample the wall several times to be sure. Note that, for more advanced operations, it is also important to know the _position_ that the robot acquired or lost a wall. 23 | 24 | The front wall can be more tricky. The size of the response from a wall ahead can be quite small until you get rather close. And, of course, small angular errors may change that response quite a lot. The trick is to not try and identify a wall ahead until you are definitely close enough to be sure of doing so reliably. For UKMARSBOT and the suggested sensor components, that may mean you should be no further way than about 200mm. 25 | 26 | ## Calibration 27 | 28 | So that the software can be as general purpose as possible, the sensor readings should be _normalised_. All sensors vary so a reading is taken in a redefined calibration location and that reading is used to automatically adjust the sensor readings so that, for example, the side sensors always give a normalised reading of 100 when the robot is correctly positioned with walls either side. If only normalised readings are used, then you can easily calibrate for different mazes and still have some confidence that the robot will run reliably. 29 | 30 | ## Other analogue inputs 31 | 32 | As well as the thee wall sensors, there are two more analogue inputs used in UKMARSBOT. One of these is connected to the battery supply through a pair of resistors that create a voltage divider. This channel is used to monitor the battery voltage. 33 | 34 | Another channel is connected to a network of resistors and switches so that a single channel can be used to identify 16 different settings of four DIP-switches at the rear of the robot as well as detecting presses of the single pushbutton found adjacent to the DIP switches. 35 | 36 | Special functions in the sensors module return a values that represents the state of these switches. 37 | -------------------------------------------------------------------------------- /mazerunner-core/config-ukmarsbot.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #pragma once 13 | #include 14 | 15 | // hardware configuration for for UKMARSBOT with an Arduino nano processor 16 | 17 | //**** IO CONFIGURATION ****************************************************// 18 | const uint8_t ENCODER_LEFT_CLK = 2; 19 | const uint8_t ENCODER_RIGHT_CLK = 3; 20 | const uint8_t ENCODER_LEFT_B = 4; 21 | const uint8_t ENCODER_RIGHT_B = 5; 22 | const uint8_t USER_IO = 6; 23 | const uint8_t MOTOR_LEFT_DIR = 7; 24 | const uint8_t MOTOR_RIGHT_DIR = 8; 25 | const uint8_t MOTOR_LEFT_PWM = 9; 26 | const uint8_t MOTOR_RIGHT_PWM = 10; 27 | const uint8_t EMITTER_A = 11; 28 | const uint8_t EMITTER_B = 12; 29 | 30 | // the sensor ADC channels in case we have no special use for a given channel 31 | const uint8_t SENSOR_0 = A0; 32 | const uint8_t SENSOR_1 = A1; 33 | const uint8_t SENSOR_2 = A2; 34 | const uint8_t SENSOR_3 = A3; 35 | const uint8_t SENSOR_4 = A4; 36 | const uint8_t SENSOR_5 = A5; 37 | const uint8_t SWITCHES_PIN = A6; 38 | const uint8_t BATTERY_PIN = A7; 39 | 40 | /****************************************************************************** 41 | * The switch input is driven by a resistor chain forming a potential divider. 42 | * These are the measured thresholds if using the specified resistor values. 43 | * 44 | * The adc_thresholds may need adjusting for non-standard resistors. 45 | * Use the adc_reading() method to find the ADC values for each switch 46 | * combination and enter them in this table 47 | */ 48 | const int adc_thesholds[] PROGMEM = {660, 647, 630, 614, 590, 570, 545, 522, 461, 429, 385, 343, 271, 212, 128, 44, 0}; 49 | 50 | /****************************************************************************** 51 | * FAST IO for ATMEGA328 ONLY 52 | * 53 | * There are places in the code (ADC and ENCODERS) where it is important that 54 | * you are able to access IO pins as quickly as possible. Some processor are fast 55 | * enough that this is not a problem. The ATMega328 using the Arduino framework 56 | * is not one of those cases so the macros below translate simple pin IO 57 | * functions into single machine code instructions. 58 | * 59 | * Extracted from digitalWriteFast: 60 | * Optimized digital functions for AVR microcontrollers 61 | * by Watterott electronic (www.watterott.com) 62 | * based on https://code.google.com/p/digitalwritefast 63 | * 64 | * If you are using a different processor, you will either need to reimplement 65 | * these functions or use a suitable built-in function if it is fast enough 66 | */ 67 | #if defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) 68 | #define __digitalPinToPortReg(P) (((P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : &PORTC)) 69 | #define __digitalPinToDDRReg(P) (((P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : &DDRC)) 70 | #define __digitalPinToPINReg(P) (((P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : &PINC)) 71 | #define __digitalPinToBit(P) (((P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P)-8 : (P)-14)) 72 | 73 | // general macros/defines 74 | #if !defined(BIT_READ) 75 | #define BIT_READ(value, bit) ((value) & (1UL << (bit))) 76 | #endif 77 | #if !defined(BIT_SET) 78 | #define BIT_SET(value, bit) ((value) |= (1UL << (bit))) 79 | #endif 80 | #if !defined(BIT_CLEAR) 81 | #define BIT_CLEAR(value, bit) ((value) &= ~(1UL << (bit))) 82 | #endif 83 | #if !defined(BIT_WRITE) 84 | #define BIT_WRITE(value, bit, bitvalue) (bitvalue ? BIT_SET(value, bit) : BIT_CLEAR(value, bit)) 85 | #endif 86 | 87 | #define fast_write_pin(P, V) BIT_WRITE(*__digitalPinToPortReg(P), __digitalPinToBit(P), (V)); 88 | #define fast_read_pin(P) ((int)(((BIT_READ(*__digitalPinToPINReg(P), __digitalPinToBit(P))) ? HIGH : LOW))) 89 | 90 | #else 91 | #define fast_write_pin(P, V) digitalWrite(P, V) 92 | #define fast_read_pin(P) digitalRead(P) 93 | #endif 94 | /****************************************************************************** 95 | * ATOMIC OPERATIONS for ATMEGA328 ONLY 96 | * Since the ATMega328 is an 8 bit processor it is possible that you will end 97 | * up trying to read a multi-byte quantity that is modified in an interrupt while 98 | * you are doing the read or write. The result is a corrupt value. 32 bit processors 99 | * are unlikely to suffer from this since quantities are read in a single operation. 100 | * 101 | * The AVR compiler provides a method for you to disable interrupts for the 102 | * duration of a block of code and then restore the state at the end of the block. 103 | * 104 | * It is not enough to simply turn off interrupts and then turn them back on because 105 | * you need to remember the state of the interrupt enable flag at the start of the 106 | * block. 107 | * 108 | * These macros do this for you and should be either modified for different processors 109 | * or bypassed if needed. 110 | * 111 | * Use like this: 112 | * ATOMIC { 113 | * // code to protect 114 | * } 115 | * 116 | */ 117 | #if defined(__AVR__) 118 | #include 119 | #define ATOMIC ATOMIC_BLOCK(ATOMIC_RESTORESTATE) 120 | #else 121 | #define ATOMIC 122 | #endif 123 | //***************************************************************************// 124 | 125 | /*** 126 | * Finally, a little-known provision of the compiler lets you 127 | * configure the standard printf() function to print directly 128 | * to a serial device. There is a cost overhead if you are not 129 | * using printf() or sprintf() elsewhere but it is a great convenience 130 | * if you want formatted printing to the serial port. 131 | * 132 | * To use this facility add a call to redirectPrintf() early in the 133 | * setup() function of your code. 134 | * 135 | * TODO: this does not work with the NRF52 compiler. 136 | */ 137 | 138 | #if !defined(ARDUINO_ARCH_NRF52840) 139 | // Function that printf and related will use to print 140 | int serial_putchar(char c, FILE *f) { 141 | if (c == '\n') { 142 | // TODO do we need to add carriage returns? I think not. 143 | Serial.write('\r'); 144 | } 145 | return Serial.write(c) == 1 ? 0 : 1; 146 | } 147 | 148 | FILE serial_stdout; 149 | void redirectPrintf() { 150 | // Redirect stdout so that we can use printf() with the console 151 | fdev_setup_stream(&serial_stdout, serial_putchar, NULL, _FDEV_SETUP_WRITE); 152 | stdout = &serial_stdout; 153 | } 154 | #else 155 | void redirectPrintf(){}; 156 | #endif 157 | -------------------------------------------------------------------------------- /mazerunner-core/motion.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef MOTION_H 13 | #define MOTION_H 14 | 15 | #include 16 | #include "motors.h" 17 | #include "profile.h" 18 | 19 | /*** 20 | * 21 | * The motion class handles all the higher level Locomotion tasks. It is responsible 22 | * for converting instructions from a path planner into actual movement of the 23 | * robot. 24 | * 25 | * Motion control needs to know about the dynamics and kinematics of the robot 26 | * and may need environmental data as well. In particular, the battery voltage and 27 | * characteristics of the motors will be important. 28 | * 29 | * The output from the motion controller should be suitable control signals for 30 | * the actuators that make the robot move. In UKMARSBOT, that will simply be 31 | * voltages for each of the two drive motors. Other robots may have more motors, 32 | * an independent steering system or other kinds of effectors such as grippers or 33 | * weapons. Remember that the motors may be stepper motors which will need a 34 | * slightly modified approach. 35 | * 36 | * Here, the motion control makes use of simple trapezoidal profiles to calculate 37 | * continuously varying speeds for linear and rotary motion components. 38 | * 39 | * How these speeds are converted into individual control signals for the drive 40 | * system is the job of the Motors class. Thus is should be relatively easy to 41 | * change that class to suit different drive systems. 42 | * 43 | * TODO: Motion should have be given the profilers 44 | * TODO: should it also be given the motors? 45 | */ 46 | class Motion { 47 | public: 48 | /** 49 | * Before the robot begins a sequence of moves, this method can be used to 50 | * make sure everything starts off in a known state. 51 | * 52 | * @brief Reset profiles, counters and controllers. Motors off. Steering off. 53 | */ 54 | void reset_drive_system() { 55 | motors.stop(); 56 | motors.disable_controllers(); 57 | encoders.reset(); 58 | forward.reset(); 59 | rotation.reset(); 60 | motors.reset_controllers(); 61 | motors.enable_controllers(); 62 | } 63 | 64 | void stop() { 65 | motors.stop(); 66 | } 67 | 68 | void disable_drive() { 69 | motors.disable_controllers(); 70 | } 71 | 72 | float position() { 73 | return forward.position(); 74 | } 75 | 76 | float velocity() { 77 | return forward.speed(); 78 | } 79 | 80 | float acceleration() { 81 | return forward.acceleration(); 82 | } 83 | 84 | void set_target_velocity(float velocity) { 85 | forward.set_target_speed(velocity); 86 | } 87 | 88 | float angle() { 89 | return rotation.position(); 90 | } 91 | 92 | float omega() { 93 | return rotation.speed(); 94 | } 95 | 96 | float alpha() { 97 | return rotation.acceleration(); 98 | } 99 | 100 | void start_move(float distance, float top_speed, float final_speed, float acceleration) { 101 | forward.start(distance, top_speed, final_speed, acceleration); 102 | } 103 | 104 | bool move_finished() { 105 | return forward.is_finished(); 106 | } 107 | 108 | void move(float distance, float top_speed, float final_speed, float acceleration) { 109 | forward.move(distance, top_speed, final_speed, acceleration); 110 | } 111 | 112 | void start_turn(float distance, float top_speed, float final_speed, float acceleration) { 113 | rotation.start(distance, top_speed, final_speed, acceleration); 114 | } 115 | 116 | bool turn_finished() { 117 | return rotation.is_finished(); 118 | } 119 | 120 | void turn(float distance, float top_speed, float final_speed, float acceleration) { 121 | rotation.move(distance, top_speed, final_speed, acceleration); 122 | } 123 | 124 | void update() { 125 | forward.update(); 126 | rotation.update(); 127 | } 128 | 129 | void set_position(float pos) { 130 | forward.set_position(pos); 131 | } 132 | 133 | void adjust_forward_position(float delta) { 134 | forward.adjust_position(delta); 135 | } 136 | 137 | //***************************************************************************// 138 | 139 | /** 140 | * Performs a spin turn. 141 | * 142 | * The function is given three parameters 143 | * 144 | * - angle : positive is a left turn (deg) 145 | * - omega : angular velocity of middle phase (deg/s) 146 | * - alpha : angular acceleration of in/out phases (deg/s/s) 147 | * 148 | * @brief execute an arbitrary in-place 149 | */ 150 | void spin_turn(float angle, float omega, float alpha) { 151 | forward.set_target_speed(0); 152 | while (forward.speed() != 0) { 153 | delay(2); 154 | } 155 | rotation.reset(); 156 | rotation.move(angle, omega, 0, alpha); 157 | }; 158 | 159 | //***************************************************************************// 160 | /** 161 | * These are examples of ways to use the motion control functions 162 | */ 163 | 164 | /** 165 | * The robot is assumed to be moving. This call will stop at a specific 166 | * distance. Clearly, there must be enough distance remaining for it to 167 | * brake to a halt. 168 | * 169 | * The current values for speed and acceleration are used. 170 | * 171 | * Calling this with the robot stationary is undefined. Don't do that. 172 | * 173 | * @brief bring the robot to a halt at a specific distance 174 | */ 175 | void stop_at(float position) { 176 | float remaining = position - forward.position(); 177 | forward.move(remaining, forward.speed(), 0, forward.acceleration()); 178 | } 179 | 180 | /** 181 | * The robot is assumed to be moving. This call will stop after a 182 | * specific distance has been travelled 183 | * 184 | * Clearly, there must be enough distance remaining for it to 185 | * brake to a halt. 186 | * 187 | * The current values for speed and acceleration are used. 188 | * 189 | * Calling this with the robot stationary is undefined. Don't do that. 190 | * 191 | * @brief bring the robot to a halt after a specific distance 192 | */ 193 | void stop_after(float distance) { 194 | forward.move(distance, forward.speed(), 0, forward.acceleration()); 195 | } 196 | 197 | /** 198 | * The robot is assumed to be moving. This utility function call will just 199 | * do a busy-wait until the forward profile gets to the supplied position. 200 | * 201 | * @brief wait until the given position is reached 202 | */ 203 | void wait_until_position(float position) { 204 | while (forward.position() < position) { 205 | delay(2); 206 | } 207 | } 208 | 209 | /** 210 | * The robot is assumed to be moving. This utility function call will just 211 | * do a busy-wait until the forward profile has moved by the given distance. 212 | * 213 | * @brief wait until the given distance has been travelled 214 | */ 215 | void wait_until_distance(float distance) { 216 | float target = forward.position() + distance; 217 | wait_until_position(target); 218 | } 219 | }; 220 | 221 | extern Motion motion; 222 | 223 | #endif 224 | -------------------------------------------------------------------------------- /documents/reporting.md: -------------------------------------------------------------------------------- 1 | # Reporting 2 | 3 | The UKMARSBOT controller is an Arduino nan. That means it has a built-in serial to USB bridge that makes communication with a host computer quite simple. Having the robot tethered with a bulky USB cable is less convenient. Even so there are some calibration tests - the sensor calibration in particular - that really benefit from the ability to display results on a host computer. 4 | The UKMARSBOT controller is an Arduino nan. That means it has a built-in serial to USB bridge that makes communication with a host computer quite simple. Having the robot tethered with a bulky USB cable is less convenient. Even so there are some calibration tests - the sensor calibration in particular - that really benefit from the ability to display results on a host computer. 5 | 6 | For moving tests, it is convenient to attach a Bluetooth Classic adaptor like the HC-05 and view telemetry remotely. 7 | For moving tests, it is convenient to attach a Bluetooth Classic adaptor like the HC-05 and view telemetry remotely. 8 | 9 | Several reporting functions are provided in the code as examples of the kinds of information that can be sent. Sometimes, a basic one-line response is all that is needed. At other times a stream of data from sensors or motion control variables is desirable. Data that is streamed out can be recorded at the host and displayed in tools like Excel to give a better understanding of the robot behaviour. 10 | Several reporting functions are provided in the code as examples of the kinds of information that can be sent. Sometimes, a basic one-line response is all that is needed. At other times a stream of data from sensors or motion control variables is desirable. Data that is streamed out can be recorded at the host and displayed in tools like Excel to give a better understanding of the robot behaviour. 11 | 12 | Look at the sample reports in reports.cpp to get some ideas of what might be possible. 13 | 14 | If you would rather not have the robot stream data out during tests, there is a flag in config.h that lets you disable the reporting without having to edit the test code. 15 | 16 | ## Custom reports 17 | 18 | When writing your own reporting functions you might like to bear in mind a number of points. 19 | 20 | ### Serial buffering 21 | 22 | The Arduino HardwareSerial object has an output buffer of 64 characters. All serial output comes from this buffer and when you use statements like `Serial.print(42);` the characters are actually placed in the buffer and the buffer is transferred one character at a time under interrupt control. All this means that you can send up to 64 characters in one go with very little impact on the timing of your code but only so long as the buffer is empty when you begin to send. 23 | 24 | ### Serial baud rates 25 | 26 | The default baud rate for the serial port in this code is 115200. That equates to a maximum of roughly 10,000 characters per second. At that speed, it would take the Arduino nearly 6.5ms to send out the entire 64 character buffer. It should be clear that trying to send longer lines of telemetry any faster than about 100 lines per second is likely to fill up the transmit buffer and characters will be lost. For many requirements, shorter lines, sent less frequently are going to be adequate. 27 | The default baud rate for the serial port in this code is 115200. That equates to a maximum of roughly 10,000 characters per second. At that speed, it would take the Arduino nearly 6.5ms to send out the entire 64 character buffer. It should be clear that trying to send longer lines of telemetry any faster than about 100 lines per second is likely to fill up the transmit buffer and characters will be lost. For many requirements, shorter lines, sent less frequently are going to be adequate. 28 | 29 | If, on the other hand, you are stuck with a BT module running at a default 9600 baud, how much telemetry can you manage? At 9600 baud, characters are sent at about 1000 characters per second. You might be able to send a 64 character buffer in 64ms or 15 lines per second. Not too shabby but that is risking data loss. For lower data rates, it pays to be mindful of how often you send telemetry data and how much you send. 30 | If, on the other hand, you are stuck with a BT module running at a default 9600 baud, how much telemetry can you manage? At 9600 baud, characters are sent at about 1000 characters per second. You might be able to send a 64 character buffer in 64ms or 15 lines per second. Not too shabby but that is risking data loss. For lower data rates, it pays to be mindful of how often you send telemetry data and how much you send. 31 | 32 | ### Regular reporting 33 | 34 | For some purposes, it is fine just to send out new data when you have it. If all you want to know is the battery voltage, for example, there is probably no great urgency. Quite often though, you want to be sending data out at very regular intervals. For example, you might want to know the battery voltage at specific instants so that you can gauge the effect of high speed and acceleration on the battery drain. Or you might want to observe the sensor response as you pass a maze post at speed. 35 | For some purposes, it is fine just to send out new data when you have it. If all you want to know is the battery voltage, for example, there is probably no great urgency. Quite often though, you want to be sending data out at very regular intervals. For example, you might want to know the battery voltage at specific instants so that you can gauge the effect of high speed and acceleration on the battery drain. Or you might want to observe the sensor response as you pass a maze post at speed. 36 | 37 | In these cases, getting timely reports at fixed intervals is important. It is common to see code like this in Arduino programs: 38 | 39 | ``` 40 | void loop() { 41 | Serial.println(data); 42 | delay(10); 43 | } 44 | ``` 45 | 46 | Presumably, the intention is to send out data every 10ms but what happens as you add more code to the loop? Now your delay will get longer. Or what if you are sending data out while waiting for some other task to complete? You cannot then just do nothing for 10ms - the other code must continue to run. 47 | 48 | A better technique is illustrated in many of the reporting functions in the code. You will see that reports have a fixed interval and that the reporting code is called as often as possible. Instead of just writing out data, the function checks to see if the interval has passed and, if it has, a new report time is calculated and then the data is sent. This method is much more reliable than a simple delay. 49 | A better technique is illustrated in many of the reporting functions in the code. You will see that reports have a fixed interval and that the reporting code is called as often as possible. Instead of just writing out data, the function checks to see if the interval has passed and, if it has, a new report time is calculated and then the data is sent. This method is much more reliable than a simple delay. 50 | 51 | ### Reporting by distance 52 | 53 | Although this method is not used in the current code, you might like to consider a variation on reporting that is distance based rather than time based. Normally data is sent out at fixed time intervals. That makes interpretation in charts reasonably simple to understand. Sometimes though the key feature is _distance_. For example, if you are looking at the response of the sensors as the robot passes through a cell, it might be better to have the reporting function look not at the elapsed _time_ since the last report but at the elapsed _distance_ since the last report. Move slowly and sent out new data every few mm of travel. 54 | Although this method is not used in the current code, you might like to consider a variation on reporting that is distance based rather than time based. Normally data is sent out at fixed time intervals. That makes interpretation in charts reasonably simple to understand. Sometimes though the key feature is _distance_. For example, if you are looking at the response of the sensors as the robot passes through a cell, it might be better to have the reporting function look not at the elapsed _time_ since the last report but at the elapsed _distance_ since the last report. Move slowly and sent out new data every few mm of travel. 55 | 56 | Alternatively, make sure that your time-based report has a column with the current robot position and plot the data in Excel using the position as the x-axis instead of time. 57 | Alternatively, make sure that your time-based report has a column with the current robot position and plot the data in Excel using the position as the x-axis instead of time. 58 | -------------------------------------------------------------------------------- /mazerunner-core/profile.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef PROFILE_H 13 | #define PROFILE_H 14 | 15 | #include 16 | #include "config.h" 17 | //***************************************************************************// 18 | class Profile; 19 | 20 | extern Profile forward; 21 | extern Profile rotation; 22 | 23 | /*** 24 | * The Profile class manages speed as a function of time or distance. A profile 25 | * is trapezoidal in shape and consists of up to three phases. 26 | * 27 | * - Acceleration : changing from the initial speed to the maximum speed 28 | * - Coasting : moving at the maximum speed 29 | * - Braking : changing to the final speed 30 | * 31 | * The names are only really relevant to a subset of all possible profiles since 32 | * the coasting section may not be possible if the acceleration is low or the 33 | * maximum speed is otherwise unreachable. Also, note that the initial speed may 34 | * be higher than the maximum speed so the first phase actually needs braking. 35 | * 36 | * Once started, the profile must be regularly updated from the system control 37 | * loop and this must be done before the motor control task since it uses the 38 | * current profile speed in its calculation. 39 | * 40 | * The robot has two instances of a Profile - one for forward motion and one 41 | * for rotation. These are completely independent but the combination of the 42 | * two allows the robot full freedom of motion within the constraints imposed 43 | * by the drive arrangement. That is, any combination of forward and rotary 44 | * motion is possible at all times. 45 | * 46 | * Although the units in the comments are shown as mm, the class is unit 47 | * agnostic and you can interpret the units as mm, cm, deg, bananas or anything 48 | * else. Just be consistent when using speed and acceleration. 49 | */ 50 | class Profile { 51 | public: 52 | enum State : uint8_t { 53 | PS_IDLE = 0, 54 | PS_ACCELERATING = 1, 55 | PS_BRAKING = 2, 56 | PS_FINISHED = 3, 57 | }; 58 | 59 | void reset() { 60 | ATOMIC { 61 | m_position = 0; 62 | m_speed = 0; 63 | m_target_speed = 0; 64 | m_state = PS_IDLE; 65 | } 66 | } 67 | 68 | bool is_finished() { 69 | return m_state == PS_FINISHED; 70 | } 71 | 72 | /// @brief Begin a profile. Once started, it will automatically run to completion 73 | /// Subsequent calls before completion supercede all the parameters. 74 | /// Called may monitor progress using is_finished() method 75 | /// @param distance (mm) always positive 76 | /// @param top_speed (mm/s) negative values move the robot in reverse 77 | /// @param final_speed (mm/s) 78 | /// @param acceleration (mm/s/s) 79 | 80 | void start(float distance, float top_speed, float final_speed, float acceleration) { 81 | m_sign = (distance < 0) ? -1 : +1; 82 | if (distance < 0) { 83 | distance = -distance; 84 | } 85 | if (distance < 1.0) { 86 | m_state = PS_FINISHED; 87 | return; 88 | } 89 | if (final_speed > top_speed) { 90 | final_speed = top_speed; 91 | } 92 | 93 | m_position = 0; 94 | m_final_position = distance; 95 | m_target_speed = m_sign * fabsf(top_speed); 96 | m_final_speed = m_sign * fabsf(final_speed); 97 | m_acceleration = fabsf(acceleration); 98 | if (m_acceleration >= 1) { 99 | m_one_over_acc = 1.0f / m_acceleration; 100 | } else { 101 | m_one_over_acc = 1.0; 102 | } 103 | m_state = PS_ACCELERATING; 104 | } 105 | 106 | // Start a profile and wait for it to finish. This is a blocking call. 107 | void move(float distance, float top_speed, float final_speed, float acceleration) { 108 | start(distance, top_speed, final_speed, acceleration); 109 | wait_until_finished(); 110 | } 111 | 112 | /// @brief causes the profile to immediately terminate with the speed to zero 113 | /// note that even when the state is PS_FINISHED, the profiler will 114 | /// continue to try and reach the target speed. (zero in this case) 115 | void stop() { 116 | ATOMIC { 117 | m_target_speed = 0; 118 | } 119 | finish(); 120 | } 121 | 122 | /// @brief Force a profile to finish with the target speed set to the final speed 123 | void finish() { 124 | ATOMIC { 125 | m_speed = m_target_speed; 126 | m_state = PS_FINISHED; 127 | } 128 | } 129 | 130 | void wait_until_finished() { 131 | while (m_state != PS_FINISHED) { 132 | delay(2); 133 | } 134 | } 135 | 136 | void set_state(State state) { 137 | m_state = state; 138 | } 139 | 140 | /// @brief Calculate the distance needed to get to the final speed from the 141 | /// current speed using the current acceleration. 142 | /// @return distance (mm) 143 | float get_braking_distance() { 144 | return fabsf(m_speed * m_speed - m_final_speed * m_final_speed) * 0.5 * m_one_over_acc; 145 | } 146 | 147 | /// @brief gets the distance travelled (mm) since the last call to start(). If there 148 | /// was a prior call to set_position() distance is incremented from there. 149 | /// @return distance travelled (mm) 150 | float position() { 151 | float pos; 152 | ATOMIC { 153 | pos = m_position; 154 | } 155 | return pos; 156 | } 157 | 158 | /// @brief Get the current speed 159 | /// @return 160 | float speed() { 161 | float speed; 162 | ATOMIC { 163 | speed = m_speed; 164 | } 165 | return speed; 166 | } 167 | 168 | float acceleration() { 169 | float acc; 170 | ATOMIC { 171 | acc = m_acceleration; 172 | } 173 | return acc; 174 | } 175 | 176 | void set_speed(float speed) { 177 | ATOMIC { 178 | m_speed = speed; 179 | } 180 | } 181 | void set_target_speed(float speed) { 182 | ATOMIC { 183 | m_target_speed = speed; 184 | } 185 | } 186 | 187 | // normally only used to alter position for forward error correction 188 | void adjust_position(float adjustment) { 189 | ATOMIC { 190 | m_position += adjustment; 191 | } 192 | } 193 | 194 | void set_position(float position) { 195 | ATOMIC { 196 | m_position = position; 197 | } 198 | } 199 | 200 | // update is called from within systick and should be safe from interrupts 201 | void update() { 202 | if (m_state == PS_IDLE) { 203 | return; 204 | } 205 | float delta_v = m_acceleration * LOOP_INTERVAL; 206 | float remaining = fabsf(m_final_position) - fabsf(m_position); 207 | if (m_state == PS_ACCELERATING) { 208 | if (remaining < get_braking_distance()) { 209 | m_state = PS_BRAKING; 210 | if (m_final_speed == 0) { 211 | // The magic number I normally set to be the same, numerically, as the current 212 | // acceleration. It is just a small velicity that ensures that the motion continues 213 | // past the finish point in case floating point rounding prevents that happening. 214 | // It is a nasty hack I keep meaning to find a more tidy solution for. 215 | m_target_speed = m_sign * 5.0f; // magic number to make sure we reach zero 216 | } else { 217 | m_target_speed = m_final_speed; 218 | }; 219 | } 220 | } 221 | // try to reach the target speed 222 | if (m_speed < m_target_speed) { 223 | m_speed += delta_v; 224 | if (m_speed > m_target_speed) { 225 | m_speed = m_target_speed; 226 | } 227 | } 228 | if (m_speed > m_target_speed) { 229 | m_speed -= delta_v; 230 | if (m_speed < m_target_speed) { 231 | m_speed = m_target_speed; 232 | } 233 | } 234 | // increment the position 235 | m_position += m_speed * LOOP_INTERVAL; 236 | // The number is a hack to ensure floating point rounding errors do not prevent the 237 | // loop termination. The units are mm and independent of the encoder resolution. 238 | // I figure that being within 1/8 of a mm will be close enough. 239 | if (m_state != PS_FINISHED && remaining < 0.125) { 240 | m_state = PS_FINISHED; 241 | m_target_speed = m_final_speed; 242 | } 243 | } 244 | 245 | private: 246 | volatile uint8_t m_state = PS_IDLE; 247 | volatile float m_speed = 0; 248 | volatile float m_position = 0; 249 | int8_t m_sign = 1; 250 | float m_acceleration = 0; 251 | float m_one_over_acc = 1; 252 | float m_target_speed = 0; 253 | float m_final_speed = 0; 254 | float m_final_position = 0; 255 | }; 256 | 257 | #endif -------------------------------------------------------------------------------- /mazerunner-core/adc.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef ADC_H 13 | #define ADC_H 14 | 15 | #include 16 | #include 17 | #include "config.h" 18 | 19 | /*** 20 | * The AnalogueConverter class samples a fixed number of ADC channels from 0 to 21 | * MAX_CHANNELS and makes the readings available to the rest of the program. 22 | * 23 | * Each channel is sampled once with the sensor emitters off and once with the emitters on. 24 | * 25 | * The first set of samples is stored in the array m_adc_dark[]. 26 | * 27 | * The second set, with the emitter on, is stored in m_adc_lit[]. 28 | * 29 | * For wall sensors, you should use the get_raw() method to read the (lit-dark) values. 30 | * 31 | * The class does not care what is connected to the adc channel. It just gathers readings. 32 | * 33 | * The battery monitor and analogue button channels are also converted here to avoid conflict. 34 | * Your config file should declare the channel number for these as well as the actual sensors. 35 | * 36 | * This class is specific to UKMARSBOT with a ATmega328p processor. If you are using any other 37 | * processor, you will need to re-write this class. 38 | * 39 | * To save overhead, the class works by using the conversion complete interrupt to gather 40 | * results only after a successful conversion. The entire sequence is begun by enabling the 41 | * appropriate interrupt and then starting an ADC conversion. Each interrupt action takes 42 | * about 5us and conversions take about 30us. Since each channel gets converted twice, the 43 | * entire sequence takes about 620us to complete but only uses about 100us of processor time. 44 | * 45 | * Even if the emitters are disabled, the conversions are still performed but the lit 46 | * values and dark values will be very similar. 47 | * 48 | * Although the code exists almost entirely in this header file, C++ requires the actual 49 | * instance of the class and its interrupt service routine to be somewhere in a .cpp file. 50 | * I have placed the instances in the main project file. 51 | * 52 | * TODO: some space and time could be saved by only storing the difference between the 53 | * lit and dark values but it complicates matters elsewhere if code tries to read the 54 | * values in the middle of the interrupt sequence. If you have all the channels read 55 | * in one go in a single interrupt service routine - or in systick - then that will not 56 | * be a problem. 57 | * 58 | * TODO: The inclusion in the class of information about the emitters is unfortunate but this 59 | * is the simplest scheme I could envisage. If there are two emitters, both are turned on 60 | * together in this version. It would be better to use different emitters for each sensor type. 61 | * that is left as an exercise for the reader. 62 | * 63 | * The code assumes the use of the advanced wall sensor board where there are two emitters. It 64 | * will work just as well with the basic wall sensor if you simply use the same pin name for 65 | * both emitter entries. 66 | * 67 | * TODO: The A4 and A5 channels get converted in the sequence. If you are expecting to use 68 | * these for an I2C device, they will need to be skipped. 69 | * 70 | * TODO: If only four sensor channels are used, there are opportunities to optimise this 71 | * code if you are so inclined. 72 | * 73 | * PORTING: A simulator may provide fake values as it sees fit and without the delays 74 | * or interrupts 75 | * 76 | * 77 | */ 78 | 79 | class AnalogueConverter; 80 | 81 | extern AnalogueConverter adc; 82 | 83 | enum Phase { DARK_READ, EMITTER_ON, SETTLE, LIT_READ, COMPLETE }; 84 | 85 | class AnalogueConverter { 86 | public: 87 | enum { 88 | MAX_CHANNELS = 8, 89 | }; 90 | 91 | void enable_emitters() { 92 | m_emitters_enabled = true; 93 | } 94 | 95 | void disable_emitters() { 96 | m_emitters_enabled = false; 97 | } 98 | 99 | // call this or nothing will work 100 | virtual void begin() { 101 | disable_emitters(); 102 | set_front_emitter_pin(EMITTER_FRONT); 103 | set_side_emitter_pin(EMITTER_DIAGONAL); 104 | converter_init(); 105 | m_configured = true; 106 | } 107 | 108 | void set_front_emitter_pin(uint8_t pin) { 109 | pinMode(pin, OUTPUT); 110 | m_emitter_front_pin = pin; 111 | } 112 | 113 | void set_side_emitter_pin(uint8_t pin) { 114 | pinMode(pin, OUTPUT); 115 | m_emitter_diagonal_pin = pin; 116 | } 117 | 118 | uint8_t emitter_front() { 119 | return m_emitter_front_pin; 120 | } 121 | uint8_t emitter_diagonal() { 122 | return m_emitter_diagonal_pin; 123 | } 124 | 125 | void converter_init() { 126 | // Change the clock prescaler from 128 to 32 for a 500kHz clock 127 | bitSet(ADCSRA, ADPS2); 128 | bitClear(ADCSRA, ADPS1); 129 | bitSet(ADCSRA, ADPS0); 130 | // Set the reference to AVcc and right adjust the result 131 | ADMUX = DEFAULT << 6; 132 | } 133 | 134 | void start_conversion_cycle() { 135 | if (not m_configured) { 136 | return; 137 | } 138 | 139 | m_phase = DARK_READ; // sync up the start of the sensor sequence 140 | m_channel = 0; 141 | bitSet(ADCSRA, ADIE); // enable the ADC interrupt 142 | start_conversion(m_channel); // begin a conversion to get things started 143 | } 144 | 145 | void end_conversion_cycle() { 146 | bitClear(ADCSRA, ADIE); // disable the ADC interrupt 147 | } 148 | 149 | void start_conversion(uint8_t channel) { 150 | ADMUX = (ADMUX & 0xF0) | (channel & 0x0F); // select the channel 151 | sbi(ADCSRA, ADSC); // start the conversion 152 | } 153 | 154 | int get_adc_result() { 155 | return ADC; 156 | } 157 | 158 | int get_lit(const int i) const { 159 | return m_adc_lit[i]; 160 | } 161 | 162 | int get_dark(const int i) const { 163 | return m_adc_dark[i]; 164 | } 165 | 166 | int get_raw(const int i) const { 167 | int diff; 168 | ATOMIC { 169 | diff = max(1, m_adc_lit[i] - m_adc_dark[i]); 170 | } 171 | return diff; 172 | } 173 | 174 | /// Perform a 'manual' conversion of a channel 175 | /// should not be used if the interrupt-driven sequencer is on 176 | int do_conversion(uint8_t channel) { 177 | start_conversion(channel); 178 | while (ADCSRA & (1 << ADSC)) { 179 | // do nothing 180 | } 181 | return get_adc_result(); 182 | } 183 | 184 | void callback_adc_isr() { 185 | switch (m_phase) { 186 | case DARK_READ: 187 | // cycle through all 8 channels with emitters off 188 | m_adc_dark[m_channel] = get_adc_result(); 189 | m_channel++; 190 | start_conversion(m_channel); 191 | if (m_channel >= MAX_CHANNELS) { 192 | m_phase = EMITTER_ON; 193 | } 194 | break; 195 | case EMITTER_ON: 196 | get_adc_result(); // dummy read to clear interrupt flag 197 | if (m_emitters_enabled) { 198 | digitalWrite(emitter_diagonal(), 1); 199 | digitalWrite(emitter_front(), 1); 200 | } 201 | m_channel = 0; 202 | start_conversion(m_channel); // Start a conversion to generate the interrupt 203 | m_phase = SETTLE; 204 | break; 205 | case SETTLE: 206 | // skip one cycle for the detectors to respond 207 | get_adc_result(); // dummy read clears the interrupt flag 208 | start_conversion(m_channel); 209 | m_phase = LIT_READ; 210 | break; 211 | case LIT_READ: 212 | // cycle through the channels again with the emitters on 213 | // avoid zero result so we know it is working 214 | m_adc_lit[m_channel] = max(1, get_adc_result()); 215 | m_channel++; 216 | start_conversion(m_channel); 217 | if (m_channel >= MAX_CHANNELS) { 218 | m_phase = COMPLETE; 219 | } 220 | break; 221 | case COMPLETE: 222 | default: 223 | get_adc_result(); // dummy read clears the interrupt flag 224 | // unconditionally turn off emitters for safety 225 | digitalWrite(emitter_diagonal(), 0); 226 | digitalWrite(emitter_front(), 0); 227 | bitClear(ADCSRA, ADIE); // turn off the interrupt 228 | break; 229 | } 230 | } 231 | 232 | private: 233 | volatile int m_adc_dark[MAX_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; 234 | volatile int m_adc_lit[MAX_CHANNELS]{0, 0, 0, 0, 0, 0, 0, 0}; 235 | uint8_t m_emitter_front_pin = -1; 236 | uint8_t m_emitter_diagonal_pin = -1; 237 | uint8_t m_index = 0; 238 | bool m_emitters_enabled = false; 239 | bool m_configured = false; 240 | uint8_t m_phase = 0; // used in the isr 241 | uint8_t m_channel = 0; 242 | }; 243 | 244 | #endif -------------------------------------------------------------------------------- /mazerunner-core/encoders.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include "config.h" 17 | 18 | /******************************************************************************* 19 | * 20 | * The encoders provide data for localisation of the robot. That is, the encoder 21 | * data is used to determine the position and orientation of the robot in its 22 | * environment. If it had one, the robot might also combine encoder data with 23 | * information form an IMU (gyroscope and accelerometer) to get a more accurate 24 | * and stable measure of pose (position + orientation). 25 | * 26 | * Encoders are subject to various errors and are not reliable when used alone. 27 | * In particular, simple robots like UKMARSBOt are likely to have low resolution 28 | * encoders and considerable backlash in the geartrain. 29 | * 30 | * ****************************************************************************/ 31 | 32 | // TODO: consider a single Encoder class with objects for each wheel. 33 | // Then a Localisation class would get two of these (and possibly 34 | // an IMU) to do the actual localisation. 35 | 36 | class Encoders; 37 | 38 | extern Encoders encoders; // defined in main file to keep them all together 39 | 40 | // Forward declaration of the callbacks ... 41 | // ... which cannot be defined until the Encoder class is complete. 42 | // tacky! 43 | void callback_left_encoder_isr(); 44 | void callback_right_encoder_isr(); 45 | 46 | class Encoders { 47 | public: 48 | Encoders() { 49 | reset(); 50 | }; 51 | void begin() { 52 | pinMode(ENCODER_LEFT_CLK, INPUT); 53 | pinMode(ENCODER_LEFT_B, INPUT); 54 | pinMode(ENCODER_RIGHT_CLK, INPUT); 55 | pinMode(ENCODER_RIGHT_B, INPUT); 56 | attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT_CLK), callback_left_encoder_isr, CHANGE); 57 | attachInterrupt(digitalPinToInterrupt(ENCODER_RIGHT_CLK), callback_right_encoder_isr, CHANGE); 58 | reset(); 59 | } 60 | 61 | void reset() { 62 | ATOMIC { 63 | m_left_counter = 0; 64 | m_right_counter = 0; 65 | m_robot_distance = 0; 66 | m_robot_angle = 0; 67 | m_total_right = 0; 68 | m_total_left = 0; 69 | } 70 | } 71 | 72 | /** 73 | * For the ATmega328 and ATmega4809: 74 | * Measurements indicate that even at 1500mm/s the total load due to 75 | * the encoder interrupts is less than 3% of the available bandwidth. 76 | * The ISR will respond to the XOR-ed pulse train from the encoder 77 | * Runs in constant time of around 3us per interrupt. 78 | * Would be faster with direct port access 79 | * 80 | * A more generic solution where the pin names are not constants would be slower 81 | * unless we can make their definition known at compile time. 82 | * 83 | * Note that the decoding method used here assumes that one of the channels 84 | * is a frequency-doubled clock signal. It is not decoding a 'normal' 85 | * quadrature input. 86 | */ 87 | void left_input_change() { 88 | static volatile bool oldA = false; 89 | static volatile bool oldB = false; 90 | // bool newB = digitalReadFast(ENCODER_LEFT_B); 91 | bool newB = fast_read_pin(ENCODER_LEFT_B); 92 | bool newA = fast_read_pin(ENCODER_LEFT_CLK) ^ newB; 93 | int delta = ENCODER_LEFT_POLARITY * ((oldA ^ newB) - (newA ^ oldB)); 94 | m_left_counter += delta; 95 | oldA = newA; 96 | oldB = newB; 97 | } 98 | 99 | void right_input_change() { 100 | static volatile bool oldA = false; 101 | static volatile bool oldB = false; 102 | bool newB = fast_read_pin(ENCODER_RIGHT_B); 103 | bool newA = fast_read_pin(ENCODER_RIGHT_CLK) ^ newB; 104 | int delta = ENCODER_RIGHT_POLARITY * ((oldA ^ newB) - (newA ^ oldB)); 105 | m_right_counter += delta; 106 | oldA = newA; 107 | oldB = newB; 108 | } 109 | 110 | /** 111 | * @brief update the robot speeds and positions from the encoders 112 | * 113 | * The update method is called during each control cycle from the 114 | * systick event. It will use the change in encoder value since the 115 | * last call to update values for the current speed, angular velocity, 116 | * distance travelled and robot angle. 117 | * 118 | * The recorded speeds are going to show quite a lot of noise when 119 | * the encoder resolution is poor. Positions effectively integrate out 120 | * a fair bit of the noise. 121 | * 122 | * The speeds are not recorded directly, only the changes in encoder 123 | * readings. This slightly reduces the computational load in the 124 | * systick event and the low-level controllers are only using the 125 | * changes in robot position and angle, not the speed directly. 126 | * If you need to see a value for the current speed or angular 127 | * velocity in real units, use the robot_speed() and robot_omega() 128 | * methods. 129 | * 130 | * Because update() is called from an interrupt service routine it 131 | * will be changing the values in ways that the higher level code may 132 | * not be able to know about. Always use the methods provided that 133 | * guard against unpredictable changes. 134 | * 135 | * If using an IMU, prefer that for measurement of angular velocity 136 | * and angle. 137 | */ 138 | void update() { 139 | int left_delta = 0; 140 | int right_delta = 0; 141 | // Make sure values don't change while being read. Be quick. 142 | ATOMIC { 143 | left_delta = m_left_counter; 144 | right_delta = m_right_counter; 145 | m_left_counter = 0; 146 | m_right_counter = 0; 147 | } 148 | m_total_left += left_delta; 149 | m_total_right += right_delta; 150 | float left_change = left_delta * MM_PER_COUNT_LEFT; 151 | float right_change = right_delta * MM_PER_COUNT_RIGHT; 152 | m_fwd_change = 0.5 * (right_change + left_change); 153 | m_robot_distance += m_fwd_change; 154 | m_rot_change = (right_change - left_change) * DEG_PER_MM_DIFFERENCE; 155 | m_robot_angle += m_rot_change; 156 | } 157 | 158 | /** 159 | * These convenience methods provide safe access to the recorded values 160 | * from the encoders. 161 | * 162 | * The ATOMIC_BLOCK guards ensure that the values canot change while 163 | * they are being retreived and are needed because the update() method 164 | * is called from an interrupt service routine. The guard block will 165 | * temporarily disable any other interrupts for the duration of the 166 | * block 167 | * 168 | * On 32 bit processors, they would not be needed and their use in this 169 | * code is sometimes not needed. However, the guards do not significantly 170 | * affect performance. They insert only three machine instructions per 171 | * guard block and the compiler will almost certainly inline the method 172 | * calls so there will not even be a function call overhead. 173 | */ 174 | float robot_distance() { 175 | float distance; 176 | ATOMIC { 177 | distance = m_robot_distance; 178 | } 179 | return distance; 180 | } 181 | 182 | float robot_speed() { 183 | float speed; 184 | ATOMIC { 185 | speed = LOOP_FREQUENCY * m_fwd_change; 186 | } 187 | return speed; 188 | } 189 | 190 | float robot_omega() { 191 | float omega; 192 | ATOMIC { 193 | omega = LOOP_FREQUENCY * m_rot_change; 194 | } 195 | return omega; 196 | } 197 | 198 | float robot_fwd_change() { 199 | float distance; 200 | ATOMIC { 201 | ; 202 | distance = m_fwd_change; 203 | } 204 | return distance; 205 | } 206 | 207 | float robot_rot_change() { 208 | float distance; 209 | ATOMIC { 210 | distance = m_rot_change; 211 | } 212 | return distance; 213 | } 214 | 215 | float robot_angle() { 216 | float angle; 217 | ATOMIC { 218 | angle = m_robot_angle; 219 | } 220 | return angle; 221 | } 222 | 223 | // None of the variables in this class should be directly available to the rest 224 | // of the code without a guard to ensure atomic access 225 | private: 226 | volatile float m_robot_distance; 227 | volatile float m_robot_angle; 228 | // the change in distance or angle in the last tick. 229 | float m_fwd_change = 0; 230 | float m_rot_change = 0; 231 | // internal use only to track encoder input edges 232 | int m_left_counter = 0; 233 | int m_right_counter = 0; 234 | 235 | public: 236 | /// These will overflow on a long straight but they are only used for short test runs 237 | /// so that is fine 238 | int m_total_right = 0; 239 | int m_total_left = 0; 240 | }; 241 | 242 | // A bit of indirection for convenience because the encoder instance is 243 | // unknown until the linker has done its thing 244 | // This is ugly 245 | inline void callback_left_encoder_isr() { 246 | encoders.left_input_change(); 247 | } 248 | 249 | inline void callback_right_encoder_isr() { 250 | encoders.right_input_change(); 251 | } -------------------------------------------------------------------------------- /mazerunner-core/sensors.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef SENSORS_H 13 | #define SENSORS_H 14 | 15 | #include 16 | #include 17 | #include "adc.h" 18 | #include "config.h" 19 | 20 | /** 21 | * 22 | * The Sensors class uses the data gathered by the AnalogueConverter to 23 | * provide basic sensor functions for UKMARSBOT. The sensors class tells 24 | * the robot about its environment both external and internal. In the 25 | * case of a micromouse the wall sensors are the primary means of 26 | * perception. For a line follower, there will be line and marker sensors. 27 | * 28 | * Internally, the sensors monitor battery voltage 29 | * 30 | * You must call begin() before using the sensors. 31 | * You must call update() in every control cycle. From systick() normally. 32 | * 33 | * Normally, each of the four sensors (two side and two front) would get 34 | * values for the corresponding channel by calling the AnalogueConverter's 35 | * get_raw() method. That will return the difference between the lit and 36 | * dark readings for the channel. Thus a close wall will give a larger 37 | * value and a distant wall will give a small value. The get_raw() 38 | * method will always return a value that is at least 1. 39 | * 40 | * The sensor are used in many tasks so look in the code for the various 41 | * convenience methods provided. 42 | * 43 | * TODO: it is not clear to me that all these methods should be in this class 44 | * 45 | * The normal response of the sensor is very non-linear and the gain - that is 46 | * the change in value for a small shange in distance - will vary greatly 47 | * as the distance changes. 48 | * 49 | * A linearisation function is provided in the get_distance() method. This 50 | * uses an approximation that is based on the inverse square root of the 51 | * sensor reading. You will need to supply a scaling constant that is 52 | * typically in the range 500-1500 and will be dependent on the 53 | * characteristics of the sensors you use. Do your own calibration and 54 | * define a constant in the robot config file. This method is not tied 55 | * to any sensor in particular. Just give it the calibration constant and 56 | * the sensor value. You can, for example, use it to linearise the fron 57 | * sum. 58 | * 59 | * The linearised value is limited to 200mm because the signal to noise of 60 | * small sensor readings is poor and you are unlikely to be able to reliably 61 | * measure distance out that far with the standard sensors. 62 | * 63 | * The linearity is pretty good for most sensors until you get to within 64 | * about 20mm. It is, however, almost always monotonic within 5mm or a target 65 | * so it is always possible to use the value to adjust, for example, forward 66 | * distance from a wall ahead. 67 | * 68 | */ 69 | 70 | enum { 71 | STEER_NORMAL, 72 | STEER_LEFT_WALL, 73 | STEER_RIGHT_WALL, 74 | STEERING_OFF, 75 | }; 76 | 77 | // used in the wait_for_user_start_function to indicate which sensor was occluded 78 | const uint8_t NO_START = 0; 79 | const uint8_t LEFT_START = 1; 80 | const uint8_t RIGHT_START = 2; 81 | 82 | //***************************************************************************// 83 | struct SensorChannel { 84 | int raw; // whatever the ADC gives us 85 | int value; // normalised to 100 at reference position 86 | }; 87 | 88 | class Sensors; 89 | extern Sensors sensors; 90 | 91 | class Sensors { 92 | public: 93 | /*** wall sensor variables ***/ 94 | 95 | volatile SensorChannel lfs; 96 | volatile SensorChannel lss; 97 | volatile SensorChannel rss; 98 | volatile SensorChannel rfs; 99 | 100 | volatile bool see_front_wall; 101 | volatile bool see_left_wall; 102 | volatile bool see_right_wall; 103 | 104 | public: 105 | /*** steering variables ***/ 106 | uint8_t g_steering_mode = STEER_NORMAL; 107 | // these are functions in case you want to modify them to 108 | // perform corrections or other scaling 109 | int get_front_sum() { 110 | return int(m_front_sum); 111 | } 112 | int get_front_diff() { 113 | return int(m_front_diff); 114 | } 115 | float get_steering_feedback() { 116 | return m_steering_adjustment; 117 | } 118 | float get_cross_track_error() { 119 | return m_cross_track_error; 120 | } 121 | 122 | //***************************************************************************// 123 | 124 | /** 125 | * The steering adjustment is an angular error that is added to the 126 | * current encoder angle so that the robot can be kept central in 127 | * a maze cell. 128 | * 129 | * A PD controller is used to generate the adjustment and the two constants 130 | * will need to be adjusted for the best response. You may find that only 131 | * the P term is needed 132 | * 133 | * The steering adjustment is limited to prevent over-correction. You should 134 | * experiment with that as well. 135 | * 136 | * @brief Calculate the steering adjustment from the cross-track error. 137 | * @param error calculated from wall sensors, Negative if too far right 138 | * @return steering adjustment in degrees 139 | * 140 | * TODO: It is not clear that this belongs here rather them for example, 141 | * in a Robot class. 142 | */ 143 | float calculate_steering_adjustment() { 144 | // always calculate the adjustment for testing. It may not get used. 145 | float pTerm = STEERING_KP * m_cross_track_error; 146 | float dTerm = STEERING_KD * (m_cross_track_error - m_last_steering_error); 147 | float adjustment = pTerm + dTerm * LOOP_FREQUENCY; 148 | adjustment = constrain(adjustment, -STEERING_ADJUST_LIMIT, STEERING_ADJUST_LIMIT); 149 | m_last_steering_error = m_cross_track_error; 150 | m_steering_adjustment = adjustment; 151 | return adjustment; 152 | } 153 | 154 | void set_steering_mode(uint8_t mode) { 155 | m_last_steering_error = m_cross_track_error; 156 | m_steering_adjustment = 0; 157 | g_steering_mode = mode; 158 | } 159 | 160 | /*************************************************************************** 161 | * This is just a way of letting the emitters turn on for the second read. 162 | * ADC Conversions happen anyway. 163 | * with the sensors disabled, you will probably get zero for the raw value on 164 | * all sensor channels (0..5). 165 | */ 166 | 167 | void enable() { 168 | adc.enable_emitters(); 169 | m_active = true; 170 | } 171 | 172 | void disable() { 173 | adc.disable_emitters(); 174 | m_active = false; 175 | } 176 | 177 | /*************************************************************************** 178 | * square roots and divisions are pretty slow. Don't call this from systick() 179 | * If you really want to use this in the control loop, consider pre-calculating 180 | * a table of k/sqrt(i) for some suitable range of i. The table would need to be 181 | * stored in flash. 182 | */ 183 | float get_distance(float sensor_value, float k) { 184 | float distance = k / sqrtf(sensor_value); 185 | return min(200, distance); 186 | } 187 | 188 | /*********************************** Wall tracking **************************/ 189 | // calculate the alignment errors - too far left is negative 190 | 191 | /*** 192 | * Note: Runs from the systick interrupt. DO NOT call this directly. 193 | * @brief update the global wall sensor values. 194 | * @return robot cross-track-error. Too far left is negative. 195 | */ 196 | void update() { 197 | // digitalWriteFast(LED_USER, 1); 198 | 199 | if (not m_active) { 200 | // NOTE: No values will be updated although the ADC is working 201 | m_cross_track_error = 0; 202 | m_steering_adjustment = 0; 203 | return; 204 | } 205 | 206 | // This should be the only place that the actual ADC channels are referenced 207 | // if there is only a single front sensor (Basic sensor board) then the value is 208 | // just used twice 209 | // Keep these values for calibration assistance 210 | // they should never be negative 211 | rfs.raw = adc.get_raw(RFS_ADC_CHANNEL); 212 | rss.raw = adc.get_raw(RSS_ADC_CHANNEL); 213 | lss.raw = adc.get_raw(LSS_ADC_CHANNEL); 214 | lfs.raw = adc.get_raw(LFS_ADC_CHANNEL); 215 | 216 | // this is the value that is normally used for wall detection and steering 217 | // yes, it would be faster to do integer arithmetic here 218 | lfs.value = FRONT_LEFT_SCALE * lfs.raw; 219 | lss.value = LEFT_SCALE * lss.raw; 220 | rss.value = RIGHT_SCALE * rss.raw; 221 | rfs.value = FRONT_RIGHT_SCALE * rfs.raw; 222 | 223 | // set the wall detection flags 224 | // you could calculate these on-demand in the higher level code 225 | see_left_wall = lss.value > LEFT_THRESHOLD; 226 | see_right_wall = rss.value > RIGHT_THRESHOLD; 227 | m_front_sum = lfs.value + rfs.value; 228 | m_front_diff = lfs.value - rfs.value; 229 | see_front_wall = m_front_sum > FRONT_THRESHOLD; 230 | 231 | // calculate the alignment errors - too far left is negative 232 | int error = 0; 233 | int right_error = SIDE_NOMINAL - rss.value; 234 | int left_error = SIDE_NOMINAL - lss.value; 235 | if (g_steering_mode == STEER_NORMAL) { 236 | if (sensors.see_left_wall && sensors.see_right_wall) { 237 | if (lss.value > rss.value) { 238 | error = 2 * left_error; 239 | } else { 240 | error = -2 * right_error; 241 | } 242 | // error = left_error - right_error; 243 | } else if (sensors.see_left_wall) { 244 | error = 2 * left_error; 245 | } else if (sensors.see_right_wall) { 246 | error = -2 * right_error; 247 | } 248 | } else if (g_steering_mode == STEER_LEFT_WALL) { 249 | error = 2 * left_error; 250 | } else if (g_steering_mode == STEER_RIGHT_WALL) { 251 | error = -2 * right_error; 252 | } 253 | 254 | if (m_front_sum > FRONT_WALL_RELIABILITY_LIMIT) { 255 | error = 0; 256 | } 257 | m_cross_track_error = error; 258 | calculate_steering_adjustment(); 259 | } 260 | 261 | //***************************************************************************// 262 | 263 | // Convenience functions for the sensors when used as UI components 264 | // such as when starting the robot by putting your hand in front 265 | 266 | bool occluded_left() { 267 | return lfs.raw > 100 && sensors.rfs.raw < 100; 268 | } 269 | 270 | bool occluded_right() { 271 | return lfs.raw < 100 && sensors.rfs.raw > 100; 272 | } 273 | 274 | /** 275 | * The sensors will be enabled and this function will return 276 | * values indicating which of the two front sensors was occluded by the 277 | * user. While waiting, the user LED is flashing rapidly to show that it 278 | * expects some action. The LED will go out when the user is detected. 279 | * 280 | * There is no timeout - the method will wait forever if necessary. 281 | * 282 | * There is a 250ms delay before returning to let the user get clear. 283 | * 284 | * Leaves the sensors disabled. 285 | */ 286 | uint8_t wait_for_user_start() { 287 | int state = 0; 288 | digitalWrite(LED_USER, 1); 289 | enable(); 290 | uint8_t choice = NO_START; 291 | while (choice == NO_START) { 292 | int count = 0; 293 | while (occluded_left()) { 294 | count++; 295 | delay(20); 296 | } 297 | if (count > 5) { 298 | choice = LEFT_START; 299 | break; 300 | } 301 | count = 0; 302 | while (occluded_right()) { 303 | count++; 304 | delay(20); 305 | } 306 | if (count > 5) { 307 | choice = RIGHT_START; 308 | break; 309 | } 310 | digitalWrite(LED_USER, state); 311 | state = 1 - state; 312 | delay(25); 313 | } 314 | disable(); 315 | digitalWrite(LED_USER, 0); 316 | delay(250); 317 | return choice; 318 | } 319 | 320 | private: 321 | float m_last_steering_error = 0; 322 | volatile bool m_active = false; 323 | volatile float m_cross_track_error = 0; 324 | volatile float m_steering_adjustment = 0; 325 | volatile int m_front_sum = 0; 326 | volatile int m_front_diff = 0; 327 | }; 328 | 329 | #endif -------------------------------------------------------------------------------- /mazerunner-core/motors.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef MOTORS_H 13 | #define MOTORS_H 14 | 15 | #include 16 | #include "battery.h" 17 | #include "config.h" 18 | #include "encoders.h" 19 | 20 | /*** 21 | * The Motors class is provided with two main control signals - the forward 22 | * and rotary speeds. A third input come from the steering correction mechanism 23 | * used normally when the robot is tracking a wall or line. That input could also 24 | * come from a trajectory tracker or a target seeker. 25 | * 26 | * UKMARSBOT uses DC motors and, to get best performance with least user effort, 27 | * a combination of feedforward and feedbackcontrollers is used. While this can 28 | * seem complex, the user need not be aware of all the details so long as the 29 | * appropriate system characterisation is done to provide suitable values for the 30 | * various system constants. 31 | * 32 | * Under the hood, there are a pair of position controllers which have their set 33 | * points continuously updated by the desired speeds. Odometry provides feedback 34 | * for these controllers. greater reliability and precision would be possible if 35 | * the odometry had better resolution and if an IMU were available. But, you can 36 | * get remarkably good results with the limited resources available. 37 | */ 38 | 39 | class Motors; 40 | 41 | extern Motors motors; 42 | 43 | class Motors { 44 | public: 45 | /*** 46 | * TODO: the constructor should really get at least the hardware pins 47 | * to do a safe setup. 48 | */ 49 | void enable_controllers() { 50 | m_controller_output_enabled = true; 51 | } 52 | 53 | void disable_controllers() { 54 | m_controller_output_enabled = false; 55 | } 56 | 57 | void reset_controllers() { 58 | m_old_left_speed = 0; 59 | m_old_right_speed = 0; 60 | m_fwd_error = 0; 61 | m_rot_error = 0; 62 | m_previous_fwd_error = 0; 63 | m_previous_rot_error = 0; 64 | } 65 | 66 | void stop() { 67 | set_left_motor_volts(0); 68 | set_right_motor_volts(0); 69 | } 70 | 71 | void begin() { 72 | pinMode(MOTOR_LEFT_DIR, OUTPUT); 73 | pinMode(MOTOR_RIGHT_DIR, OUTPUT); 74 | pinMode(MOTOR_LEFT_PWM, OUTPUT); 75 | pinMode(MOTOR_RIGHT_PWM, OUTPUT); 76 | digitalWrite(MOTOR_LEFT_PWM, 0); 77 | digitalWrite(MOTOR_LEFT_DIR, 0); 78 | digitalWrite(MOTOR_RIGHT_PWM, 0); 79 | digitalWrite(MOTOR_RIGHT_DIR, 0); 80 | set_pwm_frequency(); 81 | stop(); 82 | } 83 | 84 | /** 85 | * At each iteration of the main control loop we need to calculate 86 | * now outputs form the two position controllers - one for forward 87 | * motion, one for rotation. 88 | * 89 | * The current error increases by an amount determined by the speed 90 | * and the control loop interval. 91 | * 92 | * It is then decreased by the amount the robot has actually moved 93 | * in the previous loop interval. 94 | * 95 | * These changes are both done in the first line. 96 | * 97 | * After that we have a simple PD contoller. 98 | * 99 | * NOTE: the D-term constant is premultiplied in the config by the 100 | * loop frequency to save a little time. 101 | */ 102 | float position_controller() { 103 | float increment = m_velocity * LOOP_INTERVAL; 104 | m_fwd_error += increment - encoders.robot_fwd_change(); 105 | float diff = m_fwd_error - m_previous_fwd_error; 106 | m_previous_fwd_error = m_fwd_error; 107 | float output = FWD_KP * m_fwd_error + FWD_KD * diff; 108 | return output; 109 | } 110 | 111 | /** 112 | * The rotation controller is exactly like the forward controller 113 | * except that there is an additional error term from the steering. 114 | * All steering correction is done by treating the error term as an 115 | * angular velocity. Depending on your method of analysis, you might 116 | * also try feeding the steering corection back as an angular 117 | * acceleration. 118 | * 119 | * If you have a gyro, you can use the output from that instead of 120 | * the encoders. 121 | * 122 | * A separate controller calculates the steering adjustment term. 123 | */ 124 | float angle_controller(float steering_adjustment) { 125 | float increment = m_omega * LOOP_INTERVAL; 126 | m_rot_error += increment - encoders.robot_rot_change(); 127 | m_rot_error += steering_adjustment * LOOP_INTERVAL; 128 | float diff = m_rot_error - m_previous_rot_error; 129 | m_previous_rot_error = m_rot_error; 130 | float output = ROT_KP * m_rot_error + ROT_KD * diff; 131 | return output; 132 | } 133 | 134 | /** 135 | * Feed forward attempts to work out what voltage the motors would need 136 | * to run at the current speed and acceleration. 137 | * 138 | * Without this, the controller has a lot of work to do and will be 139 | * much harder to tune for good performance. 140 | * 141 | * The drive train is not symmetric and there is significant stiction. 142 | * If used with PID, a simpler, single value will be sufficient. 143 | * 144 | */ 145 | 146 | float leftFeedForward(float speed) { 147 | float leftFF = speed * SPEED_FF; 148 | if (speed > 0) { 149 | leftFF += BIAS_FF; 150 | } else if (speed < 0) { 151 | leftFF -= BIAS_FF; 152 | } else { 153 | // No bias when the speed is 0 154 | } 155 | float acc = (speed - m_old_left_speed) * LOOP_FREQUENCY; 156 | m_old_left_speed = speed; 157 | float accFF = ACC_FF * acc; 158 | leftFF += accFF; 159 | return leftFF; 160 | } 161 | 162 | float rightFeedForward(float speed) { 163 | float rightFF = speed * SPEED_FF; 164 | if (speed > 0) { 165 | rightFF += BIAS_FF; 166 | } else if (speed < 0) { 167 | rightFF -= BIAS_FF; 168 | } else { 169 | // No bias when the speed is 0 170 | } 171 | float acc = (speed - m_old_right_speed) * LOOP_FREQUENCY; 172 | m_old_right_speed = speed; 173 | float accFF = ACC_FF * acc; 174 | rightFF += accFF; 175 | return rightFF; 176 | } 177 | 178 | /** 179 | * Calculate the outputs of the feedback and feedforward controllers 180 | * for both forward and rotation, and combine them to obtain drive 181 | * voltages for the left and right motors. 182 | */ 183 | void update_controllers(float velocity, float omega, float steering_adjustment) { 184 | m_velocity = velocity; 185 | m_omega = omega; 186 | float pos_output = position_controller(); 187 | float rot_output = angle_controller(steering_adjustment); 188 | float left_output = 0; 189 | float right_output = 0; 190 | left_output = pos_output - rot_output; 191 | right_output = pos_output + rot_output; 192 | 193 | float tangent_speed = m_omega * MOUSE_RADIUS * RADIANS_PER_DEGREE; 194 | float left_speed = m_velocity - tangent_speed; 195 | float right_speed = m_velocity + tangent_speed; 196 | float left_ff = leftFeedForward(left_speed); 197 | float right_ff = rightFeedForward(right_speed); 198 | if (m_feedforward_enabled) { 199 | left_output += left_ff; 200 | right_output += right_ff; 201 | } 202 | if (m_controller_output_enabled) { 203 | set_right_motor_volts(right_output); 204 | set_left_motor_volts(left_output); 205 | } 206 | } 207 | 208 | /** 209 | * Once the motor voltages have been calculated, they need to be converted 210 | * into suitable PWM values for the motor drivers. 211 | * 212 | * In this section, the calculations for that are done, taking into account 213 | * the available battery voltage and the limits of the PWM hardware. 214 | * 215 | * If there is not enough voltage available from the battery, the output 216 | * will just saturate and the motor will not get up to speed. 217 | * 218 | * Some people add code to light up an LED whenever the drive output is 219 | * saturated. 220 | */ 221 | int pwm_compensated(float desired_voltage, float battery_voltage) { 222 | int pwm = MOTOR_MAX_PWM * desired_voltage / battery_voltage; 223 | return pwm; 224 | } 225 | 226 | void set_left_motor_volts(float volts) { 227 | volts = constrain(volts, -MAX_MOTOR_VOLTS, MAX_MOTOR_VOLTS); 228 | m_left_motor_volts = volts; 229 | int motorPWM = pwm_compensated(volts, battery.voltage()); 230 | set_left_motor_pwm(motorPWM); 231 | } 232 | 233 | void set_right_motor_volts(float volts) { 234 | volts = constrain(volts, -MAX_MOTOR_VOLTS, MAX_MOTOR_VOLTS); 235 | m_right_motor_volts = volts; 236 | int motorPWM = pwm_compensated(volts, battery.voltage()); 237 | set_right_motor_pwm(motorPWM); 238 | } 239 | 240 | /*** 241 | * PWM values are constrained to +/- 255 since the default for 242 | * analogueWrite is 8 bits. The sign is only used to determine 243 | * the direction. 244 | * 245 | * NOTE: it might be wise to check the resolution of the 246 | * analogueWrite function in other targtes 247 | */ 248 | // TODO: HARDWARE DEPENDENCY 249 | void set_left_motor_pwm(int pwm) { 250 | pwm = MOTOR_LEFT_POLARITY * constrain(pwm, -MOTOR_MAX_PWM, MOTOR_MAX_PWM); 251 | if (pwm < 0) { 252 | fast_write_pin(MOTOR_LEFT_DIR, 1); 253 | analogWrite(MOTOR_LEFT_PWM, -pwm); 254 | } else { 255 | fast_write_pin(MOTOR_LEFT_DIR, 0); 256 | analogWrite(MOTOR_LEFT_PWM, pwm); 257 | } 258 | } 259 | // TODO: HARDWARE DEPENDENCY 260 | void set_right_motor_pwm(int pwm) { 261 | pwm = MOTOR_RIGHT_POLARITY * constrain(pwm, -MOTOR_MAX_PWM, MOTOR_MAX_PWM); 262 | if (pwm < 0) { 263 | fast_write_pin(MOTOR_RIGHT_DIR, 1); 264 | analogWrite(MOTOR_RIGHT_PWM, -pwm); 265 | } else { 266 | fast_write_pin(MOTOR_RIGHT_DIR, 0); 267 | analogWrite(MOTOR_RIGHT_PWM, pwm); 268 | } 269 | } 270 | 271 | /** 272 | * Choosing the best PWM frequency for your motors depends on a lot of factors. 273 | * 274 | * For most cases, just pick the highest frequency that you can get. For the 275 | * comfort of you and any audience, pick a frequency that is outside the normal 276 | * range of human hearing. 277 | * 278 | * Extremely high frequencies can result in losses in the motor drive. That is not 279 | * going to be a problem with the standard UKMARSBOT. 280 | * 281 | */ 282 | // TODO: HARDWARE DEPENDENCY 283 | enum { PWM_488_HZ, PWM_977_HZ, PWM_3906_HZ, PWM_31250_HZ }; 284 | void set_pwm_frequency(int frequency = PWM_31250_HZ) { 285 | switch (frequency) { 286 | case PWM_31250_HZ: 287 | // Divide by 1. frequency = 31.25 kHz; 288 | bitClear(TCCR1B, CS11); 289 | bitSet(TCCR1B, CS10); 290 | break; 291 | case PWM_3906_HZ: 292 | // Divide by 8. frequency = 3.91 kHz; 293 | bitSet(TCCR1B, CS11); 294 | bitClear(TCCR1B, CS10); 295 | break; 296 | case PWM_488_HZ: 297 | default: 298 | // Divide by 64. frequency = 488Hz; 299 | bitSet(TCCR1B, CS11); 300 | bitSet(TCCR1B, CS10); 301 | break; 302 | } 303 | } 304 | 305 | /** 306 | * These getters are used for logging and debugging. 307 | */ 308 | int get_fwd_millivolts() { 309 | return 1000 * (get_right_motor_volts() + get_left_motor_volts()); 310 | } 311 | 312 | int get_rot_millivolts() { 313 | return 1000 * (get_right_motor_volts() - get_left_motor_volts()); 314 | } 315 | 316 | float get_left_motor_volts() { 317 | float volts = 0; 318 | ATOMIC { 319 | volts = m_left_motor_volts; 320 | } 321 | return volts; 322 | } 323 | 324 | float get_right_motor_volts() { 325 | float volts = 0; 326 | ATOMIC { 327 | volts = m_right_motor_volts; 328 | } 329 | return volts; 330 | } 331 | 332 | void set_speeds(float velocity, float omega) { 333 | ATOMIC { 334 | m_velocity = velocity; 335 | m_omega = omega; 336 | } 337 | } 338 | 339 | private: 340 | bool m_controller_output_enabled = true; 341 | bool m_feedforward_enabled = true; 342 | float m_previous_fwd_error = 0; 343 | float m_previous_rot_error = 0; 344 | float m_fwd_error = 0; 345 | float m_rot_error = 0; 346 | float m_velocity = 0; 347 | float m_omega = 0; 348 | float m_old_left_speed = 0; 349 | float m_old_right_speed = 0; 350 | // these are maintained only for logging 351 | float m_left_motor_volts = 0; 352 | float m_right_motor_volts = 0; 353 | }; 354 | 355 | #endif 356 | -------------------------------------------------------------------------------- /mazerunner-core/config-robot-osmium.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | #pragma once 12 | 13 | #include 14 | 15 | /***************************************************************************** 16 | * 17 | * OSMIUM is a specific implementation of UKMARSBOT CORE used for testing 18 | * 19 | * It uses a wall sensor board with four emitter-detector pairs. Gearmotors 20 | * with 20:1 ratio gearboxes and custom encoder discs with 6 magnets in each. 21 | * 22 | * The sensors consist of SFH4550 emitters and SFH309FA detectors. 23 | * 24 | *****************************************************************************/ 25 | #define NAME "OSMIUM CORE" 26 | 27 | //***** SENSOR CALIBRATION **************************************************// 28 | /** 29 | RAW side sensor values when robot is centred in a cell and no wall ahead 30 | 31 | ## ## 32 | || || 33 | || || 34 | || ______ || 35 | || / \ || 36 | || | | || 37 | || ||| ||| || 38 | || | | || 39 | || |_______| || 40 | ##============================## 41 | 42 | RAW values for the front sensor when the robot is backed up to a wall 43 | 44 | ##=============================## 45 | 46 | 47 | ______ 48 | / \ 49 | | | 50 | ||| ||| 51 | | | 52 | |_______| 53 | ##=============================## 54 | */ 55 | 56 | #if EVENT == EVENT_HOME 57 | // wall sensor thresholds and constants 58 | // RAW values for the front sensor when the robot is backed up to a wall 59 | // with another wall ahead 60 | const int FRONT_LEFT_CALIBRATION = 85; 61 | const int FRONT_RIGHT_CALIBRATION = 90; 62 | // RAW values for the side sensors when the robot is centered in a cell 63 | // and there is no wall ahead 64 | const int LEFT_CALIBRATION = 110; 65 | const int RIGHT_CALIBRATION = 80; 66 | 67 | // The front linear constant is the value of k needed to make the function 68 | // sensors.get_distance(sensor,k) return 68 when the mouse is backed up 69 | // against a wall with only a wall ahead 70 | const int FRONT_LINEAR_CONSTANT = 1030; 71 | const int FRONT_REFERENCE = 850; // reading when mouse centered with wall ahead 72 | 73 | // SS90E turn thresholds. This is the front sum reading to trigger a turn 74 | // it changes a bit if there is an adjacent wall. The threshold is set for 75 | // when the robot is 20mm past the cell boundary. That is, the distance 76 | // from the front of the mouse to the wall ahead is 92mm 77 | const int TURN_THRESHOLD_SS90E = 82; 78 | const int EXTRA_WALL_ADJUST = 5; 79 | 80 | #elif EVENT == EVENT_UK 81 | // RAW values for the front sensor when the robot is backed up to a wall 82 | const int FRONT_LEFT_CALIBRATION = 77; 83 | const int FRONT_RIGHT_CALIBRATION = 34; 84 | // RAW side sensor values when robot is centred in a cell and no wall ahead 85 | const int LEFT_CALIBRATION = 75; 86 | const int RIGHT_CALIBRATION = 72; 87 | 88 | // The front linear constant is the value of k needed to make the function 89 | // sensors.get_distance(sensor,k) return 68 when the mouse is backed up 90 | // against a wall with only a wall ahead 91 | const int FRONT_LINEAR_CONSTANT = 1030; 92 | const int FRONT_REFERENCE = 1286; // reading when mouse centered with wall ahead 93 | 94 | // SS90E turn thresholds. This is the front sum reading to trigger a turn 95 | // it changes a bit if there is an adjacent wall. The threshold is set for 96 | // when the robot is 20mm past the threshold. 97 | const int TURN_THRESHOLD_SS90E = 110; 98 | const int EXTRA_WALL_ADJUST = 6; 99 | 100 | #endif 101 | 102 | //***** IO PINS *****************************************************// 103 | // the BASIC sensor board has two LEDs 104 | // const int LED_LEFT = USER_IO; 105 | // const int LED_RIGHT = EMITTER_A; 106 | // const int LED_USER = USER_IO; 107 | // but only one emitter pin 108 | // const int EMITTER_FRONT = EMITTER_B; 109 | // const int EMITTER_DIAGONAL = EMITTER_B; 110 | 111 | // the ADVANCED sensor board has only one LED so use the value twice 112 | const int LED_LEFT = USER_IO; 113 | const int LED_RIGHT = USER_IO; 114 | const int LED_USER = USER_IO; 115 | // but two emitter pins 116 | const int EMITTER_FRONT = EMITTER_A; 117 | const int EMITTER_DIAGONAL = EMITTER_B; 118 | 119 | //***** SENSOR HARDWARE *****************************************************// 120 | // the ADC channels corresponding to the sensor inputs. There are 8 available 121 | // Channels 0..3 are normally used for sensors. 122 | // Channels 4 and 5 are available if you do not want to add an I2C device 123 | // Channel 6 is pre-allocated to the Battery monitor 124 | // Channel 7 is re-allocated to the function switch and button 125 | 126 | // NOTE - these are the AnalogueConverter channel indexes, not necessariy the 127 | // hardware ADC channel numbers 128 | 129 | // ADVANCED SENSOR 130 | const int RFS_ADC_CHANNEL = 0; 131 | const int RSS_ADC_CHANNEL = 1; 132 | const int LSS_ADC_CHANNEL = 2; 133 | const int LFS_ADC_CHANNEL = 3; 134 | 135 | // BASIC SENSOR - just repeat the front sensor to make the code cleaner 136 | // #define RFS_ADC_CHANNEL 1 137 | // #define RSS_ADC_CHANNEL 0 138 | // #define LSS_ADC_CHANNEL 2 139 | // #define LFS_ADC_CHANNEL 1 140 | // there are two other ADC channels used by the robot 141 | const int SWITCHES_ADC_CHANNEL = 6; 142 | const int BATTERY_ADC_CHANNEL = 7; 143 | //***************************************************************************// 144 | const uint32_t BAUDRATE = 115200; 145 | 146 | //***************************************************************************// 147 | // set this to zero to disable profile data logging over serial 148 | // #define DEBUG_LOGGING 1 149 | // time between logged lines when reporting is enabled (milliseconds) 150 | const int REPORTING_INTERVAL = 10; 151 | 152 | //***************************************************************************// 153 | // Some physical constants that are likely to be robot-specific 154 | // with robot against back wall, how much travel is there to the cell center? 155 | const int BACK_WALL_TO_CENTER = 48; 156 | 157 | //***************************************************************************// 158 | // We need to know about the drive mechanics. 159 | 160 | // These calibrations are manual and should make for a good starting setup. 161 | // Later you can make the robot perform fixed runs and turns to do some fine 162 | // tuning. 163 | // 164 | // Move the mouse in a straight line through 500mm of travel to work while 165 | // displaying the left and right encoder counts to work out the number of 166 | // mm per count for each wheel. A more accurate value can be achieved by 167 | // moving a longer distance. 168 | // Apply only just enough downward pressure to ensure the wheels do not slip. 169 | // Make several runs and average them. 170 | // It is normal for one wheel to show a different count to the other. 171 | 172 | const float MM_PER_COUNT_LEFT = 500.0 / 1175; 173 | const float MM_PER_COUNT_RIGHT = 500.0 / 1166; 174 | 175 | // For accurate turns we need to know how far apart the wheels are. Start by 176 | // measuring the distance between the tyre centres. 177 | // Then... 178 | // Place the mouse on the table and rotate it by hand while reporting the 179 | // turn angle through the serial port. As with forward motion, push down only 180 | // enough to prevent the wheels from slipping. Line the edge of the mouse up 181 | // with a line and turn the mouse as accurately as possible through 360 degrees. 182 | // Don't worry if it is not perfectly in-place. 183 | // If the angle displayed is less than 360, set the mouse radius smaller. If 184 | // the displayed angle is more than 360, set the mouse radius larger. 185 | const float MOUSE_RADIUS = 37.95; 186 | 187 | const float DEG_PER_MM_DIFFERENCE = (180.0 / (2 * MOUSE_RADIUS * PI)); 188 | 189 | //*** MOTION CONTROLLER CONSTANTS **********************************************// 190 | 191 | //***************************************************************************// 192 | // Control loop timing. Pre-calculate to save time in interrupts 193 | const float LOOP_FREQUENCY = 500.0; 194 | const float LOOP_INTERVAL = (1.0 / LOOP_FREQUENCY); 195 | 196 | // Dynamic performance constants 197 | // There is a video describing how to get these numbers and calculate the feedforward 198 | // constants here: https://youtu.be/BrabDeHGsa0 199 | const float FWD_KM = 475.0; // mm/s/Volt 200 | const float FWD_TM = 0.190; // forward time constant 201 | const float ROT_KM = 775.0; // deg/s/Volt 202 | const float ROT_TM = 0.210; // rotation time constant 203 | 204 | // Motor Feedforward 205 | /*** 206 | * Speed Feedforward is used to add a drive voltage proportional to the motor speed 207 | * The units are Volts per mm/s and the value will be different for each 208 | * robot where the motor + gearbox + wheel diamter + robot weight are different 209 | * You can experimentally determine a suitable value by turning off the controller 210 | * and then commanding a set voltage to the motors. The same voltage is applied to 211 | * each motor. Have the robot report its speed regularly or have it measure 212 | * its steady state speed after a period of acceleration. 213 | * Do this for several applied voltages from 0.5 Volts to 5 Volts in steps of 0.5V 214 | * Plot a chart of steady state speed against voltage. The slope of that graph is 215 | * the speed feedforward, SPEED_FF. 216 | * Note that the line will not pass through the origin because there will be 217 | * some minimum voltage needed just to ovecome friction and get the wheels to turn at all. 218 | * That minimum voltage is the BIAS_FF. It is not dependent upon speed but is expressed 219 | * here as a fraction for comparison. 220 | * 221 | * If you want to find out more about how these equations come about and the theory 222 | * behind the method and its derivation have a look at this video: 223 | * https://youtu.be/qKoPRacXk9Q 224 | * 225 | * And this paper: 226 | * https://github.com/ukmars/motorlab/blob/main/documents/dirty-pd-controller.pdf 227 | * 228 | * 229 | */ 230 | const float MAX_MOTOR_VOLTS = 6.0; 231 | 232 | const float SPEED_FF = (1.0 / FWD_KM); 233 | const float ACC_FF = (FWD_TM / FWD_KM); 234 | const float BIAS_FF = 0.121; 235 | const float TOP_SPEED = (6.0 - BIAS_FF) / SPEED_FF; 236 | 237 | //*** MOTION CONTROL CONSTANTS **********************************************// 238 | 239 | // forward motion controller constants 240 | const float FWD_ZETA = 0.707; 241 | const float FWD_TD = FWD_TM; 242 | 243 | const float FWD_KP = 16 * FWD_TM / (FWD_KM * FWD_ZETA * FWD_ZETA * FWD_TD * FWD_TD); 244 | const float FWD_KD = LOOP_FREQUENCY * (8 * FWD_TM - FWD_TD) / (FWD_KM * FWD_TD); 245 | 246 | // rotation motion controller constants 247 | const float ROT_ZETA = 0.707; 248 | const float ROT_TD = ROT_TM; 249 | 250 | const float ROT_KP = 16 * ROT_TM / (ROT_KM * ROT_ZETA * ROT_ZETA * ROT_TD * ROT_TD); 251 | const float ROT_KD = LOOP_FREQUENCY * (8 * ROT_TM - ROT_TD) / (ROT_KM * ROT_TD); 252 | 253 | // controller constants for the steering controller 254 | const float STEERING_KP = 0.25; 255 | const float STEERING_KD = 0.00; 256 | const float STEERING_ADJUST_LIMIT = 10.0; // deg/s 257 | 258 | // encoder polarity is either 1 or -1 and is used to account for reversal of the encoder phases 259 | #define ENCODER_LEFT_POLARITY (-1) 260 | #define ENCODER_RIGHT_POLARITY (1) 261 | 262 | // similarly, the motors may be wired with different polarity and that is defined here so that 263 | // setting a positive voltage always moves the robot forwards 264 | #define MOTOR_LEFT_POLARITY (1) 265 | #define MOTOR_RIGHT_POLARITY (-1) 266 | 267 | //***************************************************************************// 268 | 269 | //***** PERFORMANCE CONSTANTS************************************************// 270 | // search and run speeds in mm/s and mm 271 | const int SEARCH_SPEED = 400; 272 | const int SEARCH_ACCELERATION = 2000; 273 | const int SEARCH_TURN_SPEED = 300; 274 | const int SMOOTH_TURN_SPEED = 500; 275 | const int FAST_TURN_SPEED = 600; 276 | const int FAST_RUN_SPEED_MAX = 2500; 277 | 278 | const float FAST_RUN_ACCELERATION = 3000; 279 | 280 | const int OMEGA_SPIN_TURN = 360; 281 | const int ALPHA_SPIN_TURN = 3600; 282 | 283 | //***** SENSOR SCALING ******************************************************// 284 | // This is the normalised value seen by the front sensor when the mouse is 285 | // in its calibration position 286 | const int SIDE_NOMINAL = 100; 287 | const int FRONT_NOMINAL = 100; 288 | 289 | // The side sensors are not reliable close to a wall ahead. This value 290 | // is the limit for the front sensor reading, above which the side sensors 291 | // are likely to give innaccurate readings because of reflections from 292 | // the wall ahead 293 | const int FRONT_WALL_RELIABILITY_LIMIT = 100; 294 | 295 | // Sensor brightness adjustment factor. The compiler calculates these so it saves processor time 296 | const float FRONT_LEFT_SCALE = (float)FRONT_NOMINAL / FRONT_LEFT_CALIBRATION; 297 | const float FRONT_RIGHT_SCALE = (float)FRONT_NOMINAL / FRONT_RIGHT_CALIBRATION; 298 | const float LEFT_SCALE = (float)SIDE_NOMINAL / LEFT_CALIBRATION; 299 | const float RIGHT_SCALE = (float)SIDE_NOMINAL / RIGHT_CALIBRATION; 300 | 301 | // the values above which, a wall is seen 302 | const int LEFT_THRESHOLD = 40; // minimum value to register a wall 303 | const int RIGHT_THRESHOLD = 40; // minimum value to register a wall 304 | const int FRONT_THRESHOLD = 20; // minimum value to register a wall 305 | 306 | // the distance through the cell at which the corresponding sensor 307 | // will see a falling edge 308 | const int LEFT_EDGE_POS = 90; 309 | const int RIGHT_EDGE_POS = 90; 310 | 311 | // clang-format off 312 | // These take no storage - the compiler uses the values directly 313 | const TurnParameters turn_params[4] = { 314 | // speed, entry, exit, angle, omega, alpha, sensor threshold 315 | {SEARCH_TURN_SPEED, 70, 80, 90.0, 287.0, 2866.0, TURN_THRESHOLD_SS90E}, // 0 => SS90EL 316 | {SEARCH_TURN_SPEED, 70, 80, -90.0, 287.0, 2866.0, TURN_THRESHOLD_SS90E}, // 0 => SS90ER 317 | {SEARCH_TURN_SPEED, 70, 80, 90.0, 287.0, 2866.0, TURN_THRESHOLD_SS90E}, // 0 => SS90L 318 | {SEARCH_TURN_SPEED, 70, 80, -90.0, 287.0, 2866.0, TURN_THRESHOLD_SS90E}, // 0 => SS90R 319 | }; 320 | // clang-format on 321 | 322 | //***************************************************************************// 323 | // Battery resistor bridge //Derek Hall// 324 | // The battery measurement is performed by first reducing the battery voltage 325 | // with a potential divider formed by two resistors. Here they are named R1 and R2 326 | // though that may not be their designation on the schematics. 327 | // 328 | // Resistor R1 is the high-side resistor and connects to the battery supply 329 | // Resistor R2 is the low-side resistor and connects to ground 330 | // Battery voltage is measured at the junction of these resistors 331 | // The ADC port used for the conversion will have a full scale reading (FSR) that 332 | // depends on the device being used. Typically that will be 1023 for a 10-bit ADC as 333 | // found on an Arduino but it may be 4095 if you have a 12-bit ADC. 334 | // Finally, the ADC converter on your processor will have a reference voltage. On 335 | // the Arduinos for example, this is 5 Volts. Thus, a full scale reading of 336 | // 1023 would represent 5 Volts, 511 would be 2.5Volts and so on. 337 | // 338 | // in this section you can enter the appropriate values for your ADC and potential 339 | // divider setup to ensure that the battery voltage reading performed by the sensors 340 | // is as accurate as possible. 341 | // 342 | // By calculating the battery multiplier here, you can be sure that the actual 343 | // battery voltage calulation is done as efficiently as possible. 344 | // The compiler will do all these calculations so your program does not have to. 345 | 346 | const float BATTERY_R1 = 10000.0; // resistor to battery + 347 | const float BATTERY_R2 = 10000.0; // resistor to Gnd 348 | const float BATTERY_DIVIDER_RATIO = BATTERY_R2 / (BATTERY_R1 + BATTERY_R2); 349 | const float ADC_FSR = 1023.0; // The maximum reading for the ADC 350 | const float ADC_REF_VOLTS = 5.0; // Reference voltage of ADC 351 | 352 | const float BATTERY_MULTIPLIER = (ADC_REF_VOLTS / ADC_FSR / BATTERY_DIVIDER_RATIO); 353 | 354 | const int MOTOR_MAX_PWM = 255; 355 | 356 | // the position in the cell where the sensors are sampled. 357 | const float SENSING_POSITION = 170.0; 358 | -------------------------------------------------------------------------------- /mazerunner-core/cli.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef CLI_H_ 13 | #define CLI_H_ 14 | 15 | #include 16 | #include 17 | #include "config.h" 18 | #include "maze.h" 19 | #include "mouse.h" 20 | #include "reporting.h" 21 | #include "sensors.h" 22 | 23 | const int MAX_ARGC = 16; 24 | #define MAX_DIGITS 8 25 | 26 | /// These are the long commands that we can use. 27 | /// The constraints of the ATMega architecture make this all a bit sketchy 28 | const char cmdHelp[] PROGMEM = "HELP"; 29 | const char cmdSEARCH[] PROGMEM = "SEARCH"; 30 | const char *const commands[] PROGMEM = {cmdHelp, cmdSEARCH}; 31 | const int CMD_COUNT = sizeof(commands) / sizeof(char *); 32 | 33 | /*** 34 | * The Args class is little more than an array of pointers to strings and a counter. 35 | * Input lines can be parsed into tokens and each token has its location stored in 36 | * the array. 37 | * 38 | * During command line processing, a temporary instance of Args will be created 39 | * on the stack and a reference to that passed around to any omethds that may 40 | * need to see the contents. Control eventually returns to the function that 41 | * created the instance and when that terminates, the information is lost. This 42 | * means that memory is only used when needed but bear in mind that the instance 43 | * uses an extra 34 bytes on the stack. 44 | * 45 | * For convenience when debugging, there is a method that displays all the tokens 46 | * on one line. 47 | * 48 | */ 49 | struct Args { 50 | char *argv[MAX_ARGC]; 51 | int argc = 0; 52 | void print() const { 53 | for (int i = 0; i < argc; i++) { 54 | Serial.print(argv[i]); 55 | Serial.print(' '); 56 | } 57 | Serial.println(); 58 | } 59 | }; 60 | 61 | /*** 62 | * Scan a character array for an integer. 63 | * Begin scan at line[pos] 64 | * Assumes no leading spaces. 65 | * Stops at first non-digit. 66 | * MODIFIES pos so that it points to the first non-digit 67 | * MODIFIES value ONLY IF a valid integer is converted 68 | * RETURNS the number of digits found and converted 69 | * 70 | * optimisations are possible but may not be worth the effort 71 | */ 72 | inline uint8_t read_integer(const char *line, int &value) { 73 | char *ptr = (char *)line; 74 | char c = *ptr++; 75 | bool is_minus = false; 76 | uint8_t digits = 0; 77 | if (c == '-') { 78 | is_minus = true; 79 | c = *ptr++; 80 | } 81 | int32_t number = 0; 82 | while (c >= '0' and c <= '9') { 83 | if (digits++ < MAX_DIGITS) { 84 | number = 10 * number + (c - '0'); 85 | } 86 | c = *ptr++; 87 | } 88 | if (digits > 0) { 89 | value = is_minus ? -number : number; 90 | } 91 | return digits; 92 | } 93 | 94 | /*** 95 | * Scan a character array for a float. 96 | * This is a much simplified and limited version of the library function atof() 97 | * It will not convert exponents and has a limited range of valid values. 98 | * They should be more than adequate for the robot parameters however. 99 | * Begin scan at line[pos] 100 | * Assumes no leading spaces. 101 | * Only scans MAX_DIGITS characters 102 | * Stops at first non-digit, or decimal point. 103 | * MODIFIES pos so that it points to the first character after the number 104 | * MODIFIES value ONLY IF a valid float is converted 105 | * RETURNS the number of digits found an converted 106 | * 107 | * optimisations are possible but may not be worth the effort 108 | * 109 | * exponents are not handled 110 | * 111 | * Example usage: 112 | * 113 | * const char *input = " -123.45 more"; 114 | * float v; 115 | * const char *next; 116 | * uint8_t count = read_float(input, v, next); 117 | * // v == -123.45 118 | * // next points to " more" 119 | * 120 | * 121 | * 122 | */ 123 | uint8_t read_float(const char *line, float &value, const char *&endptr) { 124 | const char *ptr = line; 125 | char c = *ptr; 126 | uint8_t digits = 0; 127 | 128 | // Skip leading whitespace 129 | while (c == ' ' || c == '\t') { 130 | c = *++ptr; 131 | } 132 | 133 | bool is_minus = false; 134 | if (c == '-') { 135 | is_minus = true; 136 | c = *++ptr; 137 | } 138 | 139 | float a = 0.0f; 140 | int exponent = 0; 141 | 142 | // Parse integer part 143 | while (c >= '0' && c <= '9') { 144 | if (digits++ < MAX_DIGITS) { 145 | a = a * 10.0f + (c - '0'); 146 | } 147 | c = *++ptr; 148 | } 149 | 150 | // Parse fractional part 151 | if (c == '.') { 152 | c = *++ptr; 153 | while (c >= '0' && c <= '9') { 154 | if (digits++ < MAX_DIGITS) { 155 | a = a * 10.0f + (c - '0'); 156 | exponent--; 157 | } 158 | c = *++ptr; 159 | } 160 | } 161 | 162 | float b = a; 163 | while (exponent < 0) { 164 | b *= 0.1f; 165 | exponent++; 166 | } 167 | 168 | if (digits > 0) { 169 | value = is_minus ? -b : b; 170 | } 171 | 172 | endptr = ptr; // Return updated pointer 173 | return digits; 174 | } 175 | 176 | // #define MAX_DIGITS 8 177 | const int INPUT_BUFFER_SIZE = 32; 178 | 179 | class CommandLineInterface { 180 | public: 181 | /*** 182 | * Read characters from the serial port into the buffer. 183 | * return true if there is a complete line available 184 | * return false if not. 185 | * 186 | * Input is echoed back through the serial port and can be 187 | * edited by the user using the backspace key. Accepted 188 | * characters are converted to upper case for convenience. 189 | * 190 | * Lines are terminated with a LINEFEED character which is 191 | * echoed but not placed in the buffer. 192 | * 193 | * All printable characters are placed in a buffer with a 194 | * maximum length of just 32 characters. You could make this 195 | * longer but there should be little practical need. 196 | * 197 | * All other characters are ignored. 198 | * 199 | */ 200 | const char BACKSPACE = 0x08; 201 | bool process_serial_data() { 202 | while (Serial.available()) { 203 | char c = Serial.read(); 204 | c = toupper(c); 205 | if (c == '\n') { 206 | Serial.println(); 207 | Args args; 208 | if (tokenise(args, m_buffer) > 0) { 209 | execute_command(args); 210 | } 211 | clear_input_buffer(); 212 | prompt(); 213 | return true; 214 | } else if (c == BACKSPACE) { 215 | handle_backspace(); 216 | } else if (isPrintable(c)) { 217 | add_to_buffer(c); 218 | } 219 | } 220 | return false; 221 | } 222 | 223 | void handle_backspace() { 224 | if (m_index > 0) { 225 | m_index--; 226 | m_buffer[m_index] = 0; 227 | Serial.print("\b \b"); // Erases the character on the screen 228 | } 229 | } 230 | 231 | void add_to_buffer(char c) { 232 | if (m_index < INPUT_BUFFER_SIZE - 1) { 233 | Serial.print(c); 234 | m_buffer[m_index++] = c; 235 | m_buffer[m_index] = 0; 236 | } 237 | } 238 | 239 | /*** 240 | * Tokenising is a process where the input buffer is examined 241 | * for separators that divide one string from another. Here 242 | * the separator is a space character, a comma or an '=' sign. 243 | * consecutive separators are treated as one. 244 | * 245 | * The start of each separate string is recorded in an array 246 | * in the Args structure and a null character is placed at its 247 | * end. In this way, the original string gets to look like a 248 | * sequence of shorter strings - the tokens. The argv array 249 | * is a list of where each starts and you can think if it as 250 | * an array of strings. The argc element keeps count of how 251 | * many tokens were found. 252 | * * 253 | */ 254 | int tokenise(Args &args, char *line) { 255 | char *token; 256 | args.argc = 0; // just to be safe 257 | for (token = strtok(line, " ,="); token != NULL; token = strtok(NULL, " ,=")) { 258 | args.argv[args.argc] = token; 259 | args.argc++; 260 | if (args.argc >= MAX_ARGC) 261 | break; 262 | } 263 | return args.argc; 264 | } 265 | 266 | /*** 267 | * Input lines are parsed into a number of space-separated 268 | * tokens which are stored in an argument structure, Args. 269 | * 270 | * After tokenising, the arguments are examined and processed. 271 | * 272 | * Single character commands are handled separately for simplicity. 273 | * 274 | * Commands that consist of more than one token have their own 275 | * handler which executes a function that is passed a reference to 276 | * the list of tokens as an argument. 277 | * 278 | * Once a command line has been dealt with, the input buffer is 279 | * cleared. That means that new characters that arrive while a 280 | * function is executing will be lost. A side effect of that 281 | * is that commands cannot be aborted over the serial link. 282 | * 283 | * NOTES: 284 | * - serial input is dealt with by polling so you must 285 | * frequently check for new input in the main program loop. 286 | * - tokenising modifies the input buffer so no extra storage space 287 | * is used. 288 | * 289 | */ 290 | void process_input_line() { 291 | Args args; 292 | if (tokenise(args, m_buffer) > 0) { 293 | execute_command(args); 294 | } 295 | clear_input_buffer(); 296 | prompt(); 297 | } 298 | 299 | void execute_command(Args &args) { 300 | if (strlen(args.argv[0]) == 1) { 301 | run_short_cmd(args); 302 | } else { 303 | run_long_cmd(args); 304 | } 305 | } 306 | 307 | /*** 308 | * Run a complex command. These all start with a string with more than 309 | * one character in it and have optional arguments. 310 | * 311 | * The arguments will be passed on to the robot. 312 | * 313 | * e.g. 'TURN 4 500 1000 3000' might mean: 314 | * - perform a turn test 315 | * - pass integer arguments 500,1000,3000 316 | * 317 | */ 318 | int get_command_index(const Args &args) { 319 | char buffer[16]; 320 | for (int i = 0; i < CMD_COUNT; i++) { 321 | strncpy_P(buffer, (char *)pgm_read_word(&(commands[i])), sizeof(buffer) - 1); 322 | if (strcmp(buffer, args.argv[0]) == 0) { 323 | return i; 324 | } 325 | } 326 | return -1; 327 | } 328 | 329 | void run_long_cmd(const Args &args) { 330 | int index = get_command_index(args); 331 | if (index < 0) { 332 | Serial.print(F("UNKNOWN COMMAND: ")); 333 | args.print(); 334 | return; 335 | } 336 | switch (index) { 337 | case 0: 338 | help(); 339 | break; 340 | case 1: { 341 | handle_search_command(args); 342 | } break; 343 | } 344 | } 345 | 346 | void handle_search_command(const Args &args) { 347 | int x = 0; 348 | int y = 0; 349 | if (!read_integer(args.argv[1], x)) { 350 | x = 7; 351 | }; 352 | 353 | if (!read_integer(args.argv[2], y)) { 354 | y = 7; 355 | }; 356 | char buf[20]; 357 | sprintf_P(buf, PSTR("Search to %d,%d\n"), x, y); 358 | Serial.print(buf); 359 | // mouse.search_to(Location(x, y)); 360 | } 361 | 362 | /*** 363 | * Simple commands represented by a single character 364 | * 365 | */ 366 | void run_short_cmd(const Args &args) { 367 | char c = args.argv[0][0]; 368 | switch (c) { 369 | case '?': 370 | help(); 371 | break; 372 | case 'X': 373 | Serial.println(F("Reset Maze")); 374 | maze.initialise(); 375 | break; 376 | case 'W': 377 | reporter.print_maze(PLAIN); 378 | break; 379 | case 'C': 380 | reporter.print_maze(COSTS); 381 | break; 382 | case 'D': 383 | reporter.print_maze(DIRS); 384 | break; 385 | case 'B': 386 | Serial.print(F("Battery: ")); 387 | Serial.print(battery.voltage(), 2); 388 | Serial.print(F(" Volts\n")); 389 | break; 390 | case 'S': 391 | sensors.enable(); 392 | delay(10); 393 | reporter.print_wall_sensors(); 394 | sensors.disable(); 395 | break; 396 | case 'E': 397 | delay(10); 398 | reporter.report_encoders(); 399 | break; 400 | case 'F': { 401 | // simulate the function switches 402 | int function = -1; 403 | int digits = read_integer(args.argv[1], function); 404 | if (digits) { 405 | run_function(function); 406 | } 407 | } break; 408 | case 'Q': 409 | handle_calibrate_encoders_command(args); 410 | break; 411 | default: 412 | break; 413 | } 414 | } 415 | 416 | void handle_calibrate_encoders_command(const Args &args) { 417 | motors.disable_controllers(); 418 | encoders.reset(); 419 | while (!switches.button_pressed()) { 420 | delay(10); 421 | float position = encoders.robot_distance(); 422 | float angle = encoders.robot_angle(); 423 | // reporter.print_justified(encoders.m_total_right, 7); 424 | // reporter.print_justified(encoders.m_total_left, 7); 425 | // reporter.print_justified(position, 7); 426 | // reporter.print_justified(angle, 7); 427 | // Serial.println(); 428 | char buf[60]; 429 | sprintf_P(buf, PSTR("L:%4d R:%4d P:%5.2f A:%5.2f\r\n"), encoders.m_total_left, encoders.m_total_right, position, angle); 430 | Serial.print(buf); 431 | } 432 | } 433 | /** 434 | * These are the actions associated with the function switches on UKMARSBOT 435 | * 436 | * The switches are set to one of 16 combinations. Combination 0 (zero) is 437 | * ignored so that there is a 'safe' state where the robot will not do anything 438 | * unexpected. 439 | * 440 | * The function can handle any number of additional functions if called as a 441 | * result of a command line input like 'F 45'. Otherwise, a command like 'F 1' 442 | * is equivalent to selecting 1 on te function switches and then pressing 443 | * the 'start' button. 444 | * 445 | */ 446 | void run_function(int cmd) { 447 | if (cmd == 0) { 448 | return; 449 | } 450 | switch (cmd) { 451 | case 1: 452 | mouse.show_sensor_calibration(); 453 | break; 454 | case 2: 455 | mouse.search_maze(); 456 | break; 457 | case 3: { 458 | mouse.follow_to(maze.goal()); 459 | } break; 460 | case 4: 461 | mouse.test_SS90E(); 462 | break; 463 | case 5: 464 | mouse.wander_to(Location(20, 20)); // not implemented 465 | break; 466 | case 6: 467 | mouse.conf_edge_detection(); 468 | break; 469 | case 7: 470 | mouse.conf_sensor_spin_calibrate(); 471 | break; 472 | case 8: 473 | mouse.conf_log_front_sensor(); 474 | break; 475 | 476 | case 9: 477 | mouse.run(4 * FULL_CELL); 478 | ; 479 | break; 480 | 481 | default: 482 | // just to be safe... 483 | sensors.disable(); 484 | motion.reset_drive_system(); 485 | sensors.set_steering_mode(STEERING_OFF); 486 | break; 487 | } 488 | } 489 | 490 | void clear_input_buffer() { 491 | m_index = 0; 492 | m_buffer[m_index] = 0; 493 | } 494 | 495 | void prompt() { 496 | Serial.println(); 497 | Serial.print('>'); 498 | Serial.print(' '); 499 | } 500 | 501 | /*** 502 | * You may add a help text here but remember to keep it in 503 | * sync with what the robot acually does. 504 | * 505 | */ 506 | void help() { 507 | Serial.println(F("? : this text")); 508 | Serial.println(F("X : reset maze")); 509 | Serial.println(F("W : display maze walls")); 510 | Serial.println(F("C : display maze costs")); 511 | Serial.println(F("D : display maze with directions")); 512 | Serial.println(F("B : show battery voltage")); 513 | Serial.println(F("S : show sensor readings")); 514 | Serial.println(F("Q : show encoder readings")); 515 | Serial.println(F("F n : Run user function n")); 516 | Serial.println(F(" 0 = ---")); 517 | Serial.println(F(" 1 = Sensor Static Calibration")); 518 | Serial.println(F(" 2 = Search to the goal and back")); 519 | Serial.println(F(" 3 = Follow a wall to the goal")); 520 | Serial.println(F(" 4 = Test SS90E Turn")); 521 | Serial.println(F(" 5 = Wander")); 522 | Serial.println(F(" 6 = Test Edge Detect Position")); 523 | Serial.println(F(" 7 = Sensor Spin Calibration")); 524 | Serial.println(F(" 8 = Get Front Sensor table")); 525 | Serial.println(F(" 9 = move forward 4 cells")); 526 | Serial.println(F(" 10 = ")); 527 | Serial.println(F(" 11 = ")); 528 | Serial.println(F(" 12 = ")); 529 | Serial.println(F(" 13 = ")); 530 | Serial.println(F(" 14 = ")); 531 | Serial.println(F(" 15 = ")); 532 | Serial.println(F("SEARCH x y : search to location (x,y)")); 533 | Serial.println(F("HELP : this text")); 534 | } 535 | 536 | private: 537 | char m_buffer[INPUT_BUFFER_SIZE] = {0}; 538 | uint8_t m_index = 0; 539 | }; 540 | 541 | extern CommandLineInterface cli; 542 | 543 | #endif /* UI_H_ */ 544 | -------------------------------------------------------------------------------- /mazerunner-core/reporting.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * Project: mazerunner-core * 3 | * ----- * 4 | * Copyright 2022 - 2023 Peter Harrison, Micromouseonline * 5 | * ----- * 6 | * Licence: * 7 | * Use of this source code is governed by an MIT-style * 8 | * license that can be found in the LICENSE file or at * 9 | * https://opensource.org/licenses/MIT. * 10 | ******************************************************************************/ 11 | 12 | #ifndef REPORTER_H 13 | #define REPORTER_H 14 | 15 | #include 16 | #include "encoders.h" 17 | #include "maze.h" 18 | #include "motion.h" 19 | #include "motors.h" 20 | #include "profile.h" 21 | #include "sensors.h" 22 | 23 | /*** 24 | * While there is no absolute requirement for reporting in a micromouse or 25 | * similar robot, it can be of enormous help when setting up and debugging 26 | * the behaviour of the robot. 27 | * 28 | * The Reporter class and its supporting functions provide a number of pre- 29 | * written reports that let you observe things like controller state and 30 | * sensor readings. Many of these will be used a lot during development and 31 | * you can use any of them as a template for custom reports. 32 | * 33 | * Most commonly the only output device will be the normal Arduino Serial 34 | * device. However, if you have a second serial port or some other device such 35 | * as a serial logger you can divert reporter output to that. The method 36 | * set_printer() will change the destination for _all_ reporter output. Any 37 | * interaction on the command line interface will not be affected except, of 38 | * course a cli command that fires off a report may not see the output if you 39 | * have changed the reporter print device. You can switch devices as often 40 | * as you like. 41 | * 42 | * I would suggest that any interactive use might leave the reporter output 43 | * destination as Serial and you could switch it to a logger, for example, 44 | * at the start of a maze run. 45 | * 46 | * Although the Serial device on an Arduino normally requires the USB connection 47 | * UKMARSBOT has a connector specially intended for a cheap Bluetooth 48 | * module like the HC-05. This can be left plugged in all the time so that 49 | * reporting can be done while running. You could also connect a serial 50 | * logger to this port and record data throughout a contest run. 51 | * 52 | * Note that the Serial device has a 64 character buffer and, at 115200 baud 53 | * 64 characters will take about 6ms to go out over the wire. 54 | * 55 | * The BT connector also makes it possible to use the interactive Command 56 | * Line Interface while the robot is running. 57 | * 58 | */ 59 | const char dir_letters[] = "FRAL"; 60 | const char hdg_letters[] = "NESW"; 61 | 62 | //***************************************************************************// 63 | 64 | /* 65 | // For future expansion, suppose you have a list of operations 66 | // as a set of defines like this: 67 | 68 | #define OP_START_MOVE 0x05 69 | #define OP_MAKE_MOVE 0x06 70 | #define OP_STOP 0x07 71 | 72 | // you might want to print their names not their values for 73 | // debugging purposes. Use the stringify operator, #, like this 74 | 75 | #define case_print(x) \ 76 | case (x): \ 77 | Serial.println(F(#x)); \ 78 | break; 79 | 80 | // and use a case statement to display the output 81 | // remmeber that each identifier string ends up in flash 82 | 83 | void print_name(int operation) { 84 | switch (operation) { 85 | case_print(OP_START_MOVE); 86 | case_print(OP_MAKE_MOVE); 87 | case_print(OP_STOP); 88 | default: 89 | Serial.println(F("UNKNOWN")); 90 | } 91 | } 92 | */ 93 | //***************************************************************************// 94 | 95 | enum MazeView { PLAIN, COSTS, DIRS }; 96 | //***************************************************************************// 97 | 98 | static Stream& printer = Serial; 99 | 100 | class Reporter; 101 | extern Reporter reporter; 102 | class Reporter { 103 | uint32_t s_start_time = millis(); 104 | uint32_t s_report_time = millis(); 105 | uint32_t s_report_interval = REPORTING_INTERVAL; 106 | 107 | public: 108 | void set_printer(Stream& stream) { 109 | printer = stream; 110 | } 111 | 112 | // simple formatting methods for printing maze costs 113 | void print_hex_2(unsigned char value) { 114 | if (value < 16) { 115 | printer.print('0'); 116 | } 117 | printer.print(value, HEX); 118 | } 119 | 120 | void print_justified(int32_t value, int width) { 121 | int v = value; 122 | int w = width; 123 | w--; 124 | if (v < 0) { 125 | w--; 126 | } 127 | while (v /= 10) { 128 | w--; 129 | } 130 | while (w > 0) { 131 | printer.write(' '); 132 | --w; 133 | } 134 | printer.print(value); 135 | } 136 | 137 | /** 138 | * The profile reporter will send out a table of space separated 139 | * data so that the results can be saved to a file or imported to 140 | * a spreadsheet or other analyss software. 141 | * 142 | * Always send the header first because that restarts the elapsed 143 | * time count. 144 | * 145 | * The data includes 146 | * time - in milliseconds since the header was sent 147 | * robotPos - position in mm as reported by the encoders 148 | * robotAngle - angle in degrees as reported by the encoders 149 | * fwdPos - forward profiler setpoint in mm 150 | * fwdSpeed - forward profiler current speed in mm/s 151 | * rotpos - rotation profiler setpoint in deg 152 | * rotSpeed - rotation profiler current speed in deg/s 153 | * fwdmVolts - voltage sent to the motors for forward control 154 | * rotmVolts - voltage sent to the motors for rotation control 155 | * 156 | */ 157 | void report_profile_header() { 158 | printer.println(F("time robotPos robotAngle fwdPos fwdSpeed rotpos rotSpeed fwdmVolts rotmVolts")); 159 | s_start_time = millis(); 160 | s_report_time = s_start_time; 161 | } 162 | 163 | void report_profile() { 164 | if (millis() >= s_report_time) { 165 | s_report_time += s_report_interval; 166 | print_justified(int(encoders.robot_distance()), 6); 167 | print_justified(int(encoders.robot_angle()), 6); 168 | print_justified(int(motion.position()), 6); 169 | print_justified(int(motion.velocity()), 6); 170 | print_justified(int(motion.angle()), 6); 171 | print_justified(int(motion.omega()), 6); 172 | print_justified(motors.get_fwd_millivolts(), 6); 173 | print_justified(motors.get_rot_millivolts(), 6); 174 | printer.println(); 175 | } 176 | } 177 | 178 | //***************************************************************************// 179 | 180 | /*** 181 | * The full sensor track report can be used to help calibrate the sensors and 182 | * check the operation of the steering. 183 | * 184 | * You could also use it to examine the sensor readings as walls are detected or lost. 185 | * 186 | * To avoid sending more data that is needed, the default is to send only the 187 | * normalised sensor readings. Call with the argument set true to see raw sensor 188 | * readings for calibration checks. 189 | * 190 | */ 191 | void report_sensor_track_header() { 192 | printer.println(F(" time pos angle lfs lss rss rfs cte steer")); 193 | s_start_time = millis(); 194 | s_report_time = s_start_time; 195 | } 196 | 197 | void report_sensor_track(bool use_raw = false) { 198 | if (millis() >= s_report_time) { 199 | s_report_time += s_report_interval; 200 | print_justified(int(millis() - s_start_time), 6); 201 | print_justified(int(encoders.robot_distance()), 6); 202 | print_justified(int(encoders.robot_angle()), 6); 203 | if (use_raw) { 204 | print_justified(sensors.lfs.raw, 6); 205 | print_justified(sensors.lss.raw, 6); 206 | print_justified(sensors.rss.raw, 6); 207 | print_justified(sensors.rfs.raw, 6); 208 | } else { 209 | print_justified(sensors.lfs.value, 6); 210 | print_justified(sensors.lss.value, 6); 211 | print_justified(sensors.rss.value, 6); 212 | print_justified(sensors.rfs.value, 6); 213 | } 214 | printer.print(' '); 215 | printer.print(sensors.get_cross_track_error()); 216 | printer.print(' '); 217 | printer.print(sensors.get_steering_feedback()); 218 | printer.println(); 219 | } 220 | } 221 | 222 | void report_radial_track_header() { 223 | printer.println(F(" angle lfs lss rss rfs cte steer")); 224 | s_start_time = millis(); 225 | s_report_time = s_start_time; 226 | } 227 | void report_radial_track(bool use_raw = false) { 228 | static int recorded_angle = INT16_MAX; 229 | int this_angle = (int)encoders.robot_angle(); 230 | if (recorded_angle != this_angle) { 231 | recorded_angle = this_angle; 232 | print_justified(recorded_angle, 6); 233 | if (use_raw) { 234 | print_justified(sensors.lfs.raw, 6); 235 | print_justified(sensors.lss.raw, 6); 236 | print_justified(sensors.rss.raw, 6); 237 | print_justified(sensors.rfs.raw, 6); 238 | } else { 239 | print_justified(sensors.lfs.value, 6); 240 | print_justified(sensors.lss.value, 6); 241 | print_justified(sensors.rss.value, 6); 242 | print_justified(sensors.rfs.value, 6); 243 | } 244 | printer.print(' '); 245 | printer.print(sensors.get_cross_track_error()); 246 | printer.print(' '); 247 | printer.print(sensors.get_steering_feedback()); 248 | printer.println(); 249 | } 250 | } 251 | 252 | void report_encoders() { 253 | print_justified(int(encoders.robot_distance()), 6); 254 | print_justified(int(encoders.robot_angle()), 6); 255 | printer.print(F(" | ")); 256 | print_justified(encoders.m_total_left, 6); 257 | print_justified(encoders.m_total_right, 6); 258 | printer.println(); 259 | } 260 | 261 | //***************************************************************************// 262 | 263 | /*** 264 | * This specialised report is meant to be used when calibrating the forward 265 | * sensors. The robot would be placed up agains a wall ahead and commanded to 266 | * back up slowly for some fixed distance - 180mm seems appropriate. As it 267 | * backs up the sensor readings are transmitted so that you can build a table 268 | * of expected sensor readings against distance from the front wall. This 269 | * will be useful in setting up things like turn thresholds and to allow the 270 | * robot to adjust its position in a maze cell. 271 | * 272 | * In normal operation, UKMARSBOT has its front edge 22 mm from the wall ahead. 273 | * 274 | */ 275 | 276 | void front_sensor_track_header() { 277 | printer.println(F(" dist front_sum front_diff, distance")); 278 | } 279 | 280 | void front_sensor_track() { 281 | static int position = INT16_MAX; 282 | if (position != (int)encoders.robot_distance()) { 283 | position = encoders.robot_distance(); 284 | print_justified(position, 7); 285 | print_justified(sensors.get_front_sum(), 7); 286 | print_justified(sensors.get_front_diff(), 7); 287 | print_justified((int)sensors.get_distance(sensors.get_front_sum(), FRONT_LINEAR_CONSTANT), 7); 288 | printer.println(); 289 | } 290 | } 291 | 292 | //***************************************************************************// 293 | 294 | /*** 295 | * Mostly, you are likely to want to just stream out the current sensor readings 296 | * to a terminal while you check their values. 297 | * 298 | * This report does that, showing the raw and normalised values as well as the 299 | * sum and difference of the front sensors. 300 | * 301 | * This is possibly the most useful streamed information because you can use it 302 | * with the robot in a maze to get basic calibration values. 303 | * 304 | * 305 | */ 306 | void wall_sensor_header() { 307 | printer.println(F("| RAW | NORMALISED | | |")); 308 | printer.println(F("| lf_ ls_ rs_ rf_ | lfs lss rss rfs | sum diff | front_dist |")); 309 | } 310 | 311 | void print_wall_sensors() { 312 | printer.print(F("|")); 313 | print_justified(sensors.lfs.raw, 6); 314 | print_justified(sensors.lss.raw, 6); 315 | print_justified(sensors.rss.raw, 6); 316 | print_justified(sensors.rfs.raw, 6); 317 | printer.print(F(" | ")); 318 | print_justified(sensors.lfs.value, 6); 319 | print_justified(sensors.lss.value, 6); 320 | print_justified(sensors.rss.value, 6); 321 | print_justified(sensors.rfs.value, 6); 322 | printer.print(F(" | ")); 323 | print_justified(sensors.get_front_sum(), 5); 324 | print_justified(sensors.get_front_diff(), 5); 325 | printer.print(F(" | ")); 326 | print_justified((int)sensors.get_distance(sensors.get_front_sum(), FRONT_LINEAR_CONSTANT), 6); 327 | printer.print(F(" | ")); 328 | printer.println(); 329 | } 330 | 331 | // void 332 | 333 | //***************************************************************************// 334 | void print_walls() { 335 | if (sensors.see_left_wall) { 336 | printer.print('L'); 337 | } else { 338 | printer.print('-'); 339 | } 340 | if (sensors.see_front_wall) { 341 | printer.print('F'); 342 | } else { 343 | printer.print('-'); 344 | } 345 | if (sensors.see_right_wall) { 346 | printer.print('R'); 347 | } else { 348 | printer.print('-'); 349 | } 350 | } 351 | 352 | //***************************************************************************// 353 | /** 354 | * log_action_status outputs a formatted block of test to the serial device. 355 | * You could connect the Serial device to a BT radio or datalogger if you want. 356 | * 357 | * A typical block might look like this: 358 | * 359 | * {F [3,4] N 227@ 175 } 360 | * - ----- --- --- --- 361 | * | | | | `--- Robot Position (mm) 362 | * | | | `-------- Front Sensor Sum 363 | * | | `------------- Heading 364 | * | `------------------ Current Cell 365 | * ` --------------------- Action 366 | * 367 | * During the search, several such blocks will be generated in each cell 368 | * 369 | * TODO: consider a structure that can be populated during the search loop 370 | * and displayed in one line without the repetition of multiple blocks 371 | */ 372 | /// @private don't show this in doxygen output 373 | // 374 | void log_action_status(char action, char note, Location location, Heading heading) { 375 | printer.print(action); 376 | printer.print(note); 377 | printer.print('['); 378 | print_justified(location.x, 2); 379 | printer.print(','); 380 | print_justified(location.y, 2); 381 | printer.print(','); 382 | if (heading < HEADING_COUNT) { 383 | printer.print(hdg_letters[heading]); 384 | } else { 385 | printer.print('!'); 386 | } 387 | printer.print(']'); 388 | printer.print(' '); 389 | print_walls(); 390 | printer.print(' '); 391 | print_justified(sensors.get_front_sum(), 4); 392 | printer.print('@'); 393 | print_justified((int)motion.position(), 4); 394 | printer.print(' '); 395 | } 396 | 397 | //***************************************************************************// 398 | void show_adc() { 399 | while (true) { 400 | sensors.enable(); 401 | for (int i = 0; i < 4; i++) { 402 | print_justified(adc.get_raw(i), 5); 403 | printer.print(' '); 404 | } 405 | for (int i = 6; i < 8; i++) { 406 | print_justified(adc.get_dark(i), 5); 407 | printer.print(' '); 408 | } 409 | printer.println(); 410 | delay(50); 411 | } 412 | sensors.disable(); 413 | } 414 | 415 | //***************************************************************************// 416 | /** 417 | * Maze printing. 418 | * 419 | */ 420 | #define POST 'o' 421 | #define ERR '?' 422 | #define GAP F(" ") 423 | #define H_WALL F("---") 424 | #define H_EXIT F(" ") 425 | #define H_UNKN F("···") 426 | #define H_VIRT F("###") 427 | #define V_WALL '|' 428 | #define V_EXIT ' ' 429 | #define V_UNKN ':' 430 | #define V_VIRT '#' 431 | 432 | void print_h_wall(uint8_t state) { 433 | if (state == EXIT) { 434 | printer.print(H_EXIT); 435 | } else if (state == WALL) { 436 | printer.print(H_WALL); 437 | } else if (state == VIRTUAL) { 438 | printer.print(H_VIRT); 439 | } else { 440 | printer.print(H_UNKN); 441 | } 442 | } 443 | void printNorthWalls(int y) { 444 | for (int x = 0; x < MAZE_WIDTH; x++) { 445 | printer.print(POST); 446 | WallInfo walls = maze.walls(Location(x, y)); 447 | print_h_wall(walls.north & maze.get_mask()); 448 | } 449 | printer.println(POST); 450 | } 451 | 452 | void printSouthWalls(int y) { 453 | for (int x = 0; x < MAZE_WIDTH; x++) { 454 | printer.print(POST); 455 | WallInfo walls = maze.walls(Location(x, y)); 456 | print_h_wall(walls.south & maze.get_mask()); 457 | } 458 | printer.println(POST); 459 | } 460 | 461 | void print_maze(int style = PLAIN) { 462 | const char dirChars[] = "^>v<* "; 463 | maze.flood(maze.goal()); 464 | 465 | for (int y = MAZE_HEIGHT - 1; y >= 0; y--) { 466 | printNorthWalls(y); 467 | for (int x = 0; x < MAZE_WIDTH; x++) { 468 | Location location(x, y); 469 | WallInfo walls = maze.walls(location); 470 | uint8_t state = walls.west & maze.get_mask(); 471 | if (state == EXIT) { 472 | printer.print(V_EXIT); 473 | } else if (state == WALL) { 474 | printer.print(V_WALL); 475 | } else if (state == VIRTUAL) { 476 | printer.print(V_VIRT); 477 | } else { 478 | printer.print(V_UNKN); 479 | } 480 | if (style == COSTS) { 481 | print_justified((int)maze.cost(location), 3); 482 | } else if (style == DIRS) { 483 | unsigned char direction = maze.heading_to_smallest(location, NORTH); 484 | if (location == maze.goal()) { 485 | direction = DIRECTION_COUNT; 486 | } 487 | char arrow = ' '; 488 | if (direction != BLOCKED) { 489 | arrow = dirChars[direction]; 490 | } 491 | printer.print(' '); 492 | printer.print(arrow); 493 | printer.print(' '); 494 | } else { 495 | printer.print(GAP); 496 | } 497 | } 498 | printer.println(V_WALL); 499 | } 500 | printSouthWalls(0); 501 | printer.println(); 502 | } 503 | }; 504 | 505 | #endif --------------------------------------------------------------------------------