├── .gitignore ├── .arduino-ci.yaml ├── .gitlab-ci.yml ├── .github ├── ISSUE_TEMPLATE │ ├── config.yml │ └── bug-report-or-feature-request.md └── workflows │ └── ci.yaml ├── .dir-locals.el ├── components.txt ├── Doxyfile ├── library.properties ├── examples ├── BlinkLEDs │ └── BlinkLEDs.ino ├── LCDBasics │ └── LCDBasics.ino ├── RotationResist │ ├── TurnSensor.h │ ├── RotationResist.ino │ └── TurnSensor.cpp ├── RemoteControl │ ├── RemoteConstants.h │ ├── RemoteControl.ino │ └── RemoteDecoder.h ├── PowerDetect │ └── PowerDetect.ino ├── MotorTest │ └── MotorTest.ino ├── Buttons │ └── Buttons.ino ├── InertialSensors │ └── InertialSensors.ino ├── BuzzerBasics │ └── BuzzerBasics.ino ├── RCControl │ └── RCControl.ino ├── Encoders │ └── Encoders.ino ├── FaceUphill │ └── FaceUphill.ino └── Demo │ └── Demo.ino ├── local_keywords.txt ├── LICENSE.txt ├── Romi32U4Buzzer.h ├── update_components.sh ├── USBPause.h ├── Romi32U4Motors.cpp ├── Romi32U4Buttons.h ├── Romi32U4LCD.h ├── Romi32U4.h ├── Pushbutton.cpp ├── Romi32U4Motors.h ├── keywords.txt ├── Romi32U4Encoders.h ├── Romi32U4Encoders.cpp ├── README.md ├── PololuHD44780.cpp ├── Pushbutton.h ├── PololuBuzzer.h ├── FastGPIO.h ├── PololuHD44780.h └── PololuBuzzer.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | /docs/ 2 | /out/ 3 | -------------------------------------------------------------------------------- /.arduino-ci.yaml: -------------------------------------------------------------------------------- 1 | only_boards: 2 | - arduino:avr:leonardo 3 | library_archives: 4 | - LSM6=https://github.com/pololu/lsm6-arduino/archive/1.0.0.tar.gz=046cw10nv6cr8624f6j2rw5kf5cazrb9n8sda92m0n2971zc3k48 5 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: $CI_REGISTRY_IMAGE/nixos/nix:2.3.6 2 | 3 | stages: 4 | - ci 5 | 6 | ci: 7 | stage: ci 8 | tags: 9 | - nix 10 | script: 11 | - nix-shell -I nixpkgs=channel:nixpkgs-unstable -p arduino-ci --run "arduino-ci" 12 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/config.yml: -------------------------------------------------------------------------------- 1 | blank_issues_enabled: false 2 | contact_links: 3 | - name: Pololu Forum 4 | url: https://forum.pololu.com/ 5 | about: Do you need help getting started? Can't get this code to work at all? Having problems with electronics? Please post on our forum! 6 | -------------------------------------------------------------------------------- /.dir-locals.el: -------------------------------------------------------------------------------- 1 | ; Settings for the Emacs text editor. 2 | 3 | ( 4 | (c-mode . ((c-basic-offset . 4) (tab-width . 4) (indent-tabs-mode . nil))) 5 | (nil . ((fill-column . 80))) 6 | ("examples" . ( 7 | (nil . ((c-basic-offset . 2) (tab-width . 8) (fill-column . 65))) 8 | )) 9 | ) 10 | -------------------------------------------------------------------------------- /components.txt: -------------------------------------------------------------------------------- 1 | https://github.com/pololu/fastgpio-arduino 2.0.0 2 | https://github.com/pololu/usb-pause-arduino 2.0.0 3 | https://github.com/pololu/pushbutton-arduino 2.0.0 4 | https://github.com/pololu/pololu-buzzer-arduino 1.0.1 5 | https://github.com/pololu/pololu-hd44780-arduino 2.0.0-1-gc2ceb6f 6 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug-report-or-feature-request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report or feature request 3 | about: Did you find a specific bug in the code for this project? Do you want to request 4 | a new feature? Please open an issue! 5 | title: '' 6 | labels: '' 7 | assignees: '' 8 | 9 | --- 10 | 11 | 12 | -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | # Doxygen configuration file for generating documentation. 2 | PROJECT_NAME = "Romi32U4 library" 3 | OUTPUT_DIRECTORY = docs 4 | INLINE_INHERITED_MEMB = YES 5 | INPUT = . 6 | EXCLUDE = examples 7 | USE_MDFILE_AS_MAINPAGE = README.md 8 | RECURSIVE = YES 9 | SOURCE_BROWSER = YES 10 | USE_MATHJAX = YES 11 | GENERATE_LATEX = NO 12 | EXTRACT_STATIC = YES 13 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Romi32U4 2 | version=1.0.3 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Romi 32U4 Arduino library 6 | paragraph=This library helps interface with the on-board hardware on the Pololu Romi 32U4 Control Board. 7 | category=Device Control 8 | url=https://github.com/pololu/romi-32u4-arduino-library 9 | architectures=avr 10 | includes=Romi32U4.h 11 | -------------------------------------------------------------------------------- /examples/BlinkLEDs/BlinkLEDs.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to blink the three user LEDs on the 2 | // Romi 32U4. 3 | 4 | #include 5 | 6 | void setup() 7 | { 8 | 9 | } 10 | 11 | void loop() 12 | { 13 | // Turn the LEDs on. 14 | ledRed(1); 15 | ledYellow(1); 16 | ledGreen(1); 17 | 18 | // Wait for a second. 19 | delay(1000); 20 | 21 | // Turn the LEDs off. 22 | ledRed(0); 23 | ledYellow(0); 24 | ledGreen(0); 25 | 26 | // Wait for a second. 27 | delay(1000); 28 | } 29 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: "CI" 2 | on: 3 | pull_request: 4 | push: 5 | jobs: 6 | ci: 7 | runs-on: ubuntu-20.04 8 | steps: 9 | - name: Checkout this repository 10 | uses: actions/checkout@v2.3.4 11 | - name: Cache for arduino-ci 12 | uses: actions/cache@v2.1.3 13 | with: 14 | path: | 15 | ~/.arduino15 16 | key: ${{ runner.os }}-arduino 17 | - name: Install nix 18 | uses: cachix/install-nix-action@v12 19 | - run: nix-shell -I nixpkgs=channel:nixpkgs-unstable -p arduino-ci --run "arduino-ci" 20 | -------------------------------------------------------------------------------- /examples/LCDBasics/LCDBasics.ino: -------------------------------------------------------------------------------- 1 | // This example demonstrates basic use of the Romi 32U4 LCD. It 2 | // prints the word "hi" on the first line of the LCD and prints 3 | // the number 1234 on the second line. 4 | 5 | #include 6 | 7 | Romi32U4LCD lcd; 8 | 9 | void setup() 10 | { 11 | 12 | } 13 | 14 | void loop() 15 | { 16 | // Clear the screen. 17 | lcd.clear(); 18 | 19 | // Print a string. 20 | lcd.print("hi"); 21 | 22 | // Go to the next line. 23 | lcd.gotoXY(0, 1); 24 | 25 | // Print a number. 26 | lcd.print(1234); 27 | 28 | delay(1000); 29 | } 30 | 31 | -------------------------------------------------------------------------------- /local_keywords.txt: -------------------------------------------------------------------------------- 1 | Romi32U4LCD KEYWORD1 2 | 3 | ROMI_32U4_BUTTON_A LITERAL1 4 | ROMI_32U4_BUTTON_B LITERAL1 5 | ROMI_32U4_BUTTON_C LITERAL1 6 | Romi32U4ButtonA KEYWORD1 7 | Romi32U4ButtonB KEYWORD1 8 | Romi32U4ButtonC KEYWORD1 9 | 10 | Romi32U4Buzzer KEYWORD1 11 | 12 | Romi32U4Motors KEYWORD1 13 | flipLeftMotor KEYWORD2 14 | flipRightMotor KEYWORD2 15 | setLeftSpeed KEYWORD2 16 | setRightSpeed KEYWORD2 17 | setSpeeds KEYWORD2 18 | allowTurbo KEYWORD2 19 | 20 | Romi32U4Encoders KEYWORD1 21 | init KEYWORD2 22 | getCountsLeft KEYWORD2 23 | getCountsRight KEYWORD2 24 | getCountsAndResetLeft KEYWORD2 25 | getCountsAndResetRight KEYWORD2 26 | checkErrorLeft KEYWORD2 27 | checkErrorRight KEYWORD2 28 | 29 | ledRed KEYWORD2 30 | ledGreen KEYWORD2 31 | ledYellow KEYWORD2 32 | usbPowerPresent KEYWORD2 33 | readBatteryMillivolts KEYWORD2 34 | -------------------------------------------------------------------------------- /examples/RotationResist/TurnSensor.h: -------------------------------------------------------------------------------- 1 | // Turnsensor.h and TurnSensor.cpp provide functions for 2 | // configuring the Romi's gyro, calibrating it, and using it to 3 | // measure how much the robot has turned about its Z axis. 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | // This constant represents a turn of 45 degrees. 11 | const int32_t turnAngle45 = 0x20000000; 12 | 13 | // This constant represents a turn of 90 degrees. 14 | const int32_t turnAngle90 = turnAngle45 * 2; 15 | 16 | // This constant represents a turn of approximately 1 degree. 17 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; 18 | 19 | // These are defined in TurnSensor.cpp: 20 | void turnSensorSetup(); 21 | void turnSensorReset(); 22 | void turnSensorUpdate(); 23 | extern uint32_t turnAngle; 24 | extern int16_t turnRate; 25 | 26 | // These objects must be defined in your sketch. 27 | extern Romi32U4ButtonA buttonA; 28 | extern Romi32U4LCD lcd; 29 | extern LSM6 imu; 30 | -------------------------------------------------------------------------------- /examples/RemoteControl/RemoteConstants.h: -------------------------------------------------------------------------------- 1 | // This file contains constants for the address and commands in 2 | // the messages transmitted by the Mini IR Remote Control: 3 | // https://www.pololu.com/product/2777 4 | 5 | #pragma once 6 | 7 | const uint16_t remoteAddressByte0 = 0x00; 8 | const uint16_t remoteAddressByte1 = 0xBF; 9 | 10 | const uint8_t remoteVolMinus = 0x00; 11 | const uint8_t remotePlayPause = 0x01; 12 | const uint8_t remoteVolPlus = 0x02; 13 | 14 | const uint8_t remoteSetup = 0x04; 15 | const uint8_t remoteUp = 0x05; 16 | const uint8_t remoteStopMode = 0x06; 17 | 18 | const uint8_t remoteLeft = 0x08; 19 | const uint8_t remoteEnterSave = 0x09; 20 | const uint8_t remoteRight = 0x0A; 21 | 22 | const uint8_t remote0 = 0x0C; 23 | const uint8_t remoteDown = 0x0D; 24 | const uint8_t remoteBack = 0x0E; 25 | 26 | const uint8_t remote1 = 0x10; 27 | const uint8_t remote2 = 0x11; 28 | const uint8_t remote3 = 0x12; 29 | 30 | const uint8_t remote4 = 0x14; 31 | const uint8_t remote5 = 0x15; 32 | const uint8_t remote6 = 0x16; 33 | 34 | const uint8_t remote7 = 0x18; 35 | const uint8_t remote8 = 0x19; 36 | const uint8_t remote9 = 0x1A; 37 | 38 | 39 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017-2022 Pololu Corporation (www.pololu.com) 2 | 3 | Permission is hereby granted, free of charge, to any person 4 | obtaining a copy of this software and associated documentation 5 | files (the "Software"), to deal in the Software without 6 | restriction, including without limitation the rights to use, 7 | copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the 9 | Software is furnished to do so, subject to the following 10 | conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 17 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | -------------------------------------------------------------------------------- /examples/PowerDetect/PowerDetect.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to: 2 | // 3 | // - Measure the voltage of the Romi's batteries. 4 | // - Detect whether USB power is present. 5 | // 6 | // The results are printed to the LCD and also to the serial 7 | // monitor. 8 | // 9 | // The battery voltage can only be read when the power switching 10 | // circuit is on. Otherwise, the voltage reading displayed by 11 | // this demo will be low, but it might not be zero because the 12 | // Romi 32U4 has capacitors that take a while to discharge. 13 | 14 | #include 15 | 16 | Romi32U4LCD lcd; 17 | 18 | void setup() 19 | { 20 | } 21 | 22 | void loop() 23 | { 24 | bool usbPower = usbPowerPresent(); 25 | 26 | uint16_t batteryLevel = readBatteryMillivolts(); 27 | 28 | // Print the results to the LCD. 29 | lcd.clear(); 30 | lcd.print(F("B=")); 31 | lcd.print(batteryLevel); 32 | lcd.print(F("mV ")); 33 | lcd.gotoXY(0, 1); 34 | lcd.print(F("USB=")); 35 | lcd.print(usbPower ? 'Y' : 'N'); 36 | 37 | // Print the results to the serial monitor. 38 | Serial.print(F("USB=")); 39 | Serial.print(usbPower ? 'Y' : 'N'); 40 | Serial.print(F(" B=")); 41 | Serial.print(batteryLevel); 42 | Serial.println(F(" mV")); 43 | 44 | delay(200); 45 | } 46 | -------------------------------------------------------------------------------- /Romi32U4Buzzer.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Romi32U4Buzzer.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Plays beeps and music on the buzzer on the Romi 32U4. 10 | * 11 | * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on 12 | * the Romi 32U4 buzzer. 13 | * 14 | * Note durations are timed using a timer overflow interrupt 15 | * (`TIMER4_OVF`), which will briefly interrupt execution of your 16 | * main program at the frequency of the sound being played. In most cases, the 17 | * interrupt-handling routine is very short (several microseconds). However, 18 | * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode) 19 | * with the `play()` command, this interrupt takes much longer than normal 20 | * (perhaps several hundred microseconds) every time it starts a new note. It is 21 | * important to take this into account when writing timing-critical code. 22 | */ 23 | class Romi32U4Buzzer : public PololuBuzzer 24 | { 25 | // This is a trivial subclass of PololuBuzzer; it exists because we wanted 26 | // the Romi32U4 class names to be consistent and we didn't just use a typedef 27 | // to define it because that would make the Doxygen documentation harder to 28 | // understand. 29 | }; 30 | -------------------------------------------------------------------------------- /update_components.sh: -------------------------------------------------------------------------------- 1 | # This script helps us copy all of the library files we need from 2 | # other repositories and keep track of what versions were copied. 3 | 4 | library() 5 | { 6 | LIBDIR=$1 7 | KEYWORDS=$LIBDIR/keywords.txt 8 | COMPONENT_VERSIONS=$LIBDIR/components.txt 9 | 10 | echo -n > $COMPONENT_VERSIONS 11 | (cat $LIBDIR/local_keywords.txt; echo) > $KEYWORDS 12 | } 13 | 14 | copylib() 15 | { 16 | local origin=$1 17 | shift 18 | 19 | local dir=$1 20 | shift 21 | 22 | (cd $dir && git diff --quiet) || { 23 | echo "error: uncommitted changes in $dir" 24 | return 25 | } 26 | 27 | echo Copying from $dir 28 | 29 | # Copy the code files. 30 | until [ -z "$1" ] 31 | do 32 | cp $dir/$1 $LIBDIR 33 | shift 34 | done 35 | 36 | # Copy the keywords. 37 | (cat $dir/keywords.txt; echo) >> $KEYWORDS 38 | 39 | # Add an entry to components.txt. 40 | local ver=`cd $dir && git describe --tags` 41 | echo $origin $ver >> $COMPONENT_VERSIONS 42 | } 43 | 44 | library . 45 | copylib https://github.com/pololu/fastgpio-arduino ../fastgpio-arduino FastGPIO.h 46 | copylib https://github.com/pololu/usb-pause-arduino ../usb-pause-arduino USBPause.h 47 | copylib https://github.com/pololu/pushbutton-arduino ../pushbutton-arduino Pushbutton{.cpp,.h} 48 | copylib https://github.com/pololu/pololu-buzzer-arduino ../pololu-buzzer-arduino/PololuBuzzer PololuBuzzer{.cpp,.h} 49 | copylib https://github.com/pololu/pololu-hd44780-arduino ../pololu-hd44780-arduino PololuHD44780{.cpp,.h} 50 | 51 | 52 | -------------------------------------------------------------------------------- /examples/MotorTest/MotorTest.ino: -------------------------------------------------------------------------------- 1 | // This example drives each motor on the Romi forward, then 2 | // backward. The yellow user LED is on when a motor should be 3 | // running forward and off when a motor should be running 4 | // backward. 5 | 6 | #include 7 | 8 | Romi32U4Motors motors; 9 | Romi32U4ButtonA buttonA; 10 | 11 | void setup() 12 | { 13 | // Wait for the user to press button A. 14 | buttonA.waitForButton(); 15 | 16 | // Delay so that the robot does not move away while the user is 17 | // still touching it. 18 | delay(1000); 19 | } 20 | 21 | void loop() 22 | { 23 | // Run left motor forward. 24 | ledYellow(1); 25 | for (int speed = 0; speed <= 400; speed++) 26 | { 27 | motors.setLeftSpeed(speed); 28 | delay(2); 29 | } 30 | for (int speed = 400; speed >= 0; speed--) 31 | { 32 | motors.setLeftSpeed(speed); 33 | delay(2); 34 | } 35 | 36 | // Run left motor backward. 37 | ledYellow(0); 38 | for (int speed = 0; speed >= -400; speed--) 39 | { 40 | motors.setLeftSpeed(speed); 41 | delay(2); 42 | } 43 | for (int speed = -400; speed <= 0; speed++) 44 | { 45 | motors.setLeftSpeed(speed); 46 | delay(2); 47 | } 48 | 49 | // Run right motor forward. 50 | ledYellow(1); 51 | for (int speed = 0; speed <= 400; speed++) 52 | { 53 | motors.setRightSpeed(speed); 54 | delay(2); 55 | } 56 | for (int speed = 400; speed >= 0; speed--) 57 | { 58 | motors.setRightSpeed(speed); 59 | delay(2); 60 | } 61 | 62 | // Run right motor backward. 63 | ledYellow(0); 64 | for (int speed = 0; speed >= -400; speed--) 65 | { 66 | motors.setRightSpeed(speed); 67 | delay(2); 68 | } 69 | for (int speed = -400; speed <= 0; speed++) 70 | { 71 | motors.setRightSpeed(speed); 72 | delay(2); 73 | } 74 | 75 | delay(500); 76 | } 77 | -------------------------------------------------------------------------------- /examples/Buttons/Buttons.ino: -------------------------------------------------------------------------------- 1 | // This example demonstrates three different ways to 2 | // interface with a user pushbutton on the Romi 32U4. 3 | 4 | #include 5 | 6 | // These objects provide access to the Romi 32U4's 7 | // on-board buttons. 8 | Romi32U4ButtonA buttonA; 9 | Romi32U4ButtonB buttonB; 10 | Romi32U4ButtonC buttonC; 11 | Romi32U4LCD lcd; 12 | 13 | void setup() 14 | { 15 | lcd.clear(); 16 | lcd.print(F("Press A")); 17 | 18 | // Method 1: Use the waitForButton() function, which blocks and 19 | // doesn't return until a button press and release are 20 | // detected. This function takes care of button debouncing. 21 | buttonA.waitForButton(); 22 | 23 | lcd.clear(); 24 | } 25 | 26 | void loop() 27 | { 28 | // Method 2: Directly read the state of the button with the 29 | // isPressed() function. This method is non-blocking and 30 | // provides no debouncing. 31 | if (buttonB.isPressed()) 32 | { 33 | // Whenever the button is pressed, turn on the yellow LED. 34 | ledYellow(1); 35 | } 36 | else 37 | { 38 | // Whenever the button is not pressed, turn off the yellow 39 | // LED. 40 | ledYellow(0); 41 | } 42 | 43 | // Method 3: Call getSingleDebouncedPress() regularly in a 44 | // loop, which returns true to report a single button press or 45 | // false otherwise. This function is non-blocking and takes 46 | // care of button debouncing. 47 | static int cPressedCount = 0; 48 | if (buttonC.getSingleDebouncedPress()) 49 | { 50 | cPressedCount += 1; 51 | Serial.print(F("Button C was pressed ")); 52 | Serial.print(cPressedCount); 53 | Serial.println(F(" times.")); 54 | 55 | lcd.clear(); 56 | lcd.print(cPressedCount); 57 | } 58 | 59 | // If you use non-blocking functions like isPressed() and 60 | // getSingleDebouncedPress(), then you can monitor multiple 61 | // buttons at the same time and also take care of other tasks 62 | // at the same time. In this example, we blink the red LED 63 | // while monitoring the buttons. 64 | ledRed(millis() % 1024 < 100); 65 | } 66 | -------------------------------------------------------------------------------- /examples/InertialSensors/InertialSensors.ino: -------------------------------------------------------------------------------- 1 | // This example reads the raw values from the LSM6DS33 2 | // accelerometer and gyro and prints those raw values to the 3 | // serial monitor. 4 | // 5 | // The accelerometer readings can be converted to units of g 6 | // using the conversion factors specified in the "Mechanical 7 | // characteristics" table in the LSM6DS33 datasheet. We use a 8 | // full scale (FS) setting of +/- 16 g, so the conversion factor 9 | // is 0.488 mg/LSB (least-significant bit). A raw reading of 10 | // 2048 would correspond to 1 g. 11 | // 12 | // The gyro readings can be converted to degrees per second (dps) 13 | // using the "Mechanical characteristics" table in the LSM6DS33 14 | // datasheet. We use a full scale (FS) of +/- 1000 dps so the 15 | // conversion factor is 35 mdps/LSB. A raw reading of 2571 16 | // would correspond to 90 dps. 17 | // 18 | // To run this sketch, you will need to install the LSM6 library: 19 | // 20 | // https://github.com/pololu/lsm6-arduino 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | LSM6 imu; 27 | 28 | char report[120]; 29 | 30 | void setup() 31 | { 32 | Wire.begin(); 33 | 34 | if (!imu.init()) 35 | { 36 | // Failed to detect the LSM6. 37 | ledRed(1); 38 | while(1) 39 | { 40 | Serial.println(F("Failed to detect the LSM6.")); 41 | delay(100); 42 | } 43 | } 44 | 45 | imu.enableDefault(); 46 | 47 | // Set the gyro full scale to 1000 dps because the default 48 | // value is too low, and leave the other settings the same. 49 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 50 | 51 | // Set the accelerometer full scale to 16 g because the default 52 | // value is too low, and leave the other settings the same. 53 | imu.writeReg(LSM6::CTRL1_XL, 0b10000100); 54 | } 55 | 56 | void loop() 57 | { 58 | imu.read(); 59 | 60 | snprintf_P(report, sizeof(report), 61 | PSTR("A: %6d %6d %6d G: %6d %6d %6d"), 62 | imu.a.x, imu.a.y, imu.a.z, 63 | imu.g.x, imu.g.y, imu.g.z); 64 | Serial.println(report); 65 | 66 | delay(100); 67 | } 68 | -------------------------------------------------------------------------------- /USBPause.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see https://www.pololu.com/ 2 | 3 | /*! \file USBPause.h 4 | * 5 | * This is the main file for the USBPause library. 6 | * 7 | * For an overview of this library, see 8 | * https://github.com/pololu/usb-pause-arduino. That is the main repository for 9 | * this library, though copies may exist in other repositories. */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | /*! This class disables USB interrupts in its constructor when it is created and 16 | * restores them to their previous state in its destructor when it is 17 | * destroyed. This class is tailored to the behavior of the Arduino core USB 18 | * code, so it might have to change if that code changes. 19 | * 20 | * This class assumes that the only USB interrupts enabled are general device 21 | * interrupts and endpoint 0 interrupts. 22 | * 23 | * It also assumes that the endpoint 0 interrupts will not enable or disable any 24 | * of the general device interrupts. 25 | */ 26 | class USBPause 27 | { 28 | /// The saved value of the UDIEN register. 29 | uint8_t savedUDIEN; 30 | 31 | /// The saved value of the UENUM register. 32 | uint8_t savedUENUM; 33 | 34 | /// The saved value of the UEIENX register for endpoint 0. 35 | uint8_t savedUEIENX0; 36 | 37 | public: 38 | 39 | USBPause() 40 | { 41 | // Disable the general USB interrupt. This must be done 42 | // first, because the general USB interrupt might change the 43 | // state of the EP0 interrupt, but not the other way around. 44 | savedUDIEN = UDIEN; 45 | UDIEN = 0; 46 | 47 | // Select endpoint 0. 48 | savedUENUM = UENUM; 49 | UENUM = 0; 50 | 51 | // Disable endpoint 0 interrupts. 52 | savedUEIENX0 = UEIENX; 53 | UEIENX = 0; 54 | } 55 | 56 | ~USBPause() 57 | { 58 | // Restore endpoint 0 interrupts. 59 | UENUM = 0; 60 | UEIENX = savedUEIENX0; 61 | 62 | // Restore endpoint selection. 63 | UENUM = savedUENUM; 64 | 65 | // Restore general device interrupt. 66 | UDIEN = savedUDIEN; 67 | } 68 | 69 | }; 70 | -------------------------------------------------------------------------------- /examples/BuzzerBasics/BuzzerBasics.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to make sounds with the buzzer on the 2 | // Romi 32U4. 3 | // 4 | // This example demonstrates the use of the playFrequency(), 5 | // playNote(), and playFromProgramSpace() functions, which play 6 | // entirely in the background, requiring no further action from 7 | // the user once the function is called. The CPU is then free to 8 | // execute other code while the buzzer plays. 9 | // 10 | // This example also shows how to use the stopPlaying() function 11 | // to stop the buzzer, and it shows how to use the isPlaying() 12 | // function to tell whether the buzzer is still playing or not. 13 | 14 | #include 15 | 16 | Romi32U4Buzzer buzzer; 17 | 18 | // Store this song in program space using the PROGMEM macro. 19 | // Later we will play it directly from program space, bypassing 20 | // the need to load it all into RAM first. 21 | const char fugue[] PROGMEM = 22 | "! O5 L16 agafaea dac+adaea fac#e>c#e MS afaf ML gc#gc# MS fdfd ML eee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d" 30 | "e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc# 5 | 6 | #define THROTTLE_PIN 0 // Throttle channel from RC receiver. 7 | #define STEERING_PIN 1 // Steering channel from RC receiver. 8 | 9 | // Maximum motor speed. 10 | #define MAX_SPEED 300 11 | 12 | // Pulse width difference from 1500 us (microseconds) to ignore, 13 | // to compensate for control centering offset. 14 | #define PULSE_WIDTH_DEADBAND 40 15 | 16 | // Pulse width difference from 1500 us to be treated as full 17 | // scale input. For example, a value of 350 means any pulse 18 | // width <= 1150 us or >= 1850 us is considered full scale. 19 | #define PULSE_WIDTH_RANGE 350 20 | 21 | Romi32U4Motors motors; 22 | 23 | void setup() 24 | { 25 | } 26 | 27 | void loop() 28 | { 29 | // Read one pulse from each pin. 30 | int throttle = pulseIn(THROTTLE_PIN, HIGH); 31 | int steering = pulseIn(STEERING_PIN, HIGH); 32 | 33 | int leftSpeed, rightSpeed; 34 | 35 | if (throttle > 0 && steering > 0) 36 | { 37 | // Both RC signals are good. Turn on the yellow LED. 38 | ledYellow(1); 39 | 40 | // RC signals encode information in pulse width centered on 41 | // 1500 us; subtract 1500 to get a value centered on 0. 42 | throttle -= 1500; 43 | steering -= 1500; 44 | 45 | // Apply deadband. 46 | if (abs(throttle) <= PULSE_WIDTH_DEADBAND) 47 | { 48 | throttle = 0; 49 | } 50 | if (abs(steering) <= PULSE_WIDTH_DEADBAND) 51 | { 52 | steering = 0; 53 | } 54 | 55 | // Convert from pulse widths to speeds. 56 | int throttleSpeed = (long)throttle * MAX_SPEED / PULSE_WIDTH_RANGE; 57 | int steeringSpeed = (long)steering * MAX_SPEED / PULSE_WIDTH_RANGE; 58 | 59 | // Mix throttle and steering inputs to obtain left & right 60 | // motor speeds. 61 | leftSpeed = throttleSpeed - steeringSpeed; 62 | rightSpeed = throttleSpeed + steeringSpeed; 63 | 64 | // Cap the speeds to their maximum values. 65 | leftSpeed = constrain(leftSpeed, -MAX_SPEED, MAX_SPEED); 66 | rightSpeed = constrain(rightSpeed, -MAX_SPEED, MAX_SPEED); 67 | } 68 | else 69 | { 70 | // At least one RC signal is not good. Turn off the LED and 71 | // stop motors. 72 | ledYellow(0); 73 | 74 | leftSpeed = 0; 75 | rightSpeed = 0; 76 | } 77 | 78 | motors.setSpeeds(leftSpeed, rightSpeed); 79 | } 80 | -------------------------------------------------------------------------------- /examples/RotationResist/RotationResist.ino: -------------------------------------------------------------------------------- 1 | // This demo shows how the Romi can use its gyroscope to detect 2 | // when it is being rotated, and use the motors to resist that 3 | // rotation. If you place the robot on a rotating platter and 4 | // spin it, it should keep pointing in approximately the same 5 | // direction. 6 | // 7 | // This code was tested on a Romi with a 120:1 Mini Plastic 8 | // Gearmotor HP and 6 NiMH batteries. If you have different 9 | // motors or batteries, you might need to adjust the PID 10 | // constants. 11 | // 12 | // Be careful to not move the robot for a few seconds after 13 | // starting it while the gyro is being calibrated. During the 14 | // gyro calibration, the yellow LED is on and the words "Gyro 15 | // cal" are displayed on the LCD. 16 | // 17 | // After the gyro calibration is done, press button A to start 18 | // the demo. If you try to turn the Romi, or put it on a surface 19 | // that is turning, it will drive its motors to counteract the 20 | // turning. 21 | // 22 | // This sketch only uses the Z axis of the gyro, so it is 23 | // possible to pick up the Romi, rotate it about its X and Y 24 | // axes, and then put it down facing in a new position. 25 | // 26 | // To run this sketch, you will need to install the LSM6 library: 27 | // 28 | // https://github.com/pololu/lsm6-arduino 29 | 30 | 31 | #include 32 | #include 33 | #include 34 | #include "TurnSensor.h" 35 | 36 | const int16_t maxSpeed = 300; 37 | 38 | Romi32U4LCD lcd; 39 | Romi32U4ButtonA buttonA; 40 | Romi32U4Motors motors; 41 | LSM6 imu; 42 | 43 | void setup() 44 | { 45 | turnSensorSetup(); 46 | delay(500); 47 | turnSensorReset(); 48 | 49 | lcd.clear(); 50 | lcd.print(F("Try to")); 51 | lcd.gotoXY(0, 1); 52 | lcd.print(F("turn me!")); 53 | } 54 | 55 | void loop() 56 | { 57 | // Read the gyro to update turnAngle, the estimation of how far 58 | // the robot has turned, and turnRate, the estimation of how 59 | // fast it is turning. 60 | turnSensorUpdate(); 61 | 62 | // Calculate the motor turn speed using proportional and 63 | // derivative PID terms. Here we are a using a proportional 64 | // constant of 15 and a derivative constant of 1/73. 65 | int32_t turnSpeed = -(int32_t)turnAngle / (turnAngle1 / 15) 66 | - turnRate / 73; 67 | 68 | // Constrain our motor speeds to be between 69 | // -maxSpeed and maxSpeed. 70 | turnSpeed = constrain(turnSpeed, -maxSpeed, maxSpeed); 71 | 72 | motors.setSpeeds(-turnSpeed, turnSpeed); 73 | } 74 | -------------------------------------------------------------------------------- /Romi32U4Motors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define PWM_L 10 8 | #define PWM_R 9 9 | #define DIR_L 16 10 | #define DIR_R 15 11 | 12 | bool Romi32U4Motors::flipLeft = false; 13 | bool Romi32U4Motors::flipRight = false; 14 | 15 | int16_t Romi32U4Motors::maxSpeed = 300; 16 | 17 | // initialize timer1 to generate the proper PWM outputs to the motor drivers 18 | void Romi32U4Motors::init2() 19 | { 20 | FastGPIO::Pin::setOutputLow(); 21 | FastGPIO::Pin::setOutputLow(); 22 | FastGPIO::Pin::setOutputLow(); 23 | FastGPIO::Pin::setOutputLow(); 24 | 25 | // Timer 1 configuration 26 | // prescaler: clockI/O / 1 27 | // outputs enabled 28 | // phase-correct PWM 29 | // top of 400 30 | // 31 | // PWM frequency calculation 32 | // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz 33 | TCCR1A = 0b10100000; 34 | TCCR1B = 0b00010001; 35 | ICR1 = 400; 36 | OCR1A = 0; 37 | OCR1B = 0; 38 | } 39 | 40 | void Romi32U4Motors::flipLeftMotor(bool flip) 41 | { 42 | flipLeft = flip; 43 | } 44 | 45 | void Romi32U4Motors::flipRightMotor(bool flip) 46 | { 47 | flipRight = flip; 48 | } 49 | 50 | void Romi32U4Motors::setLeftSpeed(int16_t speed) 51 | { 52 | init(); 53 | 54 | bool reverse = 0; 55 | 56 | if (speed < 0) 57 | { 58 | speed = -speed; // Make speed a positive quantity. 59 | reverse = 1; // Preserve the direction. 60 | } 61 | if (speed > maxSpeed) 62 | { 63 | speed = maxSpeed; 64 | } 65 | 66 | OCR1B = speed; 67 | 68 | FastGPIO::Pin::setOutput(reverse ^ flipLeft); 69 | } 70 | 71 | void Romi32U4Motors::setRightSpeed(int16_t speed) 72 | { 73 | init(); 74 | 75 | bool reverse = 0; 76 | 77 | if (speed < 0) 78 | { 79 | speed = -speed; // Make speed a positive quantity. 80 | reverse = 1; // Preserve the direction. 81 | } 82 | if (speed > maxSpeed) 83 | { 84 | speed = maxSpeed; 85 | } 86 | 87 | OCR1A = speed; 88 | 89 | FastGPIO::Pin::setOutput(reverse ^ flipRight); 90 | } 91 | 92 | void Romi32U4Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) 93 | { 94 | setLeftSpeed(leftSpeed); 95 | setRightSpeed(rightSpeed); 96 | } 97 | 98 | void Romi32U4Motors::allowTurbo(bool turbo) 99 | { 100 | maxSpeed = turbo ? 400 : 300; 101 | } 102 | -------------------------------------------------------------------------------- /Romi32U4Buttons.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /** \file Romi32U4Buttons.h **/ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | /*! The pin number for the pin connected to button A on the Romi 32U4. */ 13 | #define ROMI_32U4_BUTTON_A 14 14 | 15 | /*! The pin number for the pin connected to button B on the Romi 32U4. Note 16 | * that this is not an official Arduino pin number so it cannot be used with 17 | * functions like digitalRead, but it can be used with the FastGPIO library. */ 18 | #define ROMI_32U4_BUTTON_B IO_D5 19 | 20 | /*! The pin number for the pin conencted to button C on the Romi 32U4. */ 21 | #define ROMI_32U4_BUTTON_C 17 22 | 23 | /*! \brief Interfaces with button A on the Romi 32U4. */ 24 | class Romi32U4ButtonA : public Pushbutton 25 | { 26 | public: 27 | Romi32U4ButtonA() : Pushbutton(ROMI_32U4_BUTTON_A) 28 | { 29 | } 30 | }; 31 | 32 | /*! \brief Interfaces with button B on the Romi 32U4. 33 | * 34 | * The pin used for button B is also used for the TX LED. 35 | * 36 | * This class temporarily disables USB interrupts because the Arduino core code 37 | * has USB interrupts enabled that sometimes write to the pin this button is on. 38 | * 39 | * This class temporarily sets the pin to be an input without a pull-up 40 | * resistor. The pull-up resistor is not needed because of the resistors on the 41 | * board. */ 42 | class Romi32U4ButtonB : public PushbuttonBase 43 | { 44 | public: 45 | 46 | virtual bool isPressed() 47 | { 48 | USBPause usbPause; 49 | FastGPIO::PinLoan loan; 50 | FastGPIO::Pin::setInputPulledUp(); 51 | _delay_us(3); 52 | return !FastGPIO::Pin::isInputHigh(); 53 | } 54 | }; 55 | 56 | /*! \brief Interfaces with button C on the Romi 32U4. 57 | * 58 | * The pin used for button C is also used for the RX LED. 59 | * 60 | * This class temporarily disables USB interrupts because the Arduino core code 61 | * has USB interrupts enabled that sometimes write to the pin this button is on. 62 | * 63 | * This class temporarily sets the pin to be an input without a pull-up 64 | * resistor. The pull-up resistor is not needed because of the resistors on the 65 | * board. */ 66 | class Romi32U4ButtonC : public PushbuttonBase 67 | { 68 | public: 69 | 70 | virtual bool isPressed() 71 | { 72 | USBPause usbPause; 73 | FastGPIO::PinLoan loan; 74 | FastGPIO::Pin::setInputPulledUp(); 75 | _delay_us(3); 76 | return !FastGPIO::Pin::isInputHigh(); 77 | } 78 | }; 79 | 80 | -------------------------------------------------------------------------------- /Romi32U4LCD.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Romi32U4LCD.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | /*! \brief Writes data to the LCD on the Romi 32U4. 12 | * 13 | * This library is similar to the Arduino 14 | * [LiquidCrystal library](http://arduino.cc/en/Reference/LiquidCrystal), 15 | * but it has some extra features needed on the Romi 32U4: 16 | * 17 | * * This class disables USB interrupts temporarily while writing to the LCD so 18 | * that the USB interrupts will not change the RXLED and TXLED pins, which 19 | * double as LCD data lines. 20 | * * This class restores the RS, DB4, DB5, DB6, and DB7 pins to their previous 21 | * states when it is done using them so that those pins can also be used for 22 | * other purposes such as controlling LEDs. 23 | * 24 | * This class inherits from the Arduino Print class, so you can call the 25 | * `print()` function on it with a variety of arguments. See the 26 | * [Arduino print() documentation](http://arduino.cc/en/Serial/Print) 27 | * for more information. 28 | * 29 | * For detailed information about HD44780 LCD interface, including what 30 | * characters can be displayed, see the 31 | * [HD44780 datasheet](http://www.pololu.com/file/0J72/HD44780.pdf). 32 | */ 33 | class Romi32U4LCD : public PololuHD44780Base 34 | { 35 | // Pin assignments 36 | static const uint8_t rs = 4, e = 11, db4 = 14, db5 = 17, db6 = 13, db7 = IO_D5; 37 | 38 | public: 39 | 40 | virtual void initPins() 41 | { 42 | FastGPIO::Pin::setOutputLow(); 43 | } 44 | 45 | virtual void send(uint8_t data, bool rsValue, bool only4bits) 46 | { 47 | // Temporarily disable USB interrupts because they write some pins 48 | // we are using as LCD pins. 49 | USBPause usbPause; 50 | 51 | // Save the state of the RS and data pins. The state automatically 52 | // gets restored before this function returns. 53 | FastGPIO::PinLoan loanRS; 54 | FastGPIO::PinLoan loanDB4; 55 | FastGPIO::PinLoan loanDB5; 56 | FastGPIO::PinLoan loanDB6; 57 | FastGPIO::PinLoan loanDB7; 58 | 59 | // Drive the RS pin high or low. 60 | FastGPIO::Pin::setOutput(rsValue); 61 | 62 | // Send the data. 63 | if (!only4bits) { sendNibble(data >> 4); } 64 | sendNibble(data & 0x0F); 65 | } 66 | 67 | private: 68 | 69 | void sendNibble(uint8_t data) 70 | { 71 | FastGPIO::Pin::setOutput(data >> 0 & 1); 72 | FastGPIO::Pin::setOutput(data >> 1 & 1); 73 | FastGPIO::Pin::setOutput(data >> 2 & 1); 74 | FastGPIO::Pin::setOutput(data >> 3 & 1); 75 | 76 | FastGPIO::Pin::setOutputHigh(); 77 | _delay_us(1); // Must be at least 450 ns. 78 | FastGPIO::Pin::setOutputLow(); 79 | _delay_us(1); // Must be at least 550 ns. 80 | } 81 | }; 82 | 83 | -------------------------------------------------------------------------------- /Romi32U4.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Romi32U4.h 4 | * 5 | * \brief Main header file for the Romi32U4 library. 6 | * 7 | * This file includes all the other headers files provided by the library. 8 | */ 9 | 10 | #pragma once 11 | 12 | #ifndef __AVR_ATmega32U4__ 13 | #error "This library only supports the ATmega32U4. Try selecting A-Star 32U4 in the Boards menu." 14 | #endif 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // TODO: servo support 24 | 25 | /*! \brief Turns the red user LED (RX) on or off. 26 | 27 | @param on 1 to turn on the LED, 0 to turn it off. 28 | 29 | The red user LED is on pin 17, which is also known as PB0, SS, and RXLED. The 30 | Arduino core code uses this LED to indicate when it receives data over USB, so 31 | it might be hard to control this LED when USB is connected. */ 32 | inline void ledRed(bool on) 33 | { 34 | FastGPIO::Pin<17>::setOutput(!on); 35 | } 36 | 37 | /*! \brief Turns the yellow user LED on pin 13 on or off. 38 | 39 | @param on 1 to turn on the LED, 0 to turn it off. */ 40 | inline void ledYellow(bool on) 41 | { 42 | FastGPIO::Pin<13>::setOutput(on); 43 | } 44 | 45 | /*! \brief Turns the green user LED (TX) on or off. 46 | 47 | @param on 1 to turn on the LED, 0 to turn it off. 48 | 49 | The green user LED is pin PD5, which is also known as TXLED. The Arduino core 50 | code uses this LED to indicate when it receives data over USB, so it might be 51 | hard to control this LED when USB is connected. */ 52 | inline void ledGreen(bool on) 53 | { 54 | FastGPIO::Pin::setOutput(!on); 55 | } 56 | 57 | /*! \brief Returns true if USB power is detected. 58 | 59 | This function returns true if power is detected on the board's USB port and 60 | returns false otherwise. It uses the ATmega32U4's VBUS line, which is directly 61 | connected to the power pin of the USB connector. 62 | 63 | \sa A method for detecting whether the board's virtual COM port is open: 64 | http://arduino.cc/en/Serial/IfSerial */ 65 | inline bool usbPowerPresent() 66 | { 67 | return USBSTA >> VBUS & 1; 68 | } 69 | 70 | /*! \brief Reads the battery voltage and returns it in millivolts. 71 | 72 | If this function returns a number below 5500, the actual battery voltage might 73 | be significantly lower than the value returned. */ 74 | inline uint16_t readBatteryMillivolts() 75 | { 76 | const uint8_t sampleCount = 8; 77 | uint16_t sum = 0; 78 | for (uint8_t i = 0; i < sampleCount; i++) 79 | { 80 | sum += analogRead(A1); 81 | } 82 | 83 | // VBAT = 3 * millivolt reading = 3 * raw * 5000/1024 84 | // = raw * 1875 / 128 85 | // The correction term below makes it so that we round to the 86 | // nearest whole number instead of always rounding down. 87 | const uint32_t correction = 64 * sampleCount - 1; 88 | return ((uint32_t)sum * 1875 + correction) / (128 * sampleCount); 89 | } 90 | -------------------------------------------------------------------------------- /examples/Encoders/Encoders.ino: -------------------------------------------------------------------------------- 1 | // This program shows how to read the encoders on the Romi 32U4. 2 | // The encoders can tell you how far, and in which direction each 3 | // motor has turned. 4 | // 5 | // You can press button A on the Romi to drive both motors 6 | // forward at full speed. You can press button C to drive both 7 | // motors in reverse at full speed. 8 | // 9 | // Encoder counts are printed to the LCD and to the serial 10 | // monitor. 11 | // 12 | // On the LCD, the top line shows the counts from the left 13 | // encoder, and the bottom line shows the counts from the right 14 | // encoder. Encoder errors should not happen, but if one does 15 | // happen then the buzzer will beep and an exclamation mark will 16 | // appear temporarily on the LCD. 17 | // 18 | // In the serial monitor, the first and second numbers represent 19 | // counts from the left and right encoders, respectively. The 20 | // third and fourth numbers represent errors from the left and 21 | // right encoders, respectively. 22 | 23 | #include 24 | 25 | Romi32U4Encoders encoders; 26 | Romi32U4LCD lcd; 27 | Romi32U4Buzzer buzzer; 28 | Romi32U4Motors motors; 29 | Romi32U4ButtonA buttonA; 30 | Romi32U4ButtonC buttonC; 31 | 32 | const char encoderErrorLeft[] PROGMEM = "!= 100) 49 | { 50 | lastDisplayTime = millis(); 51 | 52 | int16_t countsLeft = encoders.getCountsLeft(); 53 | int16_t countsRight = encoders.getCountsRight(); 54 | 55 | bool errorLeft = encoders.checkErrorLeft(); 56 | bool errorRight = encoders.checkErrorRight(); 57 | 58 | if (errorLeft) 59 | { 60 | // An error occurred on the left encoder channel. 61 | // Display it on the LCD for the next 10 iterations and 62 | // also beep. 63 | displayErrorLeftCountdown = 10; 64 | buzzer.playFromProgramSpace(encoderErrorLeft); 65 | } 66 | 67 | if (errorRight) 68 | { 69 | // An error occurred on the right encoder channel. 70 | // Display it on the LCD for the next 10 iterations and 71 | // also beep. 72 | displayErrorRightCountdown = 10; 73 | buzzer.playFromProgramSpace(encoderErrorRight); 74 | } 75 | 76 | // Update the LCD with encoder counts and error info. 77 | lcd.clear(); 78 | lcd.print(countsLeft); 79 | lcd.gotoXY(0, 1); 80 | lcd.print(countsRight); 81 | 82 | if (displayErrorLeftCountdown) 83 | { 84 | // Show an exclamation point on the first line to 85 | // indicate an error from the left encoder. 86 | lcd.gotoXY(7, 0); 87 | lcd.print('!'); 88 | displayErrorLeftCountdown--; 89 | } 90 | 91 | if (displayErrorRightCountdown) 92 | { 93 | // Show an exclamation point on the second line to 94 | // indicate an error from the left encoder. 95 | lcd.gotoXY(7, 1); 96 | lcd.print('!'); 97 | displayErrorRightCountdown--; 98 | } 99 | 100 | // Send the information to the serial monitor also. 101 | snprintf_P(report, sizeof(report), 102 | PSTR("%6d %6d %1d %1d"), 103 | countsLeft, countsRight, errorLeft, errorRight); 104 | Serial.println(report); 105 | } 106 | 107 | if (buttonA.isPressed()) 108 | { 109 | motors.setSpeeds(300, 300); 110 | } 111 | else if (buttonC.isPressed()) 112 | { 113 | motors.setSpeeds(-300, -300); 114 | } 115 | else 116 | { 117 | motors.setSpeeds(0, 0); 118 | } 119 | } 120 | -------------------------------------------------------------------------------- /Pushbutton.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include "Pushbutton.h" 4 | 5 | // \cond 6 | 7 | PushbuttonStateMachine::PushbuttonStateMachine() 8 | { 9 | state = 0; 10 | } 11 | 12 | // state 0: The value is considered to be true. 13 | // state 1: The value was considered to be true, but there 14 | // was a recent false reading so it might be falling. 15 | // state 2: The value is considered to be false. 16 | // state 3: The value was considered to be false, but there 17 | // was a recent true reading so it might be rising. 18 | // 19 | // The transition from state 3 to state 0 is the point where we 20 | // have successfully detected a rising edge an we return true. 21 | // 22 | // The prevTimeMillis variable holds the last time that we 23 | // transitioned to states 1 or 3. 24 | bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value) 25 | { 26 | uint16_t timeMillis = millis(); 27 | 28 | switch (state) 29 | { 30 | case 0: 31 | // If value is false, proceed to the next state. 32 | if (!value) 33 | { 34 | prevTimeMillis = timeMillis; 35 | state = 1; 36 | } 37 | break; 38 | 39 | case 1: 40 | if (value) 41 | { 42 | // The value is true or bouncing, so go back to previous (initial) 43 | // state. 44 | state = 0; 45 | } 46 | else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15) 47 | { 48 | // It has been at least 15 ms and the value is still false, so 49 | // proceed to the next state. 50 | state = 2; 51 | } 52 | break; 53 | 54 | case 2: 55 | // If the value is true, proceed to the next state. 56 | if (value) 57 | { 58 | prevTimeMillis = timeMillis; 59 | state = 3; 60 | } 61 | break; 62 | 63 | case 3: 64 | if (!value) 65 | { 66 | // The value is false or bouncing, so go back to previous state. 67 | state = 2; 68 | } 69 | else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15) 70 | { 71 | // It has been at least 15 ms and the value is still true, so 72 | // go back to the initial state and report this rising edge. 73 | state = 0; 74 | return true; 75 | } 76 | break; 77 | } 78 | 79 | return false; 80 | } 81 | 82 | // \endcond 83 | 84 | void PushbuttonBase::waitForPress() 85 | { 86 | do 87 | { 88 | while (!isPressed()); // wait for button to be pressed 89 | delay(10); // debounce the button press 90 | } 91 | while (!isPressed()); // if button isn't still pressed, loop 92 | } 93 | 94 | void PushbuttonBase::waitForRelease() 95 | { 96 | do 97 | { 98 | while (isPressed()); // wait for button to be released 99 | delay(10); // debounce the button release 100 | } 101 | while (isPressed()); // if button isn't still released, loop 102 | } 103 | 104 | void PushbuttonBase::waitForButton() 105 | { 106 | waitForPress(); 107 | waitForRelease(); 108 | } 109 | 110 | bool PushbuttonBase::getSingleDebouncedPress() 111 | { 112 | return pressState.getSingleDebouncedRisingEdge(isPressed()); 113 | } 114 | 115 | bool PushbuttonBase::getSingleDebouncedRelease() 116 | { 117 | return releaseState.getSingleDebouncedRisingEdge(!isPressed()); 118 | } 119 | 120 | Pushbutton::Pushbutton(uint8_t pin, uint8_t pullUp, uint8_t defaultState) 121 | { 122 | initialized = false; 123 | _pin = pin; 124 | _pullUp = pullUp; 125 | _defaultState = defaultState; 126 | } 127 | 128 | void Pushbutton::init2() 129 | { 130 | if (_pullUp == PULL_UP_ENABLED) 131 | { 132 | pinMode(_pin, INPUT_PULLUP); 133 | } 134 | else 135 | { 136 | pinMode(_pin, INPUT); // high impedance 137 | } 138 | 139 | delayMicroseconds(5); // give pull-up time to stabilize 140 | } 141 | 142 | bool Pushbutton::isPressed() 143 | { 144 | init(); // initialize if needed 145 | return digitalRead(_pin) != _defaultState; 146 | } 147 | -------------------------------------------------------------------------------- /examples/FaceUphill/FaceUphill.ino: -------------------------------------------------------------------------------- 1 | // This demo uses the Romi 32U4's accelerometer to detect whether 2 | // it is on a slanted surface. If it is on a slanted surface, 3 | // then it uses the motors to face uphill. 4 | // 5 | // It also uses the encoders to avoid rolling down the surface. 6 | // 7 | // Please note that this example is not very robust and it might 8 | // be hard to modify it to behave differently. The accelerometer 9 | // readings are affected by the movement of the Romi, so if you 10 | // change the code for controlling the motors, you might also affect 11 | // the accelerometer readings. 12 | // 13 | // Also, if the robot is pointing directly downhill, it might not 14 | // move, because the y component of the acceleration would be 15 | // close to 0. 16 | // 17 | // To run this sketch, you will need to install the LSM6 library: 18 | // 19 | // https://github.com/pololu/lsm6-arduino 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | const int16_t maxSpeed = 150; 26 | 27 | Romi32U4Motors motors; 28 | Romi32U4LCD lcd; 29 | Romi32U4ButtonA buttonA; 30 | Romi32U4Encoders encoders; 31 | LSM6 imu; 32 | 33 | void setup() 34 | { 35 | // Start I2C and initialize the LSM6 accelerometer/gyro. 36 | Wire.begin(); 37 | imu.init(); 38 | imu.enableDefault(); 39 | 40 | // Set the gyro full scale to 1000 dps because the default 41 | // value is too low, and leave the other settings the same. 42 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 43 | 44 | // Set the accelerometer full scale to 16 g because the default 45 | // value is too low, and leave the other settings the same. 46 | imu.writeReg(LSM6::CTRL1_XL, 0b10000100); 47 | 48 | lcd.clear(); 49 | lcd.print(F("Press A")); 50 | buttonA.waitForPress(); 51 | lcd.clear(); 52 | } 53 | 54 | void loop() 55 | { 56 | // Read the acceleration from the LSM6DS33. 57 | // A value of 2048 corresponds to approximately 1 g. 58 | imu.read(); 59 | int16_t x = imu.a.x; 60 | int16_t y = imu.a.y; 61 | int32_t magnitudeSquared = (int32_t)x * x + (int32_t)y * y; 62 | 63 | // Display the X and Y acceleration values on the LCD 64 | // every 150 ms. 65 | static uint8_t lastDisplayTime; 66 | if ((uint8_t)(millis() - lastDisplayTime) > 150) 67 | { 68 | lastDisplayTime = millis(); 69 | lcd.gotoXY(0, 0); 70 | lcd.print(x); 71 | lcd.print(F(" ")); 72 | lcd.gotoXY(0, 1); 73 | lcd.print(y); 74 | lcd.print(F(" ")); 75 | } 76 | 77 | // Use the encoders to see how much we should drive forward. 78 | // If the robot rolls downhill, the encoder counts will become 79 | // negative, resulting in a positive forwardSpeed to counteract 80 | // the rolling. 81 | int16_t forwardSpeed = -(encoders.getCountsLeft() + encoders.getCountsRight()); 82 | forwardSpeed = constrain(forwardSpeed, -maxSpeed, maxSpeed); 83 | 84 | // See if we are actually on an incline. 85 | // 2048 * sin(5 deg) = 178 86 | int16_t turnSpeed; 87 | if (magnitudeSquared > (int32_t)178 * 178) 88 | { 89 | // We are on an incline of more than 5 degrees, so 90 | // try to face uphill using a feedback algorithm. 91 | turnSpeed = y / 2; 92 | ledYellow(1); 93 | } 94 | else 95 | { 96 | // We not on a noticeable incline, so don't turn. 97 | turnSpeed = 0; 98 | ledYellow(0); 99 | } 100 | 101 | // To face uphill, we need to turn so that the X acceleration 102 | // is negative and the Y acceleration is 0. Therefore, when 103 | // the Y acceleration is positive, we want to turn to the 104 | // left (counter-clockwise). 105 | int16_t leftSpeed = forwardSpeed - turnSpeed; 106 | int16_t rightSpeed = forwardSpeed + turnSpeed; 107 | 108 | // Constrain the speeds to be between -maxSpeed and maxSpeed. 109 | leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed); 110 | rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed); 111 | 112 | motors.setSpeeds(leftSpeed, rightSpeed); 113 | } 114 | -------------------------------------------------------------------------------- /Romi32U4Motors.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Romi32U4Motors.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Controls motor speed and direction on the Romi 32U4. 10 | * 11 | * This library uses Timer 1, so it will conflict with any other libraries using 12 | * that timer. */ 13 | class Romi32U4Motors 14 | { 15 | public: 16 | 17 | /** \brief Flips the direction of the left motor. 18 | * 19 | * You can call this function with an argument of \c true if the left motor 20 | * of your Romi was not wired in the standard way and you want a 21 | * positive speed argument to correspond to forward movement. 22 | * 23 | * \param flip If true, then positive motor speeds will correspond to the 24 | * direction pin being high. If false, then positive motor speeds will 25 | * correspond to the direction pin being low. 26 | */ 27 | static void flipLeftMotor(bool flip); 28 | 29 | /** \brief Flips the direction of the right motor. 30 | * 31 | * You can call this function with an argument of \c true if the right motor 32 | * of your Romi was not wired in the standard way and you want a 33 | * positive speed argument to correspond to forward movement. 34 | * 35 | * \param flip If true, then positive motor speeds will correspond to the 36 | * direction pin being high. If false, then positive motor speeds will 37 | * correspond to the direction pin being low. */ 38 | static void flipRightMotor(bool flip); 39 | 40 | /** \brief Sets the speed for the left motor. 41 | * 42 | * \param speed A number from -300 to 300 representing the speed and 43 | * direction of the left motor. Values of -300 or less result in full speed 44 | * reverse, and values of 300 or more result in full speed forward. */ 45 | static void setLeftSpeed(int16_t speed); 46 | 47 | /** \brief Sets the speed for the right motor. 48 | * 49 | * \param speed A number from -300 to 300 representing the speed and 50 | * direction of the right motor. Values of -300 or less result in full speed 51 | * reverse, and values of 300 or more result in full speed forward. */ 52 | static void setRightSpeed(int16_t speed); 53 | 54 | /** \brief Sets the speed for both motors. 55 | * 56 | * \param leftSpeed A number from -300 to 300 representing the speed and 57 | * direction of the right motor. Values of -300 or less result in full speed 58 | * reverse, and values of 300 or more result in full speed forward. 59 | * \param rightSpeed A number from -300 to 300 representing the speed and 60 | * direction of the right motor. Values of -300 or less result in full speed 61 | * reverse, and values of 300 or more result in full speed forward. */ 62 | static void setSpeeds(int16_t leftSpeed, int16_t rightSpeed); 63 | 64 | /** \brief Turns turbo mode on or off. 65 | * 66 | * By default turbo mode is off. When turbo mode is on, the range of speeds 67 | * accepted by the other functions in this library becomes -400 to 400 68 | * (instead of -300 to 300). Turning turbo mode on allows the Romi to move 69 | * faster but could decrease the lifetime of the motors. 70 | * 71 | * This function does not have any immediate effect on the speed of the 72 | * motors; it just changes the behavior of the other functions in this 73 | * library. 74 | * 75 | * \param turbo If true, turns turbo mode on. 76 | * If false, turns turbo mode off. */ 77 | static void allowTurbo(bool turbo); 78 | 79 | private: 80 | 81 | static inline void init() 82 | { 83 | static bool initialized = false; 84 | 85 | if (!initialized) 86 | { 87 | initialized = true; 88 | init2(); 89 | } 90 | } 91 | 92 | static void init2(); 93 | 94 | static int16_t maxSpeed; 95 | static bool flipLeft; 96 | static bool flipRight; 97 | }; 98 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Romi32U4LCD KEYWORD1 2 | 3 | ROMI_32U4_BUTTON_A LITERAL1 4 | ROMI_32U4_BUTTON_B LITERAL1 5 | ROMI_32U4_BUTTON_C LITERAL1 6 | Romi32U4ButtonA KEYWORD1 7 | Romi32U4ButtonB KEYWORD1 8 | Romi32U4ButtonC KEYWORD1 9 | 10 | Romi32U4Buzzer KEYWORD1 11 | 12 | Romi32U4Motors KEYWORD1 13 | flipLeftMotor KEYWORD2 14 | flipRightMotor KEYWORD2 15 | setLeftSpeed KEYWORD2 16 | setRightSpeed KEYWORD2 17 | setSpeeds KEYWORD2 18 | allowTurbo KEYWORD2 19 | 20 | Romi32U4Encoders KEYWORD1 21 | init KEYWORD2 22 | getCountsLeft KEYWORD2 23 | getCountsRight KEYWORD2 24 | getCountsAndResetLeft KEYWORD2 25 | getCountsAndResetRight KEYWORD2 26 | checkErrorLeft KEYWORD2 27 | checkErrorRight KEYWORD2 28 | 29 | ledRed KEYWORD2 30 | ledGreen KEYWORD2 31 | ledYellow KEYWORD2 32 | usbPowerPresent KEYWORD2 33 | readBatteryMillivolts KEYWORD2 34 | 35 | FastGPIO KEYWORD1 36 | Pin KEYWORD1 37 | 38 | setOutputLow KEYWORD2 39 | setOutputHigh KEYWORD2 40 | setOutputToggle KEYWORD2 41 | setOutput KEYWORD2 42 | setOutputValueLow KEYWORD2 43 | setOutputValueHigh KEYWORD2 44 | setOutputValueToggle KEYWORD2 45 | setOutputValue KEYWORD2 46 | setInput KEYWORD2 47 | setInputPulledUp KEYWORD2 48 | isInputHigh KEYWORD2 49 | isOutput KEYWORD2 50 | isOutputValueHigh KEYWORD2 51 | getState KEYWORD2 52 | setState KEYWORD2 53 | 54 | PinLoan KEYWORD1 55 | 56 | IO_B0 LITERAL1 57 | IO_B1 LITERAL1 58 | IO_B2 LITERAL1 59 | IO_B3 LITERAL1 60 | IO_B4 LITERAL1 61 | IO_B5 LITERAL1 62 | IO_B6 LITERAL1 63 | IO_B7 LITERAL1 64 | IO_C0 LITERAL1 65 | IO_C1 LITERAL1 66 | IO_C2 LITERAL1 67 | IO_C3 LITERAL1 68 | IO_C4 LITERAL1 69 | IO_C5 LITERAL1 70 | IO_C6 LITERAL1 71 | IO_C7 LITERAL1 72 | IO_D0 LITERAL1 73 | IO_D1 LITERAL1 74 | IO_D2 LITERAL1 75 | IO_D3 LITERAL1 76 | IO_D4 LITERAL1 77 | IO_D5 LITERAL1 78 | IO_D6 LITERAL1 79 | IO_D7 LITERAL1 80 | IO_E0 LITERAL1 81 | IO_E2 LITERAL1 82 | IO_E6 LITERAL1 83 | IO_F0 LITERAL1 84 | IO_F1 LITERAL1 85 | IO_F4 LITERAL1 86 | IO_F5 LITERAL1 87 | IO_F6 LITERAL1 88 | IO_F7 LITERAL1 89 | IO_NONE LITERAL1 90 | 91 | USBPause KEYWORD1 92 | Pushbutton KEYWORD1 93 | 94 | waitForPress KEYWORD2 95 | waitForRelease KEYWORD2 96 | waitForButton KEYWORD2 97 | isPressed KEYWORD2 98 | getSingleDebouncedPress KEYWORD2 99 | getSingleDebouncedRelease KEYWORD2 100 | 101 | PushbuttonBase KEYWORD1 102 | 103 | PushbuttonStateMachine KEYWORD1 104 | 105 | getSingleDebouncedRisingEdge KEYWORD2 106 | 107 | ZUMO_BUTTON LITERAL1 108 | PULL_UP_DISABLED LITERAL1 109 | PULL_UP_ENABLED LITERAL1 110 | DEFAULT_STATE_LOW LITERAL1 111 | DEFAULT_STATE_HIGH LITERAL1 112 | 113 | PololuBuzzer KEYWORD1 114 | 115 | playFrequency KEYWORD2 116 | playNote KEYWORD2 117 | play KEYWORD2 118 | playFromProgramSpace KEYWORD2 119 | isPlaying KEYWORD2 120 | stopPlaying KEYWORD2 121 | playMode KEYWORD2 122 | playCheck KEYWORD2 123 | 124 | PLAY_AUTOMATIC LITERAL1 125 | PLAY_CHECK LITERAL1 126 | NOTE_C LITERAL1 127 | NOTE_C_SHARP LITERAL1 128 | NOTE_D_FLAT LITERAL1 129 | NOTE_D LITERAL1 130 | NOTE_D_SHARP LITERAL1 131 | NOTE_E_FLAT LITERAL1 132 | NOTE_E LITERAL1 133 | NOTE_F LITERAL1 134 | NOTE_F_SHARP LITERAL1 135 | NOTE_G_FLAT LITERAL1 136 | NOTE_G LITERAL1 137 | NOTE_G_SHARP LITERAL1 138 | NOTE_A_FLAT LITERAL1 139 | NOTE_A LITERAL1 140 | NOTE_A_SHARP LITERAL1 141 | NOTE_B_FLAT LITERAL1 142 | NOTE_B LITERAL1 143 | SILENT_NOTE LITERAL1 144 | DIV_BY_10 LITERAL1 145 | PololuHD44780Base KEYWORD1 146 | 147 | initPins KEYWORD2 148 | init KEYWORD2 149 | reinitialize KEYWORD2 150 | send KEYWORD2 151 | clear KEYWORD2 152 | loadCustomCharacter KEYWORD2 153 | loadCustomCharacterFromRam KEYWORD2 154 | createChar KEYWORD2 155 | gotoXY KEYWORD2 156 | setCursor KEYWORD2 157 | noDisplay KEYWORD2 158 | display KEYWORD2 159 | noCursor KEYWORD2 160 | cursor KEYWORD2 161 | noBlink KEYWORD2 162 | blink KEYWORD2 163 | cursorSolid KEYWORD2 164 | cursorBlinking KEYWORD2 165 | scrollDisplayLeft KEYWORD2 166 | scrollDisplayRight KEYWORD2 167 | home KEYWORD2 168 | leftToRight KEYWORD2 169 | rightToleft KEYWORD2 170 | autoscroll KEYWORD2 171 | noAutoscroll KEYWORD2 172 | command KEYWORD2 173 | write KEYWORD2 174 | 175 | PololuHD44780 KEYWORD1 176 | -------------------------------------------------------------------------------- /Romi32U4Encoders.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Romi32U4Encoders.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Reads counts from the encoders on the Romi 32U4. 10 | * 11 | * This class allows you to read counts from the encoders on the Romi 32U4, 12 | * which lets you tell how much each motor has turned and in what direction. 13 | * 14 | * The encoders are monitored in the background using interrupts, so your code 15 | * can perform other tasks without missing encoder counts. 16 | * 17 | * To read the left encoder, this class uses an interrupt service routine (ISR) 18 | * for PCINT0_vect, so there will be a compile-time conflict with any other code 19 | * that defines a pin-change ISR. 20 | * 21 | * To read the right encoder, this class calls 22 | * [attachInterrupt()](http://arduino.cc/en/Reference/attachInterrupt), so there 23 | * will be a compile-time conflict with any other code that defines an ISR for 24 | * an external interrupt directly instead of using attachInterrupt(). 25 | * 26 | * The standard Romi motors have a gear ratio of 3952:33 (approximately 120:1). 27 | * The standard Romi encoders give 12 counts per revolution. Therefore, one 28 | * revolution of a Romi wheel corresponds to 12*3952/33 (approximately 1437.09) 29 | * encoder counts as returned by this library. 30 | */ 31 | class Romi32U4Encoders 32 | { 33 | 34 | public: 35 | 36 | /*! This function initializes the encoders if they have not been initialized 37 | * already and starts listening for counts. This 38 | * function is called automatically whenever you call any other function in 39 | * this class, so you should not normally need to call it in your code. */ 40 | static void init() 41 | { 42 | static bool initialized = 0; 43 | if (!initialized) 44 | { 45 | initialized = true; 46 | init2(); 47 | } 48 | } 49 | 50 | /*! Returns the number of counts that have been detected from the left-side 51 | * encoder. These counts start at 0. Positive counts correspond to forward 52 | * movement of the left side of the Romi, while negative counts correspond 53 | * to backwards movement. 54 | * 55 | * The count is returned as a signed 16-bit integer. When the count goes 56 | * over 32767, it will overflow down to -32768. When the count goes below 57 | * -32768, it will overflow up to 32767. */ 58 | static int16_t getCountsLeft(); 59 | 60 | /*! This function is just like getCountsLeft() except it applies to the 61 | * right-side encoder. */ 62 | static int16_t getCountsRight(); 63 | 64 | /*! This function is just like getCountsLeft() except it also clears the 65 | * counts before returning. If you call this frequently enough, you will 66 | * not have to worry about the count overflowing. */ 67 | static int16_t getCountsAndResetLeft(); 68 | 69 | /*! This function is just like getCountsAndResetLeft() except it applies to 70 | * the right-side encoder. */ 71 | static int16_t getCountsAndResetRight(); 72 | 73 | /*! This function returns true if an error was detected on the left-side 74 | * encoder. It resets the error flag automatically, so it will only return 75 | * true if an error was detected since the last time checkErrorLeft() was 76 | * called. 77 | * 78 | * If an error happens, it means that both of the encoder outputs changed at 79 | * the same time from the perspective of the ISR, so the ISR was unable to 80 | * tell what direction the motor was moving, and the encoder count could be 81 | * inaccurate. The most likely cause for an error is that the interrupt 82 | * service routine for the encoders could not be started soon enough. If 83 | * you get encoder errors, make sure you are not disabling interrupts for 84 | * extended periods of time in your code. */ 85 | static bool checkErrorLeft(); 86 | 87 | /*! This function is just like checkErrorLeft() except it applies to 88 | * the right-side encoder. */ 89 | static bool checkErrorRight(); 90 | 91 | private: 92 | 93 | static void init2(); 94 | }; 95 | -------------------------------------------------------------------------------- /Romi32U4Encoders.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define LEFT_XOR 8 9 | #define LEFT_B IO_E2 10 | #define RIGHT_XOR 7 11 | #define RIGHT_B 23 12 | 13 | static volatile bool lastLeftA; 14 | static volatile bool lastLeftB; 15 | static volatile bool lastRightA; 16 | static volatile bool lastRightB; 17 | 18 | static volatile bool errorLeft; 19 | static volatile bool errorRight; 20 | 21 | // These count variables are uint16_t instead of int16_t because 22 | // signed integer overflow is undefined behavior in C++. 23 | static volatile uint16_t countLeft; 24 | static volatile uint16_t countRight; 25 | 26 | ISR(PCINT0_vect) 27 | { 28 | bool newLeftB = FastGPIO::Pin::isInputHigh(); 29 | bool newLeftA = FastGPIO::Pin::isInputHigh() ^ newLeftB; 30 | 31 | countLeft += (lastLeftA ^ newLeftB) - (newLeftA ^ lastLeftB); 32 | 33 | if((lastLeftA ^ newLeftA) & (lastLeftB ^ newLeftB)) 34 | { 35 | errorLeft = true; 36 | } 37 | 38 | lastLeftA = newLeftA; 39 | lastLeftB = newLeftB; 40 | } 41 | 42 | static void rightISR() 43 | { 44 | bool newRightB = FastGPIO::Pin::isInputHigh(); 45 | bool newRightA = FastGPIO::Pin::isInputHigh() ^ newRightB; 46 | 47 | countRight += (lastRightA ^ newRightB) - (newRightA ^ lastRightB); 48 | 49 | if((lastRightA ^ newRightA) & (lastRightB ^ newRightB)) 50 | { 51 | errorRight = true; 52 | } 53 | 54 | lastRightA = newRightA; 55 | lastRightB = newRightB; 56 | } 57 | 58 | void Romi32U4Encoders::init2() 59 | { 60 | // Set the pins as pulled-up inputs. 61 | FastGPIO::Pin::setInputPulledUp(); 62 | FastGPIO::Pin::setInputPulledUp(); 63 | FastGPIO::Pin::setInputPulledUp(); 64 | FastGPIO::Pin::setInputPulledUp(); 65 | 66 | // Enable pin-change interrupt on PB4 for left encoder, and disable other 67 | // pin-change interrupts. 68 | PCICR = (1 << PCIE0); 69 | PCMSK0 = (1 << PCINT4); 70 | PCIFR = (1 << PCIF0); // Clear its interrupt flag by writing a 1. 71 | 72 | // Enable interrupt on PE6 for the right encoder. We use attachInterrupt 73 | // instead of defining ISR(INT6_vect) ourselves so that this class will be 74 | // compatible with other code that uses attachInterrupt. 75 | attachInterrupt(4, rightISR, CHANGE); 76 | 77 | // Initialize the variables. It's good to do this after enabling the 78 | // interrupts in case the interrupts fired by accident as we were enabling 79 | // them. 80 | lastLeftB = FastGPIO::Pin::isInputHigh(); 81 | lastLeftA = FastGPIO::Pin::isInputHigh() ^ lastLeftB; 82 | countLeft = 0; 83 | errorLeft = 0; 84 | 85 | lastRightB = FastGPIO::Pin::isInputHigh(); 86 | lastRightA = FastGPIO::Pin::isInputHigh() ^ lastRightB; 87 | countRight = 0; 88 | errorRight = 0; 89 | } 90 | 91 | int16_t Romi32U4Encoders::getCountsLeft() 92 | { 93 | init(); 94 | 95 | cli(); 96 | int16_t counts = countLeft; 97 | sei(); 98 | return counts; 99 | } 100 | 101 | int16_t Romi32U4Encoders::getCountsRight() 102 | { 103 | init(); 104 | 105 | cli(); 106 | int16_t counts = countRight; 107 | sei(); 108 | return counts; 109 | } 110 | 111 | int16_t Romi32U4Encoders::getCountsAndResetLeft() 112 | { 113 | init(); 114 | 115 | cli(); 116 | int16_t counts = countLeft; 117 | countLeft = 0; 118 | sei(); 119 | return counts; 120 | } 121 | 122 | int16_t Romi32U4Encoders::getCountsAndResetRight() 123 | { 124 | init(); 125 | 126 | cli(); 127 | int16_t counts = countRight; 128 | countRight = 0; 129 | sei(); 130 | return counts; 131 | } 132 | 133 | bool Romi32U4Encoders::checkErrorLeft() 134 | { 135 | init(); 136 | 137 | bool error = errorLeft; 138 | errorLeft = 0; 139 | return error; 140 | } 141 | 142 | bool Romi32U4Encoders::checkErrorRight() 143 | { 144 | init(); 145 | 146 | bool error = errorRight; 147 | errorRight = 0; 148 | return error; 149 | } 150 | -------------------------------------------------------------------------------- /examples/RotationResist/TurnSensor.cpp: -------------------------------------------------------------------------------- 1 | // Turnsensor.h and TurnSensor.cpp provide functions for 2 | // configuring the romi's LSM6DS33 gyro, calibrating it, and 3 | // using it to measure how much the robot has turned about its Z 4 | // axis. 5 | 6 | #include 7 | #include "TurnSensor.h" 8 | 9 | // turnAngle is a 32-bit unsigned integer representing the amount 10 | // the robot has turned since the last time turnSensorReset was 11 | // called. This is computed solely using the Z axis of the gyro, 12 | // so it could be inaccurate if the robot is rotated about the X 13 | // or Y axes. 14 | // 15 | // Our convention is that a value of 0x20000000 represents a 45 16 | // degree counter-clockwise rotation. This means that a uint32_t 17 | // can represent any angle between 0 degrees and 360 degrees. If 18 | // you cast it to a signed 32-bit integer by writing 19 | // (int32_t)turnAngle, that integer can represent any angle 20 | // between -180 degrees and 180 degrees. 21 | uint32_t turnAngle = 0; 22 | 23 | // turnRate is the current angular rate of the gyro, in units of 24 | // 0.035 degrees per second. 25 | int16_t turnRate; 26 | 27 | // This is the average reading obtained from the gyro's Z axis 28 | // during calibration. 29 | int16_t gyroOffset; 30 | 31 | // This variable helps us keep track of how much time has passed 32 | // between readings of the gyro. 33 | uint16_t gyroLastUpdate = 0; 34 | 35 | // This should be called in setup() to enable and calibrate the 36 | // gyro. It uses the LCD, yellow LED, and button A. While the LCD 37 | // is displaying "Gyro cal", you should be careful to hold the robot 38 | // still. 39 | void turnSensorSetup() 40 | { 41 | Wire.begin(); 42 | imu.init(); 43 | 44 | imu.enableDefault(); 45 | 46 | // Set the gyro full scale to 1000 dps because the default 47 | // value is too low, and leave the other settings the same. 48 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 49 | 50 | lcd.clear(); 51 | lcd.print(F("Gyro cal")); 52 | 53 | // Turn on the yellow LED in case the LCD is not available. 54 | ledYellow(1); 55 | 56 | // Delay to give the user time to remove their finger. 57 | delay(500); 58 | 59 | // Calibrate the gyro. 60 | int32_t total = 0; 61 | for (uint16_t i = 0; i < 1024; i++) 62 | { 63 | // Wait for new data to be available, then read it. 64 | while(!imu.readReg(LSM6::STATUS_REG) & 0x08); 65 | imu.read(); 66 | 67 | // Add the Z axis reading to the total. 68 | total += imu.g.z; 69 | } 70 | ledYellow(0); 71 | gyroOffset = total / 1024; 72 | 73 | // Display the angle (in degrees from -180 to 180) until the 74 | // user presses A. 75 | lcd.clear(); 76 | turnSensorReset(); 77 | while (!buttonA.getSingleDebouncedRelease()) 78 | { 79 | turnSensorUpdate(); 80 | lcd.gotoXY(0, 0); 81 | lcd.print((((int32_t)turnAngle >> 16) * 360) >> 16); 82 | lcd.print(F(" ")); 83 | } 84 | lcd.clear(); 85 | } 86 | 87 | // This should be called to set the starting point for measuring 88 | // a turn. After calling this, turnAngle will be 0. 89 | void turnSensorReset() 90 | { 91 | gyroLastUpdate = micros(); 92 | turnAngle = 0; 93 | } 94 | 95 | // Read the gyro and update the angle. This should be called as 96 | // frequently as possible while using the gyro to do turns. 97 | void turnSensorUpdate() 98 | { 99 | // Read the measurements from the gyro. 100 | imu.readGyro(); 101 | turnRate = imu.g.z - gyroOffset; 102 | 103 | // Figure out how much time has passed since the last update (dt) 104 | uint16_t m = micros(); 105 | uint16_t dt = m - gyroLastUpdate; 106 | gyroLastUpdate = m; 107 | 108 | // Multiply dt by turnRate in order to get an estimation of how 109 | // much the robot has turned since the last update. 110 | // (angular change = angular velocity * time) 111 | int32_t d = (int32_t)turnRate * dt; 112 | 113 | // The units of d are gyro digits times microseconds. We need 114 | // to convert those to the units of turnAngle, where 2^29 units 115 | // represents 45 degrees. The conversion from gyro digits to 116 | // degrees per second (dps) is determined by the sensitivity of 117 | // the gyro: 0.035 degrees per second per digit. 118 | // 119 | // (0.035 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) 120 | // = 7340032/17578125 unit/(digit*us) 121 | turnAngle += (int64_t)d * 7340032 / 17578125; 122 | } 123 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Romi32U4 library 2 | 3 | [www.pololu.com](https://www.pololu.com/) 4 | 5 | ## Summary 6 | 7 | This is a C++ library for the Arduino IDE that helps access the on-board hardware of the [Romi 32U4 Control Board](https://www.pololu.com/product/3544). 8 | 9 | The Romi 32U4 Control Board turns the Romi chassis into a programmable, Arduino-compatible robot. It has an integrated AVR ATmega32U4 microcontroller, motor drivers, encoders, buzzer, buttons, and an LSM6DS33 accelerometer and gyro. 10 | 11 | This library does not include code for accessing the LSM6DS33. If you want to access it, you should install the separate [LSM6 library](https://github.com/pololu/lsm6-arduino). 12 | 13 | This library is very similar to the [Zumo32U4](https://github.com/pololu/zumo-32u4-arduino-library) library. 14 | 15 | ## Installing the library 16 | 17 | If you are using version 1.6.2 or later of the [Arduino software (IDE)](http://www.arduino.cc/en/Main/Software), you can use the Library Manager to install this library: 18 | 19 | 1. In the Arduino IDE, open the "Sketch" menu, select "Include Library", then "Manage Libraries...". 20 | 2. Search for "Romi32U4". 21 | 3. Click the Romi32U4 entry in the list. 22 | 4. Click "Install". 23 | 24 | If this does not work, you can manually install the library: 25 | 26 | 1. Download the [latest release archive from GitHub](https://github.com/pololu/romi-32u4-arduino-library) and decompress it. 27 | 2. Rename the folder "romi-32u4-arduino-library-master" to "Romi32U4". 28 | 3. Move the "Romi32U4" folder into the "libraries" directory inside your Arduino sketchbook directory. You can view your sketchbook location by opening the "File" menu and selecting "Preferences" in the Arduino IDE. If there is not already a "libraries" folder in that location, you should make the folder yourself. 29 | 4. After installing the library, restart the Arduino IDE. 30 | 31 | ## Examples 32 | 33 | Several example sketches are available that show how to use the library. You can access them from the Arduino IDE by opening the "File" menu, selecting "Examples", and then selecting "Romi32U4". If you cannot find these examples, the library was probably installed incorrectly and you should retry the installation instructions above. 34 | 35 | ## Classes and functions 36 | 37 | The main classes and functions provided by the library are listed below: 38 | 39 | * Romi32U4ButtonA 40 | * Romi32U4ButtonB 41 | * Romi32U4ButtonC 42 | * Romi32U4Buzzer 43 | * Romi32U4Encoders 44 | * Romi32U4LCD 45 | * Romi32U4Motors 46 | * ledRed() 47 | * ledGreen() 48 | * ledYellow() 49 | * usbPowerPresent() 50 | * readBatteryMillivolts() 51 | 52 | ## Component libraries 53 | 54 | This library also includes copies of several other Arduino libraries inside it which are used to help implement the classes and functions above. 55 | 56 | * [FastGPIO](https://github.com/pololu/fastgpio-arduino) 57 | * [PololuBuzzer](https://github.com/pololu/pololu-buzzer-arduino) 58 | * [PololuHD44780](https://github.com/pololu/pololu-hd44780-arduino) 59 | * [Pushbutton](https://github.com/pololu/pushbutton-arduino) 60 | * [QTRSensors](https://github.com/pololu/qtr-sensors-arduino) 61 | * [USBPause](https://github.com/pololu/usb-pause-arduino) 62 | 63 | You can use these libraries in your sketch automatically without any extra installation steps and without needing to add any extra `#include` lines to your sketch. 64 | 65 | You should avoid adding extra `#include` lines such as `#include ` because then the Arduino IDE might try to use the standalone Pushbutton library (if you previously installed it), and it would conflict with the copy of the Pushbutton code included in this library. The only `#include` line needed to access all features of this library are: 66 | 67 | ~~~{.cpp} 68 | #include 69 | ~~~ 70 | 71 | ## Documentation 72 | 73 | For complete documentation, see https://pololu.github.io/romi-32u4-arduino-library. If you are already on that page, then click on the links in the "Classes and functions" section above. 74 | 75 | ## Version history 76 | 77 | * 1.0.3 (2022-09-06): Fixed a bug in the Encoders demo that could prevent encoder errors from being shown properly on the display. 78 | * 1.0.2 (2017-07-17): Fixed a bug that caused errors for the right encoder to be reported as errors for the left encoder. 79 | * 1.0.1 (2017-02-23): 80 | * Changed the internal `Romi32U4Motors::maxSpeed` variable to be an `int16_t` so it can be compared to other `int16_t` variables without warnings. 81 | * Fixed the InterialSensors and Demo examples to not use a compass. 82 | * Fixed some comments. 83 | * 1.0.0 (2017-02-06): Original release. 84 | -------------------------------------------------------------------------------- /PololuHD44780.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | 5 | #define LCD_CLEAR 0x01 6 | #define LCD_SHOW_BLINK 0x0F 7 | #define LCD_SHOW_SOLID 0x0E 8 | #define LCD_HIDE 0x0C 9 | #define LCD_CURSOR_L 0x10 10 | #define LCD_CURSOR_R 0x14 11 | #define LCD_SHIFT_L 0x18 12 | #define LCD_SHIFT_R 0x1C 13 | 14 | PololuHD44780Base::PololuHD44780Base() 15 | { 16 | initialized = false; 17 | } 18 | 19 | void PololuHD44780Base::init2() 20 | { 21 | // The startup procedure comes from Figure 24 of the HD44780 datasheet. The 22 | // delay times in the later part of this function come from Table 6. 23 | 24 | initPins(); 25 | 26 | // We need to wait at least 15 ms after VCC reaches 4.5 V. 27 | // 28 | // Assumption: The AVR's power-on reset is already configured to wait for 29 | // tens of milliseconds, so no delay is needed here. 30 | 31 | sendCommand4Bit(3); // Function set 32 | _delay_us(4200); // Needs to be at least 4.1 ms. 33 | sendCommand4Bit(3); // Function set 34 | _delay_us(150); // Needs to be at least 100 us. 35 | sendCommand4Bit(3); // Function set 36 | 37 | sendCommand4Bit(0b0010); // 4-bit interface 38 | sendCommand(0b00101000); // 4-bit, 2 line, 5x8 dots font 39 | 40 | setDisplayControl(0b000); // display off, cursor off, blinking off 41 | clear(); 42 | setEntryMode(0b10); // cursor shifts right, no auto-scrolling 43 | setDisplayControl(0b100); // display on, cursor off, blinking off 44 | } 45 | 46 | void PololuHD44780Base::sendAndDelay(uint8_t data, bool rsValue, bool only4bit) 47 | { 48 | init(); 49 | 50 | send(data, rsValue, only4bit); 51 | 52 | // Every data transfer or command takes at least 37 us to complete, and most 53 | // of them only take that long according to the HD44780 datasheet. We delay 54 | // for 37 us here so we don't have to do it in lots of other places. 55 | // 56 | // NOTE: If we add support for configurations where the R/W line is 57 | // connected, then this delay and others like it should be disabled, and we 58 | // should instead wait for the busy flag before sending the next command. 59 | _delay_us(37); 60 | } 61 | 62 | size_t PololuHD44780Base::write(uint8_t data) 63 | { 64 | sendData(data); 65 | return 1; 66 | } 67 | 68 | size_t PololuHD44780Base::write(const uint8_t * buffer, size_t length) 69 | { 70 | size_t n = length; 71 | while (n--) 72 | { 73 | sendData(*buffer++); 74 | } 75 | return length; 76 | } 77 | 78 | void PololuHD44780Base::clear() 79 | { 80 | sendCommand(LCD_CLEAR); 81 | 82 | // It's not clear how long this command takes because it doesn't say in 83 | // Table 6 of the HD44780 datasheet. A good guess is that it takes 1.52 ms, 84 | // since the Return Home command does. 85 | _delay_us(2000); 86 | } 87 | 88 | void PololuHD44780Base::gotoXY(uint8_t x, uint8_t y) 89 | { 90 | // Each entry is the RAM address of a line, with its most 91 | // significant bit set for convenience. 92 | const uint8_t line_mem[] = {0x80, 0xC0, 0x94, 0xD4}; 93 | 94 | // Avoid out-of-bounds array access. 95 | if (y > 3) { y = 3; } 96 | 97 | sendCommand(line_mem[y] + x); 98 | 99 | // This could take up to 37 us according to Table 6 of the HD44780 datasheet. 100 | _delay_us(37); 101 | } 102 | 103 | void PololuHD44780Base::loadCustomCharacter(const uint8_t * picture, uint8_t number) 104 | { 105 | uint8_t address = number * 8; 106 | 107 | for(uint8_t i = 0; i < 8; i++) 108 | { 109 | // Set CG RAM address. 110 | sendCommand(0b01000000 | (address + i)); 111 | 112 | // Write character data. 113 | sendData(pgm_read_byte(picture + i)); 114 | } 115 | } 116 | 117 | void PololuHD44780Base::loadCustomCharacterFromRam(const uint8_t * picture, uint8_t number) 118 | { 119 | uint8_t address = number * 8; 120 | 121 | for(uint8_t i = 0; i < 8; i++) 122 | { 123 | // Set CG RAM address. 124 | sendCommand(0b01000000 | (address + i)); 125 | 126 | // Write character data. 127 | sendData(picture[i]); 128 | } 129 | } 130 | 131 | void PololuHD44780Base::setDisplayControl(uint8_t displayControl) 132 | { 133 | sendCommand(0b00001000 | displayControl); 134 | this->displayControl = displayControl; 135 | } 136 | 137 | void PololuHD44780Base::cursorSolid() 138 | { 139 | setDisplayControl((displayControl | 0b010) & ~0b001); 140 | } 141 | 142 | void PololuHD44780Base::cursorBlinking() 143 | { 144 | setDisplayControl((displayControl | 0b001) & ~0b010); 145 | } 146 | 147 | void PololuHD44780Base::hideCursor() 148 | { 149 | setDisplayControl(displayControl & ~0b011); 150 | } 151 | 152 | void PololuHD44780Base::noDisplay() 153 | { 154 | setDisplayControl(displayControl & ~0b100); 155 | } 156 | 157 | void PololuHD44780Base::display() 158 | { 159 | setDisplayControl(displayControl | 0b100); 160 | } 161 | 162 | void PololuHD44780Base::noCursor() 163 | { 164 | setDisplayControl(displayControl & ~0b010); 165 | } 166 | 167 | void PololuHD44780Base::cursor() 168 | { 169 | setDisplayControl(displayControl | 0b010); 170 | } 171 | 172 | void PololuHD44780Base::noBlink() 173 | { 174 | setDisplayControl(displayControl & ~0b001); 175 | } 176 | 177 | void PololuHD44780Base::blink() 178 | { 179 | setDisplayControl(displayControl | 0b001); 180 | } 181 | 182 | void PololuHD44780Base::scrollDisplayLeft() 183 | { 184 | sendCommand(0b00011000); 185 | } 186 | 187 | void PololuHD44780Base::scrollDisplayRight() 188 | { 189 | sendCommand(0b00011100); 190 | } 191 | 192 | void PololuHD44780Base::home() 193 | { 194 | sendCommand(0b00000010); 195 | _delay_us(1600); // needs to be at least 1.52 ms 196 | } 197 | 198 | void PololuHD44780Base::setEntryMode(uint8_t entryMode) 199 | { 200 | sendCommand(0b00000100 | entryMode); 201 | this->entryMode = entryMode; 202 | } 203 | 204 | void PololuHD44780Base::leftToRight() 205 | { 206 | setEntryMode(entryMode | 0b10); 207 | } 208 | 209 | void PololuHD44780Base::rightToLeft() 210 | { 211 | setEntryMode(entryMode & ~0b10); 212 | } 213 | 214 | void PololuHD44780Base::autoscroll() 215 | { 216 | setEntryMode(entryMode | 0b01); 217 | } 218 | 219 | void PololuHD44780Base::noAutoscroll() 220 | { 221 | setEntryMode(entryMode & ~0b01); 222 | } 223 | -------------------------------------------------------------------------------- /Pushbutton.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Pushbutton.h 4 | * 5 | * This is the main header file for the %Pushbutton library. 6 | * 7 | * For an overview of the library's features, see 8 | * https://github.com/pololu/pushbutton-arduino. That is the main repository 9 | * for the library, though copies of the library may exist in other 10 | * repositories. */ 11 | 12 | #pragma once 13 | 14 | #include 15 | 16 | /*! Indicates the that pull-up resistor should be disabled. */ 17 | #define PULL_UP_DISABLED 0 18 | 19 | /*! Indicates the that pull-up resistor should be enabled. */ 20 | #define PULL_UP_ENABLED 1 21 | 22 | /*! Indicates that the default (released) state of the button is when the 23 | * I/O line reads low. */ 24 | #define DEFAULT_STATE_LOW 0 25 | 26 | /*! Indicates that the default (released) state of the button is when the 27 | * I/O line reads high. */ 28 | #define DEFAULT_STATE_HIGH 1 29 | 30 | /*! The pin used for the button on the 31 | * [Zumo Shield for Arduino](http://www.pololu.com/product/2508). 32 | * 33 | * This does not really belong here in this general pushbutton library and will 34 | * probably be removed in the future. */ 35 | #define ZUMO_BUTTON 12 36 | 37 | // \cond 38 | /** \brief A state machine that detects when a boolean value changes from false 39 | * to true, with debouncing. 40 | * 41 | * This should be considered a private implementation detail of the library. 42 | */ 43 | class PushbuttonStateMachine 44 | { 45 | public: 46 | 47 | PushbuttonStateMachine(); 48 | 49 | /** This should be called repeatedly in a loop. It will return true once after 50 | * each time it detects the given value changing from false to true. */ 51 | bool getSingleDebouncedRisingEdge(bool value); 52 | 53 | private: 54 | 55 | uint8_t state; 56 | uint16_t prevTimeMillis; 57 | }; 58 | // \endcond 59 | 60 | /*! \brief General pushbutton class that handles debouncing. 61 | * 62 | * This is an abstract class used for interfacing with pushbuttons. It knows 63 | * about debouncing, but it knows nothing about how to read the current state of 64 | * the button. The functions in this class get the current state of the button 65 | * by calling isPressed(), a virtual function which must be implemented in a 66 | * subclass of PushbuttonBase, such as Pushbutton. 67 | * 68 | * Most users of this library do not need to directly use PushbuttonBase or even 69 | * know that it exists. They can use Pushbutton instead.*/ 70 | class PushbuttonBase 71 | { 72 | public: 73 | 74 | /*! \brief Waits until the button is pressed and takes care of debouncing. 75 | * 76 | * This function waits until the button is in the pressed state and then 77 | * returns. Note that if the button is already pressed when you call this 78 | * function, it will return quickly (in 10 ms). */ 79 | void waitForPress(); 80 | 81 | /*! \brief Waits until the button is released and takes care of debouncing. 82 | * 83 | * This function waits until the button is in the released state and then 84 | * returns. Note that if the button is already released when you call this 85 | * function, it will return quickly (in 10 ms). */ 86 | void waitForRelease(); 87 | 88 | /*! \brief Waits until the button is pressed and then waits until the button 89 | * is released, taking care of debouncing. 90 | * 91 | * This is equivalent to calling waitForPress() and then waitForRelease(). */ 92 | void waitForButton(); 93 | 94 | /*! \brief Uses a state machine to return true once after each time it detects 95 | * the button moving from the released state to the pressed state. 96 | * 97 | * This is a non-blocking function that is meant to be called repeatedly in a 98 | * loop. Each time it is called, it updates a state machine that monitors the 99 | * state of the button. When it detects the button changing from the released 100 | * state to the pressed state, with debouncing, it returns true. */ 101 | bool getSingleDebouncedPress(); 102 | 103 | /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state. 104 | * 105 | * This is just like getSingleDebouncedPress() except it has a separate state 106 | * machine and it watches for when the button goes from the pressed state to 107 | * the released state. 108 | * 109 | * There is no strict guarantee that every debounced button press event 110 | * returned by getSingleDebouncedPress() will have a corresponding 111 | * button release event returned by getSingleDebouncedRelease(); the two 112 | * functions use independent state machines and sample the button at different 113 | * times. */ 114 | bool getSingleDebouncedRelease(); 115 | 116 | /*! \brief indicates whether button is currently pressed without any debouncing. 117 | * 118 | * @return 1 if the button is pressed right now, 0 if it is not. 119 | * 120 | * This function must be implemented in a subclass of PushbuttonBase, such as 121 | * Pushbutton. */ 122 | virtual bool isPressed() = 0; 123 | 124 | private: 125 | 126 | PushbuttonStateMachine pressState; 127 | PushbuttonStateMachine releaseState; 128 | }; 129 | 130 | /** \brief Main class for interfacing with pushbuttons. 131 | * 132 | * This class can interface with any pushbutton whose state can be read with 133 | * the `digitalRead` function, which is part of the Arduino core. 134 | * 135 | * See https://github.com/pololu/pushbutton-arduino for an overview 136 | * of the different ways to use this class. */ 137 | class Pushbutton : public PushbuttonBase 138 | { 139 | public: 140 | 141 | /** Constructs a new instance of Pushbutton. 142 | * 143 | * @param pin The pin number of the pin. This is used as an argument to 144 | * `pinMode` and `digitalRead`. 145 | * 146 | * @param pullUp Specifies whether the pin's internal pull-up resistor should 147 | * be enabled. This should be either #PULL_UP_ENABLED (which is the default if 148 | * the argument is omitted) or #PULL_UP_DISABLED. 149 | * 150 | * @param defaultState Specifies the voltage level that corresponds to the 151 | * button's default (released) state. This should be either 152 | * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or 153 | * #DEFAULT_STATE_LOW. */ 154 | Pushbutton(uint8_t pin, uint8_t pullUp = PULL_UP_ENABLED, 155 | uint8_t defaultState = DEFAULT_STATE_HIGH); 156 | 157 | virtual bool isPressed(); 158 | 159 | private: 160 | 161 | void init() 162 | { 163 | if (!initialized) 164 | { 165 | initialized = true; 166 | init2(); 167 | } 168 | } 169 | 170 | void init2(); 171 | 172 | bool initialized; 173 | uint8_t _pin; 174 | bool _pullUp; 175 | bool _defaultState; 176 | }; 177 | -------------------------------------------------------------------------------- /examples/RemoteControl/RemoteControl.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to control the Romi 32U4 with an 2 | // infrared (IR) remote control and an IR receiver module. 3 | // 4 | // This code is designed to work with the Mini IR Remote Control 5 | // and the Vishay TSOP38438 IR receiver module, both available 6 | // from Pololu: 7 | // 8 | // https://www.pololu.com/product/2777 9 | // https://www.pololu.com/product/2471 10 | // 11 | // You will need to make the following connections between the IR 12 | // receiver and the Romi 32U4: 13 | // 14 | // - IR receiver OUT to Romi 32U4 pin 0 15 | // - IR recevier GND to Romi 32U4 GND 16 | // - IR receiver VS to Romi 32U4 5V 17 | // 18 | // The arrow buttons control the robot's movement, while the 19 | // number buttons from 1 to 3 play different notes on the buzzer. 20 | // 21 | // To change what actions are performed when a button press is 22 | // detected, you should change the processRemoteCommand and 23 | // stopCurrentCommand functions. 24 | // 25 | // If you have a different remote that uses the NEC protocol with 26 | // a 38 kHz, 940 nm infrared emitter, it should be possible to 27 | // make it instead of the mini remote from Pololu. You can use 28 | // this code to decode the messages from your remote, and then 29 | // you can edit the constants in RemoteConstants.h to match what 30 | // was transmitted from your remote. 31 | 32 | #include 33 | #include "RemoteConstants.h" 34 | #include "RemoteDecoder.h" 35 | 36 | // This variable sets the amount of time (in milliseconds) that 37 | // we wait before considering the current message from the remote 38 | // to have expired. This type of remote typically sends a repeat 39 | // command every 109 ms, so a timeout value of 115 was chosen. 40 | // You can increase this timeout to 230 if you want to be more 41 | // tolerant of errors that occur while you are holding down the 42 | // button, but it will make the robot slower to respond when you 43 | // release the button. 44 | const uint16_t messageTimeoutMs = 115; 45 | 46 | // This variable is true if the last message received from the 47 | // remote is still active, meaning that we are still performing 48 | // the action specified by the message. A message will be active 49 | // if the remote button is being held down and the robot has been 50 | // successfully receiving messages from the remote since the 51 | // button was pressed. 52 | bool messageActive = false; 53 | 54 | // This is the time that the current message from the remote was 55 | // last verified, in milliseconds. It is used to implement the 56 | // timeout defined above. 57 | uint16_t lastMessageTimeMs = 0; 58 | 59 | Romi32U4LCD lcd; 60 | Romi32U4Motors motors; 61 | Romi32U4Buzzer buzzer; 62 | Romi32U4ButtonA buttonA; 63 | 64 | RemoteDecoder decoder; 65 | 66 | // Initializes the IR sesnor by enabling a pull-up resistor for 67 | // it. 68 | void irSensorInit() 69 | { 70 | FastGPIO::Pin<0>::setInputPulledUp(); 71 | } 72 | 73 | // Reads from the IR sensor. If the pin is low then the sensor 74 | // is detecting something and this function returns true. 75 | bool irSensorRead() 76 | { 77 | return !FastGPIO::Pin<0>::isInputHigh(); 78 | } 79 | 80 | void setup() 81 | { 82 | irSensorInit(); 83 | 84 | lcd.clear(); 85 | lcd.print(F("Waiting")); 86 | } 87 | 88 | void loop() 89 | { 90 | decoder.service(irSensorRead()); 91 | 92 | // Turn on the yellow LED if a message is active. 93 | ledYellow(messageActive); 94 | 95 | // Turn on the red LED if we are in the middle of receiving a 96 | // new message from the remote. You should see the red LED 97 | // blinking about 9 times per second while you hold a remote 98 | // button down. 99 | ledRed(decoder.criticalTime()); 100 | 101 | if (decoder.criticalTime()) 102 | { 103 | // We are in the middle of receiving a message from the 104 | // remote, so we should avoid doing anything that might take 105 | // more than a few tens of microseconds, and call 106 | // decoder.service() as often as possible. 107 | } 108 | else 109 | { 110 | // We are not in a critical time, so we can do other things 111 | // as long as they do not take longer than about 7.3 ms. 112 | // Delays longer than that can cause some remote control 113 | // messages to be missed. 114 | 115 | processRemoteEvents(); 116 | } 117 | 118 | // Check how long ago the current message was last verified. 119 | // If it is longer than the timeout time, then the message has 120 | // expired and we should stop executing it. 121 | if (messageActive && (uint16_t)(millis() - lastMessageTimeMs) > messageTimeoutMs) 122 | { 123 | messageActive = false; 124 | stopCurrentCommand(); 125 | } 126 | } 127 | 128 | void processRemoteEvents() 129 | { 130 | if (decoder.getAndResetMessageFlag()) 131 | { 132 | // The remote decoder received a new message, so record what 133 | // time it was received and process it. 134 | lastMessageTimeMs = millis(); 135 | messageActive = true; 136 | processRemoteMessage(decoder.getMessage()); 137 | } 138 | 139 | if (decoder.getAndResetRepeatFlag()) 140 | { 141 | // The remote decoder receiver a "repeat" command, which is 142 | // sent about every 109 ms while the button is being held 143 | // down. It contains no data. We record what time the 144 | // repeat command was received so we can know that the 145 | // current message is still active. 146 | lastMessageTimeMs = millis(); 147 | } 148 | } 149 | 150 | void processRemoteMessage(const uint8_t * message) 151 | { 152 | // Print the raw message on the first line of the LCD, in hex. 153 | // The first two bytes are usually an address, and the third 154 | // byte is usually a command. The last byte is supposed to be 155 | // the bitwise inverse of the third byte, and if that is the 156 | // case, then we don't print it. 157 | lcd.clear(); 158 | char buffer[9]; 159 | if (message[2] + message[3] == 0xFF) 160 | { 161 | sprintf(buffer, "%02X%02X %02X ", 162 | message[0], message[1], message[2]); 163 | } 164 | else 165 | { 166 | sprintf(buffer, "%02X%02X%02X%02X", 167 | message[0], message[1], message[2], message[3]); 168 | } 169 | lcd.print(buffer); 170 | 171 | // Go to the next line of the LCD. 172 | lcd.gotoXY(0, 1); 173 | 174 | // Make sure the address matches what we expect. 175 | if (message[0] != remoteAddressByte0 || 176 | message[1] != remoteAddressByte1) 177 | { 178 | lcd.print(F("bad addr")); 179 | return; 180 | } 181 | 182 | // Make sure that the last byte is the logical inverse of the 183 | // command byte. 184 | if (message[2] + message[3] != 0xFF) 185 | { 186 | lcd.print(F("bad cmd")); 187 | return; 188 | } 189 | 190 | stopCurrentCommand(); 191 | processRemoteCommand(message[2]); 192 | } 193 | 194 | // Start running the new command. 195 | void processRemoteCommand(uint8_t command) 196 | { 197 | switch(command) 198 | { 199 | case remoteUp: 200 | lcd.print(F("up")); 201 | motors.setSpeeds(300, 300); 202 | break; 203 | 204 | case remoteDown: 205 | lcd.print(F("down")); 206 | motors.setSpeeds(-300, -300); 207 | break; 208 | 209 | case remoteLeft: 210 | lcd.print(F("left")); 211 | motors.setSpeeds(-200, 200); 212 | break; 213 | 214 | case remoteRight: 215 | lcd.print(F("right")); 216 | motors.setSpeeds(200, -200); 217 | break; 218 | 219 | case remoteStopMode: 220 | lcd.print(F("stop")); 221 | break; 222 | 223 | case remoteEnterSave: 224 | lcd.print(F("enter")); 225 | break; 226 | 227 | case remoteVolMinus: 228 | lcd.print(F("vol-")); 229 | break; 230 | 231 | case remoteVolPlus: 232 | lcd.print(F("vol+")); 233 | break; 234 | 235 | case remotePlayPause: 236 | lcd.print(F("play")); 237 | break; 238 | 239 | case remoteSetup: 240 | lcd.print(F("setup")); 241 | break; 242 | 243 | case remoteBack: 244 | lcd.print(F("back")); 245 | break; 246 | 247 | case remote0: 248 | lcd.print(F("0")); 249 | break; 250 | 251 | case remote1: 252 | lcd.print(F("1")); 253 | buzzer.playNote(NOTE_C(4), 200, 15); 254 | break; 255 | 256 | case remote2: 257 | lcd.print(F("2")); 258 | buzzer.playNote(NOTE_D(4), 200, 15); 259 | break; 260 | 261 | case remote3: 262 | lcd.print(F("3")); 263 | buzzer.playNote(NOTE_E(4), 200, 15); 264 | break; 265 | 266 | case remote4: 267 | lcd.print(F("4")); 268 | break; 269 | 270 | case remote5: 271 | lcd.print(F("5")); 272 | break; 273 | 274 | case remote6: 275 | lcd.print(F("6")); 276 | break; 277 | 278 | case remote7: 279 | lcd.print(F("7")); 280 | break; 281 | 282 | case remote8: 283 | lcd.print(F("8")); 284 | break; 285 | 286 | case remote9: 287 | lcd.print(F("9")); 288 | break; 289 | } 290 | } 291 | 292 | // Stops the current remote control command. This is called when 293 | // a new command is received or if the current command has 294 | // expired. 295 | void stopCurrentCommand() 296 | { 297 | motors.setSpeeds(0, 0); 298 | buzzer.stopPlaying(); 299 | } 300 | -------------------------------------------------------------------------------- /examples/RemoteControl/RemoteDecoder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | // Uncomment the line below to enable debugging output that 7 | // gets sent to the serial monitor. 8 | // #define REMOTE_DEBUG 9 | 10 | // Decodes remote control signals that use the NEC protocol: 11 | // http://www.sbprojects.com/knowledge/ir/nec.php 12 | class RemoteDecoder 13 | { 14 | public: 15 | // In this protocol, every high and low pulse from the IR LED 16 | // is supposed to have a length that is a multiple of 562 us, 17 | // and we will call this time "1 time unit". 18 | static const uint16_t unitPulseTimeUs = 562; 19 | 20 | // The number of bytes in the code sent by the remote. 21 | static const uint8_t messageSize = 4; 22 | 23 | // States for the low-level state machine that say what 24 | // part of the protocol we are currently processing. 25 | // 26 | // The sensor is inactive during the Idle, StartSpace, and Space states. 27 | // The sensor is active during the StartMark, Mark, and EndMark states. 28 | // 29 | // The expected sequence of states for a normal command is: 30 | // - Init, Idle, StartMark, StartSpace, 31 | // Mark+Space repeated 32 times, EndMark, Idle 32 | // 33 | // The expected sequence of states for a "repeat" command is: 34 | // - Init, Idle, StartMark, StartSpace, EndMark, Idle 35 | enum State { 36 | Init, 37 | Idle, 38 | StartMark, 39 | StartSpace, 40 | Mark, 41 | Space, 42 | EndMark, 43 | }; 44 | 45 | RemoteDecoder() 46 | { 47 | memset(message, 0, messageSize); 48 | } 49 | 50 | // If false, then it is OK for other parts of the code to do blocking 51 | // operations that take up to (16 - 3)*562 = 7306 microseconds long. 52 | // If true, then you should avoid doing any blocking operations, and 53 | // should call service() as often as possible. 54 | // 55 | // If you want to do a long blocking operation (like writing to 56 | // the LCD), make sure that this function returns false, or you 57 | // could miss commands sent by the remote. (There should be 58 | // practically no risk of receiving false commands though, regardless 59 | // of the timing of the calls to service().) 60 | bool criticalTime() 61 | { 62 | return state != Idle; 63 | } 64 | 65 | // This function returns a pointer to the last 4-byte message 66 | // received from the remote. The bytes will all be 0 if no 67 | // message has been received yet. 68 | const uint8_t * getMessage() 69 | { 70 | return message; 71 | } 72 | 73 | // Returns true once whenever a new message is received. 74 | // Call getMessage to see the data in the message. 75 | bool getAndResetMessageFlag() 76 | { 77 | if (messageFlag) 78 | { 79 | messageFlag = false; 80 | return true; 81 | } 82 | return false; 83 | } 84 | 85 | // Returns true once whenever a new repeat command is received. 86 | // This repeat command does not necessarily correspond to the 87 | // message returned by getMessage. 88 | bool getAndResetRepeatFlag() 89 | { 90 | if (repeatFlag) 91 | { 92 | repeatFlag = false; 93 | return true; 94 | } 95 | return false; 96 | } 97 | 98 | // Returns true once whenever an error happens, which could 99 | // indicate that the remote is blocked or uses a different 100 | // protocol. 101 | bool getAndResetErrorFlag() 102 | { 103 | if (errorFlag) 104 | { 105 | errorFlag = false; 106 | return true; 107 | } 108 | return false; 109 | } 110 | 111 | // This should be called as often as possible to monitor the 112 | // remote. The caller should read the state of the IR sensor 113 | // and pass it in as the first argument. The first argument 114 | // should be 1 if the sensor is actively detecting 115 | // something and 0 otherwise. 116 | void service(bool pulseOn) 117 | { 118 | // First, get the number of time units that have elapsed 119 | // in the current state. 120 | uint32_t time = timeInThisState(); 121 | switch(state) 122 | { 123 | case Init: 124 | // Init is the initial state, and also the state used after 125 | // there are any errors. Just wait for the signal to turn 126 | // off and then go to the idle state. 127 | if (!pulseOn) 128 | { 129 | changeState(Idle); 130 | } 131 | break; 132 | 133 | case Idle: 134 | // The sensors are off and we are waiting for the next 135 | // command. 136 | if (pulseOn) 137 | { 138 | changeState(StartMark); 139 | } 140 | break; 141 | 142 | case StartMark: 143 | // The sensors turned on while we were idle, so we think 144 | // this is the start mark of command, which is supposed to 145 | // be 16 units long (9 ms). 146 | if (time > 16) 147 | { 148 | #ifdef REMOTE_DEBUG 149 | Serial.println(F("Start mark too long")); 150 | #endif 151 | error(); 152 | } 153 | else if (!pulseOn) 154 | { 155 | // We allow the start mark to be as short as 2.5 time 156 | // units, because it is possible that the AVR might be 157 | // busy with other that cause the start of the start mark 158 | // to be missed for a while, and the measured time to be 159 | // shorter than it really is. 160 | if (time >= 3) 161 | { 162 | // Successfully detected the a start mark. 163 | changeState(StartSpace); 164 | } 165 | else 166 | { 167 | // This error happens a lot if the signal is bad, 168 | // because any error will lead to the Idle state, and 169 | // the remaining marks in the signal will all be 170 | // treated as if they might be a start mark. 171 | #ifdef REMOTE_DEBUG 172 | Serial.print(F("Start mark too short: ")); 173 | #endif 174 | error(); 175 | } 176 | } 177 | break; 178 | 179 | case StartSpace: 180 | // This is the space after the start mark, which is 181 | // supposed to be 8 units long for a normal command and 4 182 | // units long for a "repeat command", a special type of 183 | // command which has no data and just tells us that the 184 | // last command is still valid. 185 | if (time > 8) 186 | { 187 | #ifdef REMOTE_DEBUG 188 | Serial.println(F("Start space too long")); 189 | #endif 190 | error(); 191 | } 192 | else if (pulseOn) 193 | { 194 | if (time == 4) 195 | { 196 | // This looks like a repeat command so far. 197 | repeatCommand = true; 198 | changeState(EndMark); 199 | } 200 | else if (time == 8) 201 | { 202 | // This looks like a normal command, which will give us 203 | // a new 32-bit message. Get ready to receive the new 204 | // message. 205 | bitsReceived = 0; 206 | memset(incomingMessage, 0, messageSize); 207 | changeState(Mark); 208 | } 209 | else 210 | { 211 | #ifdef REMOTE_DEBUG 212 | Serial.println(F("Start space bad length")); 213 | #endif 214 | error(); 215 | } 216 | } 217 | break; 218 | 219 | case Mark: 220 | // This is a mark, a time when the sensor is active which 221 | // should last for one unit, and indicates that a new bit 222 | // of data will be transmitted. (We won't know the value of 223 | // that bit of data until later, when we time the length of 224 | // the space after this mark.) 225 | if (time > 1) 226 | { 227 | #ifdef REMOTE_DEBUG 228 | Serial.println(F("Mark too long")); 229 | #endif 230 | error(); 231 | } 232 | else if (!pulseOn) 233 | { 234 | if (time == 1) 235 | { 236 | // Successfully detected a mark. 237 | changeState(Space); 238 | } 239 | else 240 | { 241 | #ifdef REMOTE_DEBUG 242 | Serial.println(F("Mark too short")); 243 | #endif 244 | error(); 245 | } 246 | } 247 | break; 248 | 249 | case Space: 250 | // This is a space, a time when the sensor is not active 251 | // which immediately follows a mark. The length of this 252 | // state determines whether the next bit in the command is 253 | // 0 or 1. 254 | if (time > 3) 255 | { 256 | #ifdef REMOTE_DEBUG 257 | Serial.println(F("Space too long")); 258 | #endif 259 | error(); 260 | } 261 | else if (pulseOn) 262 | { 263 | if (time == 1) 264 | { 265 | // The next bit is 0. 266 | processNewBit(0); 267 | } 268 | else if (time == 2) 269 | { 270 | #ifdef REMOTE_DEBUG 271 | Serial.println(F("Invalid space length 2")); 272 | #endif 273 | error(); 274 | } 275 | else if (time == 3) 276 | { 277 | // The next bit is 1. 278 | processNewBit(1); 279 | } 280 | else 281 | { 282 | #ifdef REMOTE_DEBUG 283 | Serial.println(F("Space too short")); 284 | #endif 285 | error(); 286 | } 287 | } 288 | break; 289 | 290 | case EndMark: 291 | // The last part of any command is an end mark, a time when the 292 | // sensor is active for one time unit. This happens at the end of 293 | // a normal command (which has 32 bits of data) or a "repeat" command 294 | // which just says to keep doing the previous command. 295 | 296 | if (time > 1) 297 | { 298 | #ifdef REMOTE_DEBUG 299 | Serial.println(F("End mark too long")); 300 | #endif 301 | error(); 302 | } 303 | else if (!pulseOn) 304 | { 305 | if (time == 1) 306 | { 307 | // Successfully detected the end of a command. 308 | 309 | if (repeatCommand) 310 | { 311 | // Successfully received a repeat command. 312 | repeatFlag = true; 313 | 314 | #ifdef REMOTE_DEBUG 315 | Serial.println(F("REPEAT COMMAND")); 316 | #endif 317 | } 318 | else 319 | { 320 | // Successfully received a normal command that 321 | // includes a new message. 322 | memcpy(message, incomingMessage, messageSize); 323 | messageFlag = true; 324 | 325 | #ifdef REMOTE_DEBUG 326 | Serial.println(F("NORMAL COMMAND")); 327 | #endif 328 | } 329 | 330 | // Go back to the idle state to wait for the next 331 | // command. 332 | changeState(Idle); 333 | } 334 | else 335 | { 336 | #ifdef REMOTE_DEBUG 337 | Serial.println(F("End mark too short")); 338 | #endif 339 | error(); 340 | } 341 | } 342 | break; 343 | } 344 | } 345 | 346 | private: 347 | 348 | // This is called by the low-level state machine at the end of 349 | // a space state when we have figured out the next bit in the 350 | // message. This function records that bit, increments the bit 351 | // counter, and transitions to the appropriate next state. 352 | void processNewBit(bool b) 353 | { 354 | if (b) 355 | { 356 | // The bit is 1, so we need to set the appropriate spot 357 | // in our incoming message buffer. 358 | incomingMessage[bitsReceived / 8] |= 1 << bitsReceived % 8; 359 | } 360 | bitsReceived++; 361 | 362 | if (bitsReceived == messageSize * 8) 363 | { 364 | // We have received a complete message. Go to the EndMark 365 | // state so we can measure the time of the mark at the end 366 | // of the message. 367 | repeatCommand = false; 368 | changeState(EndMark); 369 | } 370 | else 371 | { 372 | // We still need more data, so go to the mark state. 373 | changeState(Mark); 374 | } 375 | } 376 | 377 | // This is called when almost anything goes wrong. 378 | void error() 379 | { 380 | errorFlag = true; 381 | changeState(Init); 382 | } 383 | 384 | // Changes to a new state and records the time that the change 385 | // was made. 386 | void changeState(State newState) 387 | { 388 | state = newState; 389 | stateStartTimeUs = micros(); 390 | } 391 | 392 | // The number of microseconds we have been in this state. 393 | uint32_t timeInThisStateUs() 394 | { 395 | return (uint32_t)(micros() - stateStartTimeUs); 396 | } 397 | 398 | // The number of time units we have spent in this state, 399 | // rounded to the nearest time unit. 400 | uint32_t timeInThisState() 401 | { 402 | return (timeInThisStateUs() + unitPulseTimeUs / 2) / unitPulseTimeUs; 403 | } 404 | 405 | /** High-level state variables ********/ 406 | 407 | // This gets set to true if a new message has been received, 408 | // and gets cleared when getAndResetMessageFlag() is called. 409 | bool messageFlag = false; 410 | 411 | // This gets set to true if a new repeat command has been 412 | // received, and gets cleared when getAndResetRepeatFlag() is 413 | // called. 414 | bool repeatFlag = false; 415 | 416 | // This gets set to true if an error happens, and gets cleared 417 | // when getAndResetErrorFlag() is called. 418 | bool errorFlag = false; 419 | 420 | // Holds the last message received. The first two bytes should 421 | // be the address, and the second two bytes should be the 422 | // command. 423 | uint8_t message[messageSize]; 424 | 425 | 426 | /** Low-level state variables *********/ 427 | 428 | // The overall state of the low-level state machine. 429 | State state = Init; 430 | 431 | // A buffer is used to hold a command code that has only 432 | // partially been received. 433 | uint8_t incomingMessage[messageSize]; 434 | 435 | // The number of bits received so far. This is set before 436 | // going into the first Mark state. It is only used during 437 | // Mark and Space states. 438 | uint16_t bitsReceived = 0; 439 | 440 | // This is only used in the EndMark state. It lets us remember 441 | // whether we are processing the final mark of an actual command 442 | // or just a repeat command. Alternatively, we could have just 443 | // made two states: EndMarkNormal and EndMarkRepeat. 444 | bool repeatCommand; 445 | 446 | // The time that we entered this state, in microseconds. 447 | // Gets updated whenever the state is changed. 448 | uint32_t stateStartTimeUs = 0; 449 | }; 450 | -------------------------------------------------------------------------------- /PololuBuzzer.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file PololuBuzzer.h 4 | * 5 | * See the PololuBuzzer class reference for more information about this library. 6 | * 7 | * The main repository for this library is 8 | * https://github.com/pololu/pololu-buzzer-arduino, though copies of this 9 | * library also exist in other repositories. 10 | * 11 | * \class PololuBuzzer PololuBuzzer.h 12 | * \brief Play beeps and music with the buzzer. 13 | * 14 | * The PololuBuzzer library allows various sounds to be played through a buzzer, 15 | * from simple beeps to complex tunes. 16 | * 17 | * On the ATmega328P/168 boards, this library uses Timer 2 and pin 3 (PD3/OC2B). 18 | * On ATmega32U4 boards, this library uses Timer 4 and pin 6 (PD7/OC4D). This 19 | * library will conflict will other libraries that use the same timer or pin. 20 | * 21 | * Note durations are timed using a timer overflow interrupt 22 | * (`TIMER2_OVF`/`TIMER4_OVF`), which will briefly interrupt execution of your 23 | * main program at the frequency of the sound being played. In most cases, the 24 | * interrupt-handling routine is very short (several microseconds). However, 25 | * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode) 26 | * with the `play()` command, this interrupt takes much longer than normal 27 | * (perhaps several hundred microseconds) every time it starts a new note. It is 28 | * important to take this into account when writing timing-critical code. 29 | * 30 | * This library is fully compatible with the OrangutanBuzzer functions 31 | * in the [Pololu AVR C/C++ Library](http://www.pololu.com/docs/0J18) 32 | * and the [ZumoBuzzer library](https://github.com/pololu/zumo-shield), 33 | * so any sequences and melodies written for those libraries will also work 34 | * with the equivalent PololuBuzzer functions. */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | /*! \brief Specifies that the sequence of notes will play with no further action 41 | * required by the user. */ 42 | #define PLAY_AUTOMATIC 0 43 | 44 | 45 | /*! \brief Specified that the user will need to call `playCheck()` regularly. */ 46 | #define PLAY_CHECK 1 47 | 48 | // n 49 | // Equal Tempered Scale is given by f = f * a 50 | // n o 51 | // 52 | // where f is chosen as A above middle C (A4) at f = 440 Hz 53 | // o o 54 | // and a is given by the twelfth root of 2 (~1.059463094359) 55 | 56 | /*! \anchor note_macros 57 | * 58 | * \name Note Macros 59 | * \a x specifies the octave of the note 60 | * @{ 61 | */ 62 | #define NOTE_C(x) ( 0 + (x)*12) 63 | #define NOTE_C_SHARP(x) ( 1 + (x)*12) 64 | #define NOTE_D_FLAT(x) ( 1 + (x)*12) 65 | #define NOTE_D(x) ( 2 + (x)*12) 66 | #define NOTE_D_SHARP(x) ( 3 + (x)*12) 67 | #define NOTE_E_FLAT(x) ( 3 + (x)*12) 68 | #define NOTE_E(x) ( 4 + (x)*12) 69 | #define NOTE_F(x) ( 5 + (x)*12) 70 | #define NOTE_F_SHARP(x) ( 6 + (x)*12) 71 | #define NOTE_G_FLAT(x) ( 6 + (x)*12) 72 | #define NOTE_G(x) ( 7 + (x)*12) 73 | #define NOTE_G_SHARP(x) ( 8 + (x)*12) 74 | #define NOTE_A_FLAT(x) ( 8 + (x)*12) 75 | #define NOTE_A(x) ( 9 + (x)*12) 76 | #define NOTE_A_SHARP(x) (10 + (x)*12) 77 | #define NOTE_B_FLAT(x) (10 + (x)*12) 78 | #define NOTE_B(x) (11 + (x)*12) 79 | 80 | /*! \brief silences buzzer for the note duration */ 81 | #define SILENT_NOTE 0xFF 82 | 83 | /*! \brief frequency bit that indicates Hz/10
84 | * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz 85 | */ 86 | #define DIV_BY_10 (1 << 15) 87 | /*! @} */ 88 | 89 | class PololuBuzzer 90 | { 91 | public: 92 | 93 | /*! \brief Plays the specified frequency for the specified duration. 94 | * 95 | * \param freq Frequency to play in Hz (or 0.1 Hz if the `DIV_BY_10` bit 96 | * is set). 97 | * \param duration Duration of the note in milliseconds. 98 | * \param volume Volume of the note (0--15). 99 | * 100 | * The \a frequency argument must be between 40 Hz and 10 kHz. If the most 101 | * significant bit of \a frequency is set, the frequency played is the value 102 | * of the lower 15 bits of \a frequency in units of 0.1 Hz. Therefore, you can 103 | * play a frequency of 44.5 Hz by using a \a frequency of `(DIV_BY_10 | 445)`. 104 | * If the most significant bit of \a frequency is not set, the units for 105 | * frequency are Hz. The \a volume argument controls the buzzer volume, with 106 | * 15 being the loudest and 0 being the quietest. A \a volume of 15 supplies 107 | * the buzzer with a 50% duty cycle PWM at the specified \a frequency. 108 | * Lowering \a volume by one halves the duty cycle (so 14 gives a 25% duty 109 | * cycle, 13 gives a 12.5% duty cycle, etc). The volume control is somewhat 110 | * crude (especially on the ATmega328/168) and should be thought of as a bonus 111 | * feature. 112 | * 113 | * This function plays the note in the background while your program continues 114 | * to execute. If you call another buzzer function while the note is playing, 115 | * the new function call will overwrite the previous and take control of the 116 | * buzzer. If you want to string notes together, you should either use the 117 | * `play()` function or put an appropriate delay after you start a note 118 | * playing. You can use the `is_playing()` function to figure out when the 119 | * buzzer is through playing its note or melody. 120 | * 121 | * ### Example ### 122 | * 123 | * ~~~{.cpp} 124 | * PololuBuzzer buzzer; 125 | * 126 | * ... 127 | * 128 | * // play a 6 kHz note for 250 ms at a lower volume 129 | * buzzer.playFrequency(6000, 250, 12); 130 | * 131 | * // wait for buzzer to finish playing the note 132 | * while (buzzer.isPlaying()); 133 | * 134 | * // play a 44.5 Hz note for 1 s at full volume 135 | * buzzer.playFrequency(DIV_BY_10 | 445, 1000, 15); 136 | * ~~~ 137 | * 138 | * \warning \a frequency × \a duration / 1000 must be no greater than 139 | * 0xFFFF (65535). This means you can't use a duration of 65535 ms for 140 | * frequencies greater than 1 kHz. For example, the maximum duration you can 141 | * use for a frequency of 10 kHz is 6553 ms. If you use a duration longer than 142 | * this, you will produce an integer overflow that can result in unexpected 143 | * behavior. 144 | */ 145 | static void playFrequency(unsigned int freq, unsigned int duration, 146 | unsigned char volume); 147 | 148 | /*! \brief Plays the specified note for the specified duration. 149 | * 150 | * \param note Note to play (see \ref note_macros "Note Macros"). 151 | * \param duration Duration of the note in milliseconds. 152 | * \param volume Volume of the note (0--15). 153 | * 154 | * The \a note argument is an enumeration for the notes of the equal tempered 155 | * scale (ETS). See \ref note_macros "Note Macros" for more information. The 156 | * \a volume argument controls the buzzer volume, with 15 being the loudest 157 | * and 0 being the quietest. A \a volume of 15 supplies the buzzer with a 50% 158 | * duty cycle PWM at the specified \a frequency. Lowering \a volume by one 159 | * halves the duty cycle (so 14 gives a 25% duty cycle, 13 gives a 12.5% duty 160 | * cycle, etc). The volume control is somewhat crude (especially on the 161 | * ATmega328/168) and should be thought of as a bonus feature. 162 | * 163 | * This function plays the note in the background while your program continues 164 | * to execute. If you call another buzzer function while the note is playing, 165 | * the new function call will overwrite the previous and take control of the 166 | * buzzer. If you want to string notes together, you should either use the 167 | * `play()` function or put an appropriate delay after you start a note 168 | * playing. You can use the `is_playing()` function to figure out when the 169 | * buzzer is through playing its note or melody. 170 | */ 171 | static void playNote(unsigned char note, unsigned int duration, 172 | unsigned char volume); 173 | 174 | /*! \brief Plays the specified sequence of notes. 175 | * 176 | * \param sequence Char array containing a sequence of notes to play. 177 | * 178 | * If the play mode is `PLAY_AUTOMATIC` (default), the sequence of notes will 179 | * play with no further action required by the user. If the play mode is 180 | * `PLAY_CHECK`, the user will need to call `playCheck()` in the main loop to 181 | * initiate the playing of each new note in the sequence. The play mode can be 182 | * changed while the sequence is playing. The sequence syntax is modeled after 183 | * the PLAY commands in GW-BASIC, with just a few differences. 184 | * 185 | * The notes are specified by the characters **C**, **D**, **E**, **F**, 186 | * **G**, **A**, and **B**, and they are played by default as "quarter notes" 187 | * with a length of 500 ms. This corresponds to a tempo of 120 beats/min. 188 | * Other durations can be specified by putting a number immediately after the 189 | * note. For example, C8 specifies C played as an eighth note, with half the 190 | * duration of a quarter note. The special note **R** plays a rest (no sound). 191 | * The sequence parser is case-insensitive and ignores spaces, which may be 192 | * used to format your music nicely. 193 | * 194 | * Various control characters alter the sound: 195 | * 196 | * 197 | * 198 | * 199 | * 200 | * 201 | * 202 | * 203 | * 204 | * 205 | * 206 | * 209 | * 210 | * 213 | * 214 | * 215 | * 216 | * 217 | * 218 | * 219 | * 220 | * 221 | * 222 | * 225 | * 226 | * 227 | * 228 | * 231 | * 232 | * 234 | * 235 | * 239 | *
Control character(s)Effect
A--GSpecifies a note that will be played.
RSpecifies a rest (no sound for the duration of the note).
+ or # after a noteRaises the preceding note one half-step.
- after a noteLowers the preceding note one half-step.
1--2000 after a noteDetermines the duration of the preceding note. For example, C16 207 | * specifies C played as a sixteenth note (1/16th the length of a 208 | * whole note).
. after a note"Dots" the preceding note, increasing the length by 50%. Each 211 | * additional dot adds half as much as the previous dot, so that "A.." 212 | * is 1.75 times the length of "A".
> before a notePlays the following note one octave higher.
< before a notePlays the following note one octave lower.
O followed by a numberSets the octave. (default: **O4**)
T followed by a numberSets the tempo in beats per minute (BPM). (default: **T120**)
L followed by a numberSets the default note duration to the type specified by the number: 223 | * 4 for quarter notes, 8 for eighth notes, 16 for sixteenth notes, 224 | * etc. (default: **L4**)
V followed by a numberSets the music volume (0--15). (default: **V15**)
MSSets all subsequent notes to play play staccato -- each note is 229 | * played for 1/2 of its allotted time, followed by an equal period of 230 | * silence.
MLSets all subsequent notes to play legato -- each note is played for 233 | * full length. This is the default setting.
!Resets the octave, tempo, duration, volume, and staccato setting to 236 | * their default values. These settings persist from one `play()` to the 237 | * next, which allows you to more conveniently break up your music into 238 | * reusable sections.
240 | * 241 | * This function plays the string of notes in the background while your 242 | * program continues to execute. If you call another buzzer function while the 243 | * melody is playing, the new function call will overwrite the previous and 244 | * take control of the buzzer. If you want to string melodies together, you 245 | * should put an appropriate delay after you start a melody playing. You can 246 | * use the `is_playing()` function to figure out when the buzzer is through 247 | * playing the melody. 248 | * 249 | * ### Example ### 250 | * 251 | * ~~~{.cpp} 252 | * PololuBuzzer buzzer; 253 | * 254 | * ... 255 | * 256 | * // play a C major scale up and back down: 257 | * buzzer.play("!L16 V8 cdefgab>cbagfedc"); 258 | * while (buzzer.isPlaying()); 259 | * 260 | * // the first few measures of Bach's fugue in D-minor 261 | * buzzer.play("!T240 L8 agafaea dac+adaea fa 279 | * 280 | * PololuBuzzer buzzer; 281 | * const char melody[] PROGMEM = "!L16 V8 cdefgab>cbagfedc"; 282 | * 283 | * ... 284 | * 285 | * buzzer.playFromProgramSpace(melody); 286 | * ~~~ 287 | */ 288 | static void playFromProgramSpace(const char *sequence); 289 | 290 | /*! \brief Controls whether `play()` sequence is played automatically or 291 | * must be driven with `playCheck()`. 292 | * 293 | * \param mode Play mode (either `PLAY_AUTOMATIC` or `PLAY_CHECK`). 294 | * 295 | * This method lets you determine whether the notes of the `play()` sequence 296 | * are played automatically in the background or are driven by the 297 | * `playCheck()` method. If \a mode is `PLAY_AUTOMATIC`, the sequence will 298 | * play automatically in the background, driven by the timer overflow 299 | * interrupt. The interrupt will take a considerable amount of time to execute 300 | * when it starts the next note in the sequence playing, so it is recommended 301 | * that you do not use automatic-play if you cannot tolerate being interrupted 302 | * for more than a few microseconds. If \a mode is `PLAY_CHECK`, you can 303 | * control when the next note in the sequence is played by calling the 304 | * `playCheck()` method at acceptable points in your main loop. If your main 305 | * loop has substantial delays, it is recommended that you use automatic-play 306 | * mode rather than play-check mode. Note that the play mode can be changed 307 | * while the sequence is being played. The mode is set to `PLAY_AUTOMATIC` by 308 | * default. 309 | */ 310 | static void playMode(unsigned char mode); 311 | 312 | /*! \brief Starts the next note in a sequence, if necessary, in `PLAY_CHECK` 313 | * mode. 314 | * 315 | * \return 0 if sequence is complete, 1 otherwise. 316 | * 317 | * This method only needs to be called if you are in `PLAY_CHECK` mode. It 318 | * checks to see whether it is time to start another note in the sequence 319 | * initiated by `play()`, and starts it if so. If it is not yet time to start 320 | * the next note, this method returns without doing anything. Call this as 321 | * often as possible in your main loop to avoid delays between notes in the 322 | * sequence. This method returns 0 (false) if the melody to be played is 323 | * complete, otherwise it returns 1 (true). 324 | */ 325 | static unsigned char playCheck(); 326 | 327 | /*! \brief Checks whether a note, frequency, or sequence is being played. 328 | * 329 | * \return 1 if the buzzer is current playing a note, frequency, or sequence; 330 | * 0 otherwise. 331 | * 332 | * This method returns 1 (true) if the buzzer is currently playing a 333 | * note/frequency or if it is still playing a sequence started by `play()`. 334 | * Otherwise, it returns 0 (false). You can poll this method to determine when 335 | * it's time to play the next note in a sequence, or you can use it as the 336 | * argument to a delay loop to wait while the buzzer is busy. 337 | */ 338 | static unsigned char isPlaying(); 339 | 340 | /*! \brief Stops any note, frequency, or melody being played. 341 | * 342 | * This method will immediately silence the buzzer and terminate any 343 | * note/frequency/melody that is currently playing. 344 | */ 345 | static void stopPlaying(); 346 | 347 | 348 | private: 349 | 350 | // initializes timer for buzzer control 351 | static void init2(); 352 | static void init(); 353 | }; 354 | -------------------------------------------------------------------------------- /FastGPIO.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file FastGPIO.h 4 | 5 | FastGPIO is a C++ header library for efficient AVR I/O. 6 | 7 | For an overview of the features of this library, see 8 | https://github.com/pololu/fastgpio-arduino. 9 | That is the main repository for this library, though copies may exist in other 10 | repositories. 11 | 12 | The FastGPIO::Pin class provides static functions for manipulating pins. See 13 | its class reference for more information. 14 | 15 | \class FastGPIO::Pin 16 | 17 | @tparam pin The pin number 18 | 19 | The FastGPIO::Pin class provides static functions for manipulating pins. This 20 | class can only be used if the pin number is known at compile time, which means 21 | it does not come from a variable that might change and it does not come from the 22 | result of a complicated calculation. 23 | 24 | Here is some example code showing how to use this class to blink an LED: 25 | 26 | ~~~{.cpp} 27 | #include 28 | 29 | #define LED_PIN 13 30 | 31 | void setup() { 32 | } 33 | 34 | void loop() { 35 | FastGPIO::Pin::setOutput(0); 36 | delay(500); 37 | FastGPIO::Pin::setOutput(1); 38 | delay(500); 39 | } 40 | ~~~ 41 | 42 | */ 43 | 44 | #pragma once 45 | #include 46 | #include 47 | 48 | /** @cond */ 49 | #define _FG_SBI(mem_addr, bit) asm volatile("sbi %0, %1\n" : \ 50 | : "I" (mem_addr - __SFR_OFFSET), "I" (bit)) 51 | #define _FG_CBI(mem_addr, bit) asm volatile("cbi %0, %1\n" : \ 52 | : "I" (mem_addr - __SFR_OFFSET), "I" (bit)) 53 | #define _FG_PIN(port, bit) { _SFR_MEM_ADDR(PIN##port), _SFR_MEM_ADDR(PORT##port), \ 54 | _SFR_MEM_ADDR(DDR##port), bit } 55 | /** @endcond */ 56 | 57 | namespace FastGPIO 58 | { 59 | /** @cond */ 60 | /** The IOStruct struct and the pinStructs array below are not documented in 61 | * the Doxygen documentation, but can be used by advanced users of this 62 | * library and are considered to be part of the public API for the purposes 63 | * of semantic versioning. 64 | */ 65 | typedef struct IOStruct 66 | { 67 | uint8_t pinAddr; 68 | uint8_t portAddr; 69 | uint8_t ddrAddr; 70 | uint8_t bit; 71 | 72 | volatile uint8_t * pin() const 73 | { 74 | return (volatile uint8_t *)(uint16_t)pinAddr; 75 | } 76 | 77 | volatile uint8_t * port() const 78 | { 79 | return (volatile uint8_t *)(uint16_t)portAddr; 80 | } 81 | 82 | volatile uint8_t * ddr() const 83 | { 84 | return (volatile uint8_t *)(uint16_t)ddrAddr; 85 | } 86 | } IOStruct; 87 | /** @endcond */ 88 | 89 | #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega168P__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) 90 | 91 | const IOStruct pinStructs[] = { 92 | _FG_PIN(D, 0), 93 | _FG_PIN(D, 1), 94 | _FG_PIN(D, 2), 95 | _FG_PIN(D, 3), 96 | _FG_PIN(D, 4), 97 | _FG_PIN(D, 5), 98 | _FG_PIN(D, 6), 99 | _FG_PIN(D, 7), 100 | _FG_PIN(B, 0), 101 | _FG_PIN(B, 1), 102 | _FG_PIN(B, 2), 103 | _FG_PIN(B, 3), 104 | _FG_PIN(B, 4), 105 | _FG_PIN(B, 5), 106 | _FG_PIN(C, 0), 107 | _FG_PIN(C, 1), 108 | _FG_PIN(C, 2), 109 | _FG_PIN(C, 3), 110 | _FG_PIN(C, 4), 111 | _FG_PIN(C, 5), 112 | _FG_PIN(C, 6), 113 | _FG_PIN(C, 7), // Null pin (IO_NONE) 114 | }; 115 | 116 | #define IO_D0 0 117 | #define IO_D1 1 118 | #define IO_D2 2 119 | #define IO_D3 3 120 | #define IO_D4 4 121 | #define IO_D5 5 122 | #define IO_D6 6 123 | #define IO_D7 7 124 | #define IO_B0 8 125 | #define IO_B1 9 126 | #define IO_B2 10 127 | #define IO_B3 11 128 | #define IO_B4 12 129 | #define IO_B5 13 130 | #define IO_C0 14 131 | #define IO_C1 15 132 | #define IO_C2 16 133 | #define IO_C3 17 134 | #define IO_C4 18 135 | #define IO_C5 19 136 | #define IO_C6 20 137 | #define IO_NONE 21 138 | 139 | #elif defined(__AVR_ATmega32U4__) 140 | 141 | const IOStruct pinStructs[] = { 142 | _FG_PIN(D, 2), 143 | _FG_PIN(D, 3), 144 | _FG_PIN(D, 1), 145 | _FG_PIN(D, 0), 146 | _FG_PIN(D, 4), 147 | _FG_PIN(C, 6), 148 | _FG_PIN(D, 7), 149 | _FG_PIN(E, 6), 150 | 151 | _FG_PIN(B, 4), 152 | _FG_PIN(B, 5), 153 | _FG_PIN(B, 6), 154 | _FG_PIN(B, 7), 155 | _FG_PIN(D, 6), 156 | _FG_PIN(C, 7), 157 | 158 | _FG_PIN(B, 3), 159 | _FG_PIN(B, 1), 160 | _FG_PIN(B, 2), 161 | _FG_PIN(B, 0), 162 | 163 | _FG_PIN(F, 7), 164 | _FG_PIN(F, 6), 165 | _FG_PIN(F, 5), 166 | _FG_PIN(F, 4), 167 | _FG_PIN(F, 1), 168 | _FG_PIN(F, 0), 169 | 170 | _FG_PIN(D, 4), 171 | _FG_PIN(D, 7), 172 | _FG_PIN(B, 4), 173 | _FG_PIN(B, 5), 174 | _FG_PIN(B, 6), 175 | _FG_PIN(D, 6), 176 | 177 | // Extra pins added by this library and not supported by the 178 | // Arduino GPIO functions: 179 | _FG_PIN(D, 5), 180 | _FG_PIN(E, 2), 181 | 182 | _FG_PIN(E, 0) // Null pin (IO_NONE) 183 | }; 184 | 185 | #define IO_D2 0 186 | #define IO_D3 1 187 | #define IO_D1 2 188 | #define IO_D0 3 189 | #define IO_D4 4 190 | #define IO_C6 5 191 | #define IO_D7 6 192 | #define IO_E6 7 193 | #define IO_B4 8 194 | #define IO_B5 9 195 | #define IO_B6 10 196 | #define IO_B7 11 197 | #define IO_D6 12 198 | #define IO_C7 13 199 | #define IO_B3 14 200 | #define IO_B1 15 201 | #define IO_B2 16 202 | #define IO_B0 17 203 | #define IO_F7 18 204 | #define IO_F6 19 205 | #define IO_F5 20 206 | #define IO_F4 21 207 | #define IO_F1 22 208 | #define IO_F0 23 209 | #define IO_D5 30 210 | #define IO_E2 31 211 | #define IO_NONE 32 212 | 213 | #else 214 | #error FastGPIO does not support this board. 215 | #endif 216 | 217 | template class Pin 218 | { 219 | public: 220 | /*! \brief Configures the pin to be an output driving low. 221 | * 222 | * This is equivalent to calling setOutput with an argument of 0, 223 | * but it has a simpler implementation which means it is more 224 | * likely to be compiled down to just 2 assembly instructions. 225 | */ 226 | static inline void setOutputLow() __attribute__((always_inline)) 227 | { 228 | _FG_CBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 229 | _FG_SBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 230 | } 231 | 232 | /*! \brief Configures the pin to be an output driving high. 233 | * 234 | * This is equivalent to calling setOutput with an argument of 1, 235 | * but it has a simpler implementation which means it is more 236 | * likely to be compiled down to just 2 assembly instructions. 237 | */ 238 | static inline void setOutputHigh() __attribute__((always_inline)) 239 | { 240 | _FG_SBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 241 | _FG_SBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 242 | } 243 | 244 | /*! \brief Configures the pin to be an output and toggles it. 245 | */ 246 | static inline void setOutputToggle() __attribute__((always_inline)) 247 | { 248 | setOutputValueToggle(); 249 | _FG_SBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 250 | } 251 | 252 | /*! \brief Sets the pin as an output. 253 | * 254 | * @param value Should be 0, LOW, or false to drive the pin low. Should 255 | * be 1, HIGH, or true to drive the pin high. 256 | * 257 | * The PORT bit is set before the DDR bit to ensure that the output is 258 | * not accidentally driven to the wrong value during the transition. 259 | */ 260 | static inline void setOutput(bool value) __attribute__((always_inline)) 261 | { 262 | setOutputValue(value); 263 | _FG_SBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 264 | } 265 | 266 | /*! \brief Sets the output value of the pin to 0. 267 | * 268 | * This is mainly intended to be used on pins that have already been 269 | * configured as an output in order to make the output drive low. 270 | * 271 | * If this is used on an input pin, it has the effect of disabling the 272 | * input pin's pull-up resistor. 273 | */ 274 | static inline void setOutputValueLow() __attribute__((always_inline)) 275 | { 276 | _FG_CBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 277 | } 278 | 279 | /*! \brief Sets the output value of the pin to 1. 280 | * 281 | * This is mainly intended to be used on pins that have already been 282 | * configured as an output in order to make the output drive low. 283 | * 284 | * If this is used on an input pin, it has the effect of enabling the 285 | * input pin's pull-up resistor. 286 | */ 287 | static inline void setOutputValueHigh() __attribute__((always_inline)) 288 | { 289 | _FG_SBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 290 | } 291 | 292 | /*! \brief Toggles the output value of the pin. 293 | * 294 | * This is mainly intended to be used on pins that have already been 295 | * configured as an output. If the pin was driving low, this function 296 | * changes it to drive high. If the pin was driving high, this function 297 | * changes it to drive low. 298 | * 299 | * If this function is used on an input pin, it has the effect of 300 | * toggling the state of the input pin's pull-up resistor. 301 | */ 302 | static inline void setOutputValueToggle() __attribute__((always_inline)) 303 | { 304 | _FG_SBI(pinStructs[pin].pinAddr, pinStructs[pin].bit); 305 | } 306 | 307 | /*! \brief Sets the output value of the pin. 308 | * 309 | * @param value Should be 0, LOW, or false to drive the pin low. Should 310 | * be 1, HIGH, or true to drive the pin high. 311 | * 312 | * This is mainly intended to be used on pins that have already been 313 | * configured as an output. 314 | * 315 | * If this function is used on an input pin, it has the effect of 316 | * toggling setting the state of the input pin's pull-up resistor. 317 | */ 318 | static inline void setOutputValue(bool value) __attribute__((always_inline)) 319 | { 320 | if (value) 321 | { 322 | _FG_SBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 323 | } 324 | else 325 | { 326 | _FG_CBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 327 | } 328 | } 329 | 330 | /*! \brief Sets a pin to be a digital input with the internal pull-up 331 | * resistor disabled. 332 | */ 333 | static inline void setInput() __attribute__((always_inline)) 334 | { 335 | _FG_CBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 336 | _FG_CBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 337 | } 338 | 339 | /*! \brief Sets a pin to be a digital input with the internal pull-up 340 | * resistor enabled. 341 | */ 342 | static inline void setInputPulledUp() __attribute__((always_inline)) 343 | { 344 | _FG_CBI(pinStructs[pin].ddrAddr, pinStructs[pin].bit); 345 | _FG_SBI(pinStructs[pin].portAddr, pinStructs[pin].bit); 346 | } 347 | 348 | /*! \brief Reads the input value of the pin. 349 | * 350 | * @return 0 if the pin is low, 1 if the pin is high. 351 | */ 352 | static inline bool isInputHigh() __attribute__((always_inline)) 353 | { 354 | return *pinStructs[pin].pin() >> pinStructs[pin].bit & 1; 355 | 356 | /* This is another option but it is less efficient in code 357 | like "if (isInputHigh()) { ... }": 358 | bool value; 359 | asm volatile( 360 | "ldi %0, 0\n" 361 | "sbic %2, %1\n" 362 | "ldi %0, 1\n" 363 | : "=d" (value) 364 | : "I" (pinStructs[pin].bit), 365 | "I" (pinStructs[pin].pinAddr - __SFR_OFFSET)); 366 | return value; 367 | */ 368 | } 369 | 370 | /*! \brief Returns 1 if the pin is configured as an output. 371 | * 372 | * @return 1 if the pin is an output, 0 if it is an input. 373 | */ 374 | static inline bool isOutput() __attribute__((always_inline)) 375 | { 376 | return *pinStructs[pin].ddr() >> pinStructs[pin].bit & 1; 377 | } 378 | 379 | /*! \brief Returns the output value of the pin. 380 | * 381 | * This is mainly intended to be called on pins that have been 382 | * configured an an output. If it is called on an input pin, the return 383 | * value indicates whether the pull-up resistor is enabled or not. 384 | */ 385 | static inline bool isOutputValueHigh() __attribute__((always_inline)) 386 | { 387 | return *pinStructs[pin].port() >> pinStructs[pin].bit & 1; 388 | } 389 | 390 | /*! \brief Returns the full 2-bit state of the pin. 391 | * 392 | * Bit 0 of this function's return value is the pin's output value. 393 | * Bit 1 of the return value is the pin direction; a value of 1 394 | * means output. All the other bits are zero. 395 | */ 396 | static uint8_t getState() 397 | { 398 | uint8_t state; 399 | asm volatile( 400 | "ldi %0, 0\n" 401 | "sbic %2, %1\n" 402 | "ori %0, 1\n" // Set state bit 0 to 1 if PORT bit is set. 403 | "sbic %3, %1\n" 404 | "ori %0, 2\n" // Set state bit 1 to 1 if DDR bit is set. 405 | : "=a" (state) 406 | : "I" (pinStructs[pin].bit), 407 | "I" (pinStructs[pin].portAddr - __SFR_OFFSET), 408 | "I" (pinStructs[pin].ddrAddr - __SFR_OFFSET)); 409 | return state; 410 | 411 | /* Equivalent C++ code: 412 | return isOutput() << 1 | isOutputValueHigh(); 413 | */ 414 | } 415 | 416 | /*! \brief Sets the full 2-bit state of the pin. 417 | * 418 | * @param state The state of the pin, as returns from getState. 419 | * All bits other than bits 0 and 1 are ignored. 420 | * 421 | * Sometimes this function will need to change both the PORT bit (which 422 | * specifies the output value) and the DDR bit (which specifies whether 423 | * the pin is an output). If the DDR bit is getting set to 0, this 424 | * function changes DDR first, and if it is getting set to 1, this 425 | * function changes DDR last. This guarantees that the intermediate pin 426 | * state is always an input state. 427 | */ 428 | static void setState(uint8_t state) 429 | { 430 | asm volatile( 431 | "bst %0, 1\n" // Set DDR to 0 if needed 432 | "brts .+2\n" 433 | "cbi %3, %1\n" 434 | "bst %0, 0\n" // Copy state bit 0 to PORT bit 435 | "brts .+2\n" 436 | "cbi %2, %1\n" 437 | "brtc .+2\n" 438 | "sbi %2, %1\n" 439 | "bst %0, 1\n" // Set DDR to 1 if needed 440 | "brtc .+2\n" 441 | "sbi %3, %1\n" 442 | : 443 | : "a" (state), 444 | "I" (pinStructs[pin].bit), 445 | "I" (pinStructs[pin].portAddr - __SFR_OFFSET), 446 | "I" (pinStructs[pin].ddrAddr - __SFR_OFFSET)); 447 | } 448 | }; 449 | 450 | /*! This class saves the state of the specified pin in its constructor when 451 | * it is created, and restores the pin to that state in its destructor. 452 | * This can be very useful if a pin is being used for multiple purposes. 453 | * It allows you to write code that temporarily changes the state of the 454 | * pin and is guaranteed to restore the state later. 455 | * 456 | * For example, if you were controlling both a button and an LED using a 457 | * single pin and you wanted to see if the button was pressed without 458 | * affecting the LED, you could write: 459 | * 460 | * ~~~{.cpp} 461 | * bool buttonIsPressed() 462 | * { 463 | * FastGPIO::PinLoan loan; 464 | * FastGPIO::Pin::setInputPulledUp(); 465 | * _delay_us(10); 466 | * return !FastGPIO::Pin::isInputHigh(); 467 | * } 468 | * ~~~ 469 | * 470 | * This is equivalent to: 471 | * 472 | * ~~~{.cpp} 473 | * bool buttonIsPressed() 474 | * { 475 | * uint8_t state = FastGPIO::Pin::getState(); 476 | * FastGPIO::Pin::setInputPulledUp(); 477 | * _delay_us(10); 478 | * bool value = !FastGPIO::Pin::isInputHigh(); 479 | * FastGPIO::Pin::setState(state); 480 | * return value; 481 | * } 482 | * ~~~ 483 | */ 484 | template class PinLoan 485 | { 486 | public: 487 | /*! \brief The state of the pin as returned from FastGPIO::Pin::getState. */ 488 | uint8_t state; 489 | 490 | PinLoan() 491 | { 492 | state = Pin::getState(); 493 | } 494 | 495 | ~PinLoan() 496 | { 497 | Pin::setState(state); 498 | } 499 | }; 500 | }; 501 | 502 | #undef _FG_PIN 503 | #undef _FG_CBI 504 | #undef _FG_SBI 505 | -------------------------------------------------------------------------------- /PololuHD44780.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file PololuHD44780.h 4 | * 5 | * This is the main header file for the %PololuHD44780 library. 6 | * 7 | * For an overview of the library's features, see 8 | * https://github.com/pololu/pololu-hd44780-arduino. That is the main 9 | * repository for the library, though copies of the library may exist in other 10 | * repositories. */ 11 | 12 | #pragma once 13 | #include 14 | #include 15 | 16 | /*! \brief General class for handling the HD44780 protocol. 17 | * 18 | * This is an abstract class that knows about the HD44780 LCD commands but 19 | * does not directly read or write from the actual LCD. To make a usable class, 20 | * you need to define a subclass of PololuHD44780Base and implement the 21 | * initPins() and send() functions. 22 | * 23 | * The subclass will inherit all the functions from PololuHD44780Base which are 24 | * documented here. It will also inherit all of the functions from the Arduino `Print` class. 25 | * For more information about what the `Print` class provides, see the [Arduino print() documentation](http://arduino.cc/en/Serial/Print) or look at [Print.h in the Arduino IDE source code](https://github.com/arduino/Arduino/blob/master/hardware/arduino/cores/arduino/Print.h). 26 | * 27 | * Most users of this library will not need to directly use this class and 28 | * should use PololuHD44780 or some other subclass of PololuHD44780Base defined 29 | * in a different library. 30 | * 31 | * ## LCD scrolling ## 32 | * 33 | * The PololuHD44780Base class provides several functions related to scrolling: 34 | * 35 | * * scrollDisplayLeft() scrolls everything on the screen one position to the left. 36 | * * scrollDisplayRight() scrolls everything on the screen one position to the right. 37 | * * autoscroll() and noAutoscroll() control whether auto-scrolling is enabled. 38 | * * home() and clear() both reset the scroll position 39 | * 40 | * The HD44780 actually stores 40 columns internally. By default, the left-most 41 | * internal columns are the ones that are actually displayed on the screen, but 42 | * the scrolling features allow that correspondence to change. The scrolling 43 | * wraps around, so it is possible to display some of the right-most columns on 44 | * the screen at the same time as some of the left-most columns. 45 | * 46 | * For the gotoXY() function, the x coordinate actually corresponds to the 47 | * internal column index. The left-most internal column has an x coordinate of 48 | * 0, and the right-most has an x coordinate of 39. 49 | * 50 | * For example, if you are controlling a 2×8 character LCD and you call 51 | * scrollDisplayLeft() 35 times (or call scrollDisplayRight() 5 times), then the 52 | * X coordinates of the columns displayed, from left to right, will be 35, 36, 53 | * 37, 38, 39, 0, 1, and 2. 54 | * 55 | */ 56 | class PololuHD44780Base : public Print 57 | { 58 | public: 59 | 60 | PololuHD44780Base(); 61 | 62 | /*! Initializes the pins so that the send() function can be called 63 | * successfully. This is the first step of initializing the LCD. */ 64 | virtual void initPins() = 0; 65 | 66 | /*! Initialize the LCD if it has not already been initialized. */ 67 | void init() 68 | { 69 | if (!initialized) 70 | { 71 | initialized = true; 72 | init2(); 73 | } 74 | } 75 | 76 | /*! Reinitialize the LCD. This performs the same initialization that is 77 | * done automatically the first time any function is called that writes to 78 | * the LCD. This is useful if you want to get it back to a totally clean 79 | * state. */ 80 | void reinitialize() 81 | { 82 | initialized = true; 83 | init2(); 84 | } 85 | 86 | /*! Sends data or commands to the LCD. 87 | * 88 | * The initPins() function will always be called before the first time this 89 | * function is called. This function does not need to worry about the 90 | * delays necessary to make sure the previous command has finished; that is 91 | * taken care of by PololuHD44780Base. 92 | * 93 | * This function, along with initPins(), comprise the hardware abstraction 94 | * layer for the LCD, and must be defined in a subclass of 95 | * PololuHD44780Base. All other functions use these two functions to 96 | * communicate with the LCD. 97 | * 98 | * @param data The data to send to the LCD. 99 | * @param rsValue True to drive the RS pin high, false to drive it low. 100 | * @param only4bits: If true, and the LCD is using a 4-bit interface, only sends 101 | * the lower 4 bits of the data. */ 102 | virtual void send(uint8_t data, bool rsValue, bool only4bits) = 0; 103 | 104 | private: 105 | 106 | void sendAndDelay(uint8_t data, bool rsValue, bool only4bit); 107 | 108 | /*! Sends an 8-bit command to the LCD. */ 109 | void sendCommand(uint8_t cmd) 110 | { 111 | sendAndDelay(cmd, false, false); 112 | } 113 | 114 | /*! Sends a 4-bit command to the LCD. */ 115 | void sendCommand4Bit(uint8_t cmd) 116 | { 117 | sendAndDelay(cmd, false, true); 118 | } 119 | 120 | /*! Sends 8 bits of a data to the LCD. */ 121 | void sendData(uint8_t data) 122 | { 123 | sendAndDelay(data, true, false); 124 | } 125 | 126 | public: 127 | 128 | /*! Clear the contents of the LCDs, resets the cursor position to the upper 129 | * left, and resets the scroll position. */ 130 | void clear(); 131 | 132 | /*! Defines a custom character. 133 | * @param picture A pointer to the character dot pattern, in program space. 134 | * @param number A number between 0 and 7. */ 135 | void loadCustomCharacter(const uint8_t * picture, uint8_t number); 136 | 137 | /*! Defines a custom character from RAM. 138 | * @param picture A pointer to the character dot pattern, in RAM. 139 | * @param number A number between 0 and 7. */ 140 | void loadCustomCharacterFromRam(const uint8_t * picture, uint8_t number); 141 | 142 | /*! This overload of loadCustomCharacter is only provided for compatibility 143 | * with OrangutanLCD; a lot of Orangutan code defines an array of chars for 144 | * custom character pictures. */ 145 | void loadCustomCharacter(const char * picture, uint8_t number) 146 | { 147 | loadCustomCharacter((const uint8_t *)picture, number); 148 | } 149 | 150 | /*! Defines a custom character. 151 | * This is provided for compatibility with the LiquidCrystal library. */ 152 | void createChar(uint8_t number, uint8_t picture[]) 153 | { 154 | loadCustomCharacterFromRam(picture, number); 155 | } 156 | 157 | /*! Change the location of the cursor. The cursor (whether visible or invisible), 158 | * is the place where the next character written to the LCD will be displayed. 159 | * 160 | * Note that the scrolling features of the LCD change the correspondence 161 | * between the `x` parameter and the physical column that the data is 162 | * displayed on. See the "LCD scrolling" section above for more information. 163 | * 164 | * @param x The number of the column to go to, with 0 being the leftmost column. 165 | * @param y The number of the row to go to, with 0 being the top row. */ 166 | void gotoXY(uint8_t x, uint8_t y); 167 | 168 | /*! Changes the location of the cursor. This is just a wrapper around 169 | * gotoXY provided for compaitibility with the LiquidCrystal library. */ 170 | void setCursor(uint8_t col, uint8_t row) 171 | { 172 | gotoXY(col, row); 173 | } 174 | 175 | /*! Turns off the display while preserving its state. 176 | * 177 | * You can turn the display on again by calling display(). */ 178 | void noDisplay(); 179 | 180 | /*! Turns the display on. This should only be needed if noDisplay() was 181 | * previously called. */ 182 | void display(); 183 | 184 | /*! Hides the solid cursor. 185 | * 186 | * This function clears the LCD's "C" configuration bit without changing 187 | * the other bits. 188 | * 189 | * If the "B" bit is set to 1, a blinking cursor will still be displayed 190 | * even after calling this function. For that reason, it is usually better 191 | * to call hideCursor() instead. This function is only provided for 192 | * compatibility with the LiquidCrystal library. */ 193 | void noCursor(); 194 | 195 | /*! Shows the solid cursor. 196 | * 197 | * This function sets the LCD's "C" configuration bit without changing the 198 | * other bits. 199 | * 200 | * The cursor will normally be a solid line in the bottom row, but there 201 | * could be a blinking rectangle superimposed on it if previous commands 202 | * have enabled the blinking cursor. For this reason, it is usually better 203 | * to call cursorSolid() or cursorBlinking() instead. This function is only 204 | * provided for compatibility with the LiquidCrystal library. */ 205 | void cursor(); 206 | 207 | /*! Hides the blinking cursor. 208 | * 209 | * This functions clears the LCD's "B" configuration bit without changing 210 | * the other bits. 211 | * 212 | * Calling this function does not enable or disable the solid cursor (a 213 | * solid line in the bottom row) so it is usually better to call 214 | * hideCursor() or cursorSolid() instead. This function is only provided 215 | * for compatibilty with the LiquidCrystal library. */ 216 | void noBlink(); 217 | 218 | /*! Shows the blinking cursor. 219 | * 220 | * This function sets the LCD's "B" configuration bit without changing the 221 | * other bits. 222 | * 223 | * The cursor will normally be a blinking rectangle, but there could also be 224 | * a row of solid black pixels at the bottom if previous commands have 225 | * enabled the solid cursor. For this reason, it is usually better to call 226 | * cursorSolid() or cursorBlinking() instead. This function is only 227 | * provided for compatibilty with the LiquidCrystal library. */ 228 | void blink(); 229 | 230 | /*! Enables a cursor that appears as a solid line in the bottom row. 231 | * 232 | * This sets the LCD's "C" configuration bit and clears its "B" bit. 233 | * 234 | * Note that the cursor will not be shown if the display is currently off 235 | * (due to a call to noDisplay()), or if the cursor position is not within 236 | * the bounds of the screen. */ 237 | void cursorSolid(); 238 | 239 | /*! Enables a cursor that appears as a blinking black rectangle. 240 | * 241 | * This sets the LCD's "C" and "B" configuration bits. 242 | * 243 | * Note that the cursor will not be shown if the display is currently off 244 | * (due to a call to noDisplay()), or if the cursor position is not within 245 | * the bounds of the screen. */ 246 | void cursorBlinking(); 247 | 248 | /*! Hides the solid and blinking cursors. 249 | * 250 | * This clears the LCD's "C" and "B" configuration bits. */ 251 | void hideCursor(); 252 | 253 | /*! Scrolls everything on the screen one position to the left. 254 | * 255 | * This command takes about 37 microseconds. */ 256 | void scrollDisplayLeft(); 257 | 258 | /*! Scrolls everything on the screen one position to the right. 259 | * 260 | * This command takes about 37 microseconds. */ 261 | void scrollDisplayRight(); 262 | 263 | /*! Resets the screen scrolling position back to the default and moves the 264 | * cursor to the upper left corner of the screen. 265 | * 266 | * This command takes about 1600 microseconds, so it would be faster to 267 | * instead call scrollDisplayLeft() or scrollDisplayRight() the appropriate 268 | * number of times and then call gotoXY(0, 0). */ 269 | void home(); 270 | 271 | /*! Puts the LCD into left-to-right mode: the cursor will shift to the right 272 | * after any character is written. This is the default behavior. */ 273 | void leftToRight(); 274 | 275 | /*! Puts the LCD into right-to-left mode: the cursor will shift to the left 276 | * after any character is written. */ 277 | void rightToLeft(); 278 | 279 | /*! Turns on auto-scrolling. 280 | * 281 | * When auto-scrolling is enabled, every time a character is written, the 282 | * screen will automatically scroll by one column in the appropriate 283 | * direction. */ 284 | void autoscroll(); 285 | 286 | /*! Turns off auto-scrolling. Auto-scrolling is off by default. */ 287 | void noAutoscroll(); 288 | 289 | //void initPrintf(); 290 | //void initPrintf(uint8_t lcdWidth, uint8_t lcdHeight); 291 | 292 | /*! Send an arbitrary command to the LCD. This is here for compatibility 293 | * with the LiquidCrystal library. */ 294 | void command(uint8_t cmd) 295 | { 296 | sendCommand(cmd); 297 | } 298 | 299 | /*! Writes a single character to the LCD. */ 300 | virtual size_t write(uint8_t c); 301 | 302 | /*! Writes multiple characters to the LCD. 303 | * 304 | * @param buffer Pointer to a string of characters in RAM, not 305 | * necessarily null-terminated. 306 | * @param size The number of characters to write to the LCD, excluding any 307 | * null termination character. */ 308 | virtual size_t write(const uint8_t * buffer, size_t size); 309 | 310 | // This allows us to easily call overrides of write that are 311 | // defined in Print. 312 | using Print::write; 313 | 314 | private: 315 | bool initialized; 316 | 317 | /* The lower three bits of this store the arguments to the 318 | * last "Display on/off control" HD44780 command that we sent. 319 | * bit 2: D: Whether the display is on. 320 | * bit 1: C: Whether the cursor is shown. 321 | * bit 0: B: Whether the cursor is blinking. */ 322 | uint8_t displayControl; 323 | 324 | /* The lower two bits of this variable store the arguments to the 325 | * last "Entry mode set" HD44780 command that we sent. 326 | * bit 1: I/D: 0 for moving the cursor to the left after data is written, 327 | * 1 for moving the cursor to the right. 328 | * bit 0: 1 for autoscrolling. */ 329 | uint8_t entryMode; 330 | 331 | void setEntryMode(uint8_t entryMode); 332 | void setDisplayControl(uint8_t displayControl); 333 | 334 | void init2(); 335 | }; 336 | 337 | /*! \brief Main class for interfacing with the HD44780 LCDs. 338 | * 339 | * This class is suitable for controlling an HD44780 LCD assuming that the LCD's 340 | * RS, E, DB4, DB5, DB6, and DB7 pins are each connected to a pin on the 341 | * microcontroller, each of those six microcontroller pins is supported by the 342 | * Arduino's `pinMode` and `digitalWrite` functions, and those pins are not 343 | * being used for any other purpose that conflicts with the LCD. 344 | * 345 | * This class sets the E pin to be an output driving low the first time you use 346 | * the LCD, and it assumes that no other code will change that pin. You cannot 347 | * use E for any other purposes because if the LCD sees a pulse on the E pin 348 | * then it might consider that to be a command or data, and the LCD state will 349 | * become corrupted. 350 | * 351 | * For the other pins (RS, DB4, DB5, and DB6), this library reconfigures them 352 | * each time they are used, so it is OK if you have other code that uses those 353 | * pins for other purposes. Before writing to the LCD, you just need to disable 354 | * any peripherals (such as UARTs) that override the output values of those 355 | * pins. 356 | * 357 | * If you cannot meet these conditions, you might be able to control your LCD 358 | * using a custom subclass of PololuHD44780Base. You can use this class as a 359 | * reference for how to do that. */ 360 | class PololuHD44780 : public PololuHD44780Base 361 | { 362 | public: 363 | /*! Creates a new instance of PololuHD44780. 364 | * 365 | * @param rs The pin number for the microcontroller pin that is 366 | * connected to the RS pin of the LCD. 367 | * @param e The pin number for the microcontroller pin that is 368 | * connected to the E pin of the LCD. 369 | * @param db4 The pin number for the microcontroller pin that is 370 | * connected to the DB4 pin of the LCD. 371 | * @param db5 The pin number for the microcontroller pin that is 372 | * connected to the DB5 pin of the LCD. 373 | * @param db6 The pin number for the microcontroller pin that is 374 | * connected to the DB6 pin of the LCD. 375 | * @param db7 The pin number for the microcontroller pin that is 376 | * connected to the DB7 pin of the LCD. 377 | */ 378 | PololuHD44780(uint8_t rs, uint8_t e, uint8_t db4, uint8_t db5, 379 | uint8_t db6, uint8_t db7) 380 | { 381 | this->rs = rs; 382 | this->e = e; 383 | this->db4 = db4; 384 | this->db5 = db5; 385 | this->db6 = db6; 386 | this->db7 = db7; 387 | } 388 | 389 | virtual void initPins() 390 | { 391 | digitalWrite(e, LOW); 392 | pinMode(e, OUTPUT); 393 | } 394 | 395 | virtual void send(uint8_t data, bool rsValue, bool only4bits) 396 | { 397 | digitalWrite(rs, rsValue); 398 | 399 | pinMode(rs, OUTPUT); 400 | pinMode(db4, OUTPUT); 401 | pinMode(db5, OUTPUT); 402 | pinMode(db6, OUTPUT); 403 | pinMode(db7, OUTPUT); 404 | 405 | if (!only4bits) { sendNibble(data >> 4); } 406 | sendNibble(data & 0x0F); 407 | } 408 | 409 | private: 410 | 411 | void sendNibble(uint8_t data) 412 | { 413 | digitalWrite(db4, data >> 0 & 1); 414 | digitalWrite(db5, data >> 1 & 1); 415 | digitalWrite(db6, data >> 2 & 1); 416 | digitalWrite(db7, data >> 3 & 1); 417 | 418 | digitalWrite(e, HIGH); 419 | _delay_us(1); // Must be at least 450 ns. 420 | digitalWrite(e, LOW); 421 | _delay_us(1); // Must be at least 550 ns. 422 | } 423 | 424 | uint8_t rs, e, db4, db5, db6, db7; 425 | }; 426 | -------------------------------------------------------------------------------- /examples/Demo/Demo.ino: -------------------------------------------------------------------------------- 1 | // This demo program shows many features of the Romi 32U4. 2 | // 3 | // It uses the buttons, LCD, and buzzer to provide a user 4 | // interface. It presents a menu to the user that lets the user 5 | // select from several different demos. 6 | // 7 | // To use this demo program, you will need to have the LCD 8 | // connected to the Romi 32U4. If you cannot see any text on the 9 | // LCD, try rotating the contrast potentiometer. 10 | // 11 | // To run this sketch, you will need to install the LSM6 library: 12 | // 13 | // https://github.com/pololu/lsm6-arduino 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | Romi32U4LCD lcd; 20 | Romi32U4Buzzer buzzer; 21 | Romi32U4ButtonA buttonA; 22 | Romi32U4ButtonB buttonB; 23 | Romi32U4ButtonC buttonC; 24 | LSM6 imu; 25 | Romi32U4Motors motors; 26 | Romi32U4Encoders encoders; 27 | 28 | char buttonMonitor(); 29 | 30 | // A couple of simple tunes, stored in program space. 31 | const char beepBrownout[] PROGMEM = "c#e>c#e MS afaf ML gc#gc# MS fdfd ML eee>ef>df>d b->c#b->c#a>df>d e>ee>ef>df>d" 48 | "e>d>c#>db>d>c#b >c#agaegfe fO6dc#dfdc# 8) { height = 8; } 163 | static const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, 255}; 164 | lcd.print(barChars[height]); 165 | } 166 | 167 | // The Menu class shows an interactive menu on the screen that 168 | // lets a user select an action and keeps track of the menu's 169 | // state. 170 | class Menu 171 | { 172 | public: 173 | struct Item 174 | { 175 | const char * name; 176 | void (* action)(); 177 | }; 178 | 179 | Menu(Item * items, uint8_t itemCount) 180 | { 181 | this->items = items; 182 | this->itemCount = itemCount; 183 | lcdItemIndex = 0; 184 | } 185 | 186 | void lcdUpdate(uint8_t index) 187 | { 188 | lcd.clear(); 189 | lcd.print(items[index].name); 190 | lcd.gotoXY(0, 1); 191 | 192 | // The string below uses special characters from the HD44780 193 | // interface datasheet. "\x7f" is a left arrow. 194 | // "\xa5" is a dot character. "\x7e" is a right arrow. 195 | lcd.print(F("\x7f" "A \xa5" "B C\x7e")); 196 | } 197 | 198 | void action(uint8_t index) 199 | { 200 | items[index].action(); 201 | } 202 | 203 | // Prompts the user to choose one of the menu items, 204 | // then runs it, then returns. 205 | void select() 206 | { 207 | lcdUpdate(lcdItemIndex); 208 | 209 | while (1) 210 | { 211 | switch (buttonMonitor()) 212 | { 213 | case 'A': 214 | // The A button was pressed so decrement the index. 215 | if (lcdItemIndex == 0) 216 | { 217 | lcdItemIndex = itemCount - 1; 218 | } 219 | else 220 | { 221 | lcdItemIndex--; 222 | } 223 | lcdUpdate(lcdItemIndex); 224 | break; 225 | 226 | case 'C': 227 | // The C button was pressed so increase the index. 228 | if (lcdItemIndex >= itemCount - 1) 229 | { 230 | lcdItemIndex = 0; 231 | } 232 | else 233 | { 234 | lcdItemIndex++; 235 | } 236 | lcdUpdate(lcdItemIndex); 237 | break; 238 | 239 | case 'B': 240 | // The B button was pressed so run the item and return. 241 | action(lcdItemIndex); 242 | return; 243 | } 244 | } 245 | } 246 | 247 | private: 248 | Item * items; 249 | uint8_t itemCount; 250 | uint8_t lcdItemIndex; 251 | }; 252 | 253 | 254 | // Blinks all three LEDs in sequence. 255 | void ledDemo() 256 | { 257 | displayBackArrow(); 258 | 259 | uint8_t state = 3; 260 | static uint16_t lastUpdateTime = millis() - 2000; 261 | while (buttonMonitor() != 'B') 262 | { 263 | if ((uint16_t)(millis() - lastUpdateTime) >= 500) 264 | { 265 | lastUpdateTime = millis(); 266 | state = state + 1; 267 | if (state >= 4) { state = 0; } 268 | 269 | switch (state) 270 | { 271 | case 0: 272 | buzzer.play("c32"); 273 | lcd.gotoXY(0, 0); 274 | lcd.print(F("Red ")); 275 | ledRed(1); 276 | ledGreen(0); 277 | ledYellow(0); 278 | break; 279 | 280 | case 1: 281 | buzzer.play("e32"); 282 | lcd.gotoXY(0, 0); 283 | lcd.print(F("Green")); 284 | ledRed(0); 285 | ledGreen(1); 286 | ledYellow(0); 287 | break; 288 | 289 | case 2: 290 | buzzer.play("g32"); 291 | lcd.gotoXY(0, 0); 292 | lcd.print(F("Yellow")); 293 | ledRed(0); 294 | ledGreen(0); 295 | ledYellow(1); 296 | break; 297 | } 298 | } 299 | } 300 | 301 | ledRed(0); 302 | ledYellow(0); 303 | ledGreen(0); 304 | } 305 | 306 | // Starts I2C and initializes the inertial sensors. 307 | void initInertialSensors() 308 | { 309 | Wire.begin(); 310 | imu.init(); 311 | imu.enableDefault(); 312 | 313 | // Set the gyro full scale to 1000 dps because the default 314 | // value is too low, and leave the other settings the same. 315 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 316 | 317 | // Set the accelerometer full scale to 16 g because the default 318 | // value is too low, and leave the other settings the same. 319 | imu.writeReg(LSM6::CTRL1_XL, 0b10000100); 320 | } 321 | 322 | // Given 3 readings for axes x, y, and z, prints the sign 323 | // and axis of the largest reading unless it is below the 324 | // given threshold. 325 | void printLargestAxis(int16_t x, int16_t y, int16_t z, uint16_t threshold) 326 | { 327 | int16_t largest = x; 328 | char axis = 'X'; 329 | 330 | if (abs(y) > abs(largest)) 331 | { 332 | largest = y; 333 | axis = 'Y'; 334 | } 335 | if (abs(z) > abs(largest)) 336 | { 337 | largest = z; 338 | axis = 'Z'; 339 | } 340 | 341 | if (abs(largest) < threshold) 342 | { 343 | lcd.print(" "); 344 | } 345 | else 346 | { 347 | bool positive = (largest > 0); 348 | lcd.print(positive ? '+' : '-'); 349 | lcd.print(axis); 350 | } 351 | } 352 | 353 | // Print the direction of the largest rotation rate measured 354 | // by the gyro and the up direction based on the 355 | // accelerometer's measurement of gravitational acceleration 356 | // (assuming gravity is the dominant force acting on the 357 | // Romi). 358 | void inertialDemo() 359 | { 360 | displayBackArrow(); 361 | 362 | lcd.gotoXY(3, 0); 363 | lcd.print(F("Rot")); 364 | lcd.gotoXY(4, 1); 365 | lcd.print(F("Up")); 366 | 367 | while (buttonMonitor() != 'B') 368 | { 369 | imu.read(); 370 | 371 | lcd.gotoXY(6, 0); 372 | printLargestAxis(imu.g.x, imu.g.y, imu.g.z, 2000); 373 | lcd.gotoXY(6, 1); 374 | printLargestAxis(imu.a.x, imu.a.y, imu.a.z, 200); 375 | } 376 | } 377 | 378 | // Provides an interface to test the motors. Holding button A or C 379 | // causes the left or right motor to accelerate; releasing the 380 | // button causes the motor to decelerate. Tapping the button while 381 | // the motor is not running reverses the direction it runs. 382 | // 383 | // If the showEncoders argument is true, encoder counts are 384 | // displayed on the first line of the LCD; otherwise, an 385 | // instructional message is shown. 386 | void motorDemoHelper(bool showEncoders) 387 | { 388 | loadCustomCharactersMotorDirs(); 389 | lcd.clear(); 390 | lcd.gotoXY(1, 1); 391 | lcd.print(F("A \7B C")); 392 | 393 | const uint16_t maxSpeed = 300; 394 | const uint8_t acceleration = 15; 395 | const uint8_t deceleration = 30; 396 | 397 | // Update the LCD and motors every 50 ms. 398 | const uint8_t updatePeriod = 50; 399 | 400 | int16_t leftSpeed = 0, rightSpeed = 0; 401 | int8_t leftDir = 1, rightDir = 1; 402 | uint16_t lastUpdateTime = millis() - 100; 403 | uint8_t btnCountA = 0, btnCountC = 0, instructCount = 0; 404 | 405 | int16_t encCountsLeft = 0, encCountsRight = 0; 406 | char buf[4]; 407 | 408 | while (buttonMonitor() != 'B') 409 | { 410 | encCountsLeft += encoders.getCountsAndResetLeft(); 411 | if (encCountsLeft < 0) { encCountsLeft += 1000; } 412 | if (encCountsLeft > 999) { encCountsLeft -= 1000; } 413 | 414 | encCountsRight += encoders.getCountsAndResetRight(); 415 | if (encCountsRight < 0) { encCountsRight += 1000; } 416 | if (encCountsRight > 999) { encCountsRight -= 1000; } 417 | 418 | if ((uint16_t)(millis() - lastUpdateTime) > updatePeriod) 419 | { 420 | lastUpdateTime = millis(); 421 | 422 | lcd.gotoXY(0, 0); 423 | if (showEncoders) 424 | { 425 | sprintf(buf, "%03d", encCountsLeft); 426 | lcd.print(buf); 427 | lcd.gotoXY(5, 0); 428 | sprintf(buf, "%03d", encCountsRight); 429 | lcd.print(buf); 430 | } 431 | else 432 | { 433 | // Cycle the instructions every 2 seconds. 434 | if (instructCount == 0) 435 | { 436 | lcd.print(F("Hold=run")); 437 | } 438 | else if (instructCount == 40) 439 | { 440 | lcd.print(F("Tap=flip")); 441 | } 442 | if (++instructCount == 80) { instructCount = 0; } 443 | } 444 | 445 | if (buttonA.isPressed()) 446 | { 447 | if (btnCountA < 4) 448 | { 449 | btnCountA++; 450 | } 451 | else 452 | { 453 | // Button has been held for more than 200 ms, so 454 | // start running the motor. 455 | leftSpeed += acceleration; 456 | } 457 | } 458 | else 459 | { 460 | if (leftSpeed == 0 && btnCountA > 0 && btnCountA < 4) 461 | { 462 | // Motor isn't running and button was pressed for 463 | // 200 ms or less, so flip the motor direction. 464 | leftDir = -leftDir; 465 | } 466 | btnCountA = 0; 467 | leftSpeed -= deceleration; 468 | } 469 | 470 | if (buttonC.isPressed()) 471 | { 472 | if (btnCountC < 4) 473 | { 474 | btnCountC++; 475 | } 476 | else 477 | { 478 | // Button has been held for more than 200 ms, so 479 | // start running the motor. 480 | rightSpeed += acceleration; 481 | } 482 | } 483 | else 484 | { 485 | if (rightSpeed == 0 && btnCountC > 0 && btnCountC < 4) 486 | { 487 | // Motor isn't running and button was pressed for 488 | // 200 ms or less, so flip the motor direction. 489 | rightDir = -rightDir; 490 | } 491 | btnCountC = 0; 492 | rightSpeed -= deceleration; 493 | } 494 | 495 | leftSpeed = constrain(leftSpeed, 0, maxSpeed); 496 | rightSpeed = constrain(rightSpeed, 0, maxSpeed); 497 | 498 | motors.setSpeeds(leftSpeed * leftDir, rightSpeed * rightDir); 499 | 500 | // Display arrows pointing the appropriate direction 501 | // (solid if the motor is running, chevrons if not). 502 | lcd.gotoXY(0, 1); 503 | if (leftSpeed == 0) 504 | { 505 | lcd.print((leftDir > 0) ? '\0' : '\1'); 506 | } 507 | else 508 | { 509 | lcd.print((leftDir > 0) ? '\2' : '\3'); 510 | } 511 | lcd.gotoXY(7, 1); 512 | if (rightSpeed == 0) 513 | { 514 | lcd.print((rightDir > 0) ? '\0' : '\1'); 515 | } 516 | else 517 | { 518 | lcd.print((rightDir > 0) ? '\2' : '\3'); 519 | } 520 | } 521 | } 522 | motors.setSpeeds(0, 0); 523 | } 524 | 525 | 526 | // Motor demo with instructions. 527 | void motorDemo() 528 | { 529 | motorDemoHelper(false); 530 | } 531 | 532 | // Motor demo with encoder counts. 533 | void encoderDemo() 534 | { 535 | motorDemoHelper(true); 536 | } 537 | 538 | // Play a song on the buzzer and display its title. 539 | void musicDemo() 540 | { 541 | displayBackArrow(); 542 | 543 | uint8_t fugueTitlePos = 0; 544 | uint16_t lastShiftTime = millis() - 2000; 545 | 546 | while (buttonMonitor() != 'B') 547 | { 548 | // Shift the song title to the left every 250 ms. 549 | if ((uint16_t)(millis() - lastShiftTime) > 250) 550 | { 551 | lastShiftTime = millis(); 552 | 553 | lcd.gotoXY(0, 0); 554 | for (uint8_t i = 0; i < 8; i++) 555 | { 556 | char c = pgm_read_byte(fugueTitle + fugueTitlePos + i); 557 | lcd.print(c); 558 | } 559 | fugueTitlePos++; 560 | 561 | if (fugueTitlePos + 8 >= strlen(fugueTitle)) 562 | { 563 | fugueTitlePos = 0; 564 | } 565 | } 566 | 567 | if (!buzzer.isPlaying()) 568 | { 569 | buzzer.playFromProgramSpace(fugue); 570 | } 571 | } 572 | } 573 | 574 | // Display the the battery (VIN) voltage and indicate whether USB 575 | // power is detected. 576 | void powerDemo() 577 | { 578 | displayBackArrow(); 579 | 580 | uint16_t lastDisplayTime = millis() - 2000; 581 | char buf[6]; 582 | 583 | while (buttonMonitor() != 'B') 584 | { 585 | if ((uint16_t)(millis() - lastDisplayTime) > 250) 586 | { 587 | bool usbPower = usbPowerPresent(); 588 | 589 | uint16_t batteryLevel = readBatteryMillivolts(); 590 | 591 | lastDisplayTime = millis(); 592 | lcd.gotoXY(0, 0); 593 | sprintf(buf, "%5d", batteryLevel); 594 | lcd.print(buf); 595 | lcd.print(F(" mV")); 596 | lcd.gotoXY(3, 1); 597 | lcd.print(F("USB=")); 598 | lcd.print(usbPower ? 'Y' : 'N'); 599 | } 600 | } 601 | } 602 | 603 | Menu::Item mainMenuItems[] = { 604 | { "LEDs", ledDemo }, 605 | { "Inertial", inertialDemo }, 606 | { "Motors", motorDemo }, 607 | { "Encoders", encoderDemo }, 608 | { "Music", musicDemo }, 609 | { "Power", powerDemo }, 610 | }; 611 | Menu mainMenu(mainMenuItems, 6); 612 | 613 | // This function watches for button presses. If a button is 614 | // pressed, it beeps a corresponding beep and it returns 'A', 615 | // 'B', or 'C' depending on what button was pressed. If no 616 | // button was pressed, it returns 0. This function is meant to 617 | // be called repeatedly in a loop. 618 | char buttonMonitor() 619 | { 620 | if (buttonA.getSingleDebouncedPress()) 621 | { 622 | buzzer.playFromProgramSpace(beepButtonA); 623 | return 'A'; 624 | } 625 | 626 | if (buttonB.getSingleDebouncedPress()) 627 | { 628 | buzzer.playFromProgramSpace(beepButtonB); 629 | return 'B'; 630 | } 631 | 632 | if (buttonC.getSingleDebouncedPress()) 633 | { 634 | buzzer.playFromProgramSpace(beepButtonC); 635 | return 'C'; 636 | } 637 | 638 | return 0; 639 | } 640 | 641 | void setup() 642 | { 643 | initInertialSensors(); 644 | 645 | loadCustomCharacters(); 646 | 647 | bool brownout = MCUSR >> BORF & 1; 648 | MCUSR = 0; 649 | if (brownout) 650 | { 651 | // The AVR was reset by a brownout reset 652 | // (VCC dropped below 4.3 V). 653 | // Play a special sound and display a note to the user. 654 | 655 | buzzer.playFromProgramSpace(beepBrownout); 656 | lcd.clear(); 657 | lcd.print(F("Brownout")); 658 | lcd.gotoXY(0, 1); 659 | lcd.print(F(" reset! ")); 660 | delay(1000); 661 | } 662 | else 663 | { 664 | buzzer.playFromProgramSpace(beepWelcome); 665 | } 666 | 667 | ledYellow(1); 668 | 669 | lcd.clear(); 670 | lcd.print(F(" Romi")); 671 | lcd.gotoXY(2, 1); 672 | lcd.print(F("32U4")); 673 | 674 | delay(1000); 675 | 676 | lcd.clear(); 677 | lcd.print(F("Demo")); 678 | lcd.gotoXY(0, 1); 679 | lcd.print(F("Program")); 680 | delay(1000); 681 | 682 | lcd.clear(); 683 | lcd.print(F("Use B to")); 684 | lcd.gotoXY(0, 1); 685 | lcd.print(F("select.")); 686 | delay(1000); 687 | 688 | lcd.clear(); 689 | lcd.print(F("Press B")); 690 | lcd.gotoXY(0, 1); 691 | lcd.print(F("-try it!")); 692 | 693 | while (buttonMonitor() != 'B'){} 694 | 695 | ledYellow(0); 696 | 697 | buzzer.playFromProgramSpace(beepThankYou); 698 | lcd.clear(); 699 | lcd.print(F(" Thank")); 700 | lcd.gotoXY(0, 1); 701 | lcd.print(F(" you!")); 702 | delay(1000); 703 | } 704 | 705 | // This function prompts the user to choose something from the 706 | // main menu, runs their selection, and then returns. 707 | void mainMenuSelect() 708 | { 709 | lcd.clear(); 710 | lcd.print(F(" Main")); 711 | lcd.gotoXY(0, 1); 712 | lcd.print(F(" Menu")); 713 | delay(1000); 714 | mainMenu.select(); 715 | } 716 | 717 | void loop() 718 | { 719 | mainMenuSelect(); 720 | } 721 | -------------------------------------------------------------------------------- /PololuBuzzer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | #include "PololuBuzzer.h" 5 | 6 | #ifdef __AVR_ATmega32U4__ 7 | 8 | // PD7 (OC4D) 9 | #define BUZZER_DDR DDRD 10 | #define BUZZER (1 << PORTD7) 11 | 12 | #define TIMER4_CLK_8 0x4 // 2 MHz 13 | 14 | #define ENABLE_TIMER_INTERRUPT() TIMSK4 = (1 << TOIE4) 15 | #define DISABLE_TIMER_INTERRUPT() TIMSK4 = 0 16 | 17 | #else // 168P or 328P 18 | 19 | // PD3 (OC2B) 20 | #define BUZZER_DDR DDRD 21 | #define BUZZER (1 << PORTD3) 22 | 23 | #define TIMER2_CLK_32 0x3 // 500 kHz 24 | 25 | static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024}; 26 | 27 | #define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2) 28 | #define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0 29 | 30 | #endif 31 | 32 | unsigned char buzzerInitialized = 0; 33 | volatile unsigned char buzzerFinished = 1; // flag: 0 while playing 34 | const char * volatile buzzerSequence = 0; 35 | 36 | // declaring these globals as static means they won't conflict 37 | // with globals in other .cpp files that share the same name 38 | static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit 39 | static volatile char play_mode_setting = PLAY_AUTOMATIC; 40 | 41 | extern volatile unsigned char buzzerFinished; // flag: 0 while playing 42 | extern const char * volatile buzzerSequence; 43 | 44 | 45 | static volatile unsigned char use_program_space; // boolean: true if we should 46 | // use program space 47 | 48 | // music settings and defaults 49 | static volatile unsigned char octave = 4; // the current octave 50 | static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note 51 | static volatile unsigned int note_type = 4; // 4 for quarter, etc 52 | static volatile unsigned int duration = 500; // the duration of a note in ms 53 | static volatile unsigned int volume = 15; // the note volume 54 | static volatile unsigned char staccato = 0; // true if playing staccato 55 | 56 | // staccato handling 57 | static volatile unsigned char staccato_rest_duration; // duration of a staccato rest, 58 | // or zero if it is time to play a note 59 | 60 | static void nextNote(); 61 | 62 | #ifdef __AVR_ATmega32U4__ 63 | 64 | // Timer4 overflow interrupt 65 | ISR (TIMER4_OVF_vect) 66 | { 67 | if (buzzerTimeout-- == 0) 68 | { 69 | DISABLE_TIMER_INTERRUPT(); 70 | sei(); // re-enable global interrupts (nextNote() is very slow) 71 | TCCR4B = (TCCR4B & 0xF0) | TIMER4_CLK_8; // select IO clock 72 | unsigned int top = (F_CPU/16) / 1000; // set TOP for freq = 1 kHz: 73 | TC4H = top >> 8; // top 2 bits... (TC4H temporarily stores top 2 bits of 10-bit accesses) 74 | OCR4C = top; // and bottom 8 bits 75 | TC4H = 0; // 0% duty cycle: top 2 bits... 76 | OCR4D = 0; // and bottom 8 bits 77 | buzzerFinished = 1; 78 | if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC)) 79 | nextNote(); 80 | } 81 | } 82 | 83 | #else 84 | 85 | // Timer2 overflow interrupt 86 | ISR (TIMER2_OVF_vect) 87 | { 88 | if (buzzerTimeout-- == 0) 89 | { 90 | DISABLE_TIMER_INTERRUPT(); 91 | sei(); // re-enable global interrupts (nextNote() is very slow) 92 | TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock 93 | OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz 94 | OCR2B = 0; // 0% duty cycle 95 | buzzerFinished = 1; 96 | if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC)) 97 | nextNote(); 98 | } 99 | } 100 | 101 | #endif 102 | 103 | // this is called by playFrequency() 104 | inline void PololuBuzzer::init() 105 | { 106 | if (!buzzerInitialized) 107 | { 108 | buzzerInitialized = 1; 109 | init2(); 110 | } 111 | } 112 | 113 | // initializes timer4 (32U4) or timer2 (328P) for buzzer control 114 | void PololuBuzzer::init2() 115 | { 116 | DISABLE_TIMER_INTERRUPT(); 117 | 118 | #ifdef __AVR_ATmega32U4__ 119 | TCCR4A = 0x00; // bits 7 and 6 clear: normal port op., OC4A disconnected 120 | // bits 5 and 4 clear: normal port op., OC4B disconnected 121 | // bit 3 clear: no force output compare for channel A 122 | // bit 2 clear: no force output compare for channel B 123 | // bit 1 clear: disable PWM for channel A 124 | // bit 0 clear: disable PWM for channel B 125 | 126 | TCCR4B = 0x04; // bit 7 clear: disable PWM inversion 127 | // bit 6 clear: no prescaler reset 128 | // bits 5 and 4 clear: dead time prescaler 1 129 | // bit 3 clear, 2 set, 1-0 clear: timer clock = CK/8 130 | 131 | TCCR4C = 0x09; // bits 7 and 6 clear: normal port op., OC4A disconnected 132 | // bits 5 and 4 clear: normal port op., OC4B disconnected 133 | // bit 3 set, 2 clear: clear OC4D on comp match when upcounting, 134 | // set OC4D on comp match when downcounting 135 | // bit 1 clear: no force output compare for channel D 136 | // bit 0 set: enable PWM for channel 4 137 | 138 | TCCR4D = 0x01; // bit 7 clear: disable fault protection interrupt 139 | // bit 6 clear: disable fault protection mode 140 | // bit 5 clear: disable fault protection noise canceler 141 | // bit 4 clear: falling edge triggers fault 142 | // bit 3 clear: disable fault protection analog comparator 143 | // bit 2 clear: fault protection interrupt flag 144 | // bit 1 clear, 0 set: select waveform generation mode, 145 | // phase- and frequency-correct PWM, TOP = OCR4C, 146 | // OCR4D set at BOTTOM, TOV4 flag set at BOTTOM 147 | 148 | // This sets timer 4 to run in phase- and frequency-correct PWM mode, 149 | // where TOP = OCR4C, OCR4D is updated at BOTTOM, TOV1 Flag is set on BOTTOM. 150 | // OC4D is cleared on compare match when upcounting, set on compare 151 | // match when downcounting; OC4A and OC4B are disconnected. 152 | 153 | unsigned int top = (F_CPU/16) / 1000; // set TOP for freq = 1 kHz: 154 | TC4H = top >> 8; // top 2 bits... 155 | OCR4C = top; // and bottom 8 bits 156 | TC4H = 0; // 0% duty cycle: top 2 bits... 157 | OCR4D = 0; // and bottom 8 bits 158 | #else 159 | TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected 160 | // bit 5 set, 4 clear: clear OC2B on comp match when upcounting, 161 | // set OC2B on comp match when downcounting 162 | // bits 3 and 2: not used 163 | // bit 1 clear, 0 set: combine with bit 3 of TCCR2B... 164 | 165 | TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A 166 | // bit 6 clear: no force output compare for channel B 167 | // bits 5 and 4: not used 168 | // bit 3 set: combine with bits 1 and 0 of TCCR2A to 169 | // select waveform generation mode 5, phase-correct PWM, 170 | // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM 171 | // bit 2 clear, 1-0 set: timer clock = clkT2S/32 172 | 173 | // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A, 174 | // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared 175 | // on compare match when upcounting, set on compare match when downcounting; 176 | // OC2A is disconnected. 177 | // Note: if the PWM frequency and duty cycle are changed, the first 178 | // cycle of the new frequency will be at the old duty cycle, since 179 | // the duty cycle (OCR2B) is not updated until TOP. 180 | 181 | 182 | OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz 183 | OCR2B = 0; // 0% duty cycle 184 | #endif 185 | 186 | BUZZER_DDR |= BUZZER; // buzzer pin set as an output 187 | sei(); 188 | } 189 | 190 | 191 | // Set up the timer to play the desired frequency (in Hz or .1 Hz) for the 192 | // the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz. 193 | // volume controls buzzer volume, with 15 being loudest and 0 being quietest. 194 | // Note: frequency*duration/1000 must be less than 0xFFFF (65535). This 195 | // means that you can't use a max duration of 65535 ms for frequencies 196 | // greater than 1 kHz. For example, the max duration you can use for a 197 | // frequency of 10 kHz is 6553 ms. If you use a duration longer than this, 198 | // you will cause an integer overflow that produces unexpected behavior. 199 | void PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur, 200 | unsigned char volume) 201 | { 202 | init(); // initializes the buzzer if necessary 203 | buzzerFinished = 0; 204 | 205 | unsigned int timeout; 206 | unsigned char multiplier = 1; 207 | 208 | if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set 209 | { // then the true frequency is freq/10 210 | multiplier = 10; // (gives higher resolution for small freqs) 211 | freq &= ~DIV_BY_10; // clear DIV_BY_10 bit 212 | } 213 | 214 | unsigned char min = 40 * multiplier; 215 | if (freq < min) // min frequency allowed is 40 Hz 216 | freq = min; 217 | if (multiplier == 1 && freq > 10000) 218 | freq = 10000; // max frequency allowed is 10kHz 219 | 220 | #ifdef __AVR_ATmega32U4__ 221 | unsigned long top; 222 | unsigned char dividerExponent = 0; 223 | 224 | // calculate necessary clock source and counter top value to get freq 225 | top = (unsigned int)(((F_CPU/2 * multiplier) + (freq >> 1))/ freq); 226 | 227 | while (top > 1023) 228 | { 229 | dividerExponent++; 230 | top = (unsigned int)((((F_CPU/2 >> (dividerExponent)) * multiplier) + (freq >> 1))/ freq); 231 | } 232 | #else 233 | unsigned int top; 234 | unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz) 235 | unsigned int divider = cs2_divider[newCS2]; 236 | 237 | // calculate necessary clock source and counter top value to get freq 238 | top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq); 239 | 240 | while (top > 255) 241 | { 242 | divider = cs2_divider[++newCS2]; 243 | top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq); 244 | } 245 | #endif 246 | 247 | // set timeout (duration): 248 | if (multiplier == 10) 249 | freq = (freq + 5) / 10; 250 | 251 | if (freq == 1000) 252 | timeout = dur; // duration for silent notes is exact 253 | else 254 | timeout = (unsigned int)((long)dur * freq / 1000); 255 | 256 | if (volume > 15) 257 | volume = 15; 258 | 259 | DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers 260 | 261 | #ifdef __AVR_ATmega32U4__ 262 | TCCR4B = (TCCR4B & 0xF0) | (dividerExponent + 1); // select timer 4 clock prescaler: divider = 2^n if CS4 = n+1 263 | TC4H = top >> 8; // set timer 4 pwm frequency: top 2 bits... 264 | OCR4C = top; // and bottom 8 bits 265 | unsigned int width = top >> (16 - volume); // set duty cycle (volume): 266 | TC4H = width >> 8; // top 2 bits... 267 | OCR4D = width; // and bottom 8 bits 268 | buzzerTimeout = timeout; // set buzzer duration 269 | 270 | TIFR4 |= 0xFF; // clear any pending t4 overflow int. 271 | #else 272 | TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler 273 | OCR2A = top; // set timer 2 pwm frequency 274 | OCR2B = top >> (16 - volume); // set duty cycle (volume) 275 | buzzerTimeout = timeout; // set buzzer duration 276 | 277 | TIFR2 |= 0xFF; // clear any pending t2 overflow int. 278 | #endif 279 | 280 | ENABLE_TIMER_INTERRUPT(); 281 | } 282 | 283 | 284 | 285 | // Determine the frequency for the specified note, then play that note 286 | // for the desired duration (in ms). This is done without using floats 287 | // and without having to loop. volume controls buzzer volume, with 15 being 288 | // loudest and 0 being quietest. 289 | // Note: frequency*duration/1000 must be less than 0xFFFF (65535). This 290 | // means that you can't use a max duration of 65535 ms for frequencies 291 | // greater than 1 kHz. For example, the max duration you can use for a 292 | // frequency of 10 kHz is 6553 ms. If you use a duration longer than this, 293 | // you will cause an integer overflow that produces unexpected behavior. 294 | void PololuBuzzer::playNote(unsigned char note, unsigned int dur, 295 | unsigned char volume) 296 | { 297 | // note = key + octave * 12, where 0 <= key < 12 298 | // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57) 299 | // A note is converted to a frequency by the formula: 300 | // Freq(n) = Freq(0) * a^n 301 | // where 302 | // Freq(0) is chosen as A4, which is 440 Hz 303 | // and 304 | // a = 2 ^ (1/12) 305 | // n is the number of notes you are away from A4. 306 | // One can see that the frequency will double every 12 notes. 307 | // This function exploits this property by defining the frequencies of the 308 | // 12 lowest notes allowed and then doubling the appropriate frequency 309 | // the appropriate number of times to get the frequency for the specified 310 | // note. 311 | 312 | // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz) 313 | // if note = 57, freq = 440 Hz (A4 - central value of ET Scale) 314 | // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz) 315 | // if note = 255, freq = 1 kHz and buzzer is silent (silent note) 316 | 317 | // The most significant bit of freq is the "divide by 10" bit. If set, 318 | // the units for frequency are .1 Hz, not Hz, and freq must be divided 319 | // by 10 to get the true frequency in Hz. This allows for an extra digit 320 | // of resolution for low frequencies without the need for using floats. 321 | 322 | unsigned int freq = 0; 323 | unsigned char offset_note = note - 16; 324 | 325 | if (note == SILENT_NOTE || volume == 0) 326 | { 327 | freq = 1000; // silent notes => use 1kHz freq (for cycle counter) 328 | playFrequency(freq, dur, 0); 329 | return; 330 | } 331 | 332 | if (note <= 16) 333 | offset_note = 0; 334 | else if (offset_note > 95) 335 | offset_note = 95; 336 | 337 | unsigned char exponent = offset_note / 12; 338 | 339 | // frequency table for the lowest 12 allowed notes 340 | // frequencies are specified in tenths of a Hertz for added resolution 341 | switch (offset_note - exponent * 12) // equivalent to (offset_note % 12) 342 | { 343 | case 0: // note E1 = 41.2 Hz 344 | freq = 412; 345 | break; 346 | case 1: // note F1 = 43.7 Hz 347 | freq = 437; 348 | break; 349 | case 2: // note F#1 = 46.3 Hz 350 | freq = 463; 351 | break; 352 | case 3: // note G1 = 49.0 Hz 353 | freq = 490; 354 | break; 355 | case 4: // note G#1 = 51.9 Hz 356 | freq = 519; 357 | break; 358 | case 5: // note A1 = 55.0 Hz 359 | freq = 550; 360 | break; 361 | case 6: // note A#1 = 58.3 Hz 362 | freq = 583; 363 | break; 364 | case 7: // note B1 = 61.7 Hz 365 | freq = 617; 366 | break; 367 | case 8: // note C2 = 65.4 Hz 368 | freq = 654; 369 | break; 370 | case 9: // note C#2 = 69.3 Hz 371 | freq = 693; 372 | break; 373 | case 10: // note D2 = 73.4 Hz 374 | freq = 734; 375 | break; 376 | case 11: // note D#2 = 77.8 Hz 377 | freq = 778; 378 | break; 379 | } 380 | 381 | if (exponent < 7) 382 | { 383 | freq = freq << exponent; // frequency *= 2 ^ exponent 384 | if (exponent > 1) // if the frequency is greater than 160 Hz 385 | freq = (freq + 5) / 10; // we don't need the extra resolution 386 | else 387 | freq += DIV_BY_10; // else keep the added digit of resolution 388 | } 389 | else 390 | freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow 391 | 392 | if (volume > 15) 393 | volume = 15; 394 | playFrequency(freq, dur, volume); // set buzzer this freq/duration 395 | } 396 | 397 | 398 | 399 | // Returns 1 if the buzzer is currently playing, otherwise it returns 0 400 | unsigned char PololuBuzzer::isPlaying() 401 | { 402 | return !buzzerFinished || buzzerSequence != 0; 403 | } 404 | 405 | 406 | // Plays the specified sequence of notes. If the play mode is 407 | // PLAY_AUTOMATIC, the sequence of notes will play with no further 408 | // action required by the user. If the play mode is PLAY_CHECK, 409 | // the user will need to call playCheck() in the main loop to initiate 410 | // the playing of each new note in the sequence. The play mode can 411 | // be changed while the sequence is playing. 412 | // This is modeled after the PLAY commands in GW-BASIC, with just a 413 | // few differences. 414 | // 415 | // The notes are specified by the characters C, D, E, F, G, A, and 416 | // B, and they are played by default as "quarter notes" with a 417 | // length of 500 ms. This corresponds to a tempo of 120 418 | // beats/min. Other durations can be specified by putting a number 419 | // immediately after the note. For example, C8 specifies C played as an 420 | // eighth note, with half the duration of a quarter note. The special 421 | // note R plays a rest (no sound). 422 | // 423 | // Various control characters alter the sound: 424 | // '>' plays the next note one octave higher 425 | // 426 | // '<' plays the next note one octave lower 427 | // 428 | // '+' or '#' after a note raises any note one half-step 429 | // 430 | // '-' after a note lowers any note one half-step 431 | // 432 | // '.' after a note "dots" it, increasing the length by 433 | // 50%. Each additional dot adds half as much as the 434 | // previous dot, so that "A.." is 1.75 times the length of 435 | // "A". 436 | // 437 | // 'O' followed by a number sets the octave (default: O4). 438 | // 439 | // 'T' followed by a number sets the tempo (default: T120). 440 | // 441 | // 'L' followed by a number sets the default note duration to 442 | // the type specified by the number: 4 for quarter notes, 8 443 | // for eighth notes, 16 for sixteenth notes, etc. 444 | // (default: L4) 445 | // 446 | // 'V' followed by a number from 0-15 sets the music volume. 447 | // (default: V15) 448 | // 449 | // 'MS' sets all subsequent notes to play staccato - each note is played 450 | // for 1/2 of its allotted time, followed by an equal period 451 | // of silence. 452 | // 453 | // 'ML' sets all subsequent notes to play legato - each note is played 454 | // for its full length. This is the default setting. 455 | // 456 | // '!' resets all persistent settings to their defaults. 457 | // 458 | // The following plays a c major scale up and back down: 459 | // play("L16 V8 cdefgab>cbagfedc"); 460 | // 461 | // Here is an example from Bach: 462 | // play("T240 L8 a gafaeada c+adaeafa > 8; // top 2 bits... (TC4H temporarily stores top 2 bits of 10-bit accesses) 491 | OCR4C = top; // and bottom 8 bits 492 | TC4H = 0; // 0% duty cycle: top 2 bits... 493 | OCR4D = 0; // and bottom 8 bits 494 | #else 495 | TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock 496 | OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz 497 | OCR2B = 0; // 0% duty cycle 498 | #endif 499 | 500 | buzzerFinished = 1; 501 | buzzerSequence = 0; 502 | } 503 | 504 | // Gets the current character, converting to lower-case and skipping 505 | // spaces. For any spaces, this automatically increments sequence! 506 | static char currentCharacter() 507 | { 508 | char c = 0; 509 | do 510 | { 511 | if(use_program_space) 512 | c = pgm_read_byte(buzzerSequence); 513 | else 514 | c = *buzzerSequence; 515 | 516 | if(c >= 'A' && c <= 'Z') 517 | c += 'a'-'A'; 518 | } while(c == ' ' && (buzzerSequence ++)); 519 | 520 | return c; 521 | } 522 | 523 | // Returns the numerical argument specified at buzzerSequence[0] and 524 | // increments sequence to point to the character immediately after the 525 | // argument. 526 | static unsigned int getNumber() 527 | { 528 | unsigned int arg = 0; 529 | 530 | // read all digits, one at a time 531 | char c = currentCharacter(); 532 | while(c >= '0' && c <= '9') 533 | { 534 | arg *= 10; 535 | arg += c-'0'; 536 | buzzerSequence ++; 537 | c = currentCharacter(); 538 | } 539 | 540 | return arg; 541 | } 542 | 543 | static void nextNote() 544 | { 545 | unsigned char note = 0; 546 | unsigned char rest = 0; 547 | unsigned char tmp_octave = octave; // the octave for this note 548 | unsigned int tmp_duration; // the duration of this note 549 | unsigned int dot_add; 550 | 551 | char c; // temporary variable 552 | 553 | // if we are playing staccato, after every note we play a rest 554 | if(staccato && staccato_rest_duration) 555 | { 556 | PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0); 557 | staccato_rest_duration = 0; 558 | return; 559 | } 560 | 561 | parse_character: 562 | 563 | // Get current character 564 | c = currentCharacter(); 565 | buzzerSequence ++; 566 | 567 | // Interpret the character. 568 | switch(c) 569 | { 570 | case '>': 571 | // shift the octave temporarily up 572 | tmp_octave ++; 573 | goto parse_character; 574 | case '<': 575 | // shift the octave temporarily down 576 | tmp_octave --; 577 | goto parse_character; 578 | case 'a': 579 | note = NOTE_A(0); 580 | break; 581 | case 'b': 582 | note = NOTE_B(0); 583 | break; 584 | case 'c': 585 | note = NOTE_C(0); 586 | break; 587 | case 'd': 588 | note = NOTE_D(0); 589 | break; 590 | case 'e': 591 | note = NOTE_E(0); 592 | break; 593 | case 'f': 594 | note = NOTE_F(0); 595 | break; 596 | case 'g': 597 | note = NOTE_G(0); 598 | break; 599 | case 'l': 600 | // set the default note duration 601 | note_type = getNumber(); 602 | duration = whole_note_duration/note_type; 603 | goto parse_character; 604 | case 'm': 605 | // set music staccato or legato 606 | if(currentCharacter() == 'l') 607 | staccato = false; 608 | else 609 | { 610 | staccato = true; 611 | staccato_rest_duration = 0; 612 | } 613 | buzzerSequence ++; 614 | goto parse_character; 615 | case 'o': 616 | // set the octave permanently 617 | octave = tmp_octave = getNumber(); 618 | goto parse_character; 619 | case 'r': 620 | // Rest - the note value doesn't matter. 621 | rest = 1; 622 | break; 623 | case 't': 624 | // set the tempo 625 | whole_note_duration = 60*400/getNumber()*10; 626 | duration = whole_note_duration/note_type; 627 | goto parse_character; 628 | case 'v': 629 | // set the volume 630 | volume = getNumber(); 631 | goto parse_character; 632 | case '!': 633 | // reset to defaults 634 | octave = 4; 635 | whole_note_duration = 2000; 636 | note_type = 4; 637 | duration = 500; 638 | volume = 15; 639 | staccato = 0; 640 | // reset temp variables that depend on the defaults 641 | tmp_octave = octave; 642 | tmp_duration = duration; 643 | goto parse_character; 644 | default: 645 | buzzerSequence = 0; 646 | return; 647 | } 648 | 649 | note += tmp_octave*12; 650 | 651 | // handle sharps and flats 652 | c = currentCharacter(); 653 | while(c == '+' || c == '#') 654 | { 655 | buzzerSequence ++; 656 | note ++; 657 | c = currentCharacter(); 658 | } 659 | while(c == '-') 660 | { 661 | buzzerSequence ++; 662 | note --; 663 | c = currentCharacter(); 664 | } 665 | 666 | // set the duration of just this note 667 | tmp_duration = duration; 668 | 669 | // If the input is 'c16', make it a 16th note, etc. 670 | if(c > '0' && c < '9') 671 | tmp_duration = whole_note_duration/getNumber(); 672 | 673 | // Handle dotted notes - the first dot adds 50%, and each 674 | // additional dot adds 50% of the previous dot. 675 | dot_add = tmp_duration/2; 676 | while(currentCharacter() == '.') 677 | { 678 | buzzerSequence ++; 679 | tmp_duration += dot_add; 680 | dot_add /= 2; 681 | } 682 | 683 | if(staccato) 684 | { 685 | staccato_rest_duration = tmp_duration / 2; 686 | tmp_duration -= staccato_rest_duration; 687 | } 688 | 689 | // this will re-enable the timer overflow interrupt 690 | PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume); 691 | } 692 | 693 | 694 | // This puts play() into a mode where instead of advancing to the 695 | // next note in the sequence automatically, it waits until the 696 | // function playCheck() is called. The idea is that you can 697 | // put playCheck() in your main loop and avoid potential 698 | // delays due to the note sequence being checked in the middle of 699 | // a time sensitive calculation. It is recommended that you use 700 | // this function if you are doing anything that can't tolerate 701 | // being interrupted for more than a few microseconds. 702 | // Note that the play mode can be changed while a sequence is being 703 | // played. 704 | // 705 | // Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the 706 | // default), playMode(PLAY_CHECK) sets it to a mode where you have 707 | // to call playCheck(). 708 | void PololuBuzzer::playMode(unsigned char mode) 709 | { 710 | play_mode_setting = mode; 711 | 712 | // We want to check to make sure that we didn't miss a note if we 713 | // are going out of play-check mode. 714 | if(mode == PLAY_AUTOMATIC) 715 | playCheck(); 716 | } 717 | 718 | 719 | // Checks whether it is time to start another note, and starts 720 | // it if so. If it is not yet time to start the next note, this method 721 | // returns without doing anything. Call this as often as possible 722 | // in your main loop to avoid delays between notes in the sequence. 723 | // 724 | // Returns true if it is still playing. 725 | unsigned char PololuBuzzer::playCheck() 726 | { 727 | if(buzzerFinished && buzzerSequence != 0) 728 | nextNote(); 729 | return buzzerSequence != 0; 730 | } 731 | --------------------------------------------------------------------------------