├── .arduino-ci.yaml ├── .dir-locals.el ├── .github ├── ISSUE_TEMPLATE │ ├── bug-report-or-feature-request.md │ └── config.yml └── workflows │ └── ci.yaml ├── .gitignore ├── .gitlab-ci.yml ├── Balboa32U4.h ├── Balboa32U4Buttons.h ├── Balboa32U4Buzzer.h ├── Balboa32U4Encoders.cpp ├── Balboa32U4Encoders.h ├── Balboa32U4LCD.h ├── Balboa32U4LineSensors.h ├── Balboa32U4Motors.cpp ├── Balboa32U4Motors.h ├── Doxyfile ├── FastGPIO.h ├── LICENSE.txt ├── PololuBuzzer.cpp ├── PololuBuzzer.h ├── PololuHD44780.cpp ├── PololuHD44780.h ├── Pushbutton.cpp ├── Pushbutton.h ├── QTRSensors.cpp ├── QTRSensors.h ├── README.md ├── USBPause.h ├── components.txt ├── examples ├── Balancer │ ├── Balance.cpp │ ├── Balance.h │ └── Balancer.ino ├── BlinkLEDs │ └── BlinkLEDs.ino ├── Buttons │ └── Buttons.ino ├── BuzzerBasics │ └── BuzzerBasics.ino ├── Demo │ └── Demo.ino ├── Encoders │ └── Encoders.ino ├── InertialSensors │ └── InertialSensors.ino ├── LCDBasics │ └── LCDBasics.ino ├── LineSensorTest │ └── LineSensorTest.ino ├── MotorTest │ └── MotorTest.ino └── PowerDetect │ └── PowerDetect.ino ├── keywords.txt ├── library.properties ├── local_keywords.txt └── update_components.sh /.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 | - LIS3MDL=https://github.com/pololu/lis3mdl-arduino/archive/1.0.0.tar.gz=1r0m2967gdsx23p65zbhfl4khrswyrc81hjiqdh278csar7zcwga 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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /docs 2 | /out/ 3 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /Balboa32U4.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Balboa32U4.h 4 | * 5 | * \brief Main header file for the Balboa32U4 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 | #include 23 | 24 | // TODO: servo support 25 | 26 | /*! \brief Turns the red user LED (RX) on or off. 27 | 28 | @param on 1 to turn on the LED, 0 to turn it off. 29 | 30 | The red user LED is on pin 17, which is also known as PB0, SS, and RXLED. The 31 | Arduino core code uses this LED to indicate when it receives data over USB, so 32 | it might be hard to control this LED when USB is connected. */ 33 | inline void ledRed(bool on) 34 | { 35 | FastGPIO::Pin<17>::setOutput(!on); 36 | } 37 | 38 | /*! \brief Turns the yellow user LED on pin 13 on or off. 39 | 40 | @param on 1 to turn on the LED, 0 to turn it off. */ 41 | inline void ledYellow(bool on) 42 | { 43 | FastGPIO::Pin<13>::setOutput(on); 44 | } 45 | 46 | /*! \brief Turns the green user LED (TX) on or off. 47 | 48 | @param on 1 to turn on the LED, 0 to turn it off. 49 | 50 | The green user LED is pin PD5, which is also known as TXLED. The Arduino core 51 | code uses this LED to indicate when it receives data over USB, so it might be 52 | hard to control this LED when USB is connected. */ 53 | inline void ledGreen(bool on) 54 | { 55 | FastGPIO::Pin::setOutput(!on); 56 | } 57 | 58 | /*! \brief Returns true if USB power is detected. 59 | 60 | This function returns true if power is detected on the board's USB port and 61 | returns false otherwise. It uses the ATmega32U4's VBUS line, which is directly 62 | connected to the power pin of the USB connector. 63 | 64 | \sa A method for detecting whether the board's virtual COM port is open: 65 | http://arduino.cc/en/Serial/IfSerial */ 66 | inline bool usbPowerPresent() 67 | { 68 | return USBSTA >> VBUS & 1; 69 | } 70 | 71 | /*! \brief Reads the battery voltage and returns it in millivolts. 72 | 73 | If this function returns a number below 5500, the actual battery voltage might 74 | be significantly lower than the value returned. */ 75 | inline uint16_t readBatteryMillivolts() 76 | { 77 | const uint8_t sampleCount = 8; 78 | uint16_t sum = 0; 79 | for (uint8_t i = 0; i < sampleCount; i++) 80 | { 81 | sum += analogRead(A1); 82 | } 83 | 84 | // VBAT = 3 * millivolt reading = 3 * raw * 5000/1024 85 | // = raw * 1875 / 128 86 | // The correction term below makes it so that we round to the 87 | // nearest whole number instead of always rounding down. 88 | const uint32_t correction = 64 * sampleCount - 1; 89 | return ((uint32_t)sum * 1875 + correction) / (128 * sampleCount); 90 | } 91 | -------------------------------------------------------------------------------- /Balboa32U4Buttons.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /** \file Balboa32U4Buttons.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 Balboa 32U4. */ 13 | #define BALBOA_32U4_BUTTON_A 14 14 | 15 | /*! The pin number for the pin connected to button B on the Balboa 32U4. */ 16 | #define BALBOA_32U4_BUTTON_B IO_D5 17 | 18 | /*! The pin number for the pin conencted to button C on the Balboa 32U4. */ 19 | #define BALBOA_32U4_BUTTON_C 17 20 | 21 | /*! \brief Interfaces with button A on the Balboa 32U4. */ 22 | class Balboa32U4ButtonA : public Pushbutton 23 | { 24 | public: 25 | Balboa32U4ButtonA() : Pushbutton(BALBOA_32U4_BUTTON_A) 26 | { 27 | } 28 | }; 29 | 30 | /*! \brief Interfaces with button B on the Balboa 32U4. 31 | * 32 | * The pin used for button B is also used for the TX LED. 33 | * 34 | * This class temporarily disables USB interrupts because the Arduino core code 35 | * has USB interrupts enabled that sometimes write to the pin this button is on. 36 | * 37 | * This class temporarily sets the pin to be an input without a pull-up 38 | * resistor. The pull-up resistor is not needed because of the resistors on the 39 | * board. */ 40 | class Balboa32U4ButtonB : public PushbuttonBase 41 | { 42 | public: 43 | 44 | virtual bool isPressed() 45 | { 46 | USBPause usbPause; 47 | FastGPIO::PinLoan loan; 48 | FastGPIO::Pin::setInputPulledUp(); 49 | _delay_us(3); 50 | return !FastGPIO::Pin::isInputHigh(); 51 | } 52 | }; 53 | 54 | /*! \brief Interfaces with button C on the Balboa 32U4. 55 | * 56 | * The pin used for button C is also used for the RX LED. 57 | * 58 | * This class temporarily disables USB interrupts because the Arduino core code 59 | * has USB interrupts enabled that sometimes write to the pin this button is on. 60 | * 61 | * This class temporarily sets the pin to be an input without a pull-up 62 | * resistor. The pull-up resistor is not needed because of the resistors on the 63 | * board. */ 64 | class Balboa32U4ButtonC : public PushbuttonBase 65 | { 66 | public: 67 | 68 | virtual bool isPressed() 69 | { 70 | USBPause usbPause; 71 | FastGPIO::PinLoan loan; 72 | FastGPIO::Pin::setInputPulledUp(); 73 | _delay_us(3); 74 | return !FastGPIO::Pin::isInputHigh(); 75 | } 76 | }; 77 | 78 | -------------------------------------------------------------------------------- /Balboa32U4Buzzer.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Balboa32U4Buzzer.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Plays beeps and music on the buzzer on the Balboa 32U4. 10 | * 11 | * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on 12 | * the Balboa 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 Balboa32U4Buzzer : public PololuBuzzer 24 | { 25 | // This is a trivial subclass of PololuBuzzer; it exists because we wanted 26 | // the Balboa32U4 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 | -------------------------------------------------------------------------------- /Balboa32U4Encoders.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 Balboa32U4Encoders::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 Balboa32U4Encoders::getCountsLeft() 92 | { 93 | init(); 94 | 95 | cli(); 96 | int16_t counts = countLeft; 97 | sei(); 98 | return counts; 99 | } 100 | 101 | int16_t Balboa32U4Encoders::getCountsRight() 102 | { 103 | init(); 104 | 105 | cli(); 106 | int16_t counts = countRight; 107 | sei(); 108 | return counts; 109 | } 110 | 111 | int16_t Balboa32U4Encoders::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 Balboa32U4Encoders::getCountsAndResetRight() 123 | { 124 | init(); 125 | 126 | cli(); 127 | int16_t counts = countRight; 128 | countRight = 0; 129 | sei(); 130 | return counts; 131 | } 132 | 133 | bool Balboa32U4Encoders::checkErrorLeft() 134 | { 135 | init(); 136 | 137 | bool error = errorLeft; 138 | errorLeft = 0; 139 | return error; 140 | } 141 | 142 | bool Balboa32U4Encoders::checkErrorRight() 143 | { 144 | init(); 145 | 146 | bool error = errorRight; 147 | errorRight = 0; 148 | return error; 149 | } 150 | -------------------------------------------------------------------------------- /Balboa32U4Encoders.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Balboa32U4Encoders.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Reads counts from the encoders on the Balboa 32U4. 10 | * 11 | * This class allows you to read counts from the encoders on the Balboa 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 Balboa motors have a gear ratio of 3952:33 (approximately 120:1). 27 | * The standard Balboa encoders give 12 counts per revolution. Therefore, one 28 | * revolution of a Balboa wheel corresponds to 12*3952/33 (approximately 1437.09) 29 | * encoder counts as returned by this library. 30 | */ 31 | class Balboa32U4Encoders 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 Balboa, 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 | -------------------------------------------------------------------------------- /Balboa32U4LCD.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Balboa32U4LCD.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | /*! \brief Writes data to the LCD on the Balboa 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 Balboa 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 Balboa32U4LCD : 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 | -------------------------------------------------------------------------------- /Balboa32U4LineSensors.h: -------------------------------------------------------------------------------- 1 | /** \file Balboa32U4LineSensors.h **/ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | /** \brief Gets readings from the five line sensors on the reflectance sensor 8 | * array. 9 | * 10 | * Before using the sensors with this class, you must call either 11 | * setCenterAligned() or setEdgeAligned() depending on the way your sensor 12 | * array is mounted on the Balboa. See the [product page for the sensor 13 | * array](https://www.pololu.com/product/3577) for an explanation and pictures 14 | * of the two options. 15 | * 16 | * If the alignment is set appropriately with one of the above functions, the 17 | * readLineBlack() and readLineWhite() methods will always return values that 18 | * increase from left to right, with 0 corresponding to the leftmost sensor and 19 | * 4000 corresponding to the rightmost sensor. (Note that “forward” refers to 20 | * the rotation direction that would cause a balancing Balboa to move in the 21 | * direction its battery cover is facing, and left and right are defined 22 | * accordingly.) 23 | * 24 | * See the [Usage Notes in the QTRSensors 25 | * documentation](https://pololu.github.io/qtr-sensors-arduino/md_usage.html) 26 | * for an overview of how the methods inherited from the QTRSensors library can 27 | * be used and some example code. 28 | */ 29 | class Balboa32U4LineSensors : public QTRSensors 30 | { 31 | public: 32 | 33 | Balboa32U4LineSensors() 34 | { 35 | setTypeRC(); 36 | } 37 | 38 | /** \brief Configures this object to use a center-aligned sensor array. 39 | * 40 | * This function configures this object to interface with a reflectance 41 | * sensor array using the center-aligned mounting option. In this 42 | * configuration, S5 is on the left side of the Balboa, S1 is on the right, 43 | * and CTRL is connected to pin 12. */ 44 | void setCenterAligned() 45 | { 46 | setSensorPins((const uint8_t[]){ 5, A4, A3, A2, A0 }, 5); 47 | setEmitterPin(12); 48 | } 49 | 50 | /** \brief Configures this object to use a center-aligned sensor array. 51 | * 52 | * This function configures this object to interface with a reflectance 53 | * sensor array using the center-aligned mounting option. In this 54 | * configuration, S1 is on the left side of the Balboa, S5 is on the right, 55 | * and CTRL is connected to pin 5. */ 56 | void setEdgeAligned() 57 | { 58 | setSensorPins((const uint8_t[]){ A4, A3, A2, A0, 12 }, 5); 59 | setEmitterPin(5); 60 | } 61 | }; 62 | 63 | -------------------------------------------------------------------------------- /Balboa32U4Motors.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 Balboa32U4Motors::flipLeft = false; 13 | bool Balboa32U4Motors::flipRight = false; 14 | 15 | int16_t Balboa32U4Motors::maxSpeed = 300; 16 | 17 | // initialize timer1 to generate the proper PWM outputs to the motor drivers 18 | void Balboa32U4Motors::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 Balboa32U4Motors::flipLeftMotor(bool flip) 41 | { 42 | flipLeft = flip; 43 | } 44 | 45 | void Balboa32U4Motors::flipRightMotor(bool flip) 46 | { 47 | flipRight = flip; 48 | } 49 | 50 | void Balboa32U4Motors::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 Balboa32U4Motors::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 Balboa32U4Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) 93 | { 94 | setLeftSpeed(leftSpeed); 95 | setRightSpeed(rightSpeed); 96 | } 97 | 98 | void Balboa32U4Motors::allowTurbo(bool turbo) 99 | { 100 | maxSpeed = turbo ? 400 : 300; 101 | } 102 | -------------------------------------------------------------------------------- /Balboa32U4Motors.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Balboa32U4Motors.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Controls motor speed and direction on the Balboa 32U4. 10 | * 11 | * This library uses Timer 1, so it will conflict with any other libraries using 12 | * that timer. */ 13 | class Balboa32U4Motors 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 Balboa 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 Balboa 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 Balboa 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 | -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | # Doxygen configuration file for generating documentation. 2 | PROJECT_NAME = "Balboa32U4 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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /QTRSensors.cpp: -------------------------------------------------------------------------------- 1 | #include "QTRSensors.h" 2 | #include 3 | 4 | void QTRSensors::setTypeRC() 5 | { 6 | _type = QTRType::RC; 7 | _maxValue = _timeout; 8 | } 9 | 10 | void QTRSensors::setTypeAnalog() 11 | { 12 | _type = QTRType::Analog; 13 | _maxValue = 1023; // Arduino analogRead() returns a 10-bit value by default 14 | } 15 | 16 | void QTRSensors::setSensorPins(const uint8_t * pins, uint8_t sensorCount) 17 | { 18 | if (sensorCount > QTRMaxSensors) { sensorCount = QTRMaxSensors; } 19 | 20 | // (Re)allocate and initialize the array if necessary. 21 | uint8_t * oldSensorPins = _sensorPins; 22 | _sensorPins = (uint8_t *)realloc(_sensorPins, sizeof(uint8_t) * sensorCount); 23 | if (_sensorPins == nullptr) 24 | { 25 | // Memory allocation failed; don't continue. 26 | free(oldSensorPins); // deallocate any memory used by old array 27 | return; 28 | } 29 | 30 | for (uint8_t i = 0; i < sensorCount; i++) 31 | { 32 | _sensorPins[i] = pins[i]; 33 | } 34 | 35 | _sensorCount = sensorCount; 36 | 37 | // Any previous calibration values are no longer valid, and the calibration 38 | // arrays might need to be reallocated if the sensor count was changed. 39 | calibrationOn.initialized = false; 40 | calibrationOff.initialized = false; 41 | } 42 | 43 | void QTRSensors::setTimeout(uint16_t timeout) 44 | { 45 | if (timeout > 32767) { timeout = 32767; } 46 | _timeout = timeout; 47 | if (_type == QTRType::RC) { _maxValue = timeout; } 48 | } 49 | 50 | void QTRSensors::setSamplesPerSensor(uint8_t samples) 51 | { 52 | if (samples > 64) { samples = 64; } 53 | _samplesPerSensor = samples; 54 | } 55 | 56 | void QTRSensors::setEmitterPin(uint8_t emitterPin) 57 | { 58 | releaseEmitterPins(); 59 | 60 | _oddEmitterPin = emitterPin; 61 | pinMode(_oddEmitterPin, OUTPUT); 62 | 63 | _emitterPinCount = 1; 64 | } 65 | 66 | void QTRSensors::setEmitterPins(uint8_t oddEmitterPin, uint8_t evenEmitterPin) 67 | { 68 | releaseEmitterPins(); 69 | 70 | _oddEmitterPin = oddEmitterPin; 71 | _evenEmitterPin = evenEmitterPin; 72 | pinMode(_oddEmitterPin, OUTPUT); 73 | pinMode(_evenEmitterPin, OUTPUT); 74 | 75 | _emitterPinCount = 2; 76 | } 77 | 78 | void QTRSensors::releaseEmitterPins() 79 | { 80 | if (_oddEmitterPin != QTRNoEmitterPin) 81 | { 82 | pinMode(_oddEmitterPin, INPUT); 83 | _oddEmitterPin = QTRNoEmitterPin; 84 | } 85 | 86 | if (_evenEmitterPin != QTRNoEmitterPin) 87 | { 88 | pinMode(_evenEmitterPin, INPUT); 89 | _evenEmitterPin = QTRNoEmitterPin; 90 | } 91 | 92 | _emitterPinCount = 0; 93 | } 94 | 95 | void QTRSensors::setDimmingLevel(uint8_t dimmingLevel) 96 | { 97 | if (dimmingLevel > 31) { dimmingLevel = 31; } 98 | _dimmingLevel = dimmingLevel; 99 | } 100 | 101 | // emitters defaults to QTREmitters::All; wait defaults to true 102 | void QTRSensors::emittersOff(QTREmitters emitters, bool wait) 103 | { 104 | bool pinChanged = false; 105 | 106 | // Use odd emitter pin in these cases: 107 | // - 1 emitter pin, emitters = all 108 | // - 2 emitter pins, emitters = all 109 | // - 2 emitter pins, emitters = odd 110 | if (emitters == QTREmitters::All || 111 | (_emitterPinCount == 2 && emitters == QTREmitters::Odd)) 112 | { 113 | // Check if pin is defined and only turn off if not already off 114 | if ((_oddEmitterPin != QTRNoEmitterPin) && 115 | (digitalRead(_oddEmitterPin) == HIGH)) 116 | { 117 | digitalWrite(_oddEmitterPin, LOW); 118 | pinChanged = true; 119 | } 120 | } 121 | 122 | // Use even emitter pin in these cases: 123 | // - 2 emitter pins, emitters = all 124 | // - 2 emitter pins, emitters = even 125 | if (_emitterPinCount == 2 && 126 | (emitters == QTREmitters::All || emitters == QTREmitters::Even)) 127 | { 128 | // Check if pin is defined and only turn off if not already off 129 | if ((_evenEmitterPin != QTRNoEmitterPin) && 130 | (digitalRead(_evenEmitterPin) == HIGH)) 131 | { 132 | digitalWrite(_evenEmitterPin, LOW); 133 | pinChanged = true; 134 | } 135 | } 136 | 137 | if (wait && pinChanged) 138 | { 139 | if (_dimmable) 140 | { 141 | // driver min is 1 ms 142 | delayMicroseconds(1200); 143 | } 144 | else 145 | { 146 | delayMicroseconds(200); 147 | } 148 | } 149 | } 150 | 151 | void QTRSensors::emittersOn(QTREmitters emitters, bool wait) 152 | { 153 | bool pinChanged = false; 154 | uint16_t emittersOnStart; 155 | 156 | // Use odd emitter pin in these cases: 157 | // - 1 emitter pin, emitters = all 158 | // - 2 emitter pins, emitters = all 159 | // - 2 emitter pins, emitters = odd 160 | if (emitters == QTREmitters::All || 161 | (_emitterPinCount == 2 && emitters == QTREmitters::Odd)) 162 | { 163 | // Check if pin is defined, and only turn on non-dimmable sensors if not 164 | // already on, but always turn dimmable sensors off and back on because 165 | // we might be changing the dimming level (emittersOnWithPin() should take 166 | // care of this) 167 | if ((_oddEmitterPin != QTRNoEmitterPin) && 168 | ( _dimmable || (digitalRead(_oddEmitterPin) == LOW))) 169 | { 170 | emittersOnStart = emittersOnWithPin(_oddEmitterPin); 171 | pinChanged = true; 172 | } 173 | } 174 | 175 | // Use even emitter pin in these cases: 176 | // - 2 emitter pins, emitters = all 177 | // - 2 emitter pins, emitters = even 178 | if (_emitterPinCount == 2 && 179 | (emitters == QTREmitters::All || emitters == QTREmitters::Even)) 180 | { 181 | // Check if pin is defined, and only turn on non-dimmable sensors if not 182 | // already on, but always turn dimmable sensors off and back on because 183 | // we might be changing the dimming level (emittersOnWithPin() should take 184 | // care of this) 185 | if ((_evenEmitterPin != QTRNoEmitterPin) && 186 | (_dimmable || (digitalRead(_evenEmitterPin) == LOW))) 187 | { 188 | emittersOnStart = emittersOnWithPin(_evenEmitterPin); 189 | pinChanged = true; 190 | } 191 | } 192 | 193 | if (wait && pinChanged) 194 | { 195 | if (_dimmable) 196 | { 197 | // Make sure it's been at least 300 us since the emitter pin was first set 198 | // high before returning. (Driver min is 250 us.) Some time might have 199 | // already passed while we set the dimming level. 200 | while ((uint16_t)(micros() - emittersOnStart) < 300) 201 | { 202 | delayMicroseconds(10); 203 | } 204 | } 205 | else 206 | { 207 | delayMicroseconds(200); 208 | } 209 | } 210 | } 211 | 212 | // assumes pin is valid (not QTRNoEmitterPin) 213 | // returns time when pin was first set high (used by emittersSelect()) 214 | uint16_t QTRSensors::emittersOnWithPin(uint8_t pin) 215 | { 216 | if (_dimmable && (digitalRead(pin) == HIGH)) 217 | { 218 | // We are turning on dimmable emitters that are already on. To avoid messing 219 | // up the dimming level, we have to turn the emitters off and back on. This 220 | // means the turn-off delay will happen even if wait = false was passed to 221 | // emittersOn(). (Driver min is 1 ms.) 222 | digitalWrite(pin, LOW); 223 | delayMicroseconds(1200); 224 | } 225 | 226 | digitalWrite(pin, HIGH); 227 | uint16_t emittersOnStart = micros(); 228 | 229 | if (_dimmable && (_dimmingLevel > 0)) 230 | { 231 | noInterrupts(); 232 | 233 | for (uint8_t i = 0; i < _dimmingLevel; i++) 234 | { 235 | delayMicroseconds(1); 236 | digitalWrite(pin, LOW); 237 | delayMicroseconds(1); 238 | digitalWrite(pin, HIGH); 239 | } 240 | 241 | interrupts(); 242 | } 243 | 244 | return emittersOnStart; 245 | } 246 | 247 | void QTRSensors::emittersSelect(QTREmitters emitters) 248 | { 249 | QTREmitters offEmitters; 250 | 251 | switch (emitters) 252 | { 253 | case QTREmitters::Odd: 254 | offEmitters = QTREmitters::Even; 255 | break; 256 | 257 | case QTREmitters::Even: 258 | offEmitters = QTREmitters::Odd; 259 | break; 260 | 261 | case QTREmitters::All: 262 | emittersOn(); 263 | return; 264 | 265 | case QTREmitters::None: 266 | emittersOff(); 267 | return; 268 | 269 | default: // invalid 270 | return; 271 | } 272 | 273 | // Turn off the off-emitters; don't wait before proceeding, but record the time. 274 | emittersOff(offEmitters, false); 275 | uint16_t turnOffStart = micros(); 276 | 277 | // Turn on the on-emitters and wait. 278 | emittersOn(emitters); 279 | 280 | if (_dimmable) 281 | { 282 | // Finish waiting for the off-emitters emitters to turn off: make sure it's been 283 | // at least 1200 us since the off-emitters was turned off before returning. 284 | // (Driver min is 1 ms.) Some time has already passed while we waited for 285 | // the on-emitters to turn on. 286 | while ((uint16_t)(micros() - turnOffStart) < 1200) 287 | { 288 | delayMicroseconds(10); 289 | } 290 | } 291 | } 292 | 293 | void QTRSensors::resetCalibration() 294 | { 295 | for (uint8_t i = 0; i < _sensorCount; i++) 296 | { 297 | if (calibrationOn.maximum) { calibrationOn.maximum[i] = 0; } 298 | if (calibrationOff.maximum) { calibrationOff.maximum[i] = 0; } 299 | if (calibrationOn.minimum) { calibrationOn.minimum[i] = _maxValue; } 300 | if (calibrationOff.minimum) { calibrationOff.minimum[i] = _maxValue; } 301 | } 302 | } 303 | 304 | void QTRSensors::calibrate(QTRReadMode mode) 305 | { 306 | // manual emitter control is not supported 307 | if (mode == QTRReadMode::Manual) { return; } 308 | 309 | if (mode == QTRReadMode::On || 310 | mode == QTRReadMode::OnAndOff) 311 | { 312 | calibrateOnOrOff(calibrationOn, QTRReadMode::On); 313 | } 314 | else if (mode == QTRReadMode::OddEven || 315 | mode == QTRReadMode::OddEvenAndOff) 316 | { 317 | calibrateOnOrOff(calibrationOn, QTRReadMode::OddEven); 318 | } 319 | 320 | if (mode == QTRReadMode::OnAndOff || 321 | mode == QTRReadMode::OddEvenAndOff || 322 | mode == QTRReadMode::Off) 323 | { 324 | calibrateOnOrOff(calibrationOff, QTRReadMode::Off); 325 | } 326 | } 327 | 328 | void QTRSensors::calibrateOnOrOff(CalibrationData & calibration, QTRReadMode mode) 329 | { 330 | uint16_t sensorValues[QTRMaxSensors]; 331 | uint16_t maxSensorValues[QTRMaxSensors]; 332 | uint16_t minSensorValues[QTRMaxSensors]; 333 | 334 | // (Re)allocate and initialize the arrays if necessary. 335 | if (!calibration.initialized) 336 | { 337 | uint16_t * oldMaximum = calibration.maximum; 338 | calibration.maximum = (uint16_t *)realloc(calibration.maximum, 339 | sizeof(uint16_t) * _sensorCount); 340 | if (calibration.maximum == nullptr) 341 | { 342 | // Memory allocation failed; don't continue. 343 | free(oldMaximum); // deallocate any memory used by old array 344 | return; 345 | } 346 | 347 | uint16_t * oldMinimum = calibration.minimum; 348 | calibration.minimum = (uint16_t *)realloc(calibration.minimum, 349 | sizeof(uint16_t) * _sensorCount); 350 | if (calibration.minimum == nullptr) 351 | { 352 | // Memory allocation failed; don't continue. 353 | free(oldMinimum); // deallocate any memory used by old array 354 | return; 355 | } 356 | 357 | // Initialize the max and min calibrated values to values that 358 | // will cause the first reading to update them. 359 | for (uint8_t i = 0; i < _sensorCount; i++) 360 | { 361 | calibration.maximum[i] = 0; 362 | calibration.minimum[i] = _maxValue; 363 | } 364 | 365 | calibration.initialized = true; 366 | } 367 | 368 | for (uint8_t j = 0; j < 10; j++) 369 | { 370 | read(sensorValues, mode); 371 | 372 | for (uint8_t i = 0; i < _sensorCount; i++) 373 | { 374 | // set the max we found THIS time 375 | if ((j == 0) || (sensorValues[i] > maxSensorValues[i])) 376 | { 377 | maxSensorValues[i] = sensorValues[i]; 378 | } 379 | 380 | // set the min we found THIS time 381 | if ((j == 0) || (sensorValues[i] < minSensorValues[i])) 382 | { 383 | minSensorValues[i] = sensorValues[i]; 384 | } 385 | } 386 | } 387 | 388 | // record the min and max calibration values 389 | for (uint8_t i = 0; i < _sensorCount; i++) 390 | { 391 | // Update maximum only if the min of 10 readings was still higher than it 392 | // (we got 10 readings in a row higher than the existing maximum). 393 | if (minSensorValues[i] > calibration.maximum[i]) 394 | { 395 | calibration.maximum[i] = minSensorValues[i]; 396 | } 397 | 398 | // Update minimum only if the max of 10 readings was still lower than it 399 | // (we got 10 readings in a row lower than the existing minimum). 400 | if (maxSensorValues[i] < calibration.minimum[i]) 401 | { 402 | calibration.minimum[i] = maxSensorValues[i]; 403 | } 404 | } 405 | } 406 | 407 | void QTRSensors::read(uint16_t * sensorValues, QTRReadMode mode) 408 | { 409 | switch (mode) 410 | { 411 | case QTRReadMode::Off: 412 | emittersOff(); 413 | // fall through 414 | case QTRReadMode::Manual: 415 | readPrivate(sensorValues); 416 | return; 417 | 418 | case QTRReadMode::On: 419 | case QTRReadMode::OnAndOff: 420 | emittersOn(); 421 | readPrivate(sensorValues); 422 | emittersOff(); 423 | break; 424 | 425 | case QTRReadMode::OddEven: 426 | case QTRReadMode::OddEvenAndOff: 427 | // Turn on odd emitters and read the odd-numbered sensors. 428 | // (readPrivate takes a 0-based array index, so start = 0 to start with 429 | // the first sensor) 430 | emittersSelect(QTREmitters::Odd); 431 | readPrivate(sensorValues, 0, 2); 432 | 433 | // Turn on even emitters and read the even-numbered sensors. 434 | // (readPrivate takes a 0-based array index, so start = 1 to start with 435 | // the second sensor) 436 | emittersSelect(QTREmitters::Even); 437 | readPrivate(sensorValues, 1, 2); 438 | 439 | emittersOff(); 440 | break; 441 | 442 | default: // invalid - do nothing 443 | return; 444 | } 445 | 446 | if (mode == QTRReadMode::OnAndOff || 447 | mode == QTRReadMode::OddEvenAndOff) 448 | { 449 | // Take a second set of readings and return the values (on + max - off). 450 | 451 | uint16_t offValues[QTRMaxSensors]; 452 | readPrivate(offValues); 453 | 454 | for (uint8_t i = 0; i < _sensorCount; i++) 455 | { 456 | sensorValues[i] += _maxValue - offValues[i]; 457 | if (sensorValues[i] > _maxValue) 458 | { 459 | // This usually doesn't happen, because the sensor reading should 460 | // go up when the emitters are turned off. 461 | sensorValues[i] = _maxValue; 462 | } 463 | } 464 | } 465 | } 466 | 467 | void QTRSensors::readCalibrated(uint16_t * sensorValues, QTRReadMode mode) 468 | { 469 | // manual emitter control is not supported 470 | if (mode == QTRReadMode::Manual) { return; } 471 | 472 | // if not calibrated, do nothing 473 | 474 | if (mode == QTRReadMode::On || 475 | mode == QTRReadMode::OnAndOff || 476 | mode == QTRReadMode::OddEvenAndOff) 477 | { 478 | if (!calibrationOn.initialized) 479 | { 480 | return; 481 | } 482 | } 483 | 484 | if (mode == QTRReadMode::Off || 485 | mode == QTRReadMode::OnAndOff || 486 | mode == QTRReadMode::OddEvenAndOff) 487 | { 488 | if (!calibrationOff.initialized) 489 | { 490 | return; 491 | } 492 | } 493 | 494 | // read the needed values 495 | read(sensorValues, mode); 496 | 497 | for (uint8_t i = 0; i < _sensorCount; i++) 498 | { 499 | uint16_t calmin, calmax; 500 | 501 | // find the correct calibration 502 | if (mode == QTRReadMode::On || 503 | mode == QTRReadMode::OddEven) 504 | { 505 | calmax = calibrationOn.maximum[i]; 506 | calmin = calibrationOn.minimum[i]; 507 | } 508 | else if (mode == QTRReadMode::Off) 509 | { 510 | calmax = calibrationOff.maximum[i]; 511 | calmin = calibrationOff.minimum[i]; 512 | } 513 | else // QTRReadMode::OnAndOff, QTRReadMode::OddEvenAndOff 514 | { 515 | if (calibrationOff.minimum[i] < calibrationOn.minimum[i]) 516 | { 517 | // no meaningful signal 518 | calmin = _maxValue; 519 | } 520 | else 521 | { 522 | // this won't go past _maxValue 523 | calmin = calibrationOn.minimum[i] + _maxValue - calibrationOff.minimum[i]; 524 | } 525 | 526 | if (calibrationOff.maximum[i] < calibrationOn.maximum[i]) 527 | { 528 | // no meaningful signal 529 | calmax = _maxValue; 530 | } 531 | else 532 | { 533 | // this won't go past _maxValue 534 | calmax = calibrationOn.maximum[i] + _maxValue - calibrationOff.maximum[i]; 535 | } 536 | } 537 | 538 | uint16_t denominator = calmax - calmin; 539 | int16_t value = 0; 540 | 541 | if (denominator != 0) 542 | { 543 | value = (((int32_t)sensorValues[i]) - calmin) * 1000 / denominator; 544 | } 545 | 546 | if (value < 0) { value = 0; } 547 | else if (value > 1000) { value = 1000; } 548 | 549 | sensorValues[i] = value; 550 | } 551 | } 552 | 553 | // Reads the first of every [step] sensors, starting with [start] (0-indexed, so 554 | // start = 0 means start with the first sensor). 555 | // For example, step = 2, start = 1 means read the *even-numbered* sensors. 556 | // start defaults to 0, step defaults to 1 557 | void QTRSensors::readPrivate(uint16_t * sensorValues, uint8_t start, uint8_t step) 558 | { 559 | if (_sensorPins == nullptr) { return; } 560 | 561 | switch (_type) 562 | { 563 | case QTRType::RC: 564 | for (uint8_t i = start; i < _sensorCount; i += step) 565 | { 566 | sensorValues[i] = _maxValue; 567 | // make sensor line an output (drives low briefly, but doesn't matter) 568 | pinMode(_sensorPins[i], OUTPUT); 569 | // drive sensor line high 570 | digitalWrite(_sensorPins[i], HIGH); 571 | } 572 | 573 | delayMicroseconds(10); // charge lines for 10 us 574 | 575 | { 576 | // disable interrupts so we can switch all the pins as close to the same 577 | // time as possible 578 | noInterrupts(); 579 | 580 | // record start time before the first sensor is switched to input 581 | // (similarly, time is checked before the first sensor is read in the 582 | // loop below) 583 | uint32_t startTime = micros(); 584 | uint16_t time = 0; 585 | 586 | for (uint8_t i = start; i < _sensorCount; i += step) 587 | { 588 | // make sensor line an input (should also ensure pull-up is disabled) 589 | pinMode(_sensorPins[i], INPUT); 590 | } 591 | 592 | interrupts(); // re-enable 593 | 594 | while (time < _maxValue) 595 | { 596 | // disable interrupts so we can read all the pins as close to the same 597 | // time as possible 598 | noInterrupts(); 599 | 600 | time = micros() - startTime; 601 | for (uint8_t i = start; i < _sensorCount; i += step) 602 | { 603 | if ((digitalRead(_sensorPins[i]) == LOW) && (time < sensorValues[i])) 604 | { 605 | // record the first time the line reads low 606 | sensorValues[i] = time; 607 | } 608 | } 609 | 610 | interrupts(); // re-enable 611 | } 612 | } 613 | return; 614 | 615 | case QTRType::Analog: 616 | // reset the values 617 | for (uint8_t i = start; i < _sensorCount; i += step) 618 | { 619 | sensorValues[i] = 0; 620 | } 621 | 622 | for (uint8_t j = 0; j < _samplesPerSensor; j++) 623 | { 624 | for (uint8_t i = start; i < _sensorCount; i += step) 625 | { 626 | // add the conversion result 627 | sensorValues[i] += analogRead(_sensorPins[i]); 628 | } 629 | } 630 | 631 | // get the rounded average of the readings for each sensor 632 | for (uint8_t i = start; i < _sensorCount; i += step) 633 | { 634 | sensorValues[i] = (sensorValues[i] + (_samplesPerSensor >> 1)) / 635 | _samplesPerSensor; 636 | } 637 | return; 638 | 639 | default: // QTRType::Undefined or invalid - do nothing 640 | return; 641 | } 642 | } 643 | 644 | uint16_t QTRSensors::readLinePrivate(uint16_t * sensorValues, QTRReadMode mode, 645 | bool invertReadings) 646 | { 647 | bool onLine = false; 648 | uint32_t avg = 0; // this is for the weighted total 649 | uint16_t sum = 0; // this is for the denominator, which is <= 64000 650 | 651 | // manual emitter control is not supported 652 | if (mode == QTRReadMode::Manual) { return 0; } 653 | 654 | readCalibrated(sensorValues, mode); 655 | 656 | for (uint8_t i = 0; i < _sensorCount; i++) 657 | { 658 | uint16_t value = sensorValues[i]; 659 | if (invertReadings) { value = 1000 - value; } 660 | 661 | // keep track of whether we see the line at all 662 | if (value > 200) { onLine = true; } 663 | 664 | // only average in values that are above a noise threshold 665 | if (value > 50) 666 | { 667 | avg += (uint32_t)value * (i * 1000); 668 | sum += value; 669 | } 670 | } 671 | 672 | if (!onLine) 673 | { 674 | // If it last read to the left of center, return 0. 675 | if (_lastPosition < (_sensorCount - 1) * 1000 / 2) 676 | { 677 | return 0; 678 | } 679 | // If it last read to the right of center, return the max. 680 | else 681 | { 682 | return (_sensorCount - 1) * 1000; 683 | } 684 | } 685 | 686 | _lastPosition = avg / sum; 687 | return _lastPosition; 688 | } 689 | 690 | // the destructor frees up allocated memory 691 | QTRSensors::~QTRSensors() 692 | { 693 | releaseEmitterPins(); 694 | 695 | if (_sensorPins) { free(_sensorPins); } 696 | if (calibrationOn.maximum) { free(calibrationOn.maximum); } 697 | if (calibrationOff.maximum) { free(calibrationOff.maximum); } 698 | if (calibrationOn.minimum) { free(calibrationOn.minimum); } 699 | if (calibrationOff.minimum) { free(calibrationOff.minimum); } 700 | } 701 | -------------------------------------------------------------------------------- /QTRSensors.h: -------------------------------------------------------------------------------- 1 | /// \file QTRSensors.h 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | /// \brief Emitter behavior when taking readings. 8 | /// 9 | /// Note that emitter control will only work if you specify a valid emitter pin 10 | /// with setEmitterPin(), and the odd/even modes will only work if you are 11 | /// using a second-generation QTR or QTRX sensor with two emitter control pins 12 | /// and you specify both pins with setEmitterPins(). 13 | enum class QTRReadMode : uint8_t { 14 | /// Each reading is made without turning on the infrared (IR) emitters. The 15 | /// reading represents ambient light levels near the sensor. 16 | Off, 17 | 18 | /// Each reading is made with the emitters on. The reading is a measure of 19 | /// reflectance. 20 | On, 21 | 22 | /// For each sensor, a reading is made in both the on and off states. The 23 | /// value returned is **on + max − off**, where **on** and **off** are 24 | /// the reading with the emitters on and off, respectively, and **max** is 25 | /// the maximum possible sensor reading. This mode can reduce the amount of 26 | /// interference from uneven ambient lighting. 27 | OnAndOff, 28 | 29 | /// The odd-numbered sensors are read with the odd-numbered emitters on, then 30 | /// the even-numbered sensors are read with the even-numbered emitters on. 31 | /// This mode can reduce interference between adjacent sensors, especially on 32 | /// QTRX sensor boards. It is only usable with second-generation QTR and QTRX 33 | /// sensor arrays that have two emitter control pins. 34 | OddEven, 35 | 36 | /// The odd and even sensors are read separately with the respective emitters 37 | /// on, then all sensors are read with emitters off and **on + max − 38 | /// off** is returned. (In other words, this mode combines OddEven and 39 | /// OnAndOff.) 40 | OddEvenAndOff, 41 | 42 | /// Calling read() with this mode prevents it from automatically controlling 43 | /// the emitters: they are left in their existing states, which allows manual 44 | /// control of the emitters for testing and advanced use. Calibrating and 45 | /// obtaining calibrated readings are not supported with this mode. 46 | Manual 47 | }; 48 | 49 | /// Sensor types. 50 | enum class QTRType : uint8_t { 51 | Undefined, 52 | RC, 53 | Analog 54 | }; 55 | 56 | /// Emitters selected to turn on or off. 57 | enum class QTREmitters : uint8_t { 58 | All, 59 | Odd, 60 | Even, 61 | None 62 | }; 63 | 64 | /// Represents an undefined emitter control pin. 65 | const uint8_t QTRNoEmitterPin = 255; 66 | 67 | /// Default timeout for RC sensors (in microseconds). 68 | const uint16_t QTRRCDefaultTimeout = 2500; 69 | 70 | /// The maximum number of sensors supported by an instance of this class. 71 | const uint8_t QTRMaxSensors = 31; 72 | 73 | /// \brief Represents a QTR sensor array. 74 | /// 75 | /// An instance of this class represents a QTR sensor array, consisting of one 76 | /// or more sensors of the same type. This could be either a single QTR sensor 77 | /// board or multiple boards controlled as a group. 78 | /// 79 | /// \if usage 80 | /// See \ref md_usage for an overview of how this library can be used and 81 | /// some example code. 82 | /// \endif 83 | class QTRSensors 84 | { 85 | public: 86 | 87 | QTRSensors() = default; 88 | 89 | ~QTRSensors(); 90 | 91 | /// \brief Specifies that the sensors are RC. 92 | /// 93 | /// Call this function to set up RC-type sensors. 94 | void setTypeRC(); 95 | 96 | /// \brief Specifies that the sensor type is analog. 97 | /// 98 | /// Call this function to set up A-type sensors. 99 | void setTypeAnalog(); 100 | 101 | /// \brief Returns the type of the sensors. 102 | /// 103 | /// \return The sensor type as a member of the ::QTRType enum. 104 | /// 105 | /// See also setTypeRC() and setTypeAnalog(). 106 | QTRType getType() { return _type; } 107 | 108 | /// \brief Sets the sensor pins. 109 | /// 110 | /// \param[in] pins A pointer to an array containing the Arduino pins that 111 | /// the sensors are connected to. 112 | /// 113 | /// \param sensorCount The number of sensors, which should match the length 114 | /// of the pins array. 115 | /// 116 | /// Example usage: 117 | /// ~~~{.cpp} 118 | /// // Set pins for four RC sensors connected to pins 6, 7, A0, and A1. 119 | /// // (Most analog pins can also be used as digital pins.) 120 | /// qtr.setTypeRC(); 121 | /// qtr.setSensorPins((const uint8_t[]){6, 7, A0, A1}, 4); 122 | /// ~~~ 123 | /// ~~~{.cpp} 124 | /// // Set pins for four analog sensors connected to pins A2, A3, A4, and A5. 125 | /// qtr.setTypeAnalog(); 126 | /// qtr.setSensorPins((const uint8_t[]){A2, A3, A4, A5}, 4); 127 | /// ~~~ 128 | /// 129 | /// If \link CalibrationData calibration data \endlink has already been 130 | /// stored, calling this method will force the storage for the calibration 131 | /// values to be reallocated and reinitialized the next time calibrate() is 132 | /// called (it sets `calibrationOn.initialized` and 133 | /// `calibrationOff.initialized` to false). 134 | void setSensorPins(const uint8_t * pins, uint8_t sensorCount); 135 | 136 | /// \brief Sets the timeout for RC sensors. 137 | /// 138 | /// \param timeout The length of time, in microseconds, beyond which you 139 | /// consider the sensor reading completely black. 140 | /// 141 | /// If the pulse length for a pin exceeds \p timeout, pulse timing will 142 | /// stop and the reading for that pin will be considered full black. It is 143 | /// recommended that you set \p timeout to be between 1000 and 3000 144 | /// µs, depending on factors like the height of your sensors and 145 | /// ambient lighting. This allows you to shorten the duration of a 146 | /// sensor-reading cycle while maintaining useful measurements of 147 | /// reflectance. The default timeout is 2500 µs. 148 | /// 149 | /// The maximum allowed timeout is 32767. 150 | /// (This prevents any possibility of an overflow when using 151 | /// QTRReadMode::OnAndOff or QTRReadMode::OddEvenAndOff). 152 | /// 153 | /// The timeout setting only applies to RC sensors. 154 | void setTimeout(uint16_t timeout); 155 | 156 | /// \brief Returns the timeout for RC sensors. 157 | /// 158 | /// \return The RC sensor timeout in microseconds. 159 | /// 160 | /// See also setTimeout(). 161 | uint16_t getTimeout() { return _timeout; } 162 | 163 | /// \brief Sets the number of analog readings to average per analog sensor. 164 | /// 165 | /// \param samples The number of 10-bit analog samples (analog-to-digital 166 | /// conversions) to average per sensor each time it is read. 167 | /// 168 | /// Increasing \p samples increases noise suppression at the cost of sample 169 | /// rate. The maximum number of samples per sensor is 64; the default is 4. 170 | /// 171 | /// The samples per sensor setting only applies to analog sensors. 172 | void setSamplesPerSensor(uint8_t samples); 173 | 174 | /// \brief Returns the number of analog readings to average per analog 175 | /// sensor. 176 | /// 177 | /// \return The samples per channel for analog sensors. 178 | /// 179 | /// See also setSamplesPerSensor(). 180 | uint16_t getSamplesPerSensor() { return _samplesPerSensor; } 181 | 182 | /// \brief Sets the emitter control pin for the sensors. 183 | /// 184 | /// \param emitterPin The Arduino digital pin that controls whether the IR 185 | /// LEDs are on or off. 186 | /// 187 | /// Specifying an emitter pin is optional, and the pin is not present on 188 | /// some QTR sensor boards. If a valid pin is connected and specified, the 189 | /// emitters will only be turned on during a reading; otherwise, the IR 190 | /// emitters will always be on. No emitter pin is specified by default. 191 | /// 192 | /// With second-generation QTR or QTRX sensor arrays that have two emitter 193 | /// control pins, you can control all of the emitters together by 194 | /// specifying a single emitter pin connected to either the CTRL ODD or 195 | /// CTRL EVEN pin on the sensor board. For independent control of the odd- 196 | /// and even-numbered emitters, see setEmitterPins(). 197 | /// 198 | /// If you call this function after an emitter pin/pins have already been 199 | /// specified, any existing emitter pins will be released; see also 200 | /// releaseEmitterPins(). 201 | void setEmitterPin(uint8_t emitterPin); 202 | 203 | /// \brief Sets separate odd and even emitter control pins for the sensors. 204 | /// 205 | /// \param oddEmitterPin The Arduino digital pin that controls the 206 | /// odd-numbered IR LEDs. 207 | /// 208 | /// \param evenEmitterPin The Arduino digital pin that controls the 209 | /// even-numbered IR LEDs. 210 | /// 211 | /// This function only works with second-generation QTR or QTRX sensor 212 | /// arrays that have two emitter control pins. To specify a single emitter 213 | /// pin for all sensors, see setEmitterPin(). 214 | /// 215 | /// If you call this function after an emitter pin/pins have already been 216 | /// specified, any existing emitter pins will be released; see also 217 | /// releaseEmitterPins(). 218 | void setEmitterPins(uint8_t oddEmitterPin, uint8_t evenEmitterPin); 219 | 220 | /// \brief Releases emitter pin/pins that have been set. 221 | /// 222 | /// This function releases any emitter pins that were previously specified, 223 | /// making them inputs and stopping further control of the emitters through 224 | /// them. 225 | /// 226 | /// See also setEmitterPin() and setEmitterPins(). 227 | void releaseEmitterPins(); 228 | 229 | /// \brief Returns the number of emitter control pins in use. 230 | /// 231 | /// \return The number of emitter control pins previously specified (1 with 232 | /// setEmitterPin() or 2 with setEmitterPins()). If no emitter pins have 233 | /// been specified (the default), or if previously specified pins were 234 | /// released with releaseEmitterPins(), this function returns 0. 235 | uint8_t getEmitterPinCount() { return _emitterPinCount; } 236 | 237 | /// \brief Returns the emitter control pin. 238 | /// 239 | /// \return The Arduino digital pin number of the emitter control pin 240 | /// (QTRNoEmitterPin if undefined). 241 | /// 242 | /// This function is intended for use when there is a single emitter pin 243 | /// specified; you can use getOddEmitterPin() and getEvenEmitterPin() 244 | /// instead when two are specified. 245 | /// 246 | /// See also setEmitterPin(). 247 | uint8_t getEmitterPin() { return _oddEmitterPin; } 248 | 249 | /// \brief Returns the odd emitter control pin. 250 | /// 251 | /// \return The Arduino digital pin number of the odd emitter control pin 252 | /// (QTRNoEmitterPin if undefined). 253 | /// 254 | /// This function is intended for use when there are separate odd and even 255 | /// emitter pins specified; you can use getEmitterPin() instead when only 256 | /// one is specified. 257 | /// 258 | /// See also getEvenEmitterPin() and setEmitterPins(). 259 | uint8_t getOddEmitterPin() { return _oddEmitterPin; } 260 | 261 | /// \brief Returns the even emitter control pin. 262 | /// 263 | /// \return The Arduino digital pin number of the even emitter control pin 264 | /// (QTRNoEmitterPin if undefined). 265 | /// 266 | /// This function is intended for use when there are separate odd and even 267 | /// emitter pins specified; you can use getEmitterPin() instead when only 268 | /// one is specified. 269 | /// 270 | /// See also getOddEmitterPin() and setEmitterPins(). 271 | uint8_t getEvenEmitterPin() { return _evenEmitterPin; } 272 | 273 | /// \brief Specifies that the sensors are dimmable. 274 | /// 275 | /// Calling this function is optional when setting up second-generation QTR 276 | /// or QTRX sensors. By default, the library assumes the sensors are 277 | /// dimmable. 278 | /// 279 | /// For first-generation QTR sensors, see setNonDimmable(). 280 | void setDimmable() { _dimmable = true; } 281 | 282 | /// \brief Specifies that the sensors are non-dimmable. 283 | /// 284 | /// Call this function to set up first-generation QTR sensors and allow 285 | /// them to be read slightly faster (since their emitters can be turned on 286 | /// and off slightly more quickly than those on dimmable sensors). 287 | /// 288 | /// See also setDimmable(). 289 | void setNonDimmable() { _dimmable = false; } 290 | 291 | /// \brief Returns whether the sensors are dimmable. 292 | /// 293 | /// \return True if this object is configured to treat the sensors as 294 | /// dimmable, false otherwise. 295 | /// 296 | /// See also setDimmable() and setNonDimmable(). 297 | bool getDimmable() { return _dimmable; } 298 | 299 | /// \brief Sets the dimming level. 300 | /// 301 | /// \param dimmingLevel The dimming level (0 to 31). A dimming level of 0 302 | /// corresponds to full current and brightness, with higher dimming levels 303 | /// meaning lower currents. 304 | /// 305 | /// See your sensor board's product page or documentation for details on 306 | /// the relationship of the dimming level to the LED current. 307 | /// 308 | /// The dimming level will take effect the next time emittersOn() is called 309 | /// (either from your own program or by one of the library's read methods), 310 | /// and it will be applied again whenever the emitters are turned on after 311 | /// that. 312 | /// 313 | /// This setting is only used by dimmable sensors, and an emitter control 314 | /// pin/pins must be connected and defined for dimming to be applied. 315 | void setDimmingLevel(uint8_t dimmingLevel); 316 | 317 | /// \brief Returns the dimming level. 318 | /// 319 | /// \return The dimming level. 320 | /// 321 | /// See also setDimmingLevel(). 322 | uint8_t getDimmingLevel() { return _dimmingLevel; } 323 | 324 | /// \brief Turns the IR LEDs off. 325 | /// 326 | /// \param emitters Which emitters to turn off, as a member of the 327 | /// ::QTREmitters enum. The default is QTREmitters::All. 328 | /// 329 | /// \param wait If true (the default), this function delays to give the 330 | /// sensors time to turn off before returning. Otherwise, it returns 331 | /// immediately. 332 | /// 333 | /// This function is mainly for use by the read() method. Since read() 334 | /// normally turns the emitters on and off automatically for each reading, 335 | /// calling this function yourself will not affect the readings unless the 336 | /// read mode is QTRReadMode::Manual, which tells read() to leave the 337 | /// emitters alone. 338 | void emittersOff(QTREmitters emitters = QTREmitters::All, bool wait = true); 339 | 340 | /// \brief Turns the IR LEDs on. 341 | /// 342 | /// \param emitters Which emitters to turn on, as a member of the 343 | /// ::QTREmitters enum. The default is QTREmitters::All. 344 | /// 345 | /// \param wait If true (the default), this function delays to give the 346 | /// sensors time to turn on before returning. Otherwise, it returns 347 | /// immediately. 348 | /// 349 | /// If the sensors are dimmable and a dimming level is set, this function 350 | /// will apply the dimming level after turning the emitters on. 351 | /// 352 | /// This function is mainly for use by the read() method. Since read() 353 | /// normally turns the emitters on and off automatically for each reading, 354 | /// calling this function yourself will not affect the readings unless the 355 | /// read mode is QTRReadMode::Manual, which tells read() to leave the 356 | /// emitters alone. 357 | void emittersOn(QTREmitters emitters = QTREmitters::All, bool wait = true); 358 | 359 | /// \brief Turns on the selected emitters and turns off the other emitters 360 | /// with optimized timing. 361 | /// 362 | /// \param emitters Which emitters to turn on, as a member of the 363 | /// ::QTREmitters enum. The other emitters will be turned off. 364 | /// 365 | /// This function turns on the selected emitters while it waits for the 366 | /// other emitters to turn off. For example, 367 | /// `emittersSelect(QTREmitters::Odd)` turns on the odd-numbered emitters 368 | /// while turning off the even-numbered emitters. Using this method avoids 369 | /// unnecessary delays compared to calling emittersOff() and emittersOn() 370 | /// separately, but it still waits for all emitters to be in the right 371 | /// states before returning. 372 | void emittersSelect(QTREmitters emitters); 373 | 374 | /// \brief Reads the sensors for calibration. 375 | /// 376 | /// \param mode The emitter behavior during calibration, as a member of the 377 | /// ::QTRReadMode enum. The default is QTRReadMode::On. Manual emitter 378 | /// control with QTRReadMode::Manual is not supported. 379 | /// 380 | /// This method reads the sensors 10 times and uses the results for 381 | /// calibration. The sensor values are not returned; instead, the maximum 382 | /// and minimum values found over time are stored in #calibrationOn and/or 383 | /// #calibrationOff for use by the readCalibrated() method. 384 | /// 385 | /// If the storage for the calibration values has not been initialized, 386 | /// this function will (re)allocate the arrays and initialize the maximum 387 | /// and minimum values to 0 and the maximum possible sensor reading, 388 | /// respectively, so that the very first calibration sensor reading will 389 | /// update both of them. 390 | /// 391 | /// Note that the `minimum` and `maximum` pointers in the CalibrationData 392 | /// structs will point to arrays of length \p sensorCount, as specified in 393 | /// setSensorPins(), and they will only be allocated when calibrate() is 394 | /// called. If you only calibrate with the emitters on, the calibration 395 | /// arrays that hold the off values will not be allocated (and vice versa). 396 | /// 397 | /// \if usage 398 | /// See \ref md_usage for more information and example code. 399 | /// \endif 400 | void calibrate(QTRReadMode mode = QTRReadMode::On); 401 | 402 | /// \brief Resets all calibration that has been done. 403 | void resetCalibration(); 404 | 405 | /// \brief Reads the raw sensor values into an array. 406 | /// 407 | /// \param[out] sensorValues A pointer to an array in which to store the 408 | /// raw sensor readings. There **MUST** be space in the array for as many 409 | /// values as there were sensors specified in setSensorPins(). 410 | /// 411 | /// \param mode The emitter behavior during the read, as a member of the 412 | /// ::QTRReadMode enum. The default is QTRReadMode::On. 413 | /// 414 | /// Example usage: 415 | /// ~~~{.cpp} 416 | /// uint16_t sensorValues[8]; 417 | /// qtr.read(sensorValues); 418 | /// ~~~ 419 | /// 420 | /// The values returned are a measure of the reflectance in abstract units, 421 | /// with higher values corresponding to lower reflectance (e.g. a black 422 | /// surface or a void). 423 | /// 424 | /// Analog sensors will return a raw value between 0 and 1023 (like 425 | /// Arduino's `analogRead()` function). 426 | /// 427 | /// RC sensors will return a raw value in microseconds between 0 and the 428 | /// timeout setting configured with setTimeout() (the default timeout is 429 | /// 2500 µs). 430 | /// 431 | /// \if usage 432 | /// See \ref md_usage for more information and example code. 433 | /// \endif 434 | void read(uint16_t * sensorValues, QTRReadMode mode = QTRReadMode::On); 435 | 436 | /// \brief Reads the sensors and provides calibrated values between 0 and 437 | /// 1000. 438 | /// 439 | /// \param[out] sensorValues A pointer to an array in which to store the 440 | /// calibrated sensor readings. There **MUST** be space in the array for 441 | /// as many values as there were sensors specified in setSensorPins(). 442 | /// 443 | /// \param mode The emitter behavior during the read, as a member of the 444 | /// ::QTRReadMode enum. The default is QTRReadMode::On. Manual emitter 445 | /// control with QTRReadMode::Manual is not supported. 446 | /// 447 | /// 0 corresponds to the minimum value stored in #calibrationOn or 448 | /// #calibrationOff, depending on \p mode, and 1000 corresponds to the 449 | /// maximum value. Calibration values are typically obtained by calling 450 | /// calibrate(), and they are stored separately for each sensor, so that 451 | /// differences in the sensors are accounted for automatically. 452 | /// 453 | /// \if usage 454 | /// See \ref md_usage for more information and example code. 455 | /// \endif 456 | void readCalibrated(uint16_t * sensorValues, QTRReadMode mode = QTRReadMode::On); 457 | 458 | /// \brief Reads the sensors, provides calibrated values, and returns an 459 | /// estimated black line position. 460 | /// 461 | /// \param[out] sensorValues A pointer to an array in which to store the 462 | /// calibrated sensor readings. There **MUST** be space in the array for 463 | /// as many values as there were sensors specified in setSensorPins(). 464 | /// 465 | /// \param mode The emitter behavior during the read, as a member of the 466 | /// ::QTRReadMode enum. The default is QTRReadMode::On. Manual emitter 467 | /// control with QTRReadMode::Manual is not supported. 468 | /// 469 | /// \return An estimate of the position of a black line under the sensors. 470 | /// 471 | /// The estimate is made using a weighted average of the sensor indices 472 | /// multiplied by 1000, so that a return value of 0 indicates that the line 473 | /// is directly below sensor 0, a return value of 1000 indicates that the 474 | /// line is directly below sensor 1, 2000 indicates that it's below sensor 475 | /// 2000, etc. Intermediate values indicate that the line is between two 476 | /// sensors. The formula is (where \f$v_0\f$ represents the value from the 477 | /// first sensor): 478 | /// 479 | /// \f[ 480 | /// {(0 \times v_0) + (1000 \times v_1) + (2000 \times v_2) + \cdots 481 | /// \over 482 | /// v_0 + v_1 + v_2 + \cdots} 483 | /// \f] 484 | /// 485 | /// As long as your sensors aren’t spaced too far apart relative to the 486 | /// line, this returned value is designed to be monotonic, which makes it 487 | /// great for use in closed-loop PID control. Additionally, this method 488 | /// remembers where it last saw the line, so if you ever lose the line to 489 | /// the left or the right, its line position will continue to indicate the 490 | /// direction you need to go to reacquire the line. For example, if sensor 491 | /// 4 is your rightmost sensor and you end up completely off the line to 492 | /// the left, this function will continue to return 4000. 493 | /// 494 | /// This function is intended to detect a black (or dark-colored) line on a 495 | /// white (or light-colored) background. For a white line, see 496 | /// readLineWhite(). 497 | /// 498 | /// \if usage 499 | /// See \ref md_usage for more information and example code. 500 | /// \endif 501 | uint16_t readLineBlack(uint16_t * sensorValues, QTRReadMode mode = QTRReadMode::On) 502 | { 503 | return readLinePrivate(sensorValues, mode, false); 504 | } 505 | 506 | /// \brief Reads the sensors, provides calibrated values, and returns an 507 | /// estimated white line position. 508 | /// 509 | /// \param[out] sensorValues A pointer to an array in which to store the 510 | /// calibrated sensor readings. There **MUST** be space in the array for 511 | /// as many values as there were sensors specified in setSensorPins(). 512 | /// 513 | /// \param mode The emitter behavior during the read, as a member of the 514 | /// ::QTRReadMode enum. The default is QTRReadMode::On. Manual emitter 515 | /// control with QTRReadMode::Manual is not supported. 516 | /// 517 | /// \return An estimate of the position of a white line under the sensors. 518 | /// 519 | /// This function is intended to detect a white (or light-colored) line on 520 | /// a black (or dark-colored) background. For a black line, see 521 | /// readLineBlack(). 522 | /// 523 | /// \if usage 524 | /// See \ref md_usage for more information and example code. 525 | /// \endif 526 | uint16_t readLineWhite(uint16_t * sensorValues, QTRReadMode mode = QTRReadMode::On) 527 | { 528 | return readLinePrivate(sensorValues, mode, true); 529 | } 530 | 531 | 532 | /// \brief Stores sensor calibration data. 533 | /// 534 | /// See calibrate() and readCalibrated() for details. 535 | struct CalibrationData 536 | { 537 | /// Whether array pointers have been allocated and initialized. 538 | bool initialized = false; 539 | /// Lowest readings seen during calibration. 540 | uint16_t * minimum = nullptr; 541 | /// Highest readings seen during calibration. 542 | uint16_t * maximum = nullptr; 543 | }; 544 | 545 | /// \name Calibration data 546 | /// 547 | /// See calibrate() and readCalibrated() for details. 548 | /// 549 | /// These variables are made public so that you can use them for your own 550 | /// calculations and do things like saving the values to EEPROM, performing 551 | /// sanity checking, etc. 552 | /// \{ 553 | 554 | /// Data from calibrating with emitters on. 555 | CalibrationData calibrationOn; 556 | 557 | /// Data from calibrating with emitters off. 558 | CalibrationData calibrationOff; 559 | 560 | /// \} 561 | 562 | private: 563 | 564 | uint16_t emittersOnWithPin(uint8_t pin); 565 | 566 | // Handles the actual calibration, including (re)allocating and 567 | // initializing the storage for the calibration values if necessary. 568 | void calibrateOnOrOff(CalibrationData & calibration, QTRReadMode mode); 569 | 570 | void readPrivate(uint16_t * sensorValues, uint8_t start = 0, uint8_t step = 1); 571 | 572 | uint16_t readLinePrivate(uint16_t * sensorValues, QTRReadMode mode, bool invertReadings); 573 | 574 | QTRType _type = QTRType::Undefined; 575 | 576 | uint8_t * _sensorPins = nullptr; 577 | uint8_t _sensorCount = 0; 578 | 579 | uint16_t _timeout = QTRRCDefaultTimeout; // only used for RC sensors 580 | uint16_t _maxValue = QTRRCDefaultTimeout; // the maximum value returned by readPrivate() 581 | uint8_t _samplesPerSensor = 4; // only used for analog sensors 582 | 583 | uint8_t _oddEmitterPin = QTRNoEmitterPin; // also used for single emitter pin 584 | uint8_t _evenEmitterPin = QTRNoEmitterPin; 585 | uint8_t _emitterPinCount = 0; 586 | 587 | bool _dimmable = true; 588 | uint8_t _dimmingLevel = 0; 589 | 590 | uint16_t _lastPosition = 0; 591 | }; 592 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Balboa32U4 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 Balboa 32U4 control board, which is part of the [Balboa 32U4 balancing robot](https://www.pololu.com/category/210/balboa-robot-and-acessories). The board is based on the Arduino-compatible ATmega32U4 MCU and integrates motor drivers, encoders, buzzer, buttons, an LSM6DS33 accelerometer and gyro, and an LIS3MDL compass. 8 | 9 | The library also makes it easier to interface with the optional [5-Channel reflectance sensor array](https://www.pololu.com/product/3577) that you can add to the Balboa 32U4. 10 | 11 | This library does not include code for accessing the LSM6DS33 or LIS3MDL. If you want to access those sensors, you should install the separate [LSM6](https://github.com/pololu/lsm6-arduino) and [LIS3MDL](https://github.com/pololu/lis3mdl-arduino) libraries. 12 | 13 | This library is very similar to the [Romi32U4](https://github.com/pololu/romi-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 "Balboa32U4". 21 | 3. Click the Balboa32U4 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/balboa-32u4-arduino-library) and decompress it. 27 | 2. Rename the folder "balboa-32u4-arduino-library-master" to "Balboa32U4". 28 | 3. Move the "Balboa32U4" 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 "Balboa32U4". 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 | * Balboa32U4ButtonA 40 | * Balboa32U4ButtonB 41 | * Balboa32U4ButtonC 42 | * Balboa32U4Buzzer 43 | * Balboa32U4Encoders 44 | * Balboa32U4LCD 45 | * Balboa32U4LineSensors 46 | * Balboa32U4Motors 47 | * ledRed() 48 | * ledGreen() 49 | * ledYellow() 50 | * usbPowerPresent() 51 | * readBatteryMillivolts() 52 | 53 | ## Component libraries 54 | 55 | This library also includes copies of several other Arduino libraries inside it which are used to help implement the classes and functions above. 56 | 57 | * [FastGPIO](https://github.com/pololu/fastgpio-arduino) 58 | * [PololuBuzzer](https://github.com/pololu/pololu-buzzer-arduino) 59 | * [PololuHD44780](https://github.com/pololu/pololu-hd44780-arduino) 60 | * [Pushbutton](https://github.com/pololu/pushbutton-arduino) 61 | * [QTRSensors](https://github.com/pololu/qtr-sensors-arduino) 62 | * [USBPause](https://github.com/pololu/usb-pause-arduino) 63 | 64 | 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. 65 | 66 | 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: 67 | 68 | ~~~{.cpp} 69 | #include 70 | ~~~ 71 | 72 | ## Documentation 73 | 74 | For complete documentation, see https://pololu.github.io/balboa-32u4-arduino-library. If you are already on that page, then click on the links in the "Classes and functions" section above. 75 | 76 | ## Version history 77 | 78 | * 1.1.2 (2022-09-06): Fixed a bug in the Encoders demo that could prevent encoder errors from being shown properly on the display. 79 | * 1.1.1 (2019-04-05): Fixed narrowing conversion warning/error in Demo.ino. 80 | * 1.1.0 (2019-04-04): Added Balboa32U4LineSensors class and LineSensorTest example. Improved Balancer example to measure lying down angle and more effectively decide whether it is balancing. 81 | * 1.0.1 (2017-07-17): Fixed a bug that caused errors from the right encoder to get reported as errors from the left encoder. 82 | * 1.0.0 (2017-03-08): Original release. 83 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /components.txt: -------------------------------------------------------------------------------- 1 | https://github.com/pololu/fastgpio-arduino 2.0.0-1-ga4ecf04 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-3-ge9fca83 6 | https://github.com/pololu/qtr-sensors-arduino 4.0.0-1-gbaccd9a 7 | -------------------------------------------------------------------------------- /examples/Balancer/Balance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Balance.h" 3 | 4 | int32_t gYZero; 5 | int32_t angle; // millidegrees 6 | int32_t angleRate; // degrees/s 7 | int32_t distanceLeft; 8 | int32_t speedLeft; 9 | int32_t driveLeft; 10 | int32_t distanceRight; 11 | int32_t speedRight; 12 | int32_t driveRight; 13 | int16_t motorSpeed; 14 | bool isBalancingStatus = false; 15 | bool balanceUpdateDelayedStatus; 16 | 17 | bool isBalancing() 18 | { 19 | return isBalancingStatus; 20 | } 21 | 22 | bool balanceUpdateDelayed() 23 | { 24 | return balanceUpdateDelayedStatus; 25 | } 26 | 27 | void balanceSetup() 28 | { 29 | // Initialize IMU. 30 | Wire.begin(); 31 | if (!imu.init()) 32 | { 33 | while(true) 34 | { 35 | Serial.println("Failed to detect and initialize IMU!"); 36 | delay(200); 37 | } 38 | } 39 | imu.enableDefault(); 40 | imu.writeReg(LSM6::CTRL2_G, 0b01011000); // 208 Hz, 1000 deg/s 41 | 42 | // Wait for IMU readings to stabilize. 43 | delay(1000); 44 | 45 | // Calibrate the gyro. 46 | int32_t total = 0; 47 | for (int i = 0; i < CALIBRATION_ITERATIONS; i++) 48 | { 49 | imu.read(); 50 | total += imu.g.y; 51 | delay(1); 52 | } 53 | 54 | gYZero = total / CALIBRATION_ITERATIONS; 55 | } 56 | 57 | // This function contains the core algorithm for balancing a 58 | // Balboa 32U4 robot. 59 | void balance() 60 | { 61 | // Adjust toward angle=0 with timescale ~10s, to compensate for 62 | // gyro drift. More advanced AHRS systems use the 63 | // accelerometer as a reference for finding the zero angle, but 64 | // this is a simpler technique: for a balancing robot, as long 65 | // as it is balancing, we know that the angle must be zero on 66 | // average, or we would fall over. 67 | angle = angle * 999 / 1000; 68 | 69 | // This variable measures how close we are to our basic 70 | // balancing goal - being on a trajectory that would cause us 71 | // to rise up to the vertical position with zero speed left at 72 | // the top. This is similar to the fallingAngleOffset used 73 | // for LED feedback and a calibration procedure discussed at 74 | // the end of Balancer.ino. 75 | // 76 | // It is in units of millidegrees, like the angle variable, and 77 | // you can think of it as an angular estimate of how far off we 78 | // are from being balanced. 79 | int32_t risingAngleOffset = angleRate * ANGLE_RATE_RATIO + angle; 80 | 81 | // Combine risingAngleOffset with the distance and speed 82 | // variables, using the calibration constants defined in 83 | // Balance.h, to get our motor response. Rather than becoming 84 | // the new motor speed setting, the response is an amount that 85 | // is added to the motor speeds, since a *change* in speed is 86 | // what causes the robot to tilt one way or the other. 87 | motorSpeed += ( 88 | + ANGLE_RESPONSE * risingAngleOffset 89 | + DISTANCE_RESPONSE * (distanceLeft + distanceRight) 90 | + SPEED_RESPONSE * (speedLeft + speedRight) 91 | ) / 100 / GEAR_RATIO; 92 | 93 | if (motorSpeed > MOTOR_SPEED_LIMIT) 94 | { 95 | motorSpeed = MOTOR_SPEED_LIMIT; 96 | } 97 | if (motorSpeed < -MOTOR_SPEED_LIMIT) 98 | { 99 | motorSpeed = -MOTOR_SPEED_LIMIT; 100 | } 101 | 102 | // Adjust for differences in the left and right distances; this 103 | // will prevent the robot from rotating as it rocks back and 104 | // forth due to differences in the motors, and it allows the 105 | // robot to perform controlled turns. 106 | int16_t distanceDiff = distanceLeft - distanceRight; 107 | 108 | motors.setSpeeds( 109 | motorSpeed + distanceDiff * DISTANCE_DIFF_RESPONSE / 100, 110 | motorSpeed - distanceDiff * DISTANCE_DIFF_RESPONSE / 100); 111 | } 112 | 113 | void lyingDown() 114 | { 115 | // Reset things so it doesn't go crazy. 116 | motorSpeed = 0; 117 | distanceLeft = 0; 118 | distanceRight = 0; 119 | motors.setSpeeds(0, 0); 120 | 121 | if (angleRate > -2 && angleRate < 2) 122 | { 123 | // It's really calm, so use the accelerometer to measure the 124 | // robot's rest angle. The atan2 function returns a result 125 | // in radians, so we multiply it by 180000/pi to convert it 126 | // to millidegrees. 127 | angle = atan2(imu.a.z, imu.a.x) * 57296; 128 | 129 | distanceLeft = 0; 130 | distanceRight = 0; 131 | } 132 | } 133 | 134 | void integrateGyro() 135 | { 136 | // Convert from full-scale 1000 deg/s to deg/s. 137 | angleRate = (imu.g.y - gYZero) / 29; 138 | 139 | angle += angleRate * UPDATE_TIME_MS; 140 | } 141 | 142 | void integrateEncoders() 143 | { 144 | static int16_t lastCountsLeft; 145 | int16_t countsLeft = encoders.getCountsLeft(); 146 | speedLeft = (countsLeft - lastCountsLeft); 147 | distanceLeft += countsLeft - lastCountsLeft; 148 | lastCountsLeft = countsLeft; 149 | 150 | static int16_t lastCountsRight; 151 | int16_t countsRight = encoders.getCountsRight(); 152 | speedRight = (countsRight - lastCountsRight); 153 | distanceRight += countsRight - lastCountsRight; 154 | lastCountsRight = countsRight; 155 | } 156 | 157 | void balanceDrive(int16_t leftSpeed, int16_t rightSpeed) 158 | { 159 | driveLeft = leftSpeed; 160 | driveRight = rightSpeed; 161 | } 162 | 163 | void balanceDoDriveTicks() 164 | { 165 | distanceLeft -= driveLeft; 166 | distanceRight -= driveRight; 167 | speedLeft -= driveLeft; 168 | speedRight -= driveRight; 169 | } 170 | 171 | void balanceResetEncoders() 172 | { 173 | distanceLeft = 0; 174 | distanceRight = 0; 175 | } 176 | 177 | void balanceUpdateSensors() 178 | { 179 | imu.read(); 180 | integrateGyro(); 181 | integrateEncoders(); 182 | } 183 | 184 | void balanceUpdate() 185 | { 186 | static uint16_t lastMillis; 187 | uint16_t ms = millis(); 188 | static uint8_t count = 0; 189 | 190 | // Perform the balance updates at 100 Hz. 191 | if ((uint16_t)(ms - lastMillis) < UPDATE_TIME_MS) { return; } 192 | balanceUpdateDelayedStatus = ms - lastMillis > UPDATE_TIME_MS + 1; 193 | lastMillis = ms; 194 | 195 | balanceUpdateSensors(); 196 | balanceDoDriveTicks(); 197 | 198 | if (isBalancingStatus) 199 | { 200 | balance(); 201 | 202 | // Stop trying to balance if we have been farther from 203 | // vertical than STOP_BALANCING_ANGLE for 5 counts. 204 | if (abs(angle) > STOP_BALANCING_ANGLE) 205 | { 206 | if (++count > 5) 207 | { 208 | isBalancingStatus = false; 209 | count = 0; 210 | } 211 | } 212 | else 213 | { 214 | count = 0; 215 | } 216 | } 217 | else 218 | { 219 | lyingDown(); 220 | 221 | // Start trying to balance if we have been closer to 222 | // vertical than START_BALANCING_ANGLE for 5 counts. 223 | if (abs(angle) < START_BALANCING_ANGLE) 224 | { 225 | if (++count > 5) 226 | { 227 | isBalancingStatus = true; 228 | count = 0; 229 | } 230 | } 231 | else 232 | { 233 | count = 0; 234 | } 235 | } 236 | } 237 | -------------------------------------------------------------------------------- /examples/Balancer/Balance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // This code was developed for a Balboa unit using 50:1 motors 8 | // and 45:21 plastic gears, for an overall gear ratio of 111. 9 | // Adjust the ratio below to scale various constants in the 10 | // balancing algorithm to match your robot. 11 | const int16_t GEAR_RATIO = 111; 12 | 13 | // This constant limits the maximum motor speed. If your gear 14 | // ratio is lower than what we used, or if you are testing 15 | // changes to the code, you might want to reduce it to prevent 16 | // your robot from zooming away when things go wrong. 17 | // 18 | // If you want to use speeds faster than 300, you should add 19 | // the line "motors.allowTurbo(true);" to setup(). 20 | const int16_t MOTOR_SPEED_LIMIT = 300; 21 | 22 | // This constant relates the angle to its rate of change for a 23 | // robot that is falling from a nearly-vertical position or 24 | // rising up to that position. The relationship is nearly 25 | // linear. For example, if you have the 80mm wheels it should be 26 | // about 140, which means that the angle in millidegrees is ~140 27 | // times its rate of change in degrees per second; when the robot 28 | // has fallen by 90 degrees it will be moving at about 29 | // 90,000/140 = 642 deg/s. See the end of Balancer.ino for one 30 | // way to calibrate this value. 31 | const int16_t ANGLE_RATE_RATIO = 140; 32 | 33 | // The following three constants define a PID-like algorithm for 34 | // balancing. Each one determines how much the motors will 35 | // respond to the corresponding variable being off from zero. 36 | // See the code in Balance.cpp for exactly how they are used. To 37 | // get it balancing from scratch, start with them all at zero and 38 | // adjust them as follows: 39 | 40 | // ANGLE_RESPONSE determines the response to a combination of 41 | // angle and angle_rate; the combination measures how far the 42 | // robot is from a stable trajectory. To test this, use your 43 | // hand to flick the robot up from a resting position. With a 44 | // value that is too low, it won't stop itself in time; a high 45 | // value will cause it to slam back into the ground or oscillate 46 | // wildly back and forth. When ANGLE_RESPONSE is adjusted 47 | // properly, the robot will move just enough to stop itself. 48 | // However, after stopping itself, it will be moving and keep 49 | // moving in the same direction, usually driving faster and 50 | // faster until it reaches its maximum motor speed and falls 51 | // over. That's where the next constants come in. 52 | const int16_t ANGLE_RESPONSE = 11; 53 | 54 | // DISTANCE_RESPONSE determines how much the robot resists being 55 | // moved away from its starting point. Counterintuitively, this 56 | // constant is positive: to move forwards, the robot actually has 57 | // to first roll its wheels backwards, so that it can *fall* 58 | // forwards. When this constant is adjusted properly, the robot 59 | // will no longer zoom off in one direction, but it will drive 60 | // back and forth a few times before falling down. 61 | const int16_t DISTANCE_RESPONSE = 73; 62 | 63 | // DISTANCE_DIFF_RESPONSE determines the response to differences 64 | // between the left and right motors, preventing undesired 65 | // rotation due to differences in the motors and gearing. Unlike 66 | // DISTANCE_REPONSE, it should be negative: if the left motor is 67 | // lagging, we need to increase its speed and decrease the speed 68 | // of the right motor. If this constant is too small, the robot 69 | // will spin left and right as it rocks back and forth; if it is 70 | // too large it will become unstable. 71 | const int16_t DISTANCE_DIFF_RESPONSE = -50; 72 | 73 | // SPEED_RESPONSE supresses the large back-and-forth oscillations 74 | // caused by DISTANCE_RESPONSE. Increase this until these 75 | // oscillations die down after a few cycles; but if you increase 76 | // it too much it will tend to shudder or vibrate wildly. 77 | const int16_t SPEED_RESPONSE = 3300; 78 | 79 | // The balancing code is all based on a 100 Hz update rate; if 80 | // you change this, you will have to adjust many other things. 81 | const uint8_t UPDATE_TIME_MS = 10; 82 | 83 | // Take 100 measurements initially to calibrate the gyro. 84 | const uint8_t CALIBRATION_ITERATIONS = 100; 85 | 86 | // These values represent the angles from vertical, in 87 | // millidegrees, at which the Balboa will start and stop trying 88 | // to balance. They are different to add some hysteresis so that 89 | // the robot has to be relatively more upright before beginning 90 | // to balance but can fall a little farther before giving up. 91 | // The default values make the Balboa start balancing at 92 | // 45 degrees from vertical and stop balancing at 70 degrees from 93 | // vertical. 94 | const int32_t START_BALANCING_ANGLE = 45000; 95 | const int32_t STOP_BALANCING_ANGLE = 70000; 96 | 97 | // These variables will be accessible from your sketch. 98 | extern int32_t angle; // units: millidegrees 99 | extern int32_t angleRate; // units: degrees/s (or millidegrees/ms) 100 | extern int16_t motorSpeed; // current (average) motor speed setting 101 | 102 | // These variables must be defined in your sketch. 103 | extern LSM6 imu; 104 | extern Balboa32U4Motors motors; 105 | extern Balboa32U4Encoders encoders; 106 | 107 | // Call this in your setup() to initialize and calibrate the IMU. 108 | void balanceSetup(); 109 | 110 | // Call this in loop() to run the full balancing algorithm. 111 | void balanceUpdate(); 112 | 113 | // Call this function to set a driving speed in ticks/ms. The 114 | // way it works is that every update cycle we adjust the robot's 115 | // encoder measurements, which will cause it to drive in the 116 | // corresponding direction. Differing values for left and right 117 | // will result in a turn. 118 | void balanceDrive(int16_t leftSpeed, int16_t rightSpeed); 119 | 120 | // Returns true if (according to the balancing algorithm) the 121 | // robot is trying to balance. When it falls down it shuts off 122 | // the motors, and this function will return false. If you pick 123 | // the robot up, this function will start returning true again. 124 | bool isBalancing(); 125 | 126 | // Returns true if the last update cycle was delayed to more than 127 | // UPDATE_TIME_MS+1 milliseconds. This could indicate 128 | // computations being too long or interrupts that are delaying 129 | // the loop. 130 | bool balanceUpdateDelayed(); 131 | 132 | // Sometimes you will want to take control of the motors but keep 133 | // updating the balancing code's encoders and angle measurements 134 | // so you don't lose track of the robot's position and angle. 135 | // Call this every 10ms (UPDATE_TIME_MS) to update the sensors, 136 | // and you will be able to resume balancing immediately when you 137 | // are done. 138 | void balanceUpdateSensors(); 139 | 140 | // Call this function to reset the encoders. This is useful 141 | // after a large motion, so that robot does not try to make a 142 | // huge correction to get back to "zero". 143 | void balanceResetEncoders(); 144 | -------------------------------------------------------------------------------- /examples/Balancer/Balancer.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to make a Balboa balance on its two 2 | // wheels and drive around while balancing. 3 | // 4 | // To run this demo, you will need to install the LSM6 library: 5 | // 6 | // https://github.com/pololu/lsm6-arduino 7 | // 8 | // To use this demo, place the robot on the ground with the 9 | // circuit board facing up, and then turn it on. Be careful to 10 | // not move the robot for a few seconds after powering it on, 11 | // because that is when the gyro is calibrated. During the gyro 12 | // calibration, the red LED is lit. After the red LED turns off, 13 | // turn the robot so that it is standing up. It will detect that 14 | // you have turned it and start balancing. 15 | // 16 | // Alternatively, you can press the A button while the robot is 17 | // lying down and it will try to use its motors to kick up into 18 | // the balancing position. 19 | // 20 | // This demo is tuned for the 50:1 high-power gearmotor with 21 | // carbon brushes, 45:21 plastic gears, and 80mm wheels; you will 22 | // need to adjust the parameters in Balance.h for your robot. 23 | // 24 | // After you have gotten the robot balance well, you can 25 | // uncomment some lines in loop() to make it drive around and 26 | // play a song. 27 | 28 | #include 29 | #include 30 | #include 31 | #include "Balance.h" 32 | 33 | LSM6 imu; 34 | Balboa32U4Motors motors; 35 | Balboa32U4Encoders encoders; 36 | Balboa32U4Buzzer buzzer; 37 | Balboa32U4ButtonA buttonA; 38 | Balboa32U4ButtonB buttonB; 39 | Balboa32U4ButtonC buttonC; 40 | 41 | void setup() 42 | { 43 | // Uncomment these lines if your motors are reversed. 44 | // motors.flipLeftMotor(true); 45 | // motors.flipRightMotor(true); 46 | 47 | ledYellow(0); 48 | ledRed(1); 49 | balanceSetup(); 50 | ledRed(0); 51 | } 52 | 53 | const char song[] PROGMEM = 54 | "!O6 T240" 55 | "l32ab-b>cl8r br b-bb-a a-r gr g-4 g4" 56 | "a-r gr g-gg-f er e-r d4 e-4" 57 | "gr msd8d8ml d-4d4" 58 | "l32efg-gl8r msd8d8ml d-4d4" 59 | "grms>g16>g16>g2"); 101 | ledGreen(1); 102 | ledRed(1); 103 | ledYellow(1); 104 | while (buzzer.isPlaying()); 105 | motors.setSpeeds(-MOTOR_SPEED_LIMIT, -MOTOR_SPEED_LIMIT); 106 | delay(400); 107 | motors.setSpeeds(150, 150); 108 | for (uint8_t i = 0; i < 20; i++) 109 | { 110 | delay(UPDATE_TIME_MS); 111 | balanceUpdateSensors(); 112 | if(angle < 60000) 113 | { 114 | break; 115 | } 116 | } 117 | motorSpeed = 150; 118 | balanceResetEncoders(); 119 | } 120 | 121 | void loop() 122 | { 123 | static bool enableSong = false; 124 | static bool enableDrive = false; 125 | 126 | balanceUpdate(); 127 | 128 | if (isBalancing()) 129 | { 130 | if (enableSong) { playSong(); } 131 | if (enableDrive) { driveAround(); } 132 | } 133 | else 134 | { 135 | buzzer.stopPlaying(); 136 | balanceDrive(0, 0); // reset driving speeds 137 | 138 | if (buttonA.getSingleDebouncedPress()) 139 | { 140 | enableSong = false; 141 | enableDrive = false; 142 | standUp(); 143 | } 144 | else if (buttonB.getSingleDebouncedPress()) 145 | { 146 | enableSong = false; 147 | enableDrive = true; 148 | standUp(); 149 | } 150 | else if (buttonC.getSingleDebouncedPress()) 151 | { 152 | enableSong = true; 153 | enableDrive = true; 154 | standUp(); 155 | } 156 | } 157 | 158 | // Illuminate the red LED if the last full update was too slow. 159 | ledRed(balanceUpdateDelayed()); 160 | 161 | // Display feedback on the yellow and green LEDs depending on 162 | // the variable fallingAngleOffset. This variable is similar 163 | // to the risingAngleOffset used in Balance.cpp. 164 | // 165 | // When the robot is rising toward vertical (not falling), 166 | // angleRate and angle have opposite signs, so this variable 167 | // will just be positive or negative depending on which side of 168 | // vertical it is on. 169 | // 170 | // When the robot is falling, the variable measures how far off 171 | // it is from a trajectory starting it almost perfectly 172 | // balanced then falling to one side or the other with the 173 | // motors off. 174 | // 175 | // Since this depends on ANGLE_RATE_RATIO, it is useful for 176 | // calibration. If you have changed the wheels or added weight 177 | // to your robot, you can try checking these items, with the 178 | // motor power OFF (powered by USB): 179 | // 180 | // 1. Try letting the robot fall with the Balboa 32U4 PCB up. 181 | // The green LED should remain lit the entire time. If it 182 | // sometimes shows yellow instead of green, reduce 183 | // ANGLE_RATE_RATIO. 184 | // 185 | // 2. If it is tilted beyond vertical and given a push back to 186 | // the PCB-up side again, the yellow LED should remain lit 187 | // until it hits the ground. If you see green, increase 188 | // ANGLE_RATE_RATIO. 189 | // 190 | // In practice, it is hard to achieve both 1 and 2 perfectly, 191 | // but if you can get close, your constant will probably be 192 | // good enough for balancing. 193 | int32_t fallingAngleOffset = angleRate * ANGLE_RATE_RATIO - angle; 194 | if (fallingAngleOffset > 0) 195 | { 196 | ledYellow(1); 197 | ledGreen(0); 198 | } 199 | else 200 | { 201 | ledYellow(0); 202 | ledGreen(1); 203 | } 204 | } 205 | -------------------------------------------------------------------------------- /examples/BlinkLEDs/BlinkLEDs.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to blink the three user LEDs on the 2 | // Balboa 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 | -------------------------------------------------------------------------------- /examples/Buttons/Buttons.ino: -------------------------------------------------------------------------------- 1 | // This example demonstrates three different ways to 2 | // interface with a user pushbutton on the Balboa 32U4. 3 | 4 | #include 5 | 6 | // These objects provide access to the Balboa 32U4's 7 | // on-board buttons. 8 | Balboa32U4ButtonA buttonA; 9 | Balboa32U4ButtonB buttonB; 10 | Balboa32U4ButtonC buttonC; 11 | Balboa32U4LCD 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/BuzzerBasics/BuzzerBasics.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to make sounds with the buzzer on the 2 | // Balboa 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 | Balboa32U4Buzzer 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# 18 | #include 19 | #include 20 | #include 21 | 22 | Balboa32U4LCD lcd; 23 | Balboa32U4Buzzer buzzer; 24 | Balboa32U4ButtonA buttonA; 25 | Balboa32U4ButtonB buttonB; 26 | Balboa32U4ButtonC buttonC; 27 | LSM6 imu; 28 | LIS3MDL mag; 29 | Balboa32U4Motors motors; 30 | Balboa32U4Encoders encoders; 31 | 32 | char buttonMonitor(); 33 | 34 | // A couple of simple tunes, stored in program space. 35 | 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" 52 | "e>d>c#>db>d>c#b >c#agaegfe fO6dc#dfdc# 8) { height = 8; } 167 | static const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 168 | lcd.print(barChars[height]); 169 | } 170 | 171 | // The Menu class shows an interactive menu on the screen that 172 | // lets a user select an action and keeps track of the menu's 173 | // state. 174 | class Menu 175 | { 176 | public: 177 | struct Item 178 | { 179 | const char * name; 180 | void (* action)(); 181 | }; 182 | 183 | Menu(Item * items, uint8_t itemCount) 184 | { 185 | this->items = items; 186 | this->itemCount = itemCount; 187 | lcdItemIndex = 0; 188 | } 189 | 190 | void lcdUpdate(uint8_t index) 191 | { 192 | lcd.clear(); 193 | lcd.print(items[index].name); 194 | lcd.gotoXY(0, 1); 195 | 196 | // The string below uses special characters from the HD44780 197 | // interface datasheet. "\x7f" is a left arrow. 198 | // "\xa5" is a dot character. "\x7e" is a right arrow. 199 | lcd.print(F("\x7f" "A \xa5" "B C\x7e")); 200 | } 201 | 202 | void action(uint8_t index) 203 | { 204 | items[index].action(); 205 | } 206 | 207 | // Prompts the user to choose one of the menu items, 208 | // then runs it, then returns. 209 | void select() 210 | { 211 | lcdUpdate(lcdItemIndex); 212 | 213 | while (1) 214 | { 215 | switch (buttonMonitor()) 216 | { 217 | case 'A': 218 | // The A button was pressed so decrement the index. 219 | if (lcdItemIndex == 0) 220 | { 221 | lcdItemIndex = itemCount - 1; 222 | } 223 | else 224 | { 225 | lcdItemIndex--; 226 | } 227 | lcdUpdate(lcdItemIndex); 228 | break; 229 | 230 | case 'C': 231 | // The C button was pressed so increase the index. 232 | if (lcdItemIndex >= itemCount - 1) 233 | { 234 | lcdItemIndex = 0; 235 | } 236 | else 237 | { 238 | lcdItemIndex++; 239 | } 240 | lcdUpdate(lcdItemIndex); 241 | break; 242 | 243 | case 'B': 244 | // The B button was pressed so run the item and return. 245 | action(lcdItemIndex); 246 | return; 247 | } 248 | } 249 | } 250 | 251 | private: 252 | Item * items; 253 | uint8_t itemCount; 254 | uint8_t lcdItemIndex; 255 | }; 256 | 257 | 258 | // Blinks all three LEDs in sequence. 259 | void ledDemo() 260 | { 261 | displayBackArrow(); 262 | 263 | uint8_t state = 3; 264 | static uint16_t lastUpdateTime = millis() - 2000; 265 | while (buttonMonitor() != 'B') 266 | { 267 | if ((uint16_t)(millis() - lastUpdateTime) >= 500) 268 | { 269 | lastUpdateTime = millis(); 270 | state = state + 1; 271 | if (state >= 4) { state = 0; } 272 | 273 | switch (state) 274 | { 275 | case 0: 276 | buzzer.play("c32"); 277 | lcd.gotoXY(0, 0); 278 | lcd.print(F("Red ")); 279 | ledRed(1); 280 | ledGreen(0); 281 | ledYellow(0); 282 | break; 283 | 284 | case 1: 285 | buzzer.play("e32"); 286 | lcd.gotoXY(0, 0); 287 | lcd.print(F("Green")); 288 | ledRed(0); 289 | ledGreen(1); 290 | ledYellow(0); 291 | break; 292 | 293 | case 2: 294 | buzzer.play("g32"); 295 | lcd.gotoXY(0, 0); 296 | lcd.print(F("Yellow")); 297 | ledRed(0); 298 | ledGreen(0); 299 | ledYellow(1); 300 | break; 301 | } 302 | } 303 | } 304 | 305 | ledRed(0); 306 | ledYellow(0); 307 | ledGreen(0); 308 | } 309 | 310 | // Starts I2C and initializes the inertial sensors. 311 | void initInertialSensors() 312 | { 313 | Wire.begin(); 314 | imu.init(); 315 | imu.enableDefault(); 316 | mag.init(); 317 | mag.enableDefault(); 318 | 319 | // Set the gyro full scale to 1000 dps because the default 320 | // value is too low, and leave the other settings the same. 321 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 322 | 323 | // Set the accelerometer full scale to 16 g because the default 324 | // value is too low, and leave the other settings the same. 325 | imu.writeReg(LSM6::CTRL1_XL, 0b10000100); 326 | } 327 | 328 | // Given 3 readings for axes x, y, and z, prints the sign 329 | // and axis of the largest reading unless it is below the 330 | // given threshold. 331 | void printLargestAxis(int16_t x, int16_t y, int16_t z, uint16_t threshold) 332 | { 333 | int16_t largest = x; 334 | char axis = 'X'; 335 | 336 | if (abs(y) > abs(largest)) 337 | { 338 | largest = y; 339 | axis = 'Y'; 340 | } 341 | if (abs(z) > abs(largest)) 342 | { 343 | largest = z; 344 | axis = 'Z'; 345 | } 346 | 347 | if (abs(largest) < threshold) 348 | { 349 | lcd.print(" "); 350 | } 351 | else 352 | { 353 | bool positive = (largest > 0); 354 | lcd.print(positive ? '+' : '-'); 355 | lcd.print(axis); 356 | } 357 | } 358 | 359 | // Print the direction of the largest rotation rate measured 360 | // by the gyro and the up direction based on the 361 | // accelerometer's measurement of gravitational acceleration 362 | // (assuming gravity is the dominant force acting on the 363 | // Balboa). 364 | void inertialDemo() 365 | { 366 | displayBackArrow(); 367 | 368 | lcd.gotoXY(3, 0); 369 | lcd.print(F("Rot")); 370 | lcd.gotoXY(4, 1); 371 | lcd.print(F("Up")); 372 | 373 | while (buttonMonitor() != 'B') 374 | { 375 | imu.read(); 376 | 377 | lcd.gotoXY(6, 0); 378 | printLargestAxis(imu.g.x, imu.g.y, imu.g.z, 2000); 379 | lcd.gotoXY(6, 1); 380 | printLargestAxis(imu.a.x, imu.a.y, imu.a.z, 200); 381 | } 382 | } 383 | 384 | // Print the raw readings from the X and Y axes on the 385 | // magnetometer. (The magnetometer also has a Z axis which we do 386 | // not display because it would not fit and is less likely to be 387 | // useful.) 388 | void magnetometerDemo() 389 | { 390 | displayBackArrow(); 391 | 392 | while (buttonMonitor() != 'B') 393 | { 394 | mag.read(); 395 | 396 | char buffer[7]; 397 | 398 | sprintf(buffer, "%6d", mag.m.x); 399 | lcd.gotoXY(2, 0); 400 | lcd.print(buffer); 401 | 402 | sprintf(buffer, "%6d", mag.m.y); 403 | lcd.gotoXY(2, 1); 404 | lcd.print(buffer); 405 | } 406 | } 407 | 408 | // Provides an interface to test the motors. Holding button A or C 409 | // causes the left or right motor to accelerate; releasing the 410 | // button causes the motor to decelerate. Tapping the button while 411 | // the motor is not running reverses the direction it runs. 412 | // 413 | // If the showEncoders argument is true, encoder counts are 414 | // displayed on the first line of the LCD; otherwise, an 415 | // instructional message is shown. 416 | void motorDemoHelper(bool showEncoders) 417 | { 418 | loadCustomCharactersMotorDirs(); 419 | lcd.clear(); 420 | lcd.gotoXY(1, 1); 421 | lcd.print(F("A \7B C")); 422 | 423 | const uint16_t maxSpeed = 300; 424 | const uint8_t acceleration = 15; 425 | const uint8_t deceleration = 30; 426 | 427 | // Update the LCD and motors every 50 ms. 428 | const uint8_t updatePeriod = 50; 429 | 430 | int16_t leftSpeed = 0, rightSpeed = 0; 431 | int8_t leftDir = 1, rightDir = 1; 432 | uint16_t lastUpdateTime = millis() - 100; 433 | uint8_t btnCountA = 0, btnCountC = 0, instructCount = 0; 434 | 435 | int16_t encCountsLeft = 0, encCountsRight = 0; 436 | char buf[4]; 437 | 438 | while (buttonMonitor() != 'B') 439 | { 440 | encCountsLeft += encoders.getCountsAndResetLeft(); 441 | if (encCountsLeft < 0) { encCountsLeft += 1000; } 442 | if (encCountsLeft > 999) { encCountsLeft -= 1000; } 443 | 444 | encCountsRight += encoders.getCountsAndResetRight(); 445 | if (encCountsRight < 0) { encCountsRight += 1000; } 446 | if (encCountsRight > 999) { encCountsRight -= 1000; } 447 | 448 | if ((uint16_t)(millis() - lastUpdateTime) > updatePeriod) 449 | { 450 | lastUpdateTime = millis(); 451 | 452 | lcd.gotoXY(0, 0); 453 | if (showEncoders) 454 | { 455 | sprintf(buf, "%03d", encCountsLeft); 456 | lcd.print(buf); 457 | lcd.gotoXY(5, 0); 458 | sprintf(buf, "%03d", encCountsRight); 459 | lcd.print(buf); 460 | } 461 | else 462 | { 463 | // Cycle the instructions every 2 seconds. 464 | if (instructCount == 0) 465 | { 466 | lcd.print(F("Hold=run")); 467 | } 468 | else if (instructCount == 40) 469 | { 470 | lcd.print(F("Tap=flip")); 471 | } 472 | if (++instructCount == 80) { instructCount = 0; } 473 | } 474 | 475 | if (buttonA.isPressed()) 476 | { 477 | if (btnCountA < 4) 478 | { 479 | btnCountA++; 480 | } 481 | else 482 | { 483 | // Button has been held for more than 200 ms, so 484 | // start running the motor. 485 | leftSpeed += acceleration; 486 | } 487 | } 488 | else 489 | { 490 | if (leftSpeed == 0 && btnCountA > 0 && btnCountA < 4) 491 | { 492 | // Motor isn't running and button was pressed for 493 | // 200 ms or less, so flip the motor direction. 494 | leftDir = -leftDir; 495 | } 496 | btnCountA = 0; 497 | leftSpeed -= deceleration; 498 | } 499 | 500 | if (buttonC.isPressed()) 501 | { 502 | if (btnCountC < 4) 503 | { 504 | btnCountC++; 505 | } 506 | else 507 | { 508 | // Button has been held for more than 200 ms, so 509 | // start running the motor. 510 | rightSpeed += acceleration; 511 | } 512 | } 513 | else 514 | { 515 | if (rightSpeed == 0 && btnCountC > 0 && btnCountC < 4) 516 | { 517 | // Motor isn't running and button was pressed for 518 | // 200 ms or less, so flip the motor direction. 519 | rightDir = -rightDir; 520 | } 521 | btnCountC = 0; 522 | rightSpeed -= deceleration; 523 | } 524 | 525 | leftSpeed = constrain(leftSpeed, 0, maxSpeed); 526 | rightSpeed = constrain(rightSpeed, 0, maxSpeed); 527 | 528 | motors.setSpeeds(leftSpeed * leftDir, rightSpeed * rightDir); 529 | 530 | // Display arrows pointing the appropriate direction 531 | // (solid if the motor is running, chevrons if not). 532 | lcd.gotoXY(0, 1); 533 | if (leftSpeed == 0) 534 | { 535 | lcd.print((leftDir > 0) ? '\0' : '\1'); 536 | } 537 | else 538 | { 539 | lcd.print((leftDir > 0) ? '\2' : '\3'); 540 | } 541 | lcd.gotoXY(7, 1); 542 | if (rightSpeed == 0) 543 | { 544 | lcd.print((rightDir > 0) ? '\0' : '\1'); 545 | } 546 | else 547 | { 548 | lcd.print((rightDir > 0) ? '\2' : '\3'); 549 | } 550 | } 551 | } 552 | motors.setSpeeds(0, 0); 553 | } 554 | 555 | 556 | // Motor demo with instructions. 557 | void motorDemo() 558 | { 559 | motorDemoHelper(false); 560 | } 561 | 562 | // Motor demo with encoder counts. 563 | void encoderDemo() 564 | { 565 | motorDemoHelper(true); 566 | } 567 | 568 | // Play a song on the buzzer and display its title. 569 | void musicDemo() 570 | { 571 | displayBackArrow(); 572 | 573 | uint8_t fugueTitlePos = 0; 574 | uint16_t lastShiftTime = millis() - 2000; 575 | 576 | while (buttonMonitor() != 'B') 577 | { 578 | // Shift the song title to the left every 250 ms. 579 | if ((uint16_t)(millis() - lastShiftTime) > 250) 580 | { 581 | lastShiftTime = millis(); 582 | 583 | lcd.gotoXY(0, 0); 584 | for (uint8_t i = 0; i < 8; i++) 585 | { 586 | char c = pgm_read_byte(fugueTitle + fugueTitlePos + i); 587 | lcd.print(c); 588 | } 589 | fugueTitlePos++; 590 | 591 | if (fugueTitlePos + 8 >= strlen(fugueTitle)) 592 | { 593 | fugueTitlePos = 0; 594 | } 595 | } 596 | 597 | if (!buzzer.isPlaying()) 598 | { 599 | buzzer.playFromProgramSpace(fugue); 600 | } 601 | } 602 | } 603 | 604 | // Display the the battery (VIN) voltage and indicate whether USB 605 | // power is detected. 606 | void powerDemo() 607 | { 608 | displayBackArrow(); 609 | 610 | uint16_t lastDisplayTime = millis() - 2000; 611 | char buf[6]; 612 | 613 | while (buttonMonitor() != 'B') 614 | { 615 | if ((uint16_t)(millis() - lastDisplayTime) > 250) 616 | { 617 | bool usbPower = usbPowerPresent(); 618 | 619 | uint16_t batteryLevel = readBatteryMillivolts(); 620 | 621 | lastDisplayTime = millis(); 622 | lcd.gotoXY(0, 0); 623 | sprintf(buf, "%5d", batteryLevel); 624 | lcd.print(buf); 625 | lcd.print(F(" mV")); 626 | lcd.gotoXY(3, 1); 627 | lcd.print(F("USB=")); 628 | lcd.print(usbPower ? 'Y' : 'N'); 629 | } 630 | } 631 | } 632 | 633 | Menu::Item mainMenuItems[] = { 634 | { "LEDs", ledDemo }, 635 | { "Inertial", inertialDemo }, 636 | { "Magnet", magnetometerDemo }, 637 | { "Motors", motorDemo }, 638 | { "Encoders", encoderDemo }, 639 | { "Music", musicDemo }, 640 | { "Power", powerDemo }, 641 | }; 642 | Menu mainMenu(mainMenuItems, 7); 643 | 644 | // This function watches for button presses. If a button is 645 | // pressed, it beeps a corresponding beep and it returns 'A', 646 | // 'B', or 'C' depending on what button was pressed. If no 647 | // button was pressed, it returns 0. This function is meant to 648 | // be called repeatedly in a loop. 649 | char buttonMonitor() 650 | { 651 | if (buttonA.getSingleDebouncedPress()) 652 | { 653 | buzzer.playFromProgramSpace(beepButtonA); 654 | return 'A'; 655 | } 656 | 657 | if (buttonB.getSingleDebouncedPress()) 658 | { 659 | buzzer.playFromProgramSpace(beepButtonB); 660 | return 'B'; 661 | } 662 | 663 | if (buttonC.getSingleDebouncedPress()) 664 | { 665 | buzzer.playFromProgramSpace(beepButtonC); 666 | return 'C'; 667 | } 668 | 669 | return 0; 670 | } 671 | 672 | void setup() 673 | { 674 | initInertialSensors(); 675 | 676 | loadCustomCharacters(); 677 | 678 | bool brownout = MCUSR >> BORF & 1; 679 | MCUSR = 0; 680 | if (brownout) 681 | { 682 | // The AVR was reset by a brownout reset 683 | // (VCC dropped below 4.3 V). 684 | // Play a special sound and display a note to the user. 685 | 686 | buzzer.playFromProgramSpace(beepBrownout); 687 | lcd.clear(); 688 | lcd.print(F("Brownout")); 689 | lcd.gotoXY(0, 1); 690 | lcd.print(F(" reset! ")); 691 | delay(1000); 692 | } 693 | else 694 | { 695 | buzzer.playFromProgramSpace(beepWelcome); 696 | } 697 | 698 | ledYellow(1); 699 | 700 | lcd.clear(); 701 | lcd.print(F(" Balboa")); 702 | lcd.gotoXY(2, 1); 703 | lcd.print(F("32U4")); 704 | 705 | delay(1000); 706 | 707 | lcd.clear(); 708 | lcd.print(F("Demo")); 709 | lcd.gotoXY(0, 1); 710 | lcd.print(F("Program")); 711 | delay(1000); 712 | 713 | lcd.clear(); 714 | lcd.print(F("Use B to")); 715 | lcd.gotoXY(0, 1); 716 | lcd.print(F("select.")); 717 | delay(1000); 718 | 719 | lcd.clear(); 720 | lcd.print(F("Press B")); 721 | lcd.gotoXY(0, 1); 722 | lcd.print(F("-try it!")); 723 | 724 | while (buttonMonitor() != 'B'){} 725 | 726 | ledYellow(0); 727 | 728 | buzzer.playFromProgramSpace(beepThankYou); 729 | lcd.clear(); 730 | lcd.print(F(" Thank")); 731 | lcd.gotoXY(0, 1); 732 | lcd.print(F(" you!")); 733 | delay(1000); 734 | } 735 | 736 | // This function prompts the user to choose something from the 737 | // main menu, runs their selection, and then returns. 738 | void mainMenuSelect() 739 | { 740 | lcd.clear(); 741 | lcd.print(F(" Main")); 742 | lcd.gotoXY(0, 1); 743 | lcd.print(F(" Menu")); 744 | delay(1000); 745 | mainMenu.select(); 746 | } 747 | 748 | void loop() 749 | { 750 | mainMenuSelect(); 751 | } 752 | -------------------------------------------------------------------------------- /examples/Encoders/Encoders.ino: -------------------------------------------------------------------------------- 1 | // This program shows how to read the encoders on the Balboa 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 Balboa 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 | Balboa32U4Encoders encoders; 26 | Balboa32U4LCD lcd; 27 | Balboa32U4Buzzer buzzer; 28 | Balboa32U4Motors motors; 29 | Balboa32U4ButtonA buttonA; 30 | Balboa32U4ButtonC 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 | -------------------------------------------------------------------------------- /examples/InertialSensors/InertialSensors.ino: -------------------------------------------------------------------------------- 1 | // This example reads the raw values from the LSM6DS33 2 | // accelerometer and gyro and and the LIS3MDL magnetometer on the 3 | // Balboa 32U4, and prints those raw values to the serial 4 | // monitor. 5 | // 6 | // The accelerometer readings can be converted to units of g 7 | // using the conversion factors specified in the "Mechanical 8 | // characteristics" table in the LSM6DS33 datasheet. We use a 9 | // full scale (FS) setting of +/- 16 g, so the conversion factor 10 | // is 0.488 mg/LSB (least-significant bit). A raw reading of 11 | // 2048 would correspond to 1 g. 12 | // 13 | // The gyro readings can be converted to degrees per second (dps) 14 | // using the "Mechanical characteristics" table in the LSM6DS33 15 | // datasheet. We use a full scale (FS) of +/- 1000 dps so the 16 | // conversion factor is 35 mdps/LSB. A raw reading of 2571 17 | // would correspond to 90 dps. 18 | // 19 | // The magnetometer readings are more difficult to interpret and 20 | // will usually require calibration. 21 | // 22 | // To run this sketch, you will need to install the LSM6 and 23 | // LIS3MDL libraries: 24 | // 25 | // https://github.com/pololu/lsm6-arduino 26 | // https://github.com/pololu/lis3mdl-arduino 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | LSM6 imu; 34 | LIS3MDL mag; 35 | 36 | char report[120]; 37 | 38 | void setup() 39 | { 40 | Wire.begin(); 41 | 42 | if (!mag.init()) 43 | { 44 | // Failed to detect the LIS3MDL. 45 | ledRed(1); 46 | while(1) 47 | { 48 | Serial.println(F("Failed to detect the LIS3MDL.")); 49 | delay(100); 50 | } 51 | } 52 | 53 | mag.enableDefault(); 54 | 55 | if (!imu.init()) 56 | { 57 | // Failed to detect the LSM6. 58 | ledRed(1); 59 | while(1) 60 | { 61 | Serial.println(F("Failed to detect the LSM6.")); 62 | delay(100); 63 | } 64 | } 65 | 66 | imu.enableDefault(); 67 | 68 | // Set the gyro full scale to 1000 dps because the default 69 | // value is too low, and leave the other settings the same. 70 | imu.writeReg(LSM6::CTRL2_G, 0b10001000); 71 | 72 | // Set the accelerometer full scale to 16 g because the default 73 | // value is too low, and leave the other settings the same. 74 | imu.writeReg(LSM6::CTRL1_XL, 0b10000100); 75 | } 76 | 77 | void loop() 78 | { 79 | imu.read(); 80 | mag.read(); 81 | 82 | snprintf_P(report, sizeof(report), 83 | PSTR("A: %6d %6d %6d M: %6d %6d %6d G: %6d %6d %6d"), 84 | imu.a.x, imu.a.y, imu.a.z, 85 | mag.m.x, mag.m.y, mag.m.z, 86 | imu.g.x, imu.g.y, imu.g.z); 87 | Serial.println(report); 88 | 89 | delay(100); 90 | } 91 | -------------------------------------------------------------------------------- /examples/LCDBasics/LCDBasics.ino: -------------------------------------------------------------------------------- 1 | // This example demonstrates basic use of the Balboa 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 | Balboa32U4LCD 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 | -------------------------------------------------------------------------------- /examples/LineSensorTest/LineSensorTest.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to read the raw values from the five line sensors on 2 | // the Balboa32U4 Reflectance Sensor Array. This example is useful if you are 3 | // having trouble with the sensors or just want to characterize their behavior. 4 | // 5 | // Make sure the configuration of the sensor object, in the setup() function 6 | // below, matches the orientation in which your sensor array is mounted. 7 | // 8 | // The raw (uncalibrated) values are reported to the serial monitor. You can 9 | // press the "C" button to toggle whether the IR emitters are on during the 10 | // reading. 11 | 12 | #include 13 | 14 | Balboa32U4ButtonC buttonC; 15 | Balboa32U4LineSensors lineSensors; 16 | 17 | const uint8_t SensorCount = 5; 18 | uint16_t sensorValues[SensorCount]; 19 | 20 | bool useEmitters = true; 21 | 22 | void setup() 23 | { 24 | // This program assumes your sensors are mounted in the edge-aligned 25 | // configuration (CTRL on pin 5). If you are using the sensors in the 26 | // center-aligned configuration (CTRL on pin 12), then comment out the call 27 | // to setEdgeAligned() and uncomment the call to setCenterAligned(). 28 | lineSensors.setEdgeAligned(); 29 | // lineSensors.setCenterAligned(); 30 | } 31 | 32 | // Prints a line with all the sensor readings to the serial 33 | // monitor. 34 | void printReadingsToSerial() 35 | { 36 | char buffer[80]; 37 | sprintf(buffer, "%4d %4d %4d %4d %4d %c\n", 38 | sensorValues[0], 39 | sensorValues[1], 40 | sensorValues[2], 41 | sensorValues[3], 42 | sensorValues[4], 43 | useEmitters ? 'E' : 'e' 44 | ); 45 | Serial.print(buffer); 46 | } 47 | 48 | void loop() 49 | { 50 | static uint16_t lastSampleTime = 0; 51 | 52 | if ((uint16_t)(millis() - lastSampleTime) >= 100) 53 | { 54 | lastSampleTime = millis(); 55 | 56 | // Read the line sensors. 57 | lineSensors.read(sensorValues, useEmitters ? QTRReadMode::On : QTRReadMode::Off); 58 | 59 | // Send the results to the LCD and to the serial monitor. 60 | printReadingsToSerial(); 61 | } 62 | 63 | // If button C is pressed, toggle the state of the emitters. 64 | if (buttonC.getSingleDebouncedPress()) 65 | { 66 | useEmitters = !useEmitters; 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /examples/MotorTest/MotorTest.ino: -------------------------------------------------------------------------------- 1 | // This example drives each motor on the Balboa 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 | Balboa32U4Motors motors; 9 | Balboa32U4ButtonA 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/PowerDetect/PowerDetect.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to: 2 | // 3 | // - Measure the voltage of the Balboa'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 | // Balboa 32U4 has capacitors that take a while to discharge. 13 | 14 | #include 15 | 16 | Balboa32U4LCD 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 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Balboa32U4LCD KEYWORD1 2 | 3 | BALBOA_32U4_BUTTON_A LITERAL1 4 | BALBOA_32U4_BUTTON_B LITERAL1 5 | BALBOA_32U4_BUTTON_C LITERAL1 6 | Balboa32U4ButtonA KEYWORD1 7 | Balboa32U4ButtonB KEYWORD1 8 | Balboa32U4ButtonC KEYWORD1 9 | 10 | Balboa32U4Buzzer KEYWORD1 11 | 12 | Balboa32U4Motors KEYWORD1 13 | flipLeftMotor KEYWORD2 14 | flipRightMotor KEYWORD2 15 | setLeftSpeed KEYWORD2 16 | setRightSpeed KEYWORD2 17 | setSpeeds KEYWORD2 18 | 19 | Balboa32U4Encoders KEYWORD1 20 | init KEYWORD2 21 | getCountsLeft KEYWORD2 22 | getCountsRight KEYWORD2 23 | getCountsAndResetLeft KEYWORD2 24 | getCountsAndResetRight KEYWORD2 25 | checkErrorLeft KEYWORD2 26 | checkErrorRight KEYWORD2 27 | 28 | ledRed KEYWORD2 29 | ledGreen KEYWORD2 30 | ledYellow KEYWORD2 31 | usbPowerPresent KEYWORD2 32 | readBatteryMillivolts KEYWORD2 33 | 34 | FastGPIO KEYWORD1 35 | Pin KEYWORD1 36 | 37 | setOutputLow KEYWORD2 38 | setOutputHigh KEYWORD2 39 | setOutputToggle KEYWORD2 40 | setOutput KEYWORD2 41 | setOutputValueLow KEYWORD2 42 | setOutputValueHigh KEYWORD2 43 | setOutputValueToggle KEYWORD2 44 | setOutputValue KEYWORD2 45 | setInput KEYWORD2 46 | setInputPulledUp KEYWORD2 47 | isInputHigh KEYWORD2 48 | isOutput KEYWORD2 49 | isOutputValueHigh KEYWORD2 50 | getState KEYWORD2 51 | setState KEYWORD2 52 | 53 | PinLoan KEYWORD1 54 | 55 | IO_B0 LITERAL1 56 | IO_B1 LITERAL1 57 | IO_B2 LITERAL1 58 | IO_B3 LITERAL1 59 | IO_B4 LITERAL1 60 | IO_B5 LITERAL1 61 | IO_B6 LITERAL1 62 | IO_B7 LITERAL1 63 | IO_C0 LITERAL1 64 | IO_C1 LITERAL1 65 | IO_C2 LITERAL1 66 | IO_C3 LITERAL1 67 | IO_C4 LITERAL1 68 | IO_C5 LITERAL1 69 | IO_C6 LITERAL1 70 | IO_C7 LITERAL1 71 | IO_D0 LITERAL1 72 | IO_D1 LITERAL1 73 | IO_D2 LITERAL1 74 | IO_D3 LITERAL1 75 | IO_D4 LITERAL1 76 | IO_D5 LITERAL1 77 | IO_D6 LITERAL1 78 | IO_D7 LITERAL1 79 | IO_E0 LITERAL1 80 | IO_E2 LITERAL1 81 | IO_E6 LITERAL1 82 | IO_F0 LITERAL1 83 | IO_F1 LITERAL1 84 | IO_F4 LITERAL1 85 | IO_F5 LITERAL1 86 | IO_F6 LITERAL1 87 | IO_F7 LITERAL1 88 | IO_NONE LITERAL1 89 | 90 | USBPause KEYWORD1 91 | Pushbutton KEYWORD1 92 | 93 | waitForPress KEYWORD2 94 | waitForRelease KEYWORD2 95 | waitForButton KEYWORD2 96 | isPressed KEYWORD2 97 | getSingleDebouncedPress KEYWORD2 98 | getSingleDebouncedRelease KEYWORD2 99 | 100 | PushbuttonBase KEYWORD1 101 | 102 | PushbuttonStateMachine KEYWORD1 103 | 104 | getSingleDebouncedRisingEdge KEYWORD2 105 | 106 | ZUMO_BUTTON LITERAL1 107 | PULL_UP_DISABLED LITERAL1 108 | PULL_UP_ENABLED LITERAL1 109 | DEFAULT_STATE_LOW LITERAL1 110 | DEFAULT_STATE_HIGH LITERAL1 111 | 112 | PololuBuzzer KEYWORD1 113 | 114 | playFrequency KEYWORD2 115 | playNote KEYWORD2 116 | play KEYWORD2 117 | playFromProgramSpace KEYWORD2 118 | isPlaying KEYWORD2 119 | stopPlaying KEYWORD2 120 | playMode KEYWORD2 121 | playCheck KEYWORD2 122 | 123 | PLAY_AUTOMATIC LITERAL1 124 | PLAY_CHECK LITERAL1 125 | NOTE_C LITERAL1 126 | NOTE_C_SHARP LITERAL1 127 | NOTE_D_FLAT LITERAL1 128 | NOTE_D LITERAL1 129 | NOTE_D_SHARP LITERAL1 130 | NOTE_E_FLAT LITERAL1 131 | NOTE_E LITERAL1 132 | NOTE_F LITERAL1 133 | NOTE_F_SHARP LITERAL1 134 | NOTE_G_FLAT LITERAL1 135 | NOTE_G LITERAL1 136 | NOTE_G_SHARP LITERAL1 137 | NOTE_A_FLAT LITERAL1 138 | NOTE_A LITERAL1 139 | NOTE_A_SHARP LITERAL1 140 | NOTE_B_FLAT LITERAL1 141 | NOTE_B LITERAL1 142 | SILENT_NOTE LITERAL1 143 | DIV_BY_10 LITERAL1 144 | PololuHD44780Base KEYWORD1 145 | 146 | initPins KEYWORD2 147 | init KEYWORD2 148 | reinitialize KEYWORD2 149 | send KEYWORD2 150 | clear KEYWORD2 151 | loadCustomCharacter KEYWORD2 152 | loadCustomCharacterFromRam KEYWORD2 153 | createChar KEYWORD2 154 | gotoXY KEYWORD2 155 | setCursor KEYWORD2 156 | noDisplay KEYWORD2 157 | display KEYWORD2 158 | noCursor KEYWORD2 159 | cursor KEYWORD2 160 | noBlink KEYWORD2 161 | blink KEYWORD2 162 | cursorSolid KEYWORD2 163 | cursorBlinking KEYWORD2 164 | scrollDisplayLeft KEYWORD2 165 | scrollDisplayRight KEYWORD2 166 | home KEYWORD2 167 | leftToRight KEYWORD2 168 | rightToleft KEYWORD2 169 | autoscroll KEYWORD2 170 | noAutoscroll KEYWORD2 171 | command KEYWORD2 172 | write KEYWORD2 173 | 174 | PololuHD44780 KEYWORD1 175 | ####################################### 176 | # Syntax Coloring Map for QTRSensors 177 | ####################################### 178 | 179 | ####################################### 180 | # Datatypes (KEYWORD1) 181 | ####################################### 182 | 183 | QTRSensors KEYWORD1 184 | QTRReadMode KEYWORD1 185 | QTRType KEYWORD1 186 | QTREmitters KEYWORD1 187 | CalibrationData KEYWORD1 188 | 189 | ####################################### 190 | # Methods and Functions (KEYWORD2) 191 | ####################################### 192 | 193 | setTypeRC KEYWORD2 194 | setTypeAnalog KEYWORD2 195 | getType KEYWORD2 196 | setSensorPins KEYWORD2 197 | setTimeout KEYWORD2 198 | getTimeout KEYWORD2 199 | setSamplesPerSensor KEYWORD2 200 | getSamplesPerSensor KEYWORD2 201 | setEmitterPin KEYWORD2 202 | setEmitterPins KEYWORD2 203 | releaseEmitterPins KEYWORD2 204 | getEmitterPinCount KEYWORD2 205 | getEmitterPin KEYWORD2 206 | getOddEmitterPin KEYWORD2 207 | getEvenEmitterPin KEYWORD2 208 | setDimmable KEYWORD2 209 | setNonDimmable KEYWORD2 210 | getDimmable KEYWORD2 211 | setDimmingLevel KEYWORD2 212 | getDimmingLevel KEYWORD2 213 | emittersOff KEYWORD2 214 | emittersOn KEYWORD2 215 | emittersSelect KEYWORD2 216 | calibrate KEYWORD2 217 | resetCalibration KEYWORD2 218 | read KEYWORD2 219 | readCalibrated KEYWORD2 220 | readLineBlack KEYWORD2 221 | readLineWhite KEYWORD2 222 | 223 | calibrationOn KEYWORD2 224 | calibrationOff KEYWORD2 225 | 226 | ####################################### 227 | # Constants (LITERAL1) 228 | ####################################### 229 | 230 | QTRNoEmitterPin LITERAL1 231 | QTRRCDefaultTimeout LITERAL1 232 | QTRMaxSensors LITERAL1 233 | 234 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Balboa32U4 2 | version=1.1.2 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Balboa 32U4 Arduino library 6 | paragraph=This library helps interface with the on-board hardware on the Pololu Balboa 32U4 control board. 7 | category=Device Control 8 | url=https://github.com/pololu/balboa-32u4-arduino-library 9 | architectures=avr 10 | includes=Balboa32U4.h 11 | -------------------------------------------------------------------------------- /local_keywords.txt: -------------------------------------------------------------------------------- 1 | Balboa32U4LCD KEYWORD1 2 | 3 | BALBOA_32U4_BUTTON_A LITERAL1 4 | BALBOA_32U4_BUTTON_B LITERAL1 5 | BALBOA_32U4_BUTTON_C LITERAL1 6 | Balboa32U4ButtonA KEYWORD1 7 | Balboa32U4ButtonB KEYWORD1 8 | Balboa32U4ButtonC KEYWORD1 9 | 10 | Balboa32U4Buzzer KEYWORD1 11 | 12 | Balboa32U4Motors KEYWORD1 13 | flipLeftMotor KEYWORD2 14 | flipRightMotor KEYWORD2 15 | setLeftSpeed KEYWORD2 16 | setRightSpeed KEYWORD2 17 | setSpeeds KEYWORD2 18 | 19 | Balboa32U4Encoders KEYWORD1 20 | init KEYWORD2 21 | getCountsLeft KEYWORD2 22 | getCountsRight KEYWORD2 23 | getCountsAndResetLeft KEYWORD2 24 | getCountsAndResetRight KEYWORD2 25 | checkErrorLeft KEYWORD2 26 | checkErrorRight KEYWORD2 27 | 28 | ledRed KEYWORD2 29 | ledGreen KEYWORD2 30 | ledYellow KEYWORD2 31 | usbPowerPresent KEYWORD2 32 | readBatteryMillivolts KEYWORD2 33 | -------------------------------------------------------------------------------- /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 | copylib https://github.com/pololu/qtr-sensors-arduino ../qtr-sensors-arduino QTRSensors{.cpp,.h} 51 | 52 | 53 | --------------------------------------------------------------------------------