├── .arduino-ci.yaml ├── .dir-locals.el ├── .github ├── ISSUE_TEMPLATE │ ├── bug-report-or-feature-request.md │ └── config.yml └── workflows │ └── ci.yaml ├── .gitignore ├── .gitlab-ci.yml ├── Doxyfile ├── LICENSE.txt ├── README.md ├── examples ├── Balancing │ └── Balancing.ino ├── BlinkLEDs │ └── BlinkLEDs.ino ├── BorderDetect │ └── BorderDetect.ino ├── Buttons │ └── Buttons.ino ├── BuzzerBasics │ └── BuzzerBasics.ino ├── DemoForLCDVersion │ └── DemoForLCDVersion.ino ├── DemoForOLEDVersion │ ├── DemoForOLEDVersion.ino │ ├── extended_lcd_font.h │ ├── font.cpp │ ├── splash.cpp │ ├── splash.h │ └── zumo_32u4_splash.h ├── DisplayBasics │ └── DisplayBasics.ino ├── Encoders │ └── Encoders.ino ├── FaceTowardsOpponent │ └── FaceTowardsOpponent.ino ├── FaceUphill │ └── FaceUphill.ino ├── InertialSensors │ └── InertialSensors.ino ├── LineAndProximitySensors │ ├── LineAndProximitySensors.ino │ ├── extended_lcd_font.h │ └── font.cpp ├── LineFollower │ ├── LineFollower.ino │ ├── extended_lcd_font.h │ └── font.cpp ├── LineSensorTest │ ├── LineSensorTest.ino │ ├── extended_lcd_font.h │ └── font.cpp ├── MazeSolver │ ├── GridMovement.h │ ├── MazeSolver.ino │ ├── TurnSensor.h │ ├── extended_lcd_font.h │ └── font.cpp ├── MotorTest │ └── MotorTest.ino ├── PowerDetect │ └── PowerDetect.ino ├── RCControl │ └── RCControl.ino ├── RemoteControl │ ├── RemoteConstants.h │ ├── RemoteControl.ino │ └── RemoteDecoder.h ├── RotationResist │ ├── RotationResist.ino │ └── TurnSensor.h ├── SumoCollisionDetect │ └── SumoCollisionDetect.ino └── SumoProximitySensors │ └── SumoProximitySensors.ino ├── keywords.txt ├── library.properties └── src ├── QTRSensors.cpp ├── QTRSensors.h ├── Zumo32U4.h ├── Zumo32U4Buttons.h ├── Zumo32U4Buzzer.h ├── Zumo32U4Encoders.cpp ├── Zumo32U4Encoders.h ├── Zumo32U4IMU.cpp ├── Zumo32U4IMU.h ├── Zumo32U4IRPulses.cpp ├── Zumo32U4IRPulses.h ├── Zumo32U4LCD.h ├── Zumo32U4LineSensors.h ├── Zumo32U4Motors.cpp ├── Zumo32U4Motors.h ├── Zumo32U4OLED.h ├── Zumo32U4ProximitySensors.cpp └── Zumo32U4ProximitySensors.h /.arduino-ci.yaml: -------------------------------------------------------------------------------- 1 | only_boards: 2 | - arduino:avr:leonardo 3 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /Doxyfile: -------------------------------------------------------------------------------- 1 | # Doxygen configuration file for generating documentation. 2 | PROJECT_NAME = "Zumo32U4 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 | ALWAYS_DETAILED_SEC = YES 14 | TAGFILES = ../PololuBuzzer/PololuBuzzer.tag=../pololu-buzzer-arduino \ 15 | ../PololuHD44780/PololuHD44780.tag=../pololu-hd44780-arduino \ 16 | ../PololuOLED/PololuOLED.tag=../pololu-oled-arduino \ 17 | ../Pushbutton/Pushbutton.tag=../pushbutton-arduino 18 | 19 | PREDEFINED = DOXYGEN 20 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015-2022 Pololu Corporation (www.pololu.com) 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of 4 | this software and associated documentation files (the "Software"), to deal in 5 | the Software without restriction, including without limitation the rights to 6 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 7 | the Software, and to permit persons to whom the Software is furnished to do so, 8 | subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 15 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 16 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 17 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 18 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Zumo32U4 library 2 | 3 | [www.pololu.com](http://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 [Zumo 32U4 robot](https://www.pololu.com/category/170/zumo-32u4-oled-robot) (both the [newer OLED version](https://www.pololu.com/category/170/zumo-32u4-robot) and the [original LCD version](https://www.pololu.com/category/286/original-zumo-32u4-robot)). 8 | 9 | The Zumo 32U4 robot consists of the Zumo chassis, the Zumo 32U4 Main Board, and the Zumo 32U4 Front Sensor Array. It has an integrated AVR ATmega32U4 microcontroller, motor drivers, encoders, proximity sensors, line sensors, inertial sensors, buzzer, four buttons, and display screen (graphical OLED or LCD). The user's guide for the Zumo 32U4 robot is here: 10 | 11 | https://www.pololu.com/docs/0J63 12 | 13 | Please note that this library does NOT work with the Zumo Shield for Arduino, which is a very different product. The Zumo Shield does not have an integrated microcontroller, so it must be connected to an Arduino-sized board to run. If you have the Zumo Shield, then you should not use this library and instead refer to the Zumo Shield documentation [here](https://www.pololu.com/docs/0J57). 14 | 15 | ## Installing the library 16 | 17 | Use the Library Manager in version 1.8.10 or later of the Arduino software (IDE) to install this library: 18 | 19 | 1. In the Arduino IDE, open the "Tools" menu and select "Manage Libraries...". 20 | 2. Search for "Zumo32U4". 21 | 3. Click the Zumo32U4 entry in the list. 22 | 4. Click "Install". 23 | 5. If you see a prompt asking to install missing dependencies, click "Install all". 24 | 25 | ## Usage 26 | 27 | To access all features of this library, you just need these include statements: 28 | 29 | ```cpp 30 | #include 31 | #include 32 | ``` 33 | 34 | ## Examples 35 | 36 | 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 "Zumo32U4". If you cannot find these examples, the library was probably installed incorrectly and you should retry the installation instructions above. 37 | 38 | ## Classes and functions 39 | 40 | The main classes and functions provided by the library are listed below: 41 | 42 | * Zumo32U4ButtonA 43 | * Zumo32U4ButtonB 44 | * Zumo32U4ButtonC 45 | * Zumo32U4Buzzer 46 | * Zumo32U4Encoders 47 | * Zumo32U4IMU 48 | * Zumo32U4IRPulses 49 | * Zumo32U4LCD 50 | * Zumo32U4LineSensors 51 | * Zumo32U4Motors 52 | * Zumo32U4OLED 53 | * Zumo32U4ProximitySensors 54 | * ledRed() 55 | * ledGreen() 56 | * ledYellow() 57 | * usbPowerPresent() 58 | * readBatteryMillivolts() 59 | 60 | ## Dependencies 61 | 62 | This library also references several other Arduino libraries which are used to help implement the classes and functions above. 63 | 64 | * [FastGPIO](https://github.com/pololu/fastgpio-arduino) 65 | * [PololuBuzzer](https://github.com/pololu/pololu-buzzer-arduino) 66 | * [PololuHD44780](https://github.com/pololu/pololu-hd44780-arduino) 67 | * [PololuMenu](https://github.com/pololu/pololu-menu-arduino) 68 | * [PololuOLED](https://github.com/pololu/pololu-oled-arduino) 69 | * [Pushbutton](https://github.com/pololu/pushbutton-arduino) 70 | * [USBPause](https://github.com/pololu/usb-pause-arduino) 71 | 72 | ## Documentation 73 | 74 | For complete documentation, see https://pololu.github.io/zumo-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 | * 2.0.1 (2022-09-06): Fixed a bug in the Encoders demo that could prevent encoder errors from being shown properly on the display. 79 | * 2.0.0 (2021-11-23): Added support for the Zumo 32U4 OLED. Reorganized the library's directory structure and replaced most of the bundled component libraries with Arduino Library Manager dependencies. Removed the previously bundled LSM303 and L3G libraries. 80 | * 1.2.0 (2020-09-11): Added a Zumo32U4IMU class that abstracts some details of the inertial sensors and supports different IMU types. The examples have been updated to use this class. 81 | * 1.1.4 (2017-07-17): Fixed a bug that caused errors from the right encoder to be reported as errors from the left encoder. 82 | * 1.1.3 (2016-10-12): Fixed a bug that caused the buzzer's `isPlaying` method to malfunction sometimes when link time optimization is enabled. Also incorporated some minor fixes to the QTRSensors and LSM303 libraries. 83 | * 1.1.2 (2016-03-14): Updated the Demo example so it can compile in Arduino 1.6.7. 84 | * 1.1.1 (2015-09-01): Moved the library out of the a-star repository into its own repository. Added Demo example. 85 | * 1.1.0 (2015-05-06): Updated FastGPIO to version 1.0.2. Fixed a bug in Zumo32U4ProximitySensors where the wrong array length was used. Added five demos: RotationResist, FaceUphill, RemoteControl, Balancing, and SumoProximitySensors. 86 | * 1.0.1 (2015-03-11): Improve the Buttons example. 87 | * 1.0.0 (2015-03-05): Original release. 88 | -------------------------------------------------------------------------------- /examples/Balancing/Balancing.ino: -------------------------------------------------------------------------------- 1 | /* This example shows how to use the Zumo 32U4's accelerometer 2 | and gyro to balance on its front end. 3 | 4 | Please note that the balancing algorithm in this code is not 5 | perfect: the robot tends to drift away from its starting position 6 | over time. We found that this code works better on carpet than 7 | on a hard surface. 8 | 9 | You will have to remove the Zumo's blade and front sensor array if 10 | they are installed. After removing the blade, be sure to reinstall 11 | the screws that were holding the blade. 12 | 13 | This code is sensitive to changes in the Zumo's center of mass, 14 | so make sure the display is plugged in. 15 | 16 | This code was designed for Zumos with 75:1 HP micro metal 17 | gearmotors, and it might need to be adjusted to work on Zumos 18 | with other types of motors. */ 19 | 20 | #include 21 | #include 22 | 23 | // Change next line to this if you are using the older Zumo 32U4 24 | // with a black and green LCD display (and see note for the 25 | // targetAngle constant below): 26 | // Zumo32U4LCD display; 27 | Zumo32U4OLED display; 28 | 29 | Zumo32U4ButtonA buttonA; 30 | Zumo32U4ButtonB buttonB; 31 | Zumo32U4ButtonC buttonC; 32 | Zumo32U4Motors motors; 33 | Zumo32U4Buzzer buzzer; 34 | Zumo32U4IMU imu; 35 | 36 | // This is the average reading obtained from the gyro's Y axis 37 | // during calibration. 38 | float gyroOffsetY; 39 | 40 | // This variable holds our estimation of the robot's angle based 41 | // on the gyro and the accelerometer. A value of 0 means the 42 | // robot is perfectly vertical. A value of -90 means that the 43 | // robot is horizontal and the battery holder is facing down. A 44 | // value of 90 means that the robot is horizontal and the battery 45 | // holder is facing up. 46 | float angle = 0; 47 | 48 | // This is just like "angle", but it is based solely on the 49 | // accelerometer. 50 | float aAngle = 0; 51 | 52 | // This is the angle the robot tries to balance at. Since the LCD and OLED 53 | // displays have different weights and cause the Zumo to balance differently, 54 | // the best target angle will depend on which version of the Zumo 32U4 you have. 55 | // We suggest starting with a value of 4.0 for the OLED version or 2.0 for the 56 | // LCD version. 57 | const float targetAngle = 4.0; 58 | 59 | void setup() 60 | { 61 | Wire.begin(); 62 | 63 | // Set up the inertial sensors. 64 | imu.init(); 65 | imu.enableDefault(); 66 | imu.configureForBalancing(); 67 | 68 | display.clear(); 69 | display.print(F("Gyro cal")); 70 | ledYellow(1); 71 | 72 | // Delay to give the user time to remove their finger. 73 | delay(500); 74 | 75 | // Calibrate the gyro. 76 | for (uint16_t i = 0; i < 1024; i++) 77 | { 78 | // Wait for new data to be available, then read it. 79 | while(!imu.gyroDataReady()) {} 80 | imu.readGyro(); 81 | 82 | // Add the Y axis reading to the total. 83 | gyroOffsetY += imu.g.y; 84 | } 85 | gyroOffsetY /= 1024; 86 | 87 | display.clear(); 88 | ledYellow(0); 89 | 90 | // Display the angle until the user presses A. 91 | while (!buttonA.getSingleDebouncedRelease()) 92 | { 93 | // Update the angle using the gyro as often as possible. 94 | updateAngleGyro(); 95 | 96 | // Every 20 ms (50 Hz), correct the angle using the 97 | // accelerometer and also print it. 98 | static uint8_t lastCorrectionTime = 0; 99 | uint8_t m = millis(); 100 | if ((uint8_t)(m - lastCorrectionTime) >= 20) 101 | { 102 | lastCorrectionTime = m; 103 | correctAngleAccel(); 104 | printAngles(); 105 | } 106 | } 107 | delay(500); 108 | } 109 | 110 | void loop() 111 | { 112 | // Update the angle using the gyro as often as possible. 113 | updateAngleGyro(); 114 | 115 | // Every 20 ms (50 Hz), correct the angle using the 116 | // accelerometer, print it, and set the motor speeds. 117 | static byte lastCorrectionTime = 0; 118 | byte m = millis(); 119 | if ((byte)(m - lastCorrectionTime) >= 20) 120 | { 121 | lastCorrectionTime = m; 122 | correctAngleAccel(); 123 | printAngles(); 124 | setMotors(); 125 | } 126 | } 127 | 128 | void printAngles() 129 | { 130 | display.gotoXY(0, 0); 131 | display.print(angle); 132 | display.print(F(" ")); 133 | 134 | display.gotoXY(0, 1); 135 | display.print(aAngle); 136 | display.print(" "); 137 | } 138 | 139 | // Reads the gyro and uses it to update the angle estimation. 140 | void updateAngleGyro() 141 | { 142 | // Figure out how much time has passed since the last update. 143 | static uint16_t lastUpdate = 0; 144 | uint16_t m = micros(); 145 | uint16_t dt = m - lastUpdate; 146 | lastUpdate = m; 147 | 148 | imu.readGyro(); 149 | 150 | // Calculate how much the angle has changed, in degrees, and 151 | // add it to our estimation of the current angle. The gyro's 152 | // sensitivity is 0.07 dps per digit. 153 | angle += ((float)imu.g.y - gyroOffsetY) * 70 * dt / 1000000000; 154 | } 155 | 156 | template float vector_dot(const Zumo32U4IMU::vector *a, const Zumo32U4IMU::vector *b) 157 | { 158 | return (a->x * b->x) + (a->y * b->y) + (a->z * b->z); 159 | } 160 | 161 | // Reads the accelerometer and uses it to adjust the angle 162 | // estimation. 163 | void correctAngleAccel() 164 | { 165 | imu.readAcc(); 166 | 167 | // Calculate the angle according to the accelerometer. 168 | aAngle = -atan2(imu.a.z, -imu.a.x) * 180 / M_PI; 169 | 170 | // Calculate the magnitude of the measured acceleration vector, 171 | // in units of g. 172 | Zumo32U4IMU::vector const aInG = { 173 | (float)imu.a.x / 4096, 174 | (float)imu.a.y / 4096, 175 | (float)imu.a.z / 4096} 176 | ; 177 | float mag = sqrt(vector_dot(&aInG, &aInG)); 178 | 179 | // Calculate how much weight we should give to the 180 | // accelerometer reading. When the magnitude is not close to 181 | // 1 g, we trust it less because it is being influenced by 182 | // non-gravity accelerations, so we give it a lower weight. 183 | float weight = 1 - 5 * abs(1 - mag); 184 | weight = constrain(weight, 0, 1); 185 | weight /= 10; 186 | 187 | // Adjust the angle estimation. The higher the weight, the 188 | // more the angle gets adjusted. 189 | angle = weight * aAngle + (1 - weight) * angle; 190 | } 191 | 192 | // This function uses our current angle estimation and a PID 193 | // algorithm to set the motor speeds. This is the core of the 194 | // robot's balancing algorithm. 195 | void setMotors() 196 | { 197 | int32_t speed; 198 | if (abs(angle) > 45) 199 | { 200 | // If the robot is tilted more than 45 degrees, it is 201 | // probably going to fall over. Stop the motors to prevent 202 | // it from running away. 203 | speed = 0; 204 | } 205 | else 206 | { 207 | static float lastError = 0; 208 | static float integral = 0; 209 | 210 | float error = angle - targetAngle; 211 | 212 | integral += error; 213 | integral = constrain(integral, -40, 40); 214 | 215 | float errorDifference = error - lastError; 216 | speed = error * 35 + errorDifference * 10 + integral * 5; 217 | speed = constrain(speed, -400, 400); 218 | 219 | lastError = error; 220 | } 221 | motors.setSpeeds(speed, speed); 222 | } -------------------------------------------------------------------------------- /examples/BlinkLEDs/BlinkLEDs.ino: -------------------------------------------------------------------------------- 1 | /* This example shows how to blink the three user LEDs 2 | on the Zumo 32U4. */ 3 | 4 | #include 5 | #include 6 | 7 | void setup() 8 | { 9 | 10 | } 11 | 12 | void loop() 13 | { 14 | // Turn the LEDs on. 15 | ledRed(1); 16 | ledYellow(1); 17 | ledGreen(1); 18 | 19 | // Wait for a second. 20 | delay(1000); 21 | 22 | // Turn the LEDs off. 23 | ledRed(0); 24 | ledYellow(0); 25 | ledGreen(0); 26 | 27 | // Wait for a second. 28 | delay(1000); 29 | } 30 | -------------------------------------------------------------------------------- /examples/BorderDetect/BorderDetect.ino: -------------------------------------------------------------------------------- 1 | /** This example uses the Zumo's line sensors to detect the white 2 | border around a sumo ring. When the border is detected, it 3 | backs up and turns. */ 4 | 5 | #include 6 | #include 7 | 8 | // This might need to be tuned for different lighting conditions, 9 | // surfaces, etc. 10 | #define QTR_THRESHOLD 1000 // microseconds 11 | 12 | // These might need to be tuned for different motor types. 13 | #define REVERSE_SPEED 200 // 0 is stopped, 400 is full speed 14 | #define TURN_SPEED 200 15 | #define FORWARD_SPEED 200 16 | #define REVERSE_DURATION 200 // ms 17 | #define TURN_DURATION 300 // ms 18 | 19 | // Change next line to this if you are using the older Zumo 32U4 20 | // with a black and green LCD display: 21 | // Zumo32U4LCD display; 22 | Zumo32U4OLED display; 23 | 24 | Zumo32U4ButtonA buttonA; 25 | Zumo32U4Buzzer buzzer; 26 | Zumo32U4Motors motors; 27 | Zumo32U4LineSensors lineSensors; 28 | 29 | #define NUM_SENSORS 3 30 | unsigned int lineSensorValues[NUM_SENSORS]; 31 | 32 | void waitForButtonAndCountDown() 33 | { 34 | ledYellow(1); 35 | display.clear(); 36 | display.print(F("Press A")); 37 | 38 | buttonA.waitForButton(); 39 | 40 | ledYellow(0); 41 | display.clear(); 42 | 43 | // Play audible countdown. 44 | for (int i = 0; i < 3; i++) 45 | { 46 | delay(1000); 47 | buzzer.playNote(NOTE_G(3), 200, 15); 48 | } 49 | delay(1000); 50 | buzzer.playNote(NOTE_G(4), 500, 15); 51 | delay(1000); 52 | } 53 | 54 | void setup() 55 | { 56 | // Uncomment if necessary to correct motor directions: 57 | //motors.flipLeftMotor(true); 58 | //motors.flipRightMotor(true); 59 | 60 | lineSensors.initThreeSensors(); 61 | 62 | waitForButtonAndCountDown(); 63 | } 64 | 65 | void loop() 66 | { 67 | if (buttonA.isPressed()) 68 | { 69 | // If button is pressed, stop and wait for another press to 70 | // go again. 71 | motors.setSpeeds(0, 0); 72 | buttonA.waitForRelease(); 73 | waitForButtonAndCountDown(); 74 | } 75 | 76 | lineSensors.read(lineSensorValues); 77 | 78 | if (lineSensorValues[0] < QTR_THRESHOLD) 79 | { 80 | // If leftmost sensor detects line, reverse and turn to the 81 | // right. 82 | motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED); 83 | delay(REVERSE_DURATION); 84 | motors.setSpeeds(TURN_SPEED, -TURN_SPEED); 85 | delay(TURN_DURATION); 86 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED); 87 | } 88 | else if (lineSensorValues[NUM_SENSORS - 1] < QTR_THRESHOLD) 89 | { 90 | // If rightmost sensor detects line, reverse and turn to the 91 | // left. 92 | motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED); 93 | delay(REVERSE_DURATION); 94 | motors.setSpeeds(-TURN_SPEED, TURN_SPEED); 95 | delay(TURN_DURATION); 96 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED); 97 | } 98 | else 99 | { 100 | // Otherwise, go straight. 101 | motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED); 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /examples/Buttons/Buttons.ino: -------------------------------------------------------------------------------- 1 | /* This example demonstrates three different ways to 2 | interface with a user pushbutton on the Zumo 32U4. */ 3 | 4 | #include 5 | #include 6 | 7 | // These objects provide access to the Zumo 32U4's 8 | // on-board buttons. 9 | Zumo32U4ButtonA buttonA; 10 | Zumo32U4ButtonB buttonB; 11 | Zumo32U4ButtonC buttonC; 12 | 13 | // Change next line to this if you are using the older Zumo 32U4 14 | // with a black and green LCD display: 15 | // Zumo32U4LCD display; 16 | Zumo32U4OLED display; 17 | 18 | void setup() 19 | { 20 | display.clear(); 21 | display.print(F("Press A")); 22 | 23 | /* Method 1: Use the waitForButton() function, which blocks and 24 | * doesn't return until a button press and release are 25 | * detected. This function takes care of button debouncing. */ 26 | buttonA.waitForButton(); 27 | 28 | display.clear(); 29 | } 30 | 31 | void loop() 32 | { 33 | /* Method 2: Directly read the state of the button with the 34 | * isPressed() function. This method is non-blocking and 35 | * provides no debouncing. */ 36 | if (buttonB.isPressed()) 37 | { 38 | // Whenever the button is pressed, turn on the yellow LED. 39 | ledYellow(1); 40 | } 41 | else 42 | { 43 | // Whenever the button is not pressed, turn off the yellow 44 | // LED. 45 | ledYellow(0); 46 | } 47 | 48 | /* Method 3: Call getSingleDebouncedPress() regularly in a 49 | * loop, which returns true to report a single button press or 50 | * false otherwise. This function is non-blocking and takes 51 | * care of button debouncing. */ 52 | static int cPressedCount = 0; 53 | if (buttonC.getSingleDebouncedPress()) 54 | { 55 | cPressedCount += 1; 56 | Serial.print(F("Button C was pressed ")); 57 | Serial.print(cPressedCount); 58 | Serial.println(F(" times.")); 59 | 60 | display.clear(); 61 | display.print(cPressedCount); 62 | } 63 | 64 | /* If you use non-blocking functions like isPressed() and 65 | * getSingleDebouncedPress(), then you can monitor multiple 66 | * buttons at the same time and also take care of other tasks 67 | * at the same time. In this example, we blink the red LED 68 | * while monitoring the buttons. */ 69 | ledRed(millis() % 1024 < 100); 70 | } 71 | -------------------------------------------------------------------------------- /examples/BuzzerBasics/BuzzerBasics.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the Zumo32U4PrimeBuzzer class to play a 2 | series of notes on the buzzer. 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 the 7 | 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 to 11 | stop the buzzer, and it shows how to use the isPlaying() function 12 | to tell whether the buzzer is still playing or not. */ 13 | 14 | #include 15 | #include 16 | 17 | Zumo32U4Buzzer buzzer; 18 | 19 | // Store this song in program space using the PROGMEM macro. 20 | // Later we will play it directly from program space, bypassing 21 | // the need to load it all into RAM first. 22 | const char fugue[] PROGMEM = 23 | "! 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" 31 | "e>d>c#>db>d>c#b >c#agaegfe f O6 dc#dfdc# 2 | 3 | // Replaces the default OLED font with an extended font that incorporates 4 | // several non-ASCII glyphs from the original HD44780 character set: the 5 | // forward arrow and centered dot. 6 | // 7 | // This file uses a #define hack to include a font file stored in a special 8 | // C-compatible format called X BitMap or "xbm". To edit the font, you will 9 | // need an editor that can work with this format, such as GIMP: 10 | // 11 | // https://www.gimp.org/ 12 | // 13 | // For compatibility with the PololuOLED library, characters run 14 | // vertically in an 8-by-1120 pixel image, using five rows per character 15 | // and no space in between. Also, the characters are mirrored, to match the 16 | // bit order used in the library. 17 | // 18 | // It is also allowed to use a smaller (shorter) image file, if you are not 19 | // using all the characters. In this example, we use a font file that defines 20 | // only the 134 characters from 0x20 to 0xA5; non-ASCII characters above 0xA5 21 | // are excluded. 22 | // 23 | // You can open original_font.xbm from the PololuOLED library as a starting 24 | // point. 25 | // 26 | // Consider rotating and mirroring the original font file so that you can see 27 | // the letters in their proper orientation, transforming them back before 28 | // exporting the font to "font.xbm" and renaming to "font.h". 29 | // 30 | // Open font.h in a text editor to make sure that it declares a variable 31 | // exactly like this: 32 | // 33 | // static unsigned char font_bits[] 34 | // 35 | // Our "hack" is to redefine the name font_bits as a macro below to turn that 36 | // declaration into a replacement of the variable pololuOledFont in the 37 | // library. If it doesn't match exactly, you will either get an error or 38 | // still have the original font. 39 | 40 | #define font_bits unused_placeholder; extern const uint8_t PROGMEM pololuOledFont 41 | #include "extended_lcd_font.h" 42 | -------------------------------------------------------------------------------- /examples/DemoForOLEDVersion/splash.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define zumo_32u4_splash_bits unused_placeholder; extern const uint8_t PROGMEM zumo32U4Splash 4 | #include "zumo_32u4_splash.h" 5 | -------------------------------------------------------------------------------- /examples/DemoForOLEDVersion/splash.h: -------------------------------------------------------------------------------- 1 | extern const uint8_t PROGMEM zumo32U4Splash[1024]; 2 | 3 | -------------------------------------------------------------------------------- /examples/DemoForOLEDVersion/zumo_32u4_splash.h: -------------------------------------------------------------------------------- 1 | #define zumo_32u4_splash_width 64 2 | #define zumo_32u4_splash_height 128 3 | static unsigned char zumo_32u4_splash_bits[] = { 4 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 5 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 6 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfb, 0xff, 0x07, 7 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xfb, 0xff, 0x07, 0x00, 0xf0, 0x00, 0xe0, 8 | 0xff, 0xf1, 0xff, 0x03, 0x00, 0xf0, 0x00, 0xf8, 0xff, 0xf1, 0xfc, 0x03, 9 | 0x00, 0xf0, 0x00, 0xfe, 0xff, 0x21, 0xfe, 0x01, 0x00, 0xf0, 0x80, 0xff, 10 | 0xff, 0x00, 0xfe, 0x01, 0x00, 0xf0, 0xe0, 0xff, 0x07, 0x00, 0xff, 0x01, 11 | 0x00, 0xf0, 0xf8, 0xf7, 0x07, 0x00, 0xff, 0x00, 0x00, 0xf0, 0xfe, 0xf1, 12 | 0x0f, 0x00, 0x3c, 0x00, 0x00, 0xf0, 0x7f, 0xf0, 0x1f, 0x00, 0x00, 0x00, 13 | 0x00, 0xf0, 0x1f, 0xf0, 0x7f, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x07, 0xf0, 14 | 0xff, 0x80, 0x03, 0x00, 0x00, 0xf0, 0x01, 0xf0, 0xff, 0x80, 0xff, 0x01, 15 | 0x00, 0x70, 0x00, 0xf0, 0xff, 0x08, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 16 | 0xff, 0x18, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 0xff, 0x7d, 0xfe, 0x07, 17 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd, 0xff, 0x07, 0x00, 0x00, 0xfe, 0x0f, 18 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xfe, 0x3f, 0xff, 0xff, 0xff, 0x07, 19 | 0x00, 0x00, 0xfe, 0x7f, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xfe, 0xff, 20 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 21 | 0x00, 0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 22 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x00, 23 | 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfe, 0xff, 24 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xfe, 0x7f, 0xff, 0xff, 0xff, 0x07, 25 | 0x00, 0x00, 0xfe, 0x3f, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xfe, 0x0f, 26 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 27 | 0x00, 0x00, 0x00, 0x00, 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 28 | 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0xfe, 0xff, 0x1f, 0xe0, 0x03, 0x00, 29 | 0x00, 0x00, 0xfe, 0xff, 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0xfe, 0xff, 30 | 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0xfe, 0xff, 0x1f, 0xe0, 0x03, 0x00, 31 | 0x00, 0x00, 0x1c, 0x00, 0x3f, 0xf0, 0x03, 0x00, 0x00, 0x00, 0x0e, 0x00, 32 | 0x3e, 0xf0, 0x01, 0x00, 0x00, 0x00, 0x0e, 0x00, 0xfe, 0xfc, 0x01, 0x00, 33 | 0x00, 0x00, 0x0e, 0x00, 0xfc, 0xff, 0x00, 0x00, 0x00, 0x00, 0x1e, 0x00, 34 | 0xfc, 0xff, 0x00, 0x00, 0x00, 0x00, 0xfe, 0xff, 0xf8, 0x7f, 0x00, 0x00, 35 | 0x00, 0x00, 0xfc, 0xff, 0xf0, 0x3f, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xff, 36 | 0xc0, 0x0f, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xff, 0x00, 0x00, 0x00, 0x00, 37 | 0x00, 0x00, 0x1c, 0x00, 0x00, 0x80, 0x3f, 0x00, 0x00, 0x00, 0x0e, 0x00, 38 | 0x00, 0xe0, 0xff, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x00, 0xf0, 0xff, 0x01, 39 | 0x00, 0x00, 0x0e, 0x00, 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0x1e, 0x00, 40 | 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0xfe, 0xff, 0x00, 0xfc, 0xe0, 0x07, 41 | 0x00, 0x00, 0xfc, 0xff, 0x00, 0x7c, 0xc0, 0x07, 0x00, 0x00, 0xf8, 0xff, 42 | 0x00, 0x3c, 0x80, 0x07, 0x00, 0x00, 0xe0, 0xff, 0x00, 0x3c, 0x80, 0x07, 43 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 44 | 0x00, 0x3c, 0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7c, 0xc0, 0x07, 45 | 0x00, 0x00, 0xc0, 0x07, 0x00, 0xfc, 0xe0, 0x07, 0x00, 0x00, 0xf0, 0x1f, 46 | 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0xf8, 0x3f, 0x00, 0xf8, 0xff, 0x03, 47 | 0x00, 0x00, 0xfc, 0x7f, 0x00, 0xf0, 0xff, 0x01, 0x00, 0x00, 0x3c, 0x78, 48 | 0x00, 0xe0, 0xff, 0x00, 0x00, 0x00, 0x1e, 0xf0, 0x00, 0x80, 0x3f, 0x00, 49 | 0x00, 0x00, 0x0e, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xe0, 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xe0, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x0e, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1e, 0xf0, 52 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x3c, 0x78, 0xff, 0xff, 0xff, 0x07, 53 | 0x00, 0x00, 0xfc, 0x7f, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xf8, 0x3f, 54 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xf0, 0x1f, 0xff, 0xff, 0xff, 0x07, 55 | 0x00, 0x00, 0xc0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 58 | 0x00, 0x80, 0x3f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0xff, 0x00, 59 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 60 | 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0xff, 0x03, 61 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xe0, 0x07, 0x00, 0x00, 0x00, 0x00, 62 | 0x00, 0x7c, 0xc0, 0x07, 0x00, 0x00, 0x04, 0x3e, 0x00, 0x3c, 0x80, 0x07, 63 | 0x00, 0x18, 0x0e, 0x7f, 0x00, 0x3c, 0x80, 0x07, 0x00, 0x18, 0x8c, 0xe3, 64 | 0x00, 0x3c, 0x80, 0x07, 0x00, 0xd8, 0x8c, 0xc1, 0x00, 0x3c, 0x80, 0x07, 65 | 0x00, 0xf8, 0x8c, 0xc1, 0x00, 0x7c, 0xc0, 0x07, 0x00, 0xb8, 0x8f, 0xc1, 66 | 0x00, 0xfc, 0xe0, 0x07, 0x00, 0x18, 0x87, 0xe3, 0x00, 0xf8, 0xff, 0x03, 67 | 0x00, 0x00, 0x00, 0x7f, 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0x00, 0x3e, 68 | 0x00, 0xf0, 0xff, 0x01, 0x00, 0x10, 0x0c, 0x00, 0x00, 0xe0, 0xff, 0x00, 69 | 0x00, 0x38, 0x0e, 0x00, 0x00, 0x80, 0x3f, 0x00, 0x00, 0x18, 0x8f, 0xff, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x98, 0x8f, 0xff, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0xd8, 0x0d, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x0c, 0xc0, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x70, 0x0c, 0xc0, 0xff, 0xff, 0xff, 0x07, 73 | 0x00, 0x00, 0x00, 0xc0, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 74 | 0xff, 0xff, 0xff, 0x07, 0x00, 0xf8, 0x03, 0x00, 0xff, 0xff, 0xff, 0x07, 75 | 0x00, 0xf8, 0x87, 0xff, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x8e, 0xff, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8c, 0xcd, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0x00, 0x8c, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x8e, 0xcd, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x87, 0xcd, 0x00, 0xfc, 0x7f, 0x00, 79 | 0x00, 0xf8, 0x83, 0xc1, 0x00, 0xfc, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0xfc, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x03, 81 | 0x00, 0x00, 0x83, 0xff, 0x00, 0xfc, 0xff, 0x07, 0x00, 0x80, 0x83, 0xff, 82 | 0x00, 0x00, 0xc0, 0x07, 0x00, 0xc0, 0x83, 0xc1, 0x00, 0x00, 0x80, 0x07, 83 | 0x00, 0x60, 0x83, 0xc1, 0x00, 0x00, 0x80, 0x07, 0x00, 0x30, 0x83, 0xc1, 84 | 0x00, 0x00, 0x80, 0x07, 0x00, 0xf8, 0x8f, 0xe3, 0x00, 0x00, 0x80, 0x07, 85 | 0x00, 0xf8, 0x0f, 0x7f, 0x00, 0x00, 0xc0, 0x07, 0x00, 0x00, 0x03, 0x3e, 86 | 0x00, 0xfc, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x03, 87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 88 | 0x00, 0xfc, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x7f, 0x00, 89 | 0x00, 0x00, 0x00, 0x00 }; 90 | -------------------------------------------------------------------------------- /examples/DisplayBasics/DisplayBasics.ino: -------------------------------------------------------------------------------- 1 | /* This example demonstrates basic use of the Zumo 32U4 display. 2 | It prints the word "hi" on the first line of the display and 3 | prints the number 1234 on the second line. */ 4 | 5 | #include 6 | #include 7 | 8 | // Change next line to this if you are using the older Zumo 32U4 9 | // with a black and green LCD display: 10 | // Zumo32U4LCD display; 11 | Zumo32U4OLED display; 12 | 13 | void setup() 14 | { 15 | 16 | } 17 | 18 | void loop() 19 | { 20 | // Clear the screen 21 | display.clear(); 22 | 23 | // Print a string 24 | display.print("hi"); 25 | 26 | // Go to the next line 27 | display.gotoXY(0, 1); 28 | 29 | // Print a number 30 | display.print(1234); 31 | 32 | delay(1000); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /examples/Encoders/Encoders.ino: -------------------------------------------------------------------------------- 1 | /* This program shows how to read the encoders on the Zumo 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 Zumo to drive both motors forward 6 | at full speed. You can press button C to drive both motors 7 | in reverse at full speed. 8 | 9 | Encoder counts are printed to the display and to the serial monitor. 10 | 11 | On the display, the top line shows the counts from the left encoder, 12 | and the bottom line shows the counts from the right encoder. 13 | Encoder errors should not happen, but if one does happen then the 14 | buzzer will beep and an exclamation mark will appear temporarily 15 | on the display. 16 | 17 | In the serial monitor, the first and second numbers represent 18 | counts from the left and right encoders, respectively. The third 19 | and fourth numbers represent errors from the left and right 20 | encoders, respectively. */ 21 | 22 | #include 23 | #include 24 | 25 | Zumo32U4Encoders encoders; 26 | Zumo32U4Buzzer buzzer; 27 | Zumo32U4Motors motors; 28 | Zumo32U4ButtonA buttonA; 29 | Zumo32U4ButtonC buttonC; 30 | 31 | // Change next line to this if you are using the older Zumo 32U4 32 | // with a black and green LCD display: 33 | // Zumo32U4LCD display; 34 | Zumo32U4OLED display; 35 | 36 | const char encoderErrorLeft[] PROGMEM = "!= 100) 53 | { 54 | lastDisplayTime = millis(); 55 | 56 | int16_t countsLeft = encoders.getCountsLeft(); 57 | int16_t countsRight = encoders.getCountsRight(); 58 | 59 | bool errorLeft = encoders.checkErrorLeft(); 60 | bool errorRight = encoders.checkErrorRight(); 61 | 62 | if (errorLeft) 63 | { 64 | // An error occurred on the left encoder channel. 65 | // Show it on the display for the next 10 iterations and 66 | // also beep. 67 | displayErrorLeftCountdown = 10; 68 | buzzer.playFromProgramSpace(encoderErrorLeft); 69 | } 70 | 71 | if (errorRight) 72 | { 73 | // An error occurred on the right encoder channel. 74 | // Show it on the display for the next 10 iterations and 75 | // also beep. 76 | displayErrorRightCountdown = 10; 77 | buzzer.playFromProgramSpace(encoderErrorRight); 78 | } 79 | 80 | // Update the display with encoder counts and error info. 81 | display.clear(); 82 | display.print(countsLeft); 83 | display.gotoXY(0, 1); 84 | display.print(countsRight); 85 | 86 | if (displayErrorLeftCountdown) 87 | { 88 | // Show an exclamation point on the first line to 89 | // indicate an error from the left encoder. 90 | display.gotoXY(7, 0); 91 | display.print('!'); 92 | displayErrorLeftCountdown--; 93 | } 94 | 95 | if (displayErrorRightCountdown) 96 | { 97 | // Show an exclamation point on the second line to 98 | // indicate an error from the left encoder. 99 | display.gotoXY(7, 1); 100 | display.print('!'); 101 | displayErrorRightCountdown--; 102 | } 103 | 104 | // Send the information to the serial monitor also. 105 | snprintf_P(report, sizeof(report), 106 | PSTR("%6d %6d %1d %1d"), 107 | countsLeft, countsRight, errorLeft, errorRight); 108 | Serial.println(report); 109 | } 110 | 111 | if (buttonA.isPressed()) 112 | { 113 | motors.setSpeeds(400, 400); 114 | } 115 | else if (buttonC.isPressed()) 116 | { 117 | motors.setSpeeds(-400, -400); 118 | } 119 | else 120 | { 121 | motors.setSpeeds(0, 0); 122 | } 123 | } 124 | -------------------------------------------------------------------------------- /examples/FaceTowardsOpponent/FaceTowardsOpponent.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the front proximity sensor on the Zumo 32U4 2 | Front Sensor Array to locate an opponent robot or any other 3 | reflective object. Using the motors to turn, it scans its 4 | surroundings. If it senses an object, it turns on its yellow LED 5 | and attempts to face towards that object. */ 6 | 7 | #include 8 | #include 9 | 10 | // Change next line to this if you are using the older Zumo 32U4 11 | // with a black and green LCD display: 12 | // Zumo32U4LCD display; 13 | Zumo32U4OLED display; 14 | 15 | Zumo32U4Motors motors; 16 | Zumo32U4ProximitySensors proxSensors; 17 | Zumo32U4ButtonA buttonA; 18 | 19 | // A sensors reading must be greater than or equal to this 20 | // threshold in order for the program to consider that sensor as 21 | // seeing an object. 22 | const uint8_t sensorThreshold = 1; 23 | 24 | // The maximum speed to drive the motors while turning. 400 is 25 | // full speed. 26 | const uint16_t turnSpeedMax = 400; 27 | 28 | // The minimum speed to drive the motors while turning. 400 is 29 | // full speed. 30 | const uint16_t turnSpeedMin = 100; 31 | 32 | // The amount to decrease the motor speed by during each cycle 33 | // when an object is seen. 34 | const uint16_t deceleration = 10; 35 | 36 | // The amount to increase the speed by during each cycle when an 37 | // object is not seen. 38 | const uint16_t acceleration = 10; 39 | 40 | #define LEFT 0 41 | #define RIGHT 1 42 | 43 | // Stores the last indication from the sensors about what 44 | // direction to turn to face the object. When no object is seen, 45 | // this variable helps us make a good guess about which direction 46 | // to turn. 47 | bool senseDir = RIGHT; 48 | 49 | // True if the robot is turning left (counter-clockwise). 50 | bool turningLeft = false; 51 | 52 | // True if the robot is turning right (clockwise). 53 | bool turningRight = false; 54 | 55 | // If the robot is turning, this is the speed it will use. 56 | uint16_t turnSpeed = turnSpeedMax; 57 | 58 | // The time, in milliseconds, when an object was last seen. 59 | uint16_t lastTimeObjectSeen = 0; 60 | 61 | void setup() 62 | { 63 | proxSensors.initFrontSensor(); 64 | 65 | // Wait for the user to press A before driving the motors. 66 | display.clear(); 67 | display.print(F("Press A")); 68 | buttonA.waitForButton(); 69 | display.clear(); 70 | } 71 | 72 | void turnRight() 73 | { 74 | motors.setSpeeds(turnSpeed, -turnSpeed); 75 | turningLeft = false; 76 | turningRight = true; 77 | } 78 | 79 | void turnLeft() 80 | { 81 | motors.setSpeeds(-turnSpeed, turnSpeed); 82 | turningLeft = true; 83 | turningRight = false; 84 | } 85 | 86 | void stop() 87 | { 88 | motors.setSpeeds(0, 0); 89 | turningLeft = false; 90 | turningRight = false; 91 | } 92 | 93 | void loop() 94 | { 95 | // Read the front proximity sensor and gets its left value (the 96 | // amount of reflectance detected while using the left LEDs) 97 | // and right value. 98 | proxSensors.read(); 99 | uint8_t leftValue = proxSensors.countsFrontWithLeftLeds(); 100 | uint8_t rightValue = proxSensors.countsFrontWithRightLeds(); 101 | 102 | // Determine if an object is visible or not. 103 | bool objectSeen = leftValue >= sensorThreshold || rightValue >= sensorThreshold; 104 | 105 | if (objectSeen) 106 | { 107 | // An object is visible, so we will start decelerating in 108 | // order to help the robot find the object without 109 | // overshooting or oscillating. 110 | turnSpeed -= deceleration; 111 | } 112 | else 113 | { 114 | // An object is not visible, so we will accelerate in order 115 | // to help find the object sooner. 116 | turnSpeed += acceleration; 117 | } 118 | 119 | // Constrain the turn speed so it is between turnSpeedMin and 120 | // turnSpeedMax. 121 | turnSpeed = constrain(turnSpeed, turnSpeedMin, turnSpeedMax); 122 | 123 | if (objectSeen) 124 | { 125 | // An object seen. 126 | ledYellow(1); 127 | 128 | lastTimeObjectSeen = millis(); 129 | 130 | if (leftValue < rightValue) 131 | { 132 | // The right value is greater, so the object is probably 133 | // closer to the robot's right LEDs, which means the robot 134 | // is not facing it directly. Turn to the right to try to 135 | // make it more even. 136 | turnRight(); 137 | senseDir = RIGHT; 138 | } 139 | else if (leftValue > rightValue) 140 | { 141 | // The left value is greater, so turn to the left. 142 | turnLeft(); 143 | senseDir = LEFT; 144 | } 145 | else 146 | { 147 | // The values are equal, so stop the motors. 148 | stop(); 149 | } 150 | } 151 | else 152 | { 153 | // No object is seen, so just keep turning in the direction 154 | // that we last sensed the object. 155 | ledYellow(0); 156 | 157 | if (senseDir == RIGHT) 158 | { 159 | turnRight(); 160 | } 161 | else 162 | { 163 | turnLeft(); 164 | } 165 | } 166 | 167 | display.gotoXY(0, 0); 168 | display.print(leftValue); 169 | display.print(' '); 170 | display.print(rightValue); 171 | display.gotoXY(0, 1); 172 | display.print(turningRight ? 'R' : (turningLeft ? 'L' : ' ')); 173 | display.print(' '); 174 | display.print(turnSpeed); 175 | display.print(' '); 176 | display.print(' '); 177 | } 178 | -------------------------------------------------------------------------------- /examples/FaceUphill/FaceUphill.ino: -------------------------------------------------------------------------------- 1 | /* This demo uses the Zumo 32U4's accelerometer to detect 2 | whether it is on a slanted surface. If it is on a slanted 3 | surface, then it uses the motors to face uphill. 4 | 5 | It also uses the encoders to avoid rolling down the surface. 6 | 7 | Please note that this example is not very robust and it might be 8 | hard to modify it to behave differently. The accelerometer 9 | readings are affected by the movement of the Zumo, so if you 10 | change the code for controlling the motors, you might also affect 11 | the accelerometer readings. 12 | 13 | This code was tested on a Zumo with 4 NiMH batteries and two 75:1 14 | HP micro metal gearmotors. If you have different batteries or 15 | motors, you might need to adjust the formula for computing 16 | turnSpeed. 17 | 18 | Also, if the robot is pointing directly downhill, it might not 19 | move, because the y component of the acceleration would be close 20 | to 0. */ 21 | 22 | #include 23 | #include 24 | 25 | const int16_t maxSpeed = 150; 26 | 27 | // Change next line to this if you are using the older Zumo 32U4 28 | // with a black and green LCD display: 29 | // Zumo32U4LCD display; 30 | Zumo32U4OLED display; 31 | 32 | Zumo32U4IMU imu; 33 | Zumo32U4Motors motors; 34 | Zumo32U4ButtonA buttonA; 35 | Zumo32U4Encoders encoders; 36 | 37 | void setup() 38 | { 39 | // Start I2C and initialize the IMU sensors. 40 | Wire.begin(); 41 | imu.init(); 42 | imu.enableDefault(); 43 | imu.configureForFaceUphill(); 44 | 45 | display.clear(); 46 | display.print(F("Press A")); 47 | buttonA.waitForPress(); 48 | display.clear(); 49 | } 50 | 51 | void loop() 52 | { 53 | // Read the accelerometer. 54 | // A value of 16384 corresponds to approximately 1 g. 55 | imu.readAcc(); 56 | int16_t x = imu.a.x; 57 | int16_t y = imu.a.y; 58 | int32_t magnitudeSquared = (int32_t)x * x + (int32_t)y * y; 59 | 60 | // Show the X and Y acceleration values on the display 61 | // every 150 ms. 62 | static uint8_t lastDisplayTime; 63 | if ((uint8_t)(millis() - lastDisplayTime) > 150) 64 | { 65 | lastDisplayTime = millis(); 66 | display.gotoXY(0, 0); 67 | display.print(x); 68 | display.print(F(" ")); 69 | display.gotoXY(0, 1); 70 | display.print(y); 71 | display.print(F(" ")); 72 | } 73 | 74 | // Use the encoders to see how much we should drive forward. 75 | // If the robot rolls downhill, the encoder counts will become 76 | // negative, resulting in a positive forwardSpeed to counteract 77 | // the rolling. 78 | int16_t forwardSpeed = -(encoders.getCountsLeft() + encoders.getCountsRight()); 79 | forwardSpeed = constrain(forwardSpeed, -maxSpeed, maxSpeed); 80 | 81 | // See if we are actually on an incline. 82 | // 16384 * sin(5 deg) = 1427 83 | int16_t turnSpeed; 84 | if (magnitudeSquared > (int32_t)1427 * 1427) 85 | { 86 | // We are on an incline of more than 5 degrees, so 87 | // try to face uphill using a feedback algorithm. 88 | turnSpeed = y / 16; 89 | ledYellow(1); 90 | } 91 | else 92 | { 93 | // We not on a noticeable incline, so don't turn. 94 | turnSpeed = 0; 95 | ledYellow(0); 96 | } 97 | 98 | // To face uphill, we need to turn so that the X acceleration 99 | // is negative and the Y acceleration is 0. Therefore, when 100 | // the Y acceleration is positive, we want to turn to the 101 | // left (counter-clockwise). 102 | int16_t leftSpeed = forwardSpeed - turnSpeed; 103 | int16_t rightSpeed = forwardSpeed + turnSpeed; 104 | 105 | // Constrain the speeds to be between -maxSpeed and maxSpeed. 106 | leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed); 107 | rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed); 108 | 109 | motors.setSpeeds(leftSpeed, rightSpeed); 110 | } 111 | 112 | -------------------------------------------------------------------------------- /examples/InertialSensors/InertialSensors.ino: -------------------------------------------------------------------------------- 1 | /* This example reads the raw values from the accelerometer, 2 | magnetometer, and gyro on the Zumo 32U4, and prints those raw 3 | values to the serial monitor. 4 | 5 | The accelerometer readings can be converted to units of g using 6 | the sensitivity specified in the LSM303 or LSM6DS33 datasheet. 7 | The default full-scale (FS) setting is +/- 2 g, so the conversion 8 | factor is 0.061 mg/LSB (least-significant bit). A raw reading of 9 | 16384 would correspond to 1 g. 10 | 11 | The gyro readings can be converted to degrees per second (dps) 12 | using the sensitivity specified in the L3GD20H or LSM6DS33 13 | datasheet. The default sensitivity is 8.75 mdps/LSB (least- 14 | significant bit). A raw reading of 10285 would correspond to 15 | 90 dps. 16 | 17 | The magnetometer readings are more difficult to interpret and 18 | will usually require calibration. */ 19 | 20 | #include 21 | #include 22 | 23 | Zumo32U4IMU imu; 24 | 25 | char report[120]; 26 | 27 | void setup() 28 | { 29 | Wire.begin(); 30 | 31 | if (!imu.init()) 32 | { 33 | // Failed to detect the compass. 34 | ledRed(1); 35 | while(1) 36 | { 37 | Serial.println(F("Failed to initialize IMU sensors.")); 38 | delay(100); 39 | } 40 | } 41 | 42 | imu.enableDefault(); 43 | } 44 | 45 | void loop() 46 | { 47 | imu.read(); 48 | 49 | snprintf_P(report, sizeof(report), 50 | PSTR("A: %6d %6d %6d M: %6d %6d %6d G: %6d %6d %6d"), 51 | imu.a.x, imu.a.y, imu.a.z, 52 | imu.m.x, imu.m.y, imu.m.z, 53 | imu.g.x, imu.g.y, imu.g.z); 54 | Serial.println(report); 55 | 56 | delay(100); 57 | } 58 | -------------------------------------------------------------------------------- /examples/LineAndProximitySensors/LineAndProximitySensors.ino: -------------------------------------------------------------------------------- 1 | /** This example shows how to read from the five line sensors and 2 | three proximity sensors on the Zumo32U4 Front Sensor Array and 3 | documents the different options available for configuring those 4 | sensors. 5 | 6 | The sensor readings are shown on the display and also printed to 7 | the serial monitor. */ 8 | 9 | #include 10 | #include 11 | 12 | // Change next line to this if you are using the older Zumo 32U4 13 | // with a black and green LCD display: 14 | // Zumo32U4LCD display; 15 | Zumo32U4OLED display; 16 | 17 | Zumo32U4LineSensors lineSensors; 18 | Zumo32U4ProximitySensors proxSensors; 19 | 20 | uint16_t lineSensorValues[5] = { 0, 0, 0, 0, 0 }; 21 | 22 | bool proxLeftActive; 23 | bool proxFrontActive; 24 | bool proxRightActive; 25 | 26 | void setup() 27 | { 28 | // The code below shows several ways to configure the line and 29 | // proximity sensors. If you don't want to use confiuration 1, 30 | // then comment it out and uncomment one of the other 31 | // configurations. 32 | 33 | /* Configuration 1: 34 | * - 3 line sensors (1, 3, 5) 35 | * - 3 proximity sensors (left, front, right) 36 | * 37 | * For this configuration to work, jumpers on the front sensor 38 | * array must be installed in order to connect pin 4 to RGT and 39 | * connect pin 20 to LFT. This is a good configuration for a 40 | * sumo robot. */ 41 | lineSensors.initThreeSensors(); 42 | proxSensors.initThreeSensors(); 43 | 44 | /* Configuration 2: 45 | * - 5 line sensors (1, 2, 3, 4, 5) 46 | * - 1 proximity sensor (front) 47 | * 48 | * For this configuration to work, jumpers on the front sensor 49 | * array must be installed in order to connect pin 4 to DN4 and 50 | * pin 20 to DN2. This is a good configuration for a line 51 | * follower or maze solver. */ 52 | //lineSensors.initFiveSensors(); 53 | //proxSensors.initFrontSensor(); 54 | 55 | /* Configuration 3: 56 | * - 4 line sensors (1, 3, 4, 5) 57 | * - 2 proximity sensors (left, front) 58 | * 59 | * For this configuration to work, jumpers on the front sensor 60 | * array must be installed in order to connect pin 4 to DN4 and 61 | * pin 20 to LFT. */ 62 | //uint8_t lineSensorPins[] = { SENSOR_DOWN1, SENSOR_DOWN3, SENSOR_DOWN4, SENSOR_DOWN5 }; 63 | //lineSensors.init(lineSensorPins, sizeof(lineSensorPins)); 64 | //uint8_t proxSensorPins[] = { SENSOR_LEFT, SENSOR_FRONT }; 65 | //proxSensors.init(proxSensorPins, sizeof(proxSensorPins)); 66 | 67 | /* Configuration 4: 68 | * - 4 line sensors (1, 2, 3, 5) 69 | * - 2 proximity sensors (front, right) 70 | * 71 | * For this configuration to work, jumpers on the front sensor 72 | * array must be installed in order to connect pin 4 to RGT and 73 | * pin 20 to DN2. */ 74 | //uint8_t lineSensorPins[] = { SENSOR_DOWN1, SENSOR_DOWN2, SENSOR_DOWN3, SENSOR_DOWN5 }; 75 | //lineSensors.init(lineSensorPins, sizeof(lineSensorPins)); 76 | //uint8_t proxSensorPins[] = { SENSOR_FRONT, SENSOR_RIGHT }; 77 | //proxSensors.init(proxSensorPins, sizeof(proxSensorPins)); 78 | 79 | /* Configuration 5: 80 | * This is the same as configuration 1 except that all the pins 81 | * and parameters being used have been written explicitly, so 82 | * this is a good starting point if you want to do something 83 | * customized. 84 | * 85 | * If you use custom pins for your sensors, then the helpers 86 | * used in this example like countsRightWithLeftLeds() will no 87 | * longer know which sensor to use. Instead, you should use 88 | * countsWithLeftLeds(sensorNumber) and 89 | * coutnsWithRightLeds(sensorNumber). 90 | * 91 | * In the code below, 2000 is timeout value (in milliseconds) 92 | * for the line sensors, and SENSOR_LEDON is the pin number for 93 | * the pin that controls the line sensor emitters. */ 94 | //uint8_t lineSensorPins[] = { SENSOR_DOWN1, SENSOR_DOWN3, SENSOR_DOWN5 }; 95 | //lineSensors.init(lineSensorPins, sizeof(lineSensorPins), 2000, SENSOR_LEDON); 96 | //uint8_t proxSensorPins[] = { SENSOR_LEFT, SENSOR_FRONT, SENSOR_RIGHT }; 97 | //proxSensors.init(proxSensorPins, sizeof(proxSensorPins), SENSOR_LEDON); 98 | 99 | /* After setting up the proximity sensors with one of the 100 | * methods above, you can also customize their operation: */ 101 | //proxSensors.setPeriod(420); 102 | //proxSensors.setPulseOnTimeUs(421); 103 | //proxSensors.setPulseOffTimeUs(578); 104 | //uint16_t levels[] = { 4, 15, 32, 55, 85, 120 }; 105 | //proxSensors.setBrightnessLevels(levels, sizeof(levels)/2); 106 | 107 | loadCustomCharacters(); 108 | 109 | calibrateLineSensors(); 110 | } 111 | 112 | // This function calibrates the line sensors for about 10 113 | // seconds. During this time, you should move the robot around 114 | // manually so that each of its line sensors sees a full black 115 | // surface and a full white surface. For the best calibration 116 | // results, you should also avoid exposing the sensors to 117 | // abnormal conditions during this time. 118 | void calibrateLineSensors() 119 | { 120 | // To indicate we are in calibration mode, turn on the yellow LED 121 | // and print "Line cal" on the display. 122 | ledYellow(1); 123 | display.clear(); 124 | display.print(F("Line cal")); 125 | 126 | for (uint16_t i = 0; i < 400; i++) 127 | { 128 | display.gotoXY(0, 1); 129 | display.print(i); 130 | lineSensors.calibrate(); 131 | } 132 | 133 | ledYellow(0); 134 | display.clear(); 135 | } 136 | 137 | void loadCustomCharacters() 138 | { 139 | static const char levels[] PROGMEM = { 140 | 0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63 141 | }; 142 | display.loadCustomCharacter(levels + 0, 0); // 1 bar 143 | display.loadCustomCharacter(levels + 1, 1); // 2 bars 144 | display.loadCustomCharacter(levels + 2, 2); // 3 bars 145 | display.loadCustomCharacter(levels + 3, 3); // 4 bars 146 | display.loadCustomCharacter(levels + 4, 4); // 5 bars 147 | display.loadCustomCharacter(levels + 5, 5); // 6 bars 148 | display.loadCustomCharacter(levels + 6, 6); // 7 bars 149 | } 150 | 151 | void printBar(uint8_t height) 152 | { 153 | if (height > 8) { height = 8; } 154 | const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 155 | display.print(barChars[height]); 156 | } 157 | 158 | // Prints a bar graph showing all the sensor readings on the display. 159 | void printReadingsToDisplay() 160 | { 161 | // On the first line of the display, show proximity sensor 162 | // readings. 163 | display.gotoXY(0, 0); 164 | printBar(proxSensors.countsLeftWithLeftLeds()); 165 | printBar(proxSensors.countsLeftWithRightLeds()); 166 | display.print(' '); 167 | printBar(proxSensors.countsFrontWithLeftLeds()); 168 | printBar(proxSensors.countsFrontWithRightLeds()); 169 | display.print(' '); 170 | printBar(proxSensors.countsRightWithLeftLeds()); 171 | printBar(proxSensors.countsRightWithRightLeds()); 172 | 173 | // On the second line of the display, show line sensor readings. 174 | // These calibrated sensor readings are between 0 and 1000, so 175 | // we use the map function to rescale it to 0 through 8. 176 | display.gotoXY(0, 1); 177 | for (uint8_t i = 0; i < 5; i++) 178 | { 179 | uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8); 180 | printBar(barHeight); 181 | } 182 | 183 | // On the last 3 characters of the second line, display basic 184 | // readings of the sensors taken without sending IR pulses. 185 | display.gotoXY(5, 1); 186 | printBar(proxLeftActive); 187 | printBar(proxFrontActive); 188 | printBar(proxRightActive); 189 | } 190 | 191 | // Prints a line with all the sensor readings to the serial 192 | // monitor. 193 | void printReadingsToSerial() 194 | { 195 | static char buffer[80]; 196 | sprintf(buffer, "%d %d %d %d %d %d %d %d %d %4d %4d %4d %4d %4d\n", 197 | proxSensors.countsLeftWithLeftLeds(), 198 | proxSensors.countsLeftWithRightLeds(), 199 | proxSensors.countsFrontWithLeftLeds(), 200 | proxSensors.countsFrontWithRightLeds(), 201 | proxSensors.countsRightWithLeftLeds(), 202 | proxSensors.countsRightWithRightLeds(), 203 | proxLeftActive, 204 | proxFrontActive, 205 | proxRightActive, 206 | lineSensorValues[0], 207 | lineSensorValues[1], 208 | lineSensorValues[2], 209 | lineSensorValues[3], 210 | lineSensorValues[4] 211 | ); 212 | Serial.print(buffer); 213 | } 214 | 215 | void loop() 216 | { 217 | static uint16_t lastSampleTime = 0; 218 | 219 | if ((uint16_t)(millis() - lastSampleTime) >= 100) 220 | { 221 | lastSampleTime = millis(); 222 | 223 | // Send IR pulses and read the proximity sensors. 224 | proxSensors.read(); 225 | 226 | // Just read the proximity sensors without sending pulses. 227 | proxLeftActive = proxSensors.readBasicLeft(); 228 | proxFrontActive = proxSensors.readBasicFront(); 229 | proxRightActive = proxSensors.readBasicRight(); 230 | 231 | // Read the line sensors. 232 | lineSensors.readCalibrated(lineSensorValues); 233 | 234 | // Send the results to the display and to the serial monitor. 235 | printReadingsToDisplay(); 236 | printReadingsToSerial(); 237 | } 238 | } 239 | -------------------------------------------------------------------------------- /examples/LineAndProximitySensors/extended_lcd_font.h: -------------------------------------------------------------------------------- 1 | #define font_width 8 2 | #define font_height 1120 3 | static unsigned char font_bits[] = { 4 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4f, 0x00, 0x00, 0x00, 0x07, 5 | 0x00, 0x07, 0x00, 0x14, 0x7f, 0x14, 0x7f, 0x14, 0x24, 0x2a, 0x7f, 0x2a, 6 | 0x12, 0x23, 0x13, 0x08, 0x64, 0x62, 0x36, 0x49, 0x55, 0x22, 0x50, 0x00, 7 | 0x00, 0x07, 0x00, 0x00, 0x00, 0x1c, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 8 | 0x1c, 0x00, 0x14, 0x08, 0x3e, 0x08, 0x14, 0x08, 0x08, 0x3e, 0x08, 0x08, 9 | 0x00, 0x50, 0x30, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x60, 10 | 0x60, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x3e, 0x51, 0x49, 0x45, 11 | 0x3e, 0x00, 0x42, 0x7f, 0x40, 0x00, 0x42, 0x61, 0x51, 0x49, 0x46, 0x21, 12 | 0x41, 0x45, 0x4b, 0x31, 0x18, 0x14, 0x12, 0x7f, 0x10, 0x27, 0x45, 0x45, 13 | 0x45, 0x39, 0x3c, 0x4a, 0x49, 0x49, 0x30, 0x03, 0x01, 0x71, 0x09, 0x07, 14 | 0x36, 0x49, 0x49, 0x49, 0x36, 0x06, 0x49, 0x49, 0x29, 0x1e, 0x00, 0x36, 15 | 0x36, 0x00, 0x00, 0x00, 0x56, 0x36, 0x00, 0x00, 0x08, 0x14, 0x22, 0x41, 16 | 0x00, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 17 | 0x01, 0x51, 0x09, 0x06, 0x32, 0x49, 0x79, 0x41, 0x3e, 0x7e, 0x11, 0x11, 18 | 0x11, 0x7e, 0x7f, 0x49, 0x49, 0x49, 0x36, 0x3e, 0x41, 0x41, 0x41, 0x22, 19 | 0x7f, 0x41, 0x41, 0x41, 0x3e, 0x7f, 0x49, 0x49, 0x49, 0x41, 0x7f, 0x09, 20 | 0x09, 0x09, 0x01, 0x3e, 0x41, 0x49, 0x49, 0x7a, 0x7f, 0x08, 0x08, 0x08, 21 | 0x7f, 0x00, 0x41, 0x7f, 0x41, 0x00, 0x20, 0x40, 0x41, 0x3f, 0x01, 0x7f, 22 | 0x08, 0x14, 0x22, 0x41, 0x7f, 0x40, 0x40, 0x40, 0x40, 0x7f, 0x02, 0x0c, 23 | 0x02, 0x7f, 0x7f, 0x04, 0x08, 0x10, 0x7f, 0x3e, 0x41, 0x41, 0x41, 0x3e, 24 | 0x7f, 0x09, 0x09, 0x09, 0x06, 0x3e, 0x41, 0x51, 0x21, 0x5e, 0x7f, 0x09, 25 | 0x19, 0x29, 0x46, 0x46, 0x49, 0x49, 0x49, 0x31, 0x01, 0x01, 0x7f, 0x01, 26 | 0x01, 0x3f, 0x40, 0x40, 0x40, 0x3f, 0x1f, 0x20, 0x40, 0x20, 0x1f, 0x3f, 27 | 0x40, 0x38, 0x40, 0x3f, 0x63, 0x14, 0x08, 0x14, 0x63, 0x07, 0x08, 0x70, 28 | 0x08, 0x07, 0x61, 0x51, 0x49, 0x45, 0x43, 0x00, 0x7f, 0x41, 0x41, 0x00, 29 | 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x41, 0x41, 0x7f, 0x00, 0x04, 0x02, 30 | 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x01, 0x02, 0x04, 31 | 0x00, 0x20, 0x54, 0x54, 0x54, 0x78, 0x7f, 0x48, 0x44, 0x44, 0x38, 0x38, 32 | 0x44, 0x44, 0x44, 0x20, 0x38, 0x44, 0x44, 0x48, 0x7f, 0x38, 0x54, 0x54, 33 | 0x54, 0x18, 0x08, 0x7e, 0x09, 0x01, 0x02, 0x0c, 0x52, 0x52, 0x52, 0x3e, 34 | 0x7f, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7d, 0x40, 0x00, 0x20, 0x40, 35 | 0x44, 0x3d, 0x00, 0x7f, 0x10, 0x28, 0x44, 0x00, 0x00, 0x41, 0x7f, 0x40, 36 | 0x00, 0x7c, 0x04, 0x18, 0x04, 0x78, 0x7c, 0x08, 0x04, 0x04, 0x78, 0x38, 37 | 0x44, 0x44, 0x44, 0x38, 0x7c, 0x14, 0x14, 0x14, 0x08, 0x08, 0x14, 0x14, 38 | 0x18, 0x7c, 0x7c, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x20, 39 | 0x04, 0x3f, 0x44, 0x40, 0x20, 0x3c, 0x40, 0x40, 0x20, 0x7c, 0x1c, 0x20, 40 | 0x40, 0x20, 0x1c, 0x3c, 0x40, 0x38, 0x40, 0x3c, 0x44, 0x28, 0x10, 0x28, 41 | 0x44, 0x0c, 0x50, 0x50, 0x50, 0x3c, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 42 | 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x41, 0x36, 43 | 0x08, 0x00, 0x08, 0x08, 0x2a, 0x1c, 0x08, 0x08, 0x1c, 0x2a, 0x08, 0x08, 44 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 45 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 46 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 47 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 48 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 49 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 53 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 54 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 58 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 59 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 60 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 61 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 62 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 63 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 64 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 65 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 66 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 67 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 68 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 69 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 75 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 79 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 82 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 83 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 84 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 86 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 88 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 89 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 90 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 92 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 93 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x3c, 0x04, 0x7c, 0x44, 94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 95 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 96 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 97 | 0xff, 0xff, 0xff, 0xff }; 98 | -------------------------------------------------------------------------------- /examples/LineAndProximitySensors/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Replaces the default OLED font with an extended font that incorporates 4 | // several non-ASCII glyphs from the original HD44780 character set: the 5 | // forward arrow, Greek pi, centered dot, and full box. 6 | // 7 | // This file uses a #define hack to include a font file stored in a special 8 | // C-compatible format called X BitMap or "xbm". To edit the font, you will 9 | // need an editor that can work with this format, such as GIMP: 10 | // 11 | // https://www.gimp.org/ 12 | // 13 | // For compatibility with the PololuOLED library, characters run 14 | // vertically in an 8-by-1120 pixel image, using five rows per character 15 | // and no space in between. Also, the characters are mirrored, to match the 16 | // bit order used in the library. 17 | // 18 | // It is also allowed to use a smaller (shorter) image file, if you are not 19 | // using all the characters. For example, you could use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f. 21 | // 22 | // You can open original_font.xbm from the PololuOLED library as a starting 23 | // point. 24 | // 25 | // Consider rotating and mirroring the original font file so that you can see 26 | // the letters in their proper orientation, transforming them back before 27 | // exporting the font to "font.xbm" and renaming to "font.h". 28 | // 29 | // Open font.h in a text editor to make sure that it declares a variable 30 | // exactly like this: 31 | // 32 | // static unsigned char font_bits[] 33 | // 34 | // Our "hack" is to redefine the name font_bits as a macro below to turn that 35 | // declaration into a replacement of the variable pololuOledFont in the 36 | // library. If it doesn't match exactly, you will either get an error or 37 | // still have the original font. 38 | 39 | #define font_bits unused_placeholder; extern const uint8_t PROGMEM pololuOledFont 40 | #include "extended_lcd_font.h" 41 | -------------------------------------------------------------------------------- /examples/LineFollower/LineFollower.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the line sensors on the Zumo 32U4 to follow 2 | a black line on a white background, using a PID-based algorithm. 3 | It works decently on courses with smooth, 6" radius curves and 4 | has been tested with Zumos using 75:1 HP motors. Modifications 5 | might be required for it to work well on different courses or 6 | with different motors. 7 | 8 | This demo requires a Zumo 32U4 Front Sensor Array to be 9 | connected, and jumpers on the front sensor array must be 10 | installed in order to connect pin 4 to DN4 and pin 20 to DN2. */ 11 | 12 | #include 13 | #include 14 | 15 | // This is the maximum speed the motors will be allowed to turn. 16 | // A maxSpeed of 400 lets the motors go at top speed. Decrease 17 | // this value to impose a speed limit. 18 | const uint16_t maxSpeed = 400; 19 | 20 | Zumo32U4Buzzer buzzer; 21 | Zumo32U4LineSensors lineSensors; 22 | Zumo32U4Motors motors; 23 | Zumo32U4ButtonA buttonA; 24 | 25 | // Change next line to this if you are using the older Zumo 32U4 26 | // with a black and green LCD display: 27 | // Zumo32U4LCD display; 28 | Zumo32U4OLED display; 29 | 30 | int16_t lastError = 0; 31 | 32 | #define NUM_SENSORS 5 33 | unsigned int lineSensorValues[NUM_SENSORS]; 34 | 35 | // Sets up special characters for the display so that we can show 36 | // bar graphs. 37 | void loadCustomCharacters() 38 | { 39 | static const char levels[] PROGMEM = { 40 | 0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63 41 | }; 42 | display.loadCustomCharacter(levels + 0, 0); // 1 bar 43 | display.loadCustomCharacter(levels + 1, 1); // 2 bars 44 | display.loadCustomCharacter(levels + 2, 2); // 3 bars 45 | display.loadCustomCharacter(levels + 3, 3); // 4 bars 46 | display.loadCustomCharacter(levels + 4, 4); // 5 bars 47 | display.loadCustomCharacter(levels + 5, 5); // 6 bars 48 | display.loadCustomCharacter(levels + 6, 6); // 7 bars 49 | } 50 | 51 | void printBar(uint8_t height) 52 | { 53 | if (height > 8) { height = 8; } 54 | const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 55 | display.print(barChars[height]); 56 | } 57 | 58 | void calibrateSensors() 59 | { 60 | display.clear(); 61 | 62 | // Wait 1 second and then begin automatic sensor calibration 63 | // by rotating in place to sweep the sensors over the line 64 | delay(1000); 65 | for(uint16_t i = 0; i < 120; i++) 66 | { 67 | if (i > 30 && i <= 90) 68 | { 69 | motors.setSpeeds(-200, 200); 70 | } 71 | else 72 | { 73 | motors.setSpeeds(200, -200); 74 | } 75 | 76 | lineSensors.calibrate(); 77 | } 78 | motors.setSpeeds(0, 0); 79 | } 80 | 81 | // Shows a bar graph of sensor readings on the display. 82 | // Returns after the user presses A. 83 | void showReadings() 84 | { 85 | display.clear(); 86 | 87 | while(!buttonA.getSingleDebouncedPress()) 88 | { 89 | lineSensors.readCalibrated(lineSensorValues); 90 | 91 | display.gotoXY(0, 0); 92 | for (uint8_t i = 0; i < NUM_SENSORS; i++) 93 | { 94 | uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8); 95 | printBar(barHeight); 96 | } 97 | } 98 | } 99 | 100 | void setup() 101 | { 102 | // Uncomment if necessary to correct motor directions: 103 | //motors.flipLeftMotor(true); 104 | //motors.flipRightMotor(true); 105 | 106 | lineSensors.initFiveSensors(); 107 | 108 | loadCustomCharacters(); 109 | 110 | // Play a little welcome song 111 | buzzer.play(">g32>>c32"); 112 | 113 | // Wait for button A to be pressed and released. 114 | display.clear(); 115 | display.print(F("Press A")); 116 | display.gotoXY(0, 1); 117 | display.print(F("to calib")); 118 | buttonA.waitForButton(); 119 | 120 | calibrateSensors(); 121 | 122 | showReadings(); 123 | 124 | // Play music and wait for it to finish before we start driving. 125 | display.clear(); 126 | display.print(F("Go!")); 127 | buzzer.play("L16 cdegreg4"); 128 | while(buzzer.isPlaying()); 129 | } 130 | 131 | void loop() 132 | { 133 | // Get the position of the line. Note that we *must* provide 134 | // the "lineSensorValues" argument to readLine() here, even 135 | // though we are not interested in the individual sensor 136 | // readings. 137 | int16_t position = lineSensors.readLine(lineSensorValues); 138 | 139 | // Our "error" is how far we are away from the center of the 140 | // line, which corresponds to position 2000. 141 | int16_t error = position - 2000; 142 | 143 | // Get motor speed difference using proportional and derivative 144 | // PID terms (the integral term is generally not very useful 145 | // for line following). Here we are using a proportional 146 | // constant of 1/4 and a derivative constant of 6, which should 147 | // work decently for many Zumo motor choices. You probably 148 | // want to use trial and error to tune these constants for your 149 | // particular Zumo and line course. 150 | int16_t speedDifference = error / 4 + 6 * (error - lastError); 151 | 152 | lastError = error; 153 | 154 | // Get individual motor speeds. The sign of speedDifference 155 | // determines if the robot turns left or right. 156 | int16_t leftSpeed = (int16_t)maxSpeed + speedDifference; 157 | int16_t rightSpeed = (int16_t)maxSpeed - speedDifference; 158 | 159 | // Constrain our motor speeds to be between 0 and maxSpeed. 160 | // One motor will always be turning at maxSpeed, and the other 161 | // will be at maxSpeed-|speedDifference| if that is positive, 162 | // else it will be stationary. For some applications, you 163 | // might want to allow the motor speed to go negative so that 164 | // it can spin in reverse. 165 | leftSpeed = constrain(leftSpeed, 0, (int16_t)maxSpeed); 166 | rightSpeed = constrain(rightSpeed, 0, (int16_t)maxSpeed); 167 | 168 | motors.setSpeeds(leftSpeed, rightSpeed); 169 | } 170 | -------------------------------------------------------------------------------- /examples/LineFollower/extended_lcd_font.h: -------------------------------------------------------------------------------- 1 | #define font_width 8 2 | #define font_height 1120 3 | static unsigned char font_bits[] = { 4 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4f, 0x00, 0x00, 0x00, 0x07, 5 | 0x00, 0x07, 0x00, 0x14, 0x7f, 0x14, 0x7f, 0x14, 0x24, 0x2a, 0x7f, 0x2a, 6 | 0x12, 0x23, 0x13, 0x08, 0x64, 0x62, 0x36, 0x49, 0x55, 0x22, 0x50, 0x00, 7 | 0x00, 0x07, 0x00, 0x00, 0x00, 0x1c, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 8 | 0x1c, 0x00, 0x14, 0x08, 0x3e, 0x08, 0x14, 0x08, 0x08, 0x3e, 0x08, 0x08, 9 | 0x00, 0x50, 0x30, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x60, 10 | 0x60, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x3e, 0x51, 0x49, 0x45, 11 | 0x3e, 0x00, 0x42, 0x7f, 0x40, 0x00, 0x42, 0x61, 0x51, 0x49, 0x46, 0x21, 12 | 0x41, 0x45, 0x4b, 0x31, 0x18, 0x14, 0x12, 0x7f, 0x10, 0x27, 0x45, 0x45, 13 | 0x45, 0x39, 0x3c, 0x4a, 0x49, 0x49, 0x30, 0x03, 0x01, 0x71, 0x09, 0x07, 14 | 0x36, 0x49, 0x49, 0x49, 0x36, 0x06, 0x49, 0x49, 0x29, 0x1e, 0x00, 0x36, 15 | 0x36, 0x00, 0x00, 0x00, 0x56, 0x36, 0x00, 0x00, 0x08, 0x14, 0x22, 0x41, 16 | 0x00, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 17 | 0x01, 0x51, 0x09, 0x06, 0x32, 0x49, 0x79, 0x41, 0x3e, 0x7e, 0x11, 0x11, 18 | 0x11, 0x7e, 0x7f, 0x49, 0x49, 0x49, 0x36, 0x3e, 0x41, 0x41, 0x41, 0x22, 19 | 0x7f, 0x41, 0x41, 0x41, 0x3e, 0x7f, 0x49, 0x49, 0x49, 0x41, 0x7f, 0x09, 20 | 0x09, 0x09, 0x01, 0x3e, 0x41, 0x49, 0x49, 0x7a, 0x7f, 0x08, 0x08, 0x08, 21 | 0x7f, 0x00, 0x41, 0x7f, 0x41, 0x00, 0x20, 0x40, 0x41, 0x3f, 0x01, 0x7f, 22 | 0x08, 0x14, 0x22, 0x41, 0x7f, 0x40, 0x40, 0x40, 0x40, 0x7f, 0x02, 0x0c, 23 | 0x02, 0x7f, 0x7f, 0x04, 0x08, 0x10, 0x7f, 0x3e, 0x41, 0x41, 0x41, 0x3e, 24 | 0x7f, 0x09, 0x09, 0x09, 0x06, 0x3e, 0x41, 0x51, 0x21, 0x5e, 0x7f, 0x09, 25 | 0x19, 0x29, 0x46, 0x46, 0x49, 0x49, 0x49, 0x31, 0x01, 0x01, 0x7f, 0x01, 26 | 0x01, 0x3f, 0x40, 0x40, 0x40, 0x3f, 0x1f, 0x20, 0x40, 0x20, 0x1f, 0x3f, 27 | 0x40, 0x38, 0x40, 0x3f, 0x63, 0x14, 0x08, 0x14, 0x63, 0x07, 0x08, 0x70, 28 | 0x08, 0x07, 0x61, 0x51, 0x49, 0x45, 0x43, 0x00, 0x7f, 0x41, 0x41, 0x00, 29 | 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x41, 0x41, 0x7f, 0x00, 0x04, 0x02, 30 | 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x01, 0x02, 0x04, 31 | 0x00, 0x20, 0x54, 0x54, 0x54, 0x78, 0x7f, 0x48, 0x44, 0x44, 0x38, 0x38, 32 | 0x44, 0x44, 0x44, 0x20, 0x38, 0x44, 0x44, 0x48, 0x7f, 0x38, 0x54, 0x54, 33 | 0x54, 0x18, 0x08, 0x7e, 0x09, 0x01, 0x02, 0x0c, 0x52, 0x52, 0x52, 0x3e, 34 | 0x7f, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7d, 0x40, 0x00, 0x20, 0x40, 35 | 0x44, 0x3d, 0x00, 0x7f, 0x10, 0x28, 0x44, 0x00, 0x00, 0x41, 0x7f, 0x40, 36 | 0x00, 0x7c, 0x04, 0x18, 0x04, 0x78, 0x7c, 0x08, 0x04, 0x04, 0x78, 0x38, 37 | 0x44, 0x44, 0x44, 0x38, 0x7c, 0x14, 0x14, 0x14, 0x08, 0x08, 0x14, 0x14, 38 | 0x18, 0x7c, 0x7c, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x20, 39 | 0x04, 0x3f, 0x44, 0x40, 0x20, 0x3c, 0x40, 0x40, 0x20, 0x7c, 0x1c, 0x20, 40 | 0x40, 0x20, 0x1c, 0x3c, 0x40, 0x38, 0x40, 0x3c, 0x44, 0x28, 0x10, 0x28, 41 | 0x44, 0x0c, 0x50, 0x50, 0x50, 0x3c, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 42 | 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x41, 0x36, 43 | 0x08, 0x00, 0x08, 0x08, 0x2a, 0x1c, 0x08, 0x08, 0x1c, 0x2a, 0x08, 0x08, 44 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 45 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 46 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 47 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 48 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 49 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 53 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 54 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 58 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 59 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 60 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 61 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 62 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 63 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 64 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 65 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 66 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 67 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 68 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 69 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 75 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 79 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 82 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 83 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 84 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 86 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 88 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 89 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 90 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 92 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 93 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x3c, 0x04, 0x7c, 0x44, 94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 95 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 96 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 97 | 0xff, 0xff, 0xff, 0xff }; 98 | -------------------------------------------------------------------------------- /examples/LineFollower/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Replaces the default OLED font with an extended font that incorporates 4 | // several non-ASCII glyphs from the original HD44780 character set: the 5 | // forward arrow, Greek pi, centered dot, and full box. 6 | // 7 | // This file uses a #define hack to include a font file stored in a special 8 | // C-compatible format called X BitMap or "xbm". To edit the font, you will 9 | // need an editor that can work with this format, such as GIMP: 10 | // 11 | // https://www.gimp.org/ 12 | // 13 | // For compatibility with the PololuOLED library, characters run 14 | // vertically in an 8-by-1120 pixel image, using five rows per character 15 | // and no space in between. Also, the characters are mirrored, to match the 16 | // bit order used in the library. 17 | // 18 | // It is also allowed to use a smaller (shorter) image file, if you are not 19 | // using all the characters. For example, you could use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f. 21 | // 22 | // You can open original_font.xbm from the PololuOLED library as a starting 23 | // point. 24 | // 25 | // Consider rotating and mirroring the original font file so that you can see 26 | // the letters in their proper orientation, transforming them back before 27 | // exporting the font to "font.xbm" and renaming to "font.h". 28 | // 29 | // Open font.h in a text editor to make sure that it declares a variable 30 | // exactly like this: 31 | // 32 | // static unsigned char font_bits[] 33 | // 34 | // Our "hack" is to redefine the name font_bits as a macro below to turn that 35 | // declaration into a replacement of the variable pololuOledFont in the 36 | // library. If it doesn't match exactly, you will either get an error or 37 | // still have the original font. 38 | 39 | #define font_bits unused_placeholder; extern const uint8_t PROGMEM pololuOledFont 40 | #include "extended_lcd_font.h" 41 | -------------------------------------------------------------------------------- /examples/LineSensorTest/LineSensorTest.ino: -------------------------------------------------------------------------------- 1 | /** This example shows how to read the raw values from the five 2 | line sensors on the Zumo32U4 Front Sensor Array. This example is 3 | useful if you are having trouble with the sensors or just want to 4 | characterize their behavior. 5 | 6 | The raw (uncalibrated) values are reported to the serial monitor, 7 | and also shown on the display. The first line of the display 8 | shows a bar graph of all five readings. The upper right 9 | corner shows a an "E" if the IR emitters are being used (the 10 | default) or an "e" if they are not being used. The second line 11 | shows the raw reading for the currently-selected sensor. 12 | 13 | You can press the "A" and "B" buttons to change which sensor is 14 | selected. 15 | 16 | You can press the "C" button to toggle whether the IR emitters 17 | are on during the reading. 18 | 19 | In order for the second and forth sensors to work, jumpers on the 20 | front sensor array must be installed in order to connect pin 4 to 21 | DN4 and pin 20 to DN2. */ 22 | 23 | #include 24 | #include 25 | 26 | // Change next line to this if you are using the older Zumo 32U4 27 | // with a black and green LCD display: 28 | // Zumo32U4LCD display; 29 | Zumo32U4OLED display; 30 | 31 | Zumo32U4ButtonA buttonA; 32 | Zumo32U4ButtonB buttonB; 33 | Zumo32U4ButtonC buttonC; 34 | Zumo32U4LineSensors lineSensors; 35 | Zumo32U4ProximitySensors proxSensors; 36 | 37 | #define NUM_SENSORS 5 38 | uint16_t lineSensorValues[NUM_SENSORS]; 39 | 40 | bool useEmitters = true; 41 | 42 | uint8_t selectedSensorIndex = 0; 43 | 44 | void setup() 45 | { 46 | lineSensors.initFiveSensors(); 47 | 48 | loadCustomCharacters(); 49 | } 50 | 51 | void loadCustomCharacters() 52 | { 53 | static const char levels[] PROGMEM = { 54 | 0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63 55 | }; 56 | display.loadCustomCharacter(levels + 0, 0); // 1 bar 57 | display.loadCustomCharacter(levels + 1, 1); // 2 bars 58 | display.loadCustomCharacter(levels + 2, 2); // 3 bars 59 | display.loadCustomCharacter(levels + 3, 3); // 4 bars 60 | display.loadCustomCharacter(levels + 4, 4); // 5 bars 61 | display.loadCustomCharacter(levels + 5, 5); // 6 bars 62 | display.loadCustomCharacter(levels + 6, 6); // 7 bars 63 | } 64 | 65 | void printBar(uint8_t height) 66 | { 67 | if (height > 8) { height = 8; } 68 | const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 69 | display.print(barChars[height]); 70 | } 71 | 72 | void printReadingsToDisplay() 73 | { 74 | // On the first line of the display, show the bar graph. 75 | display.gotoXY(0, 0); 76 | for (uint8_t i = 0; i < 5; i++) 77 | { 78 | uint8_t barHeight = map(lineSensorValues[i], 0, 2000, 0, 8); 79 | printBar(barHeight); 80 | } 81 | 82 | // Print "E" if the emitters are on, "e" if they are off. 83 | display.gotoXY(7, 0); 84 | display.print(useEmitters ? 'E' : 'e'); 85 | 86 | // On the second line of the display, show one raw reading. 87 | display.gotoXY(0, 1); 88 | display.print(selectedSensorIndex); 89 | display.print(F(": ")); 90 | display.print(lineSensorValues[selectedSensorIndex]); 91 | display.print(F(" ")); 92 | } 93 | 94 | // Prints a line with all the sensor readings to the serial 95 | // monitor. 96 | void printReadingsToSerial() 97 | { 98 | char buffer[80]; 99 | sprintf(buffer, "%4d %4d %4d %4d %4d %c\n", 100 | lineSensorValues[0], 101 | lineSensorValues[1], 102 | lineSensorValues[2], 103 | lineSensorValues[3], 104 | lineSensorValues[4], 105 | useEmitters ? 'E' : 'e' 106 | ); 107 | Serial.print(buffer); 108 | } 109 | 110 | void loop() 111 | { 112 | static uint16_t lastSampleTime = 0; 113 | 114 | if ((uint16_t)(millis() - lastSampleTime) >= 100) 115 | { 116 | lastSampleTime = millis(); 117 | 118 | // Read the line sensors. 119 | lineSensors.read(lineSensorValues, useEmitters ? QTR_EMITTERS_ON : QTR_EMITTERS_OFF); 120 | 121 | // Send the results to the display and to the serial monitor. 122 | printReadingsToDisplay(); 123 | printReadingsToSerial(); 124 | } 125 | 126 | // If button A is pressed, select the previous sensor. 127 | if (buttonA.getSingleDebouncedPress()) 128 | { 129 | selectedSensorIndex = (selectedSensorIndex + NUM_SENSORS - 1) % NUM_SENSORS; 130 | } 131 | 132 | // If button B is pressed, select the next sensor. 133 | if (buttonB.getSingleDebouncedPress()) 134 | { 135 | selectedSensorIndex = (selectedSensorIndex + 1) % NUM_SENSORS; 136 | } 137 | 138 | // If button C is pressed, toggle the state of the emitters. 139 | if (buttonC.getSingleDebouncedPress()) 140 | { 141 | useEmitters = !useEmitters; 142 | } 143 | } 144 | -------------------------------------------------------------------------------- /examples/LineSensorTest/extended_lcd_font.h: -------------------------------------------------------------------------------- 1 | #define font_width 8 2 | #define font_height 1120 3 | static unsigned char font_bits[] = { 4 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4f, 0x00, 0x00, 0x00, 0x07, 5 | 0x00, 0x07, 0x00, 0x14, 0x7f, 0x14, 0x7f, 0x14, 0x24, 0x2a, 0x7f, 0x2a, 6 | 0x12, 0x23, 0x13, 0x08, 0x64, 0x62, 0x36, 0x49, 0x55, 0x22, 0x50, 0x00, 7 | 0x00, 0x07, 0x00, 0x00, 0x00, 0x1c, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 8 | 0x1c, 0x00, 0x14, 0x08, 0x3e, 0x08, 0x14, 0x08, 0x08, 0x3e, 0x08, 0x08, 9 | 0x00, 0x50, 0x30, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x60, 10 | 0x60, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x3e, 0x51, 0x49, 0x45, 11 | 0x3e, 0x00, 0x42, 0x7f, 0x40, 0x00, 0x42, 0x61, 0x51, 0x49, 0x46, 0x21, 12 | 0x41, 0x45, 0x4b, 0x31, 0x18, 0x14, 0x12, 0x7f, 0x10, 0x27, 0x45, 0x45, 13 | 0x45, 0x39, 0x3c, 0x4a, 0x49, 0x49, 0x30, 0x03, 0x01, 0x71, 0x09, 0x07, 14 | 0x36, 0x49, 0x49, 0x49, 0x36, 0x06, 0x49, 0x49, 0x29, 0x1e, 0x00, 0x36, 15 | 0x36, 0x00, 0x00, 0x00, 0x56, 0x36, 0x00, 0x00, 0x08, 0x14, 0x22, 0x41, 16 | 0x00, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 17 | 0x01, 0x51, 0x09, 0x06, 0x32, 0x49, 0x79, 0x41, 0x3e, 0x7e, 0x11, 0x11, 18 | 0x11, 0x7e, 0x7f, 0x49, 0x49, 0x49, 0x36, 0x3e, 0x41, 0x41, 0x41, 0x22, 19 | 0x7f, 0x41, 0x41, 0x41, 0x3e, 0x7f, 0x49, 0x49, 0x49, 0x41, 0x7f, 0x09, 20 | 0x09, 0x09, 0x01, 0x3e, 0x41, 0x49, 0x49, 0x7a, 0x7f, 0x08, 0x08, 0x08, 21 | 0x7f, 0x00, 0x41, 0x7f, 0x41, 0x00, 0x20, 0x40, 0x41, 0x3f, 0x01, 0x7f, 22 | 0x08, 0x14, 0x22, 0x41, 0x7f, 0x40, 0x40, 0x40, 0x40, 0x7f, 0x02, 0x0c, 23 | 0x02, 0x7f, 0x7f, 0x04, 0x08, 0x10, 0x7f, 0x3e, 0x41, 0x41, 0x41, 0x3e, 24 | 0x7f, 0x09, 0x09, 0x09, 0x06, 0x3e, 0x41, 0x51, 0x21, 0x5e, 0x7f, 0x09, 25 | 0x19, 0x29, 0x46, 0x46, 0x49, 0x49, 0x49, 0x31, 0x01, 0x01, 0x7f, 0x01, 26 | 0x01, 0x3f, 0x40, 0x40, 0x40, 0x3f, 0x1f, 0x20, 0x40, 0x20, 0x1f, 0x3f, 27 | 0x40, 0x38, 0x40, 0x3f, 0x63, 0x14, 0x08, 0x14, 0x63, 0x07, 0x08, 0x70, 28 | 0x08, 0x07, 0x61, 0x51, 0x49, 0x45, 0x43, 0x00, 0x7f, 0x41, 0x41, 0x00, 29 | 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x41, 0x41, 0x7f, 0x00, 0x04, 0x02, 30 | 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x01, 0x02, 0x04, 31 | 0x00, 0x20, 0x54, 0x54, 0x54, 0x78, 0x7f, 0x48, 0x44, 0x44, 0x38, 0x38, 32 | 0x44, 0x44, 0x44, 0x20, 0x38, 0x44, 0x44, 0x48, 0x7f, 0x38, 0x54, 0x54, 33 | 0x54, 0x18, 0x08, 0x7e, 0x09, 0x01, 0x02, 0x0c, 0x52, 0x52, 0x52, 0x3e, 34 | 0x7f, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7d, 0x40, 0x00, 0x20, 0x40, 35 | 0x44, 0x3d, 0x00, 0x7f, 0x10, 0x28, 0x44, 0x00, 0x00, 0x41, 0x7f, 0x40, 36 | 0x00, 0x7c, 0x04, 0x18, 0x04, 0x78, 0x7c, 0x08, 0x04, 0x04, 0x78, 0x38, 37 | 0x44, 0x44, 0x44, 0x38, 0x7c, 0x14, 0x14, 0x14, 0x08, 0x08, 0x14, 0x14, 38 | 0x18, 0x7c, 0x7c, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x20, 39 | 0x04, 0x3f, 0x44, 0x40, 0x20, 0x3c, 0x40, 0x40, 0x20, 0x7c, 0x1c, 0x20, 40 | 0x40, 0x20, 0x1c, 0x3c, 0x40, 0x38, 0x40, 0x3c, 0x44, 0x28, 0x10, 0x28, 41 | 0x44, 0x0c, 0x50, 0x50, 0x50, 0x3c, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 42 | 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x41, 0x36, 43 | 0x08, 0x00, 0x08, 0x08, 0x2a, 0x1c, 0x08, 0x08, 0x1c, 0x2a, 0x08, 0x08, 44 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 45 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 46 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 47 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 48 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 49 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 53 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 54 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 58 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 59 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 60 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 61 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 62 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 63 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 64 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 65 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 66 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 67 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 68 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 69 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 75 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 79 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 82 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 83 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 84 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 86 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 88 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 89 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 90 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 92 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 93 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x3c, 0x04, 0x7c, 0x44, 94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 95 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 96 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 97 | 0xff, 0xff, 0xff, 0xff }; 98 | -------------------------------------------------------------------------------- /examples/LineSensorTest/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Replaces the default OLED font with an extended font that incorporates 4 | // several non-ASCII glyphs from the original HD44780 character set: the 5 | // forward arrow, Greek pi, centered dot, and full box. 6 | // 7 | // This file uses a #define hack to include a font file stored in a special 8 | // C-compatible format called X BitMap or "xbm". To edit the font, you will 9 | // need an editor that can work with this format, such as GIMP: 10 | // 11 | // https://www.gimp.org/ 12 | // 13 | // For compatibility with the PololuOLED library, characters run 14 | // vertically in an 8-by-1120 pixel image, using five rows per character 15 | // and no space in between. Also, the characters are mirrored, to match the 16 | // bit order used in the library. 17 | // 18 | // It is also allowed to use a smaller (shorter) image file, if you are not 19 | // using all the characters. For example, you could use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f. 21 | // 22 | // You can open original_font.xbm from the PololuOLED library as a starting 23 | // point. 24 | // 25 | // Consider rotating and mirroring the original font file so that you can see 26 | // the letters in their proper orientation, transforming them back before 27 | // exporting the font to "font.xbm" and renaming to "font.h". 28 | // 29 | // Open font.h in a text editor to make sure that it declares a variable 30 | // exactly like this: 31 | // 32 | // static unsigned char font_bits[] 33 | // 34 | // Our "hack" is to redefine the name font_bits as a macro below to turn that 35 | // declaration into a replacement of the variable pololuOledFont in the 36 | // library. If it doesn't match exactly, you will either get an error or 37 | // still have the original font. 38 | 39 | #define font_bits unused_placeholder; extern const uint8_t PROGMEM pololuOledFont 40 | #include "extended_lcd_font.h" 41 | -------------------------------------------------------------------------------- /examples/MazeSolver/MazeSolver.ino: -------------------------------------------------------------------------------- 1 | /* This code for the Zumo 32U4 will solve a line maze constructed 2 | with a black line on a white background, as long as there are no 3 | loops and all intersections are at right angles. It has two 4 | phases: first, it learns the maze with a left-hand-on-the-wall 5 | strategy and computes the most efficient path to the finish. 6 | Second, it follows the previously-computed path. 7 | 8 | This code is meant to run on a Zumo with 4 NiMH batteries and two 9 | 75:1 HP micro metal gearmotors. If you have different batteries 10 | or motors, you will probably need to adjust the timing and speed 11 | parameters in GridMovement.h. 12 | 13 | == Hardware setup == 14 | 15 | This demo requires a Zumo 32U4 Front Sensor Array to be 16 | connected, and jumpers on the front sensor array must be 17 | installed in order to connect pin 4 to DN4 and pin 20 to DN2. 18 | 19 | == Usage == 20 | 21 | First, place your Zumo on the maze. You should place it so that 22 | the line sensors are approximately centered above one of the 23 | black lines. 24 | 25 | Next, turn on the Zumo, and be careful to not move it for a few 26 | seconds afterwards while the gyro is being calibrated. During 27 | the gyro calibration, the yellow LED is on and the words "Gyro 28 | cal" are shown on the display. 29 | 30 | After the gyro calibration is done, press button A to start the 31 | line sensor calibration. The robot will turn left 90 degrees, 32 | return to center, turn right 90 degrees, and then return 33 | approximately to center. If the angles are incorrect or the 34 | robot turns in circles, then something is probably wrong with the 35 | gyro and you should power off and try again. 36 | 37 | The robot's position might shift during calibration. If none of 38 | the inner three line sensors can see the line, as indicated on 39 | the display, you should reposition it to center the sensors on the 40 | line. 41 | 42 | Press button A. The robot will explore the maze, searching for a 43 | large dark spot which indicates the end and computing the optimal 44 | path to reach it. When it reaches the end, it will beep. 45 | 46 | Move the robot back to the starting position, and press button A 47 | again. The robot will go directly to the end of the maze using 48 | the path it previously learned. */ 49 | 50 | #include 51 | #include 52 | 53 | // Change next line to this if you are using the older Zumo 32U4 54 | // with a black and green LCD display: 55 | // Zumo32U4LCD display; 56 | Zumo32U4OLED display; 57 | 58 | Zumo32U4Buzzer buzzer; 59 | Zumo32U4ButtonA buttonA; 60 | Zumo32U4Motors motors; 61 | Zumo32U4LineSensors lineSensors; 62 | Zumo32U4IMU imu; 63 | 64 | #include "TurnSensor.h" 65 | #include "GridMovement.h" 66 | 67 | // The path variable will store the path that the robot has 68 | // taken. It is stored as an array of characters, each of which 69 | // represents the turn that should be made at one intersection in 70 | // the sequence: 71 | // 'L' for left 72 | // 'R' for right 73 | // 'S' for straight (going straight through an intersection) 74 | // 'B' for back (U-turn) 75 | // 76 | // The simplifyPath() function checks for dead ends and removes 77 | // them from the path every time the robot makes a turn. 78 | char path[100]; 79 | uint8_t pathLength = 0; 80 | 81 | void setup() 82 | { 83 | // Uncomment if necessary to correct motor directions: 84 | //motors.flipLeftMotor(true); 85 | //motors.flipRightMotor(true); 86 | 87 | buzzer.playFromProgramSpace(PSTR("!>g32>>c32")); 88 | 89 | gridMovementSetup(); 90 | 91 | // mazeSolve() explores every segment of the maze until it 92 | // finds the optimal path. 93 | mazeSolve(); 94 | } 95 | 96 | void loop() 97 | { 98 | // The maze has been solved. Wait for the user to press a button. 99 | buttonA.waitForButton(); 100 | 101 | // Follow the simplified path computed earlier to reach the end 102 | // of the maze. 103 | mazeFollowPath(); 104 | } 105 | 106 | // This function decides which way to turn during the learning 107 | // phase of maze solving. It uses the variables found_left, 108 | // found_straight, and found_right, which indicate whether there 109 | // is an exit in each of the three directions, applying the 110 | // left-hand-on-the-wall strategy. 111 | char selectTurn(bool foundLeft, bool foundStraight, bool foundRight) 112 | { 113 | // Make a decision about how to turn. The following code 114 | // implements a left-hand-on-the-wall strategy, where we always 115 | // turn as far to the left as possible. 116 | if(foundLeft) { return 'L'; } 117 | else if(foundStraight) { return 'S'; } 118 | else if(foundRight) { return 'R'; } 119 | else { return 'B'; } 120 | } 121 | 122 | // The mazeSolve() function works by applying a 123 | // left-hand-on-the-wall strategy: the robot follows a segment 124 | // until it reaches an intersection, where it takes the leftmost 125 | // fork available to it. It records each turn it makes, and as 126 | // long as the maze has no loops, this strategy will eventually 127 | // lead it to the finish. Afterwards, the recorded path is 128 | // simplified by removing dead ends. More information can be 129 | // found in the 3pi maze solving example. 130 | void mazeSolve() 131 | { 132 | // Start with an empty path. We will add and remove entries to 133 | // the path as we solve the maze. 134 | pathLength = 0; 135 | 136 | // Play a starting tune. 137 | buzzer.playFromProgramSpace(PSTR("!L16 cdegreg4")); 138 | 139 | // Delay so the robot does not move while the user is still 140 | // touching the button. 141 | delay(1000); 142 | 143 | while(1) 144 | { 145 | // Navigate current line segment until we enter an intersection. 146 | followSegment(); 147 | 148 | // Drive stright forward to get to the center of the 149 | // intersection and check for exits to the left, right, and 150 | // straight ahead. 151 | bool foundLeft, foundStraight, foundRight; 152 | driveToIntersectionCenter(&foundLeft, &foundStraight, &foundRight); 153 | 154 | if(aboveDarkSpot()) 155 | { 156 | // We found the end of the maze, so we succeeded in solving 157 | // the maze. 158 | break; 159 | } 160 | 161 | // Choose a direction to turn. 162 | char dir = selectTurn(foundLeft, foundStraight, foundRight); 163 | 164 | // Make sure we don't overflow the pathLength buffer, 165 | // which could lead to unpredictable behavior of the 166 | // robot. 167 | if (pathLength >= sizeof(path)) 168 | { 169 | display.clear(); 170 | display.print(F("pathfull")); 171 | while(1) 172 | { 173 | ledRed(1); 174 | } 175 | } 176 | 177 | // Store the intersection in the path variable. 178 | path[pathLength] = dir; 179 | pathLength++; 180 | 181 | // Simplify the learned path. 182 | simplifyPath(); 183 | 184 | // Show the path on the display. 185 | displayPath(); 186 | 187 | // If the path is equal to "BB", it means we have searched the 188 | // whole maze and not found the path. We beep but 189 | // continue searching, because maybe the sensors missed 190 | // something earlier. 191 | if (pathLength == 2 && path[0] == 'B' && path[1] == 'B') 192 | { 193 | buzzer.playFromProgramSpace(PSTR("!>a32")); 204 | } 205 | 206 | // Re-run the maze, following the recorded path. We need to 207 | // detect intersections, but it is not necessary to identify all 208 | // the exits from them, so this function is simpler than 209 | // mazeSolve. 210 | void mazeFollowPath() 211 | { 212 | // Play a starting note. 213 | buzzer.playFromProgramSpace(PSTR("!>c32")); 214 | 215 | // Delay so the robot does not move while the user is still 216 | // touching the button. 217 | delay(1000); 218 | 219 | for(uint16_t i = 0; i < pathLength; i++) 220 | { 221 | // Follow a line segment until we get to the center of an 222 | // intersection. 223 | followSegment(); 224 | driveToIntersectionCenter(); 225 | 226 | // Make a turn according to the instruction stored in 227 | // path[i]. 228 | turn(path[i]); 229 | } 230 | 231 | // Follow the last segment. 232 | followSegment(); 233 | 234 | // The end of the path has been reached. Stop the motors and 235 | // play a note with the buzzer. 236 | motors.setSpeeds(0, 0); 237 | buzzer.playFromProgramSpace(PSTR("!>>a32")); 238 | } 239 | 240 | // Path simplification. The strategy is that whenever we 241 | // encounter a sequence xBx, we can simplify it by cutting out 242 | // the dead end. For example, LBL -> S, because a single S 243 | // bypasses the dead end represented by LBL. 244 | void simplifyPath() 245 | { 246 | // Only simplify the path if it is at least three instructions 247 | // long and the second-to-last turn was a 'B'. 248 | if(pathLength < 3 || path[pathLength - 2] != 'B') 249 | { 250 | return; 251 | } 252 | 253 | int16_t totalAngle = 0; 254 | 255 | for(uint8_t i = 1; i <= 3; i++) 256 | { 257 | switch(path[pathLength - i]) 258 | { 259 | case 'L': 260 | totalAngle += 90; 261 | break; 262 | case 'R': 263 | totalAngle -= 90; 264 | break; 265 | case 'B': 266 | totalAngle += 180; 267 | break; 268 | } 269 | } 270 | 271 | // Get the angle as a number between 0 and 360 degrees. 272 | totalAngle = totalAngle % 360; 273 | 274 | // Replace all of those turns with a single one. 275 | switch(totalAngle) 276 | { 277 | case 0: 278 | path[pathLength - 3] = 'S'; 279 | break; 280 | case 90: 281 | path[pathLength - 3] = 'L'; 282 | break; 283 | case 180: 284 | path[pathLength - 3] = 'B'; 285 | break; 286 | case 270: 287 | path[pathLength - 3] = 'R'; 288 | break; 289 | } 290 | 291 | // The path is now two steps shorter. 292 | pathLength -= 2; 293 | } 294 | 295 | void displayPath() 296 | { 297 | // Set the last character of the path to a 0 so that the print() 298 | // function can find the end of the string. This is how strings 299 | // are normally terminated in C. 300 | path[pathLength] = 0; 301 | 302 | display.clear(); 303 | display.print(path); 304 | if(pathLength > 8) 305 | { 306 | display.gotoXY(0, 1); 307 | display.print(path + 8); 308 | } 309 | } 310 | -------------------------------------------------------------------------------- /examples/MazeSolver/TurnSensor.h: -------------------------------------------------------------------------------- 1 | // Turnsensor.h provides functions for configuring the 2 | // Zumo 32U4's gyro, calibrating it, and using it to 3 | // measure how much the robot has turned about its Z axis. 4 | // 5 | // This file should be included once in your sketch, 6 | // somewhere after you define objects named buttonA, 7 | // display, and imu. 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // This constant represents a turn of 45 degrees. 14 | const int32_t turnAngle45 = 0x20000000; 15 | 16 | // This constant represents a turn of 90 degrees. 17 | const int32_t turnAngle90 = turnAngle45 * 2; 18 | 19 | // This constant represents a turn of approximately 1 degree. 20 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; 21 | 22 | /* turnAngle is a 32-bit unsigned integer representing the amount 23 | the robot has turned since the last time turnSensorReset was 24 | called. This is computed solely using the Z axis of the gyro, so 25 | it could be inaccurate if the robot is rotated about the X or Y 26 | axes. 27 | 28 | Our convention is that a value of 0x20000000 represents a 45 29 | degree counter-clockwise rotation. This means that a uint32_t 30 | can represent any angle between 0 degrees and 360 degrees. If 31 | you cast it to a signed 32-bit integer by writing 32 | (int32_t)turnAngle, that integer can represent any angle between 33 | -180 degrees and 180 degrees. */ 34 | uint32_t turnAngle = 0; 35 | 36 | // turnRate is the current angular rate of the gyro, in units of 37 | // 0.07 degrees per second. 38 | int16_t turnRate; 39 | 40 | // This is the average reading obtained from the gyro's Z axis 41 | // during calibration. 42 | int16_t gyroOffset; 43 | 44 | // This variable helps us keep track of how much time has passed 45 | // between readings of the gyro. 46 | uint16_t gyroLastUpdate = 0; 47 | 48 | 49 | // This should be called to set the starting point for measuring 50 | // a turn. After calling this, turnAngle will be 0. 51 | void turnSensorReset() 52 | { 53 | gyroLastUpdate = micros(); 54 | turnAngle = 0; 55 | } 56 | 57 | // Read the gyro and update the angle. This should be called as 58 | // frequently as possible while using the gyro to do turns. 59 | void turnSensorUpdate() 60 | { 61 | // Read the measurements from the gyro. 62 | imu.readGyro(); 63 | turnRate = imu.g.z - gyroOffset; 64 | 65 | // Figure out how much time has passed since the last update (dt) 66 | uint16_t m = micros(); 67 | uint16_t dt = m - gyroLastUpdate; 68 | gyroLastUpdate = m; 69 | 70 | // Multiply dt by turnRate in order to get an estimation of how 71 | // much the robot has turned since the last update. 72 | // (angular change = angular velocity * time) 73 | int32_t d = (int32_t)turnRate * dt; 74 | 75 | // The units of d are gyro digits times microseconds. We need 76 | // to convert those to the units of turnAngle, where 2^29 units 77 | // represents 45 degrees. The conversion from gyro digits to 78 | // degrees per second (dps) is determined by the sensitivity of 79 | // the gyro: 0.07 degrees per second per digit. 80 | // 81 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) 82 | // = 14680064/17578125 unit/(digit*us) 83 | turnAngle += (int64_t)d * 14680064 / 17578125; 84 | } 85 | 86 | /* This should be called in setup() to enable and calibrate the 87 | gyro. It uses the display, yellow LED, and button A. While the 88 | display shows "Gyro cal", you should be careful to hold the robot 89 | still. 90 | 91 | The digital zero-rate level of the gyro can be as high as 92 | 25 degrees per second, and this calibration helps us correct for 93 | that. */ 94 | void turnSensorSetup() 95 | { 96 | Wire.begin(); 97 | imu.init(); 98 | imu.enableDefault(); 99 | imu.configureForTurnSensing(); 100 | 101 | display.clear(); 102 | display.print(F("Gyro cal")); 103 | 104 | // Turn on the yellow LED in case the display is not available. 105 | ledYellow(1); 106 | 107 | // Delay to give the user time to remove their finger. 108 | delay(500); 109 | 110 | // Calibrate the gyro. 111 | int32_t total = 0; 112 | for (uint16_t i = 0; i < 1024; i++) 113 | { 114 | // Wait for new data to be available, then read it. 115 | while(!imu.gyroDataReady()) {} 116 | imu.readGyro(); 117 | 118 | // Add the Z axis reading to the total. 119 | total += imu.g.z; 120 | } 121 | ledYellow(0); 122 | gyroOffset = total / 1024; 123 | 124 | // Display the angle (in degrees from -180 to 180) until the 125 | // user presses A. 126 | display.clear(); 127 | turnSensorReset(); 128 | while (!buttonA.getSingleDebouncedRelease()) 129 | { 130 | turnSensorUpdate(); 131 | display.gotoXY(0, 0); 132 | display.print((((int32_t)turnAngle >> 16) * 360) >> 16); 133 | display.print(F(" ")); 134 | } 135 | display.clear(); 136 | } 137 | -------------------------------------------------------------------------------- /examples/MazeSolver/extended_lcd_font.h: -------------------------------------------------------------------------------- 1 | #define font_width 8 2 | #define font_height 1120 3 | static unsigned char font_bits[] = { 4 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4f, 0x00, 0x00, 0x00, 0x07, 5 | 0x00, 0x07, 0x00, 0x14, 0x7f, 0x14, 0x7f, 0x14, 0x24, 0x2a, 0x7f, 0x2a, 6 | 0x12, 0x23, 0x13, 0x08, 0x64, 0x62, 0x36, 0x49, 0x55, 0x22, 0x50, 0x00, 7 | 0x00, 0x07, 0x00, 0x00, 0x00, 0x1c, 0x22, 0x41, 0x00, 0x00, 0x41, 0x22, 8 | 0x1c, 0x00, 0x14, 0x08, 0x3e, 0x08, 0x14, 0x08, 0x08, 0x3e, 0x08, 0x08, 9 | 0x00, 0x50, 0x30, 0x00, 0x00, 0x08, 0x08, 0x08, 0x08, 0x08, 0x00, 0x60, 10 | 0x60, 0x00, 0x00, 0x20, 0x10, 0x08, 0x04, 0x02, 0x3e, 0x51, 0x49, 0x45, 11 | 0x3e, 0x00, 0x42, 0x7f, 0x40, 0x00, 0x42, 0x61, 0x51, 0x49, 0x46, 0x21, 12 | 0x41, 0x45, 0x4b, 0x31, 0x18, 0x14, 0x12, 0x7f, 0x10, 0x27, 0x45, 0x45, 13 | 0x45, 0x39, 0x3c, 0x4a, 0x49, 0x49, 0x30, 0x03, 0x01, 0x71, 0x09, 0x07, 14 | 0x36, 0x49, 0x49, 0x49, 0x36, 0x06, 0x49, 0x49, 0x29, 0x1e, 0x00, 0x36, 15 | 0x36, 0x00, 0x00, 0x00, 0x56, 0x36, 0x00, 0x00, 0x08, 0x14, 0x22, 0x41, 16 | 0x00, 0x14, 0x14, 0x14, 0x14, 0x14, 0x00, 0x41, 0x22, 0x14, 0x08, 0x02, 17 | 0x01, 0x51, 0x09, 0x06, 0x32, 0x49, 0x79, 0x41, 0x3e, 0x7e, 0x11, 0x11, 18 | 0x11, 0x7e, 0x7f, 0x49, 0x49, 0x49, 0x36, 0x3e, 0x41, 0x41, 0x41, 0x22, 19 | 0x7f, 0x41, 0x41, 0x41, 0x3e, 0x7f, 0x49, 0x49, 0x49, 0x41, 0x7f, 0x09, 20 | 0x09, 0x09, 0x01, 0x3e, 0x41, 0x49, 0x49, 0x7a, 0x7f, 0x08, 0x08, 0x08, 21 | 0x7f, 0x00, 0x41, 0x7f, 0x41, 0x00, 0x20, 0x40, 0x41, 0x3f, 0x01, 0x7f, 22 | 0x08, 0x14, 0x22, 0x41, 0x7f, 0x40, 0x40, 0x40, 0x40, 0x7f, 0x02, 0x0c, 23 | 0x02, 0x7f, 0x7f, 0x04, 0x08, 0x10, 0x7f, 0x3e, 0x41, 0x41, 0x41, 0x3e, 24 | 0x7f, 0x09, 0x09, 0x09, 0x06, 0x3e, 0x41, 0x51, 0x21, 0x5e, 0x7f, 0x09, 25 | 0x19, 0x29, 0x46, 0x46, 0x49, 0x49, 0x49, 0x31, 0x01, 0x01, 0x7f, 0x01, 26 | 0x01, 0x3f, 0x40, 0x40, 0x40, 0x3f, 0x1f, 0x20, 0x40, 0x20, 0x1f, 0x3f, 27 | 0x40, 0x38, 0x40, 0x3f, 0x63, 0x14, 0x08, 0x14, 0x63, 0x07, 0x08, 0x70, 28 | 0x08, 0x07, 0x61, 0x51, 0x49, 0x45, 0x43, 0x00, 0x7f, 0x41, 0x41, 0x00, 29 | 0x02, 0x04, 0x08, 0x10, 0x20, 0x00, 0x41, 0x41, 0x7f, 0x00, 0x04, 0x02, 30 | 0x01, 0x02, 0x04, 0x40, 0x40, 0x40, 0x40, 0x40, 0x00, 0x01, 0x02, 0x04, 31 | 0x00, 0x20, 0x54, 0x54, 0x54, 0x78, 0x7f, 0x48, 0x44, 0x44, 0x38, 0x38, 32 | 0x44, 0x44, 0x44, 0x20, 0x38, 0x44, 0x44, 0x48, 0x7f, 0x38, 0x54, 0x54, 33 | 0x54, 0x18, 0x08, 0x7e, 0x09, 0x01, 0x02, 0x0c, 0x52, 0x52, 0x52, 0x3e, 34 | 0x7f, 0x08, 0x04, 0x04, 0x78, 0x00, 0x44, 0x7d, 0x40, 0x00, 0x20, 0x40, 35 | 0x44, 0x3d, 0x00, 0x7f, 0x10, 0x28, 0x44, 0x00, 0x00, 0x41, 0x7f, 0x40, 36 | 0x00, 0x7c, 0x04, 0x18, 0x04, 0x78, 0x7c, 0x08, 0x04, 0x04, 0x78, 0x38, 37 | 0x44, 0x44, 0x44, 0x38, 0x7c, 0x14, 0x14, 0x14, 0x08, 0x08, 0x14, 0x14, 38 | 0x18, 0x7c, 0x7c, 0x08, 0x04, 0x04, 0x08, 0x48, 0x54, 0x54, 0x54, 0x20, 39 | 0x04, 0x3f, 0x44, 0x40, 0x20, 0x3c, 0x40, 0x40, 0x20, 0x7c, 0x1c, 0x20, 40 | 0x40, 0x20, 0x1c, 0x3c, 0x40, 0x38, 0x40, 0x3c, 0x44, 0x28, 0x10, 0x28, 41 | 0x44, 0x0c, 0x50, 0x50, 0x50, 0x3c, 0x44, 0x64, 0x54, 0x4c, 0x44, 0x00, 42 | 0x08, 0x36, 0x41, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x41, 0x36, 43 | 0x08, 0x00, 0x08, 0x08, 0x2a, 0x1c, 0x08, 0x08, 0x1c, 0x2a, 0x08, 0x08, 44 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 45 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 46 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 47 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 48 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 49 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 50 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 53 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 54 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 55 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 58 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 59 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x00, 0x00, 60 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 61 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 62 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 63 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 64 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 65 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 66 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 67 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 68 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 69 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 73 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 74 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 75 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 79 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 82 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 83 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 84 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 86 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 87 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 88 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 89 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 90 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 91 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 92 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 93 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x3c, 0x04, 0x7c, 0x44, 94 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 95 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 96 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 97 | 0xff, 0xff, 0xff, 0xff }; 98 | -------------------------------------------------------------------------------- /examples/MazeSolver/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Replaces the default OLED font with an extended font that incorporates 4 | // several non-ASCII glyphs from the original HD44780 character set: the 5 | // forward arrow, Greek pi, centered dot, and full box. 6 | // 7 | // This file uses a #define hack to include a font file stored in a special 8 | // C-compatible format called X BitMap or "xbm". To edit the font, you will 9 | // need an editor that can work with this format, such as GIMP: 10 | // 11 | // https://www.gimp.org/ 12 | // 13 | // For compatibility with the PololuOLED library, characters run 14 | // vertically in an 8-by-1120 pixel image, using five rows per character 15 | // and no space in between. Also, the characters are mirrored, to match the 16 | // bit order used in the library. 17 | // 18 | // It is also allowed to use a smaller (shorter) image file, if you are not 19 | // using all the characters. For example, you could use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f. 21 | // 22 | // You can open original_font.xbm from the PololuOLED library as a starting 23 | // point. 24 | // 25 | // Consider rotating and mirroring the original font file so that you can see 26 | // the letters in their proper orientation, transforming them back before 27 | // exporting the font to "font.xbm" and renaming to "font.h". 28 | // 29 | // Open font.h in a text editor to make sure that it declares a variable 30 | // exactly like this: 31 | // 32 | // static unsigned char font_bits[] 33 | // 34 | // Our "hack" is to redefine the name font_bits as a macro below to turn that 35 | // declaration into a replacement of the variable pololuOledFont in the 36 | // library. If it doesn't match exactly, you will either get an error or 37 | // still have the original font. 38 | 39 | #define font_bits unused_placeholder; extern const uint8_t PROGMEM pololuOledFont 40 | #include "extended_lcd_font.h" 41 | -------------------------------------------------------------------------------- /examples/MotorTest/MotorTest.ino: -------------------------------------------------------------------------------- 1 | /* This example drives each motor on the Zumo 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 backward. 4 | If a motor on your Zumo has been flipped, you can correct its 5 | direction by uncommenting the call to flipLeftMotor() or 6 | flipRightMotor() in the setup() function. */ 7 | 8 | #include 9 | #include 10 | 11 | Zumo32U4Motors motors; 12 | Zumo32U4ButtonA buttonA; 13 | 14 | void setup() 15 | { 16 | // Uncomment if necessary to correct motor directions: 17 | //motors.flipLeftMotor(true); 18 | //motors.flipRightMotor(true); 19 | 20 | // Wait for the user to press button A. 21 | buttonA.waitForButton(); 22 | 23 | // Delay so that the robot does not move away while the user is 24 | // still touching it. 25 | delay(1000); 26 | } 27 | 28 | void loop() 29 | { 30 | // Run left motor forward. 31 | ledYellow(1); 32 | for (int speed = 0; speed <= 400; speed++) 33 | { 34 | motors.setLeftSpeed(speed); 35 | delay(2); 36 | } 37 | for (int speed = 400; speed >= 0; speed--) 38 | { 39 | motors.setLeftSpeed(speed); 40 | delay(2); 41 | } 42 | 43 | // Run left motor backward. 44 | ledYellow(0); 45 | for (int speed = 0; speed >= -400; speed--) 46 | { 47 | motors.setLeftSpeed(speed); 48 | delay(2); 49 | } 50 | for (int speed = -400; speed <= 0; speed++) 51 | { 52 | motors.setLeftSpeed(speed); 53 | delay(2); 54 | } 55 | 56 | // Run right motor forward. 57 | ledYellow(1); 58 | for (int speed = 0; speed <= 400; speed++) 59 | { 60 | motors.setRightSpeed(speed); 61 | delay(2); 62 | } 63 | for (int speed = 400; speed >= 0; speed--) 64 | { 65 | motors.setRightSpeed(speed); 66 | delay(2); 67 | } 68 | 69 | // Run right motor backward. 70 | ledYellow(0); 71 | for (int speed = 0; speed >= -400; speed--) 72 | { 73 | motors.setRightSpeed(speed); 74 | delay(2); 75 | } 76 | for (int speed = -400; speed <= 0; speed++) 77 | { 78 | motors.setRightSpeed(speed); 79 | delay(2); 80 | } 81 | 82 | delay(500); 83 | } 84 | -------------------------------------------------------------------------------- /examples/PowerDetect/PowerDetect.ino: -------------------------------------------------------------------------------- 1 | /* This example shows how to: 2 | 3 | - Measure the voltage of the Zumo's batteries. 4 | - Detect whether USB power is present. 5 | 6 | The results are printed to the display and also to the serial 7 | monitor. 8 | 9 | The battery voltage can only be read when the power switch is in 10 | the "On" position. If the power switch is off, the voltage 11 | reading displayed by this demo will be low, but it might not be 12 | zero because the Zumo 32U4 has capacitors that take a while to 13 | discharge. */ 14 | 15 | #include 16 | #include 17 | 18 | // Change next line to this if you are using the older Zumo 32U4 19 | // with a black and green LCD display: 20 | // Zumo32U4LCD display; 21 | Zumo32U4OLED display; 22 | 23 | void setup() 24 | { 25 | 26 | } 27 | 28 | void loop() 29 | { 30 | bool usbPower = usbPowerPresent(); 31 | 32 | uint16_t batteryLevel = readBatteryMillivolts(); 33 | 34 | // Print the results to the display. 35 | display.clear(); 36 | display.print(F("B=")); 37 | display.print(batteryLevel); 38 | display.print(F("mV ")); 39 | display.gotoXY(0, 1); 40 | display.print(F("USB=")); 41 | display.print(usbPower ? 'Y' : 'N'); 42 | 43 | // Print the results to the serial monitor. 44 | Serial.print(F("USB=")); 45 | Serial.print(usbPower ? 'Y' : 'N'); 46 | Serial.print(F(" B=")); 47 | Serial.print(batteryLevel); 48 | Serial.println(F(" mV")); 49 | 50 | delay(200); 51 | } 52 | -------------------------------------------------------------------------------- /examples/RCControl/RCControl.ino: -------------------------------------------------------------------------------- 1 | /* This example shows how to read RC pulses from an external 2 | receiver and use them to control the motors on the Zumo 32U4. 3 | To use this example, connect the signals from two channels of an 4 | RC receiver to pins 0 (throttle) and 1 (steering) on the Zumo. 5 | You can use the 5V and GND pins adjacent to 0 and 1 on the top 6 | expansion headers to power the receiver, if appropriate. 7 | 8 | Pins 0 and 1 are display control lines, so you cannot use the 9 | display while running this demo. */ 10 | 11 | #include 12 | #include 13 | 14 | #define THROTTLE_PIN 0 // Throttle channel from RC receiver. 15 | #define STEERING_PIN 1 // Steering channel from RC receiver. 16 | 17 | // Maximum motor speed. 18 | #define MAX_SPEED 400 19 | 20 | // Pulse width difference from 1500 us (microseconds) to ignore, 21 | // to compensate for control centering offset. 22 | #define PULSE_WIDTH_DEADBAND 40 23 | 24 | // Pulse width difference from 1500 us to be treated as full 25 | // scale input. For example, a value of 350 means any pulse 26 | // width <= 1150 us or >= 1850 us is considered full scale. 27 | #define PULSE_WIDTH_RANGE 350 28 | 29 | Zumo32U4Motors motors; 30 | 31 | void setup() 32 | { 33 | // Uncomment if necessary to correct motor directions: 34 | //motors.flipLeftMotor(true); 35 | //motors.flipRightMotor(true); 36 | } 37 | 38 | void loop() 39 | { 40 | // Read one pulse from each pin. 41 | int throttle = pulseIn(THROTTLE_PIN, HIGH); 42 | int steering = pulseIn(STEERING_PIN, HIGH); 43 | 44 | int leftSpeed, rightSpeed; 45 | 46 | if (throttle > 0 && steering > 0) 47 | { 48 | // Both RC signals are good. Turn on the yellow LED. 49 | ledYellow(1); 50 | 51 | // RC signals encode information in pulse width centered on 52 | // 1500 us; subtract 1500 to get a value centered on 0. 53 | throttle -= 1500; 54 | steering -= 1500; 55 | 56 | // Apply deadband. 57 | if (abs(throttle) <= PULSE_WIDTH_DEADBAND) 58 | { 59 | throttle = 0; 60 | } 61 | if (abs(steering) <= PULSE_WIDTH_DEADBAND) 62 | { 63 | steering = 0; 64 | } 65 | 66 | // Convert from pulse widths to speeds. 67 | int throttleSpeed = (long)throttle * MAX_SPEED / PULSE_WIDTH_RANGE; 68 | int steeringSpeed = (long)steering * MAX_SPEED / PULSE_WIDTH_RANGE; 69 | 70 | // Mix throttle and steering inputs to obtain left & right 71 | // motor speeds. 72 | leftSpeed = throttleSpeed - steeringSpeed; 73 | rightSpeed = throttleSpeed + steeringSpeed; 74 | 75 | // Cap the speeds to their maximum values. 76 | leftSpeed = constrain(leftSpeed, -MAX_SPEED, MAX_SPEED); 77 | rightSpeed = constrain(rightSpeed, -MAX_SPEED, MAX_SPEED); 78 | } 79 | else 80 | { 81 | // At least one RC signal is not good. Turn off the LED and 82 | // stop motors. 83 | ledYellow(0); 84 | 85 | leftSpeed = 0; 86 | rightSpeed = 0; 87 | } 88 | 89 | motors.setSpeeds(leftSpeed, rightSpeed); 90 | } 91 | -------------------------------------------------------------------------------- /examples/RemoteControl/RemoteConstants.h: -------------------------------------------------------------------------------- 1 | // This file contains constants for the address and commands in 2 | // the messages transmitted by the Mini IR Remote Control: 3 | // https://www.pololu.com/product/2777 4 | 5 | #pragma once 6 | 7 | const uint16_t remoteAddressByte0 = 0x00; 8 | const uint16_t remoteAddressByte1 = 0xBF; 9 | 10 | const uint8_t remoteVolMinus = 0x00; 11 | const uint8_t remotePlayPause = 0x01; 12 | const uint8_t remoteVolPlus = 0x02; 13 | 14 | const uint8_t remoteSetup = 0x04; 15 | const uint8_t remoteUp = 0x05; 16 | const uint8_t remoteStopMode = 0x06; 17 | 18 | const uint8_t remoteLeft = 0x08; 19 | const uint8_t remoteEnterSave = 0x09; 20 | const uint8_t remoteRight = 0x0A; 21 | 22 | const uint8_t remote0 = 0x0C; 23 | const uint8_t remoteDown = 0x0D; 24 | const uint8_t remoteBack = 0x0E; 25 | 26 | const uint8_t remote1 = 0x10; 27 | const uint8_t remote2 = 0x11; 28 | const uint8_t remote3 = 0x12; 29 | 30 | const uint8_t remote4 = 0x14; 31 | const uint8_t remote5 = 0x15; 32 | const uint8_t remote6 = 0x16; 33 | 34 | const uint8_t remote7 = 0x18; 35 | const uint8_t remote8 = 0x19; 36 | const uint8_t remote9 = 0x1A; 37 | 38 | 39 | -------------------------------------------------------------------------------- /examples/RemoteControl/RemoteControl.ino: -------------------------------------------------------------------------------- 1 | /* This example shows how to use the receivers for the infrared 2 | proximity sensors on the Zumo 32U4 to detect and decode commands 3 | from an infrared remote. This code is designed to work with the 4 | Mini IR Remote Control available from Pololu: 5 | 6 | https://www.pololu.com/product/2777 7 | 8 | For this code to work, jumpers on the front sensor array should 9 | be installed in order to connect pin 4 to RGT and connect pin 20 10 | to LFT. 11 | 12 | The arrow buttons control the robot's movement, while the number 13 | buttons from 1 to 3 play different notes on the buzzer. 14 | 15 | To change what actions are performed when a button press is 16 | detected, you should change the processRemoteCommand and 17 | stopCurrentCommand functions. 18 | 19 | If you have a different remote that uses the NEC protocol with a 20 | 38 kHz, 940 nm infrared emitter, it should be possible to make it 21 | work with this code. You can use this code to decode the 22 | messages from your remote, and then you can edit the constants in 23 | RemoteConstants.h to match what was transmitted from your 24 | remote. */ 25 | 26 | #include 27 | #include 28 | #include "RemoteConstants.h" 29 | #include "RemoteDecoder.h" 30 | 31 | // This variable sets the amount of time (in milliseconds) that 32 | // we wait before considering the current message from the remote 33 | // to have expired. This type of remote typically sends a repeat 34 | // command every 109 ms, so a timeout value of 115 was chosen. 35 | // You can increase this timeout to 230 if you want to be more 36 | // tolerant of errors that occur while you are holding down the 37 | // button, but it will make the robot slower to respond when you 38 | // release the button. 39 | const uint16_t messageTimeoutMs = 115; 40 | 41 | // This variable is true if the last message received from the 42 | // remote is still active, meaning that we are still performing 43 | // the action specified by the message. A message will be active 44 | // if the remote button is being held down and the robot has been 45 | // successfully receiving messages from the remote since the 46 | // button was pressed. 47 | bool messageActive = false; 48 | 49 | // This is the time that the current message from the remote was 50 | // last verified, in milliseconds. It is used to implement the 51 | // timeout defined above. 52 | uint16_t lastMessageTimeMs = 0; 53 | 54 | // Change next line to this if you are using the older Zumo 32U4 55 | // with a black and green LCD display: 56 | // Zumo32U4LCD display; 57 | Zumo32U4OLED display; 58 | 59 | Zumo32U4Motors motors; 60 | Zumo32U4Buzzer buzzer; 61 | Zumo32U4ButtonA buttonA; 62 | 63 | RemoteDecoder decoder; 64 | 65 | void setup() 66 | { 67 | decoder.init(); 68 | 69 | display.clear(); 70 | display.print(F("Waiting")); 71 | } 72 | 73 | void loop() 74 | { 75 | decoder.service(); 76 | 77 | // Turn on the yellow LED if a message is active. 78 | ledYellow(messageActive); 79 | 80 | // Turn on the red LED if we are in the middle of receiving a 81 | // new message from the remote. You should see the red LED 82 | // blinking about 9 times per second while you hold a remote 83 | // button down. 84 | ledRed(decoder.criticalTime()); 85 | 86 | if (decoder.criticalTime()) 87 | { 88 | // We are in the middle of receiving a message from the 89 | // remote, so we should avoid doing anything that might take 90 | // more than a few tens of microseconds, and call 91 | // decoder.service() as often as possible. 92 | } 93 | else 94 | { 95 | // We are not in a critical time, so we can do other things 96 | // as long as they do not take longer than about 7.3 ms. 97 | // Delays longer than that can cause some remote control 98 | // messages to be missed. 99 | 100 | processRemoteEvents(); 101 | } 102 | 103 | // Check how long ago the current message was last verified. 104 | // If it is longer than the timeout time, then the message has 105 | // expired and we should stop executing it. 106 | if (messageActive && (uint16_t)(millis() - lastMessageTimeMs) > messageTimeoutMs) 107 | { 108 | messageActive = false; 109 | stopCurrentCommand(); 110 | } 111 | } 112 | 113 | void processRemoteEvents() 114 | { 115 | if (decoder.getAndResetMessageFlag()) 116 | { 117 | // The remote decoder received a new message, so record what 118 | // time it was received and process it. 119 | lastMessageTimeMs = millis(); 120 | messageActive = true; 121 | processRemoteMessage(decoder.getMessage()); 122 | } 123 | 124 | if (decoder.getAndResetRepeatFlag()) 125 | { 126 | // The remote decoder receiver a "repeat" command, which is 127 | // sent about every 109 ms while the button is being held 128 | // down. It contains no data. We record what time the 129 | // repeat command was received so we can know that the 130 | // current message is still active. 131 | lastMessageTimeMs = millis(); 132 | } 133 | } 134 | 135 | void processRemoteMessage(const uint8_t * message) 136 | { 137 | // Print the raw message on the first line of the display, in hex. 138 | // The first two bytes are usually an address, and the third 139 | // byte is usually a command. The last byte is supposed to be 140 | // the bitwise inverse of the third byte, and if that is the 141 | // case, then we don't print it. 142 | display.clear(); 143 | char buffer[9]; 144 | if (message[2] + message[3] == 0xFF) 145 | { 146 | sprintf(buffer, "%02X%02X %02X ", 147 | message[0], message[1], message[2]); 148 | } 149 | else 150 | { 151 | sprintf(buffer, "%02X%02X%02X%02X", 152 | message[0], message[1], message[2], message[3]); 153 | } 154 | display.print(buffer); 155 | 156 | // Go to the next line of the display. 157 | display.gotoXY(0, 1); 158 | 159 | // Make sure the address matches what we expect. 160 | if (message[0] != remoteAddressByte0 || 161 | message[1] != remoteAddressByte1) 162 | { 163 | display.print(F("bad addr")); 164 | return; 165 | } 166 | 167 | // Make sure that the last byte is the logical inverse of the 168 | // command byte. 169 | if (message[2] + message[3] != 0xFF) 170 | { 171 | display.print(F("bad cmd")); 172 | return; 173 | } 174 | 175 | stopCurrentCommand(); 176 | processRemoteCommand(message[2]); 177 | } 178 | 179 | // Start running the new command. 180 | void processRemoteCommand(uint8_t command) 181 | { 182 | switch(command) 183 | { 184 | case remoteUp: 185 | display.print(F("up")); 186 | motors.setSpeeds(400, 400); 187 | break; 188 | 189 | case remoteDown: 190 | display.print(F("down")); 191 | motors.setSpeeds(-400, -400); 192 | break; 193 | 194 | case remoteLeft: 195 | display.print(F("left")); 196 | motors.setSpeeds(-250, 250); 197 | break; 198 | 199 | case remoteRight: 200 | display.print(F("right")); 201 | motors.setSpeeds(250, -250); 202 | break; 203 | 204 | case remoteStopMode: 205 | display.print(F("stop")); 206 | break; 207 | 208 | case remoteEnterSave: 209 | display.print(F("enter")); 210 | break; 211 | 212 | case remoteVolMinus: 213 | display.print(F("vol-")); 214 | break; 215 | 216 | case remoteVolPlus: 217 | display.print(F("vol+")); 218 | break; 219 | 220 | case remotePlayPause: 221 | display.print(F("play")); 222 | break; 223 | 224 | case remoteSetup: 225 | display.print(F("setup")); 226 | break; 227 | 228 | case remoteBack: 229 | display.print(F("back")); 230 | break; 231 | 232 | case remote0: 233 | display.print(F("0")); 234 | break; 235 | 236 | case remote1: 237 | display.print(F("1")); 238 | buzzer.playNote(NOTE_C(4), 200, 15); 239 | break; 240 | 241 | case remote2: 242 | display.print(F("2")); 243 | buzzer.playNote(NOTE_D(4), 200, 15); 244 | break; 245 | 246 | case remote3: 247 | display.print(F("3")); 248 | buzzer.playNote(NOTE_E(4), 200, 15); 249 | break; 250 | 251 | case remote4: 252 | display.print(F("4")); 253 | break; 254 | 255 | case remote5: 256 | display.print(F("5")); 257 | break; 258 | 259 | case remote6: 260 | display.print(F("6")); 261 | break; 262 | 263 | case remote7: 264 | display.print(F("7")); 265 | break; 266 | 267 | case remote8: 268 | display.print(F("8")); 269 | break; 270 | 271 | case remote9: 272 | display.print(F("9")); 273 | break; 274 | } 275 | } 276 | 277 | // Stops the current remote control command. This is called when 278 | // a new command is received or if the current command has 279 | // expired. 280 | void stopCurrentCommand() 281 | { 282 | motors.setSpeeds(0, 0); 283 | buzzer.stopPlaying(); 284 | } 285 | -------------------------------------------------------------------------------- /examples/RotationResist/RotationResist.ino: -------------------------------------------------------------------------------- 1 | /* This demo shows how the Zumo can use its gyroscope to detect 2 | when it is being rotated, and use the motors to resist that 3 | rotation. 4 | 5 | This code was tested on a Zumo with 4 NiMH batteries and two 75:1 6 | HP micro metal gearmotors. If you have different batteries or 7 | motors, you might need to adjust the PID constants. 8 | 9 | Be careful to not move the robot for a few seconds after starting 10 | it while the gyro is being calibrated. During the gyro 11 | calibration, the yellow LED is on and the words "Gyro cal" are 12 | shown on the display. 13 | 14 | After the gyro calibration is done, press button A to start the 15 | demo. If you try to turn the Zumo, or put it on a surface that 16 | is turning, it will drive its motors to counteract the turning. 17 | 18 | This demo only uses the Z axis of the gyro, so it is possible to 19 | pick up the Zumo, rotate it about its X and Y axes, and then put 20 | it down facing in a new position. */ 21 | 22 | #include 23 | #include 24 | 25 | // This is the maximum speed the motors will be allowed to turn. 26 | // A maxSpeed of 400 lets the motors go at top speed. Decrease 27 | // this value to impose a speed limit. 28 | const int16_t maxSpeed = 400; 29 | 30 | // Change next line to this if you are using the older Zumo 32U4 31 | // with a black and green LCD display: 32 | // Zumo32U4LCD display; 33 | Zumo32U4OLED display; 34 | 35 | Zumo32U4ButtonA buttonA; 36 | Zumo32U4Motors motors; 37 | Zumo32U4IMU imu; 38 | 39 | #include "TurnSensor.h" 40 | 41 | void setup() 42 | { 43 | turnSensorSetup(); 44 | delay(500); 45 | turnSensorReset(); 46 | 47 | display.clear(); 48 | display.print(F("Try to")); 49 | display.gotoXY(0, 1); 50 | display.print(F("turn me!")); 51 | } 52 | 53 | void loop() 54 | { 55 | // Read the gyro to update turnAngle, the estimation of how far 56 | // the robot has turned, and turnRate, the estimation of how 57 | // fast it is turning. 58 | turnSensorUpdate(); 59 | 60 | // Calculate the motor turn speed using proportional and 61 | // derivative PID terms. Here we are a using a proportional 62 | // constant of 56 and a derivative constant of 1/20. 63 | int32_t turnSpeed = -(int32_t)turnAngle / (turnAngle1 / 56) 64 | - turnRate / 20; 65 | 66 | // Constrain our motor speeds to be between 67 | // -maxSpeed and maxSpeed. 68 | turnSpeed = constrain(turnSpeed, -maxSpeed, maxSpeed); 69 | 70 | motors.setSpeeds(-turnSpeed, turnSpeed); 71 | } 72 | -------------------------------------------------------------------------------- /examples/RotationResist/TurnSensor.h: -------------------------------------------------------------------------------- 1 | // Turnsensor.h provides functions for configuring the 2 | // Zumo 32U4's gyro, calibrating it, and using it to 3 | // measure how much the robot has turned about its Z axis. 4 | // 5 | // This file should be included once in your sketch, 6 | // somewhere after you define objects named buttonA, 7 | // display, and imu. 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | // This constant represents a turn of 45 degrees. 14 | const int32_t turnAngle45 = 0x20000000; 15 | 16 | // This constant represents a turn of 90 degrees. 17 | const int32_t turnAngle90 = turnAngle45 * 2; 18 | 19 | // This constant represents a turn of approximately 1 degree. 20 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; 21 | 22 | /* turnAngle is a 32-bit unsigned integer representing the amount 23 | the robot has turned since the last time turnSensorReset was 24 | called. This is computed solely using the Z axis of the gyro, so 25 | it could be inaccurate if the robot is rotated about the X or Y 26 | axes. 27 | 28 | Our convention is that a value of 0x20000000 represents a 45 29 | degree counter-clockwise rotation. This means that a uint32_t 30 | can represent any angle between 0 degrees and 360 degrees. If 31 | you cast it to a signed 32-bit integer by writing 32 | (int32_t)turnAngle, that integer can represent any angle between 33 | -180 degrees and 180 degrees. */ 34 | uint32_t turnAngle = 0; 35 | 36 | // turnRate is the current angular rate of the gyro, in units of 37 | // 0.07 degrees per second. 38 | int16_t turnRate; 39 | 40 | // This is the average reading obtained from the gyro's Z axis 41 | // during calibration. 42 | int16_t gyroOffset; 43 | 44 | // This variable helps us keep track of how much time has passed 45 | // between readings of the gyro. 46 | uint16_t gyroLastUpdate = 0; 47 | 48 | 49 | // This should be called to set the starting point for measuring 50 | // a turn. After calling this, turnAngle will be 0. 51 | void turnSensorReset() 52 | { 53 | gyroLastUpdate = micros(); 54 | turnAngle = 0; 55 | } 56 | 57 | // Read the gyro and update the angle. This should be called as 58 | // frequently as possible while using the gyro to do turns. 59 | void turnSensorUpdate() 60 | { 61 | // Read the measurements from the gyro. 62 | imu.readGyro(); 63 | turnRate = imu.g.z - gyroOffset; 64 | 65 | // Figure out how much time has passed since the last update (dt) 66 | uint16_t m = micros(); 67 | uint16_t dt = m - gyroLastUpdate; 68 | gyroLastUpdate = m; 69 | 70 | // Multiply dt by turnRate in order to get an estimation of how 71 | // much the robot has turned since the last update. 72 | // (angular change = angular velocity * time) 73 | int32_t d = (int32_t)turnRate * dt; 74 | 75 | // The units of d are gyro digits times microseconds. We need 76 | // to convert those to the units of turnAngle, where 2^29 units 77 | // represents 45 degrees. The conversion from gyro digits to 78 | // degrees per second (dps) is determined by the sensitivity of 79 | // the gyro: 0.07 degrees per second per digit. 80 | // 81 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) 82 | // = 14680064/17578125 unit/(digit*us) 83 | turnAngle += (int64_t)d * 14680064 / 17578125; 84 | } 85 | 86 | /* This should be called in setup() to enable and calibrate the 87 | gyro. It uses the display, yellow LED, and button A. While the 88 | display shows "Gyro cal", you should be careful to hold the robot 89 | still. 90 | 91 | The digital zero-rate level of the gyro can be as high as 92 | 25 degrees per second, and this calibration helps us correct for 93 | that. */ 94 | void turnSensorSetup() 95 | { 96 | Wire.begin(); 97 | imu.init(); 98 | imu.enableDefault(); 99 | imu.configureForTurnSensing(); 100 | 101 | display.clear(); 102 | display.print(F("Gyro cal")); 103 | 104 | // Turn on the yellow LED in case the display is not available. 105 | ledYellow(1); 106 | 107 | // Delay to give the user time to remove their finger. 108 | delay(500); 109 | 110 | // Calibrate the gyro. 111 | int32_t total = 0; 112 | for (uint16_t i = 0; i < 1024; i++) 113 | { 114 | // Wait for new data to be available, then read it. 115 | while(!imu.gyroDataReady()) {} 116 | imu.readGyro(); 117 | 118 | // Add the Z axis reading to the total. 119 | total += imu.g.z; 120 | } 121 | ledYellow(0); 122 | gyroOffset = total / 1024; 123 | 124 | // Display the angle (in degrees from -180 to 180) until the 125 | // user presses A. 126 | display.clear(); 127 | turnSensorReset(); 128 | while (!buttonA.getSingleDebouncedRelease()) 129 | { 130 | turnSensorUpdate(); 131 | display.gotoXY(0, 0); 132 | display.print((((int32_t)turnAngle >> 16) * 360) >> 16); 133 | display.print(F(" ")); 134 | } 135 | display.clear(); 136 | } 137 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Zumo32U4LCD KEYWORD1 2 | 3 | Zumo32U4OLED KEYWORD1 4 | 5 | ZUMO_32U4_BUTTON_A LITERAL1 6 | ZUMO_32U4_BUTTON_B LITERAL1 7 | ZUMO_32U4_BUTTON_C LITERAL1 8 | Zumo32U4ButtonA KEYWORD1 9 | Zumo32U4ButtonB KEYWORD1 10 | Zumo32U4ButtonC KEYWORD1 11 | 12 | Zumo32U4Buzzer KEYWORD1 13 | 14 | Zumo32U4Motors KEYWORD1 15 | flipLeftMotor KEYWORD2 16 | flipRightMotor KEYWORD2 17 | setLeftSpeed KEYWORD2 18 | setRightSpeed KEYWORD2 19 | setSpeeds KEYWORD2 20 | 21 | Zumo32U4Encoders KEYWORD1 22 | init KEYWORD2 23 | getCountsLeft KEYWORD2 24 | getCountsRight KEYWORD2 25 | getCountsAndResetLeft KEYWORD2 26 | getCountsAndResetRight KEYWORD2 27 | checkErrorLeft KEYWORD2 28 | checkErrorRight KEYWORD2 29 | 30 | Zumo32U4IRPulses KEYWORD1 31 | defaultPeriod KEYWORD2 32 | getPulseCount KEYWORD2 33 | start KEYWORD2 34 | stop KEYWORD2 35 | 36 | Zumo32U4LineSensors KEYWORD1 37 | SENSOR_DOWN1 LITERAL1 38 | SENSOR_DOWN2 LITERAL1 39 | SENSOR_DOWN3 LITERAL1 40 | SENSOR_DOWN4 LITERAL1 41 | SENSOR_DOWN5 LITERAL1 42 | SENSOR_LEDON LITERAL1 43 | initThreeSensors KEYWORD2 44 | initFiveSensors KEYWORD2 45 | 46 | Zumo32U4ProximitySensors KEYWORD1 47 | SENSOR_LEFT LITERAL1 48 | SENSOR_FRONT LITERAL1 49 | SENSOR_RIGHT LITERAL1 50 | SENSOR_NO_PIN LITERAL1 51 | initThreeSensors KEYWORD2 52 | initFrontSensor KEYWORD2 53 | getNumSensors KEYWORD2 54 | defaultPeriod KEYWORD2 55 | defaultPulseOnTimeUs KEYWORD2 56 | defaultPulseOffTimeUs KEYWORD2 57 | setPeriod KEYWORD2 58 | setBrightnessLevels KEYWORD2 59 | setPulseOnTimeUs KEYWORD2 60 | setPulseOffTimeUs KEYWORD2 61 | getNumBrightnessLevels KEYWORD2 62 | lineSensorEmittersOff KEYWORD2 63 | pullupsOn KEYWORD2 64 | readBasic KEYWORD2 65 | read KEYWORD2 66 | countsWithLeftLeds KEYWORD2 67 | countsWithRightLeds KEYWORD2 68 | countsLeftWithLeftLeds KEYWORD2 69 | countsLeftWithRightLeds KEYWORD2 70 | countsFrontWithLeftLeds KEYWORD2 71 | countsFrontWithRightLeds KEYWORD2 72 | countsRightWithLeftLeds KEYWORD2 73 | countsRightWithRightLeds KEYWORD2 74 | readBasicLeft KEYWORD2 75 | readBasicFront KEYWORD2 76 | readBasicRight KEYWORD2 77 | 78 | LSM303D_ADDR LITERAL1 79 | L3GD20H_ADDR LITERAL1 80 | LSM6DS33_ADDR LITERAL1 81 | LIS3MDL_ADDR LITERAL1 82 | LSM303D_REG_STATUS_M LITERAL1 83 | LSM303D_REG_OUT_X_L_M LITERAL1 84 | LSM303D_REG_WHO_AM_I LITERAL1 85 | LSM303D_REG_CTRL1 LITERAL1 86 | LSM303D_REG_CTRL2 LITERAL1 87 | LSM303D_REG_CTRL5 LITERAL1 88 | LSM303D_REG_CTRL6 LITERAL1 89 | LSM303D_REG_CTRL7 LITERAL1 90 | LSM303D_REG_STATUS_A LITERAL1 91 | LSM303D_REG_OUT_X_L_A LITERAL1 92 | L3GD20H_REG_WHO_AM_I LITERAL1 93 | L3GD20H_REG_CTRL1 LITERAL1 94 | L3GD20H_REG_CTRL4 LITERAL1 95 | L3GD20H_REG_STATUS LITERAL1 96 | L3GD20H_REG_OUT_X_L LITERAL1 97 | LSM6DS33_REG_WHO_AM_I LITERAL1 98 | LSM6DS33_REG_CTRL1_XL LITERAL1 99 | LSM6DS33_REG_CTRL2_G LITERAL1 100 | LSM6DS33_REG_CTRL3_C LITERAL1 101 | LSM6DS33_REG_STATUS_REG LITERAL1 102 | LSM6DS33_REG_OUTX_L_G LITERAL1 103 | LSM6DS33_REG_OUTX_L_XL LITERAL1 104 | LIS3MDL_REG_WHO_AM_I LITERAL1 105 | LIS3MDL_REG_CTRL_REG1 LITERAL1 106 | LIS3MDL_REG_CTRL_REG2 LITERAL1 107 | LIS3MDL_REG_CTRL_REG3 LITERAL1 108 | LIS3MDL_REG_CTRL_REG4 LITERAL1 109 | LIS3MDL_REG_STATUS_REG LITERAL1 110 | LIS3MDL_REG_OUT_X_L LITERAL1 111 | Zumo32U4IMUType KEYWORD1 112 | Unknown KEYWORD2 113 | LSM303D_L3GD20H KEYWORD2 114 | LSM6DS33_LIS3MDL KEYWORD2 115 | Zumo32U4IMU KEYWORD1 116 | vector KEYWORD1 117 | getLastError KEYWORD2 118 | init KEYWORD2 119 | getType KEYWORD2 120 | enableDefault KEYWORD2 121 | configureForTurnSensing KEYWORD2 122 | configureForBalancing KEYWORD2 123 | configureForFaceUphill KEYWORD2 124 | writeReg KEYWORD2 125 | readReg KEYWORD2 126 | readAcc KEYWORD2 127 | readGyro KEYWORD2 128 | readMag KEYWORD2 129 | read KEYWORD2 130 | accDataReady KEYWORD2 131 | gyroDataReady KEYWORD2 132 | magDataReady KEYWORD2 133 | 134 | ledRed KEYWORD2 135 | ledGreen KEYWORD2 136 | ledYellow KEYWORD2 137 | usbPowerPresent KEYWORD2 138 | readBatteryMillivolts KEYWORD2 139 | 140 | ####################################### 141 | # Syntax Coloring Map QTRSensors 142 | ####################################### 143 | 144 | ####################################### 145 | # Datatypes (KEYWORD1) 146 | ####################################### 147 | 148 | QTRSensorsAnalog KEYWORD1 149 | QTRSensorsRC KEYWORD1 150 | QTRSensors KEYWORD1 151 | 152 | ####################################### 153 | # Methods and Functions (KEYWORD2) 154 | ####################################### 155 | 156 | read KEYWORD2 157 | emittersOff KEYWORD2 158 | emittersOn KEYWORD2 159 | calibrate KEYWORD2 160 | readCalibrated KEYWORD2 161 | readLine KEYWORD2 162 | calibratedMinimumOn KEYWORD2 163 | calibratedMaximumOn KEYWORD2 164 | calibratedMinimumOff KEYWORD2 165 | calibratedMaximumOff KEYWORD2 166 | init KEYWORD2 167 | 168 | ####################################### 169 | # Constants (LITERAL1) 170 | ####################################### 171 | 172 | QTR_EMITTERS_OFF LITERAL1 173 | QTR_EMITTERS_ON LITERAL1 174 | QTR_EMITTERS_ON_AND_OFF LITERAL1 175 | QTR_NO_EMITTER_PIN LITERAL1 176 | 177 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Zumo32U4 2 | version=2.0.1 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Zumo 32U4 Arduino library 6 | paragraph=This is a library for the Arduino IDE that helps interface with the on-board hardware on the Pololu Zumo 32U4 robot. 7 | category=Device Control 8 | url=https://github.com/pololu/zumo-32u4-arduino-library 9 | architectures=avr 10 | includes=Zumo32U4.h 11 | depends=FastGPIO,USBPause,Pushbutton,PololuBuzzer,PololuHD44780,PololuOLED,PololuMenu 12 | -------------------------------------------------------------------------------- /src/Zumo32U4.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4.h 4 | * 5 | * \brief Main header file for the Zumo32U4 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 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // TODO: servo support 29 | 30 | /*! \brief Turns the red user LED (RX) on or off. 31 | 32 | @param on 1 to turn on the LED, 0 to turn it off. 33 | 34 | The red user LED is on pin 17, which is also known as PB0, SS, and RXLED. The 35 | Arduino core code uses this LED to indicate when it receives data over USB, so 36 | it might be hard to control this LED when USB is connected. */ 37 | inline void ledRed(bool on) 38 | { 39 | FastGPIO::Pin<17>::setOutput(!on); 40 | } 41 | 42 | /*! \brief Turns the yellow user LED on pin 13 on or off. 43 | 44 | @param on 1 to turn on the LED, 0 to turn it off. */ 45 | inline void ledYellow(bool on) 46 | { 47 | FastGPIO::Pin<13>::setOutput(on); 48 | } 49 | 50 | /*! \brief Turns the green user LED (TX) on or off. 51 | 52 | @param on 1 to turn on the LED, 0 to turn it off. 53 | 54 | The green user LED is pin PD5, which is also known as TXLED. The Arduino core 55 | code uses this LED to indicate when it receives data over USB, so it might be 56 | hard to control this LED when USB is connected. */ 57 | inline void ledGreen(bool on) 58 | { 59 | FastGPIO::Pin::setOutput(!on); 60 | } 61 | 62 | /*! \brief Returns true if USB power is detected. 63 | 64 | This function returns true if power is detected on the board's USB port and 65 | returns false otherwise. It uses the ATmega32U4's VBUS line, which is directly 66 | connected to the power pin of the USB connector. 67 | 68 | \sa A method for detecting whether the board's virtual COM port is open: 69 | http://arduino.cc/en/Serial/IfSerial */ 70 | inline bool usbPowerPresent() 71 | { 72 | return USBSTA >> VBUS & 1; 73 | } 74 | 75 | /*! \brief Reads the battery voltage and returns it in millivolts. */ 76 | inline uint16_t readBatteryMillivolts() 77 | { 78 | const uint8_t sampleCount = 8; 79 | uint16_t sum = 0; 80 | for (uint8_t i = 0; i < sampleCount; i++) 81 | { 82 | sum += analogRead(A1); 83 | } 84 | 85 | // VBAT = 2 * millivolt reading = 2 * raw * 5000/1024 86 | // = raw * 625 / 64 87 | // The correction term below makes it so that we round to the 88 | // nearest whole number instead of always rounding down. 89 | const uint32_t correction = 32 * sampleCount - 1; 90 | return ((uint32_t)sum * 625 + correction) / (64 * sampleCount); 91 | } 92 | 93 | /** 94 | 95 | \class QTRSensors 96 | For complete documentation of this class, see 97 | https://www.pololu.com/docs/0J19 98 | 99 | 100 | \class QTRSensorsRC 101 | For complete documentation of this class, see 102 | https://www.pololu.com/docs/0J19 103 | 104 | */ 105 | -------------------------------------------------------------------------------- /src/Zumo32U4Buttons.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /** \file Zumo32U4Buttons.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 Zumo 32U4. */ 13 | #define ZUMO_32U4_BUTTON_A 14 14 | 15 | /*! The pin number for the pin connected to button B on the Zumo 32U4. Note 16 | * that this is not an official Arduino pin number so it cannot be used with 17 | * functions like digitalRead, but it can be used with the FastGPIO library. */ 18 | #define ZUMO_32U4_BUTTON_B IO_D5 19 | 20 | /*! The pin number for the pin conencted to button C on the Zumo 32U4. */ 21 | #define ZUMO_32U4_BUTTON_C 17 22 | 23 | /*! \brief Interfaces with button A on the Zumo 32U4. */ 24 | class Zumo32U4ButtonA : public Pushbutton 25 | { 26 | public: 27 | Zumo32U4ButtonA() : Pushbutton(ZUMO_32U4_BUTTON_A) 28 | { 29 | } 30 | }; 31 | 32 | /*! \brief Interfaces with button B on the Zumo 32U4. 33 | * 34 | * The pin used for button B is also used for the TX LED. 35 | * 36 | * This class temporarily disables USB interrupts because the Arduino core code 37 | * has USB interrupts enabled that sometimes write to the pin this button is on. 38 | * 39 | * This class temporarily sets the pin to be an input without a pull-up 40 | * resistor. The pull-up resistor is not needed because of the resistors on the 41 | * board. */ 42 | class Zumo32U4ButtonB : public PushbuttonBase 43 | { 44 | public: 45 | 46 | virtual bool isPressed() 47 | { 48 | USBPause usbPause; 49 | FastGPIO::PinLoan loan; 50 | FastGPIO::Pin::setInputPulledUp(); 51 | _delay_us(3); 52 | return !FastGPIO::Pin::isInputHigh(); 53 | } 54 | }; 55 | 56 | /*! \brief Interfaces with button C on the Zumo 32U4. 57 | * 58 | * The pin used for button C is also used for the RX LED. 59 | * 60 | * This class temporarily disables USB interrupts because the Arduino core code 61 | * has USB interrupts enabled that sometimes write to the pin this button is on. 62 | * 63 | * This class temporarily sets the pin to be an input without a pull-up 64 | * resistor. The pull-up resistor is not needed because of the resistors on the 65 | * board. */ 66 | class Zumo32U4ButtonC : public PushbuttonBase 67 | { 68 | public: 69 | 70 | virtual bool isPressed() 71 | { 72 | USBPause usbPause; 73 | FastGPIO::PinLoan loan; 74 | FastGPIO::Pin::setInputPulledUp(); 75 | _delay_us(3); 76 | return !FastGPIO::Pin::isInputHigh(); 77 | } 78 | }; 79 | 80 | -------------------------------------------------------------------------------- /src/Zumo32U4Buzzer.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4Buzzer.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Plays beeps and music on the buzzer on the Zumo 32U4. 10 | * 11 | * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on 12 | * the Zumo 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 Zumo32U4Buzzer : public PololuBuzzer 24 | { 25 | // This is a trivial subclass of PololuBuzzer; it exists because we wanted 26 | // the Zumo32U4 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 | -------------------------------------------------------------------------------- /src/Zumo32U4Encoders.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 += (newLeftA ^ lastLeftB) - (lastLeftA ^ newLeftB); 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 += (newRightA ^ lastRightB) - (lastRightA ^ newRightB); 48 | 49 | if((lastRightA ^ newRightA) & (lastRightB ^ newRightB)) 50 | { 51 | errorRight = true; 52 | } 53 | 54 | lastRightA = newRightA; 55 | lastRightB = newRightB; 56 | } 57 | 58 | void Zumo32U4Encoders::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 Zumo32U4Encoders::getCountsLeft() 92 | { 93 | init(); 94 | 95 | cli(); 96 | int16_t counts = countLeft; 97 | sei(); 98 | return counts; 99 | } 100 | 101 | int16_t Zumo32U4Encoders::getCountsRight() 102 | { 103 | init(); 104 | 105 | cli(); 106 | int16_t counts = countRight; 107 | sei(); 108 | return counts; 109 | } 110 | 111 | int16_t Zumo32U4Encoders::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 Zumo32U4Encoders::getCountsAndResetRight() 123 | { 124 | init(); 125 | 126 | cli(); 127 | int16_t counts = countRight; 128 | countRight = 0; 129 | sei(); 130 | return counts; 131 | } 132 | 133 | bool Zumo32U4Encoders::checkErrorLeft() 134 | { 135 | init(); 136 | 137 | bool error = errorLeft; 138 | errorLeft = 0; 139 | return error; 140 | } 141 | 142 | bool Zumo32U4Encoders::checkErrorRight() 143 | { 144 | init(); 145 | 146 | bool error = errorRight; 147 | errorRight = 0; 148 | return error; 149 | } 150 | -------------------------------------------------------------------------------- /src/Zumo32U4Encoders.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4Encoders.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Reads counts from the encoders on the Zumo 32U4. 10 | * 11 | * This class allows you to read counts from the encoders on the Zumo 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 | class Zumo32U4Encoders 26 | { 27 | 28 | public: 29 | 30 | /*! This function initializes the encoders if they have not been initialized 31 | * already and starts listening for counts. This 32 | * function is called automatically whenever you call any other function in 33 | * this class, so you should not normally need to call it in your code. */ 34 | static void init() 35 | { 36 | static bool initialized = 0; 37 | if (!initialized) 38 | { 39 | initialized = true; 40 | init2(); 41 | } 42 | } 43 | 44 | /*! Returns the number of counts that have been detected from the left-side 45 | * encoder. These counts start at 0. Positive counts correspond to forward 46 | * movement of the left side of the Zumo, while negative counts correspond 47 | * to backwards movement. 48 | * 49 | * The count is returned as a signed 16-bit integer. When the count goes 50 | * over 32767, it will overflow down to -32768. When the count goes below 51 | * -32768, it will overflow up to 32767. */ 52 | static int16_t getCountsLeft(); 53 | 54 | /*! This function is just like getCountsLeft() except it applies to the 55 | * right-side encoder. */ 56 | static int16_t getCountsRight(); 57 | 58 | /*! This function is just like getCountsLeft() except it also clears the 59 | * counts before returning. If you call this frequently enough, you will 60 | * not have to worry about the count overflowing. */ 61 | static int16_t getCountsAndResetLeft(); 62 | 63 | /*! This function is just like getCountsAndResetLeft() except it applies to 64 | * the right-side encoder. */ 65 | static int16_t getCountsAndResetRight(); 66 | 67 | /*! This function returns true if an error was detected on the left-side 68 | * encoder. It resets the error flag automatically, so it will only return 69 | * true if an error was detected since the last time checkErrorLeft() was 70 | * called. 71 | * 72 | * If an error happens, it means that both of the encoder outputs changed at 73 | * the same time from the perspective of the ISR, so the ISR was unable to 74 | * tell what direction the motor was moving, and the encoder count could be 75 | * inaccurate. The most likely cause for an error is that the interrupt 76 | * service routine for the encoders could not be started soon enough. If 77 | * you get encoder errors, make sure you are not disabling interrupts for 78 | * extended periods of time in your code. */ 79 | static bool checkErrorLeft(); 80 | 81 | /*! This function is just like checkErrorLeft() except it applies to 82 | * the right-side encoder. */ 83 | static bool checkErrorRight(); 84 | 85 | private: 86 | 87 | static void init2(); 88 | }; 89 | -------------------------------------------------------------------------------- /src/Zumo32U4IMU.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define TEST_REG_ERROR -1 5 | 6 | #define LSM303D_WHO_ID 0x49 7 | #define L3GD20H_WHO_ID 0xD7 8 | #define LSM6DS33_WHO_ID 0x69 9 | #define LIS3MDL_WHO_ID 0x3D 10 | 11 | bool Zumo32U4IMU::init() 12 | { 13 | if (testReg(LSM303D_ADDR, LSM303D_REG_WHO_AM_I) == LSM303D_WHO_ID && 14 | testReg(L3GD20H_ADDR, L3GD20H_REG_WHO_AM_I) == L3GD20H_WHO_ID) 15 | { 16 | type = Zumo32U4IMUType::LSM303D_L3GD20H; 17 | return true; 18 | } 19 | else if (testReg(LSM6DS33_ADDR, LSM6DS33_REG_WHO_AM_I) == LSM6DS33_WHO_ID && 20 | testReg( LIS3MDL_ADDR, LIS3MDL_REG_WHO_AM_I) == LIS3MDL_WHO_ID) 21 | { 22 | type = Zumo32U4IMUType::LSM6DS33_LIS3MDL; 23 | return true; 24 | } 25 | else 26 | { 27 | return false; 28 | } 29 | } 30 | 31 | void Zumo32U4IMU::enableDefault() 32 | { 33 | switch (type) 34 | { 35 | case Zumo32U4IMUType::LSM303D_L3GD20H: 36 | 37 | // Accelerometer 38 | 39 | // 0x57 = 0b01010111 40 | // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) 41 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x57); 42 | if (lastError) { return; } 43 | 44 | // 0x00 = 0b00000000 45 | // AFS = 0 (+/- 2 g full scale) 46 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL2, 0x00); 47 | if (lastError) { return; } 48 | 49 | // Magnetometer 50 | 51 | // 0x64 = 0b01100100 52 | // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) 53 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL5, 0x64); 54 | if (lastError) { return; } 55 | 56 | // 0x20 = 0b00100000 57 | // MFS = 01 (+/- 4 gauss full scale) 58 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL6, 0x20); 59 | if (lastError) { return; } 60 | 61 | // 0x00 = 0b00000000 62 | // MD = 00 (continuous-conversion mode) 63 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL7, 0x00); 64 | if (lastError) { return; } 65 | 66 | // Gyro 67 | 68 | // 0x7F = 0b01111111 69 | // DR = 01 (189.4 Hz ODR); BW = 11 (70 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 70 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0x7F); 71 | if (lastError) { return; } 72 | 73 | // 0x00 = 0b00000000 74 | // FS = 00 (+/- 245 dps full scale) 75 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x00); 76 | return; 77 | 78 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 79 | 80 | // Accelerometer 81 | 82 | // 0x30 = 0b00110000 83 | // ODR = 0011 (52 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale) 84 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x30); 85 | if (lastError) { return; } 86 | 87 | // Gyro 88 | 89 | // 0x50 = 0b01010000 90 | // ODR = 0101 (208 Hz (high performance)); FS_G = 00 (+/- 245 dps full scale) 91 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x50); 92 | if (lastError) { return; } 93 | 94 | // Accelerometer + Gyro 95 | 96 | // 0x04 = 0b00000100 97 | // IF_INC = 1 (automatically increment register address) 98 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL3_C, 0x04); 99 | if (lastError) { return; } 100 | 101 | // Magnetometer 102 | 103 | // 0x70 = 0b01110000 104 | // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR) 105 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG1, 0x70); 106 | if (lastError) { return; } 107 | 108 | // 0x00 = 0b00000000 109 | // FS = 00 (+/- 4 gauss full scale) 110 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG2, 0x00); 111 | if (lastError) { return; } 112 | 113 | // 0x00 = 0b00000000 114 | // MD = 00 (continuous-conversion mode) 115 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG3, 0x00); 116 | if (lastError) { return; } 117 | 118 | // 0x0C = 0b00001100 119 | // OMZ = 11 (ultra-high-performance mode for Z) 120 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG4, 0x0C); 121 | return; 122 | 123 | default: 124 | return; 125 | } 126 | } 127 | 128 | void Zumo32U4IMU::configureForBalancing() 129 | { 130 | switch (type) 131 | { 132 | case Zumo32U4IMUType::LSM303D_L3GD20H: 133 | 134 | // Accelerometer 135 | 136 | // 0x18 = 0b00011000 137 | // AFS = 0 (+/- 2 g full scale) 138 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL2, 0x18); 139 | if (lastError) { return; } 140 | 141 | // Gyro 142 | 143 | // 0xFF = 0b11111111 144 | // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 145 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF); 146 | if (lastError) { return; } 147 | 148 | // 0x20 = 0b00100000 149 | // FS = 10 (+/- 2000 dps full scale) 150 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20); 151 | return; 152 | 153 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 154 | 155 | // Accelerometer 156 | 157 | // 0x3C = 0b00111100 158 | // ODR = 0011 (52 Hz (high performance)); FS_XL = 11 (+/- 8 g full scale) 159 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x3C); 160 | if (lastError) { return; } 161 | 162 | // Gyro 163 | 164 | // 0x7C = 0b01111100 165 | // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale) 166 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C); 167 | return; 168 | 169 | default: 170 | return; 171 | } 172 | } 173 | 174 | void Zumo32U4IMU::configureForTurnSensing() 175 | { 176 | switch (type) 177 | { 178 | case Zumo32U4IMUType::LSM303D_L3GD20H: 179 | 180 | // Gyro 181 | 182 | // 0xFF = 0b11111111 183 | // DR = 11 (757.6 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) 184 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL1, 0xFF); 185 | if (lastError) { return; } 186 | 187 | // 0x20 = 0b00100000 188 | // FS = 10 (+/- 2000 dps full scale) 189 | writeReg(L3GD20H_ADDR, L3GD20H_REG_CTRL4, 0x20); 190 | return; 191 | 192 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 193 | 194 | // Gyro 195 | 196 | // 0x7C = 0b01111100 197 | // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale) 198 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C); 199 | return; 200 | 201 | default: 202 | return; 203 | } 204 | } 205 | 206 | void Zumo32U4IMU::configureForFaceUphill() 207 | { 208 | switch (type) 209 | { 210 | case Zumo32U4IMUType::LSM303D_L3GD20H: 211 | 212 | // Accelerometer 213 | 214 | // 0x37 = 0b00110111 215 | // AODR = 0011 (12.5 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) 216 | writeReg(LSM303D_ADDR, LSM303D_REG_CTRL1, 0x37); 217 | return; 218 | 219 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 220 | 221 | // Accelerometer 222 | 223 | // 0x10 = 0b00010000 224 | // ODR = 0001 (13 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale) 225 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x10); 226 | return; 227 | 228 | default: 229 | return; 230 | } 231 | } 232 | 233 | void Zumo32U4IMU::writeReg(uint8_t addr, uint8_t reg, uint8_t value) 234 | { 235 | Wire.beginTransmission(addr); 236 | Wire.write(reg); 237 | Wire.write(value); 238 | lastError = Wire.endTransmission(); 239 | } 240 | 241 | uint8_t Zumo32U4IMU::readReg(uint8_t addr, uint8_t reg) 242 | { 243 | Wire.beginTransmission(addr); 244 | Wire.write(reg); 245 | lastError = Wire.endTransmission(); 246 | if (lastError) { return 0; } 247 | 248 | uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1); 249 | if (byteCount != 1) 250 | { 251 | lastError = 50; 252 | return 0; 253 | } 254 | return Wire.read(); 255 | } 256 | 257 | // Reads the 3 accelerometer channels and stores them in vector a 258 | void Zumo32U4IMU::readAcc(void) 259 | { 260 | switch (type) 261 | { 262 | case Zumo32U4IMUType::LSM303D_L3GD20H: 263 | // set MSB of register address for auto-increment 264 | readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_A | (1 << 7), a); 265 | return; 266 | 267 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 268 | // assumes register address auto-increment is enabled (IF_INC in CTRL3_C) 269 | readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_XL, a); 270 | return; 271 | 272 | default: 273 | return; 274 | } 275 | } 276 | 277 | // Reads the 3 gyro channels and stores them in vector g 278 | void Zumo32U4IMU::readGyro() 279 | { 280 | switch (type) 281 | { 282 | case Zumo32U4IMUType::LSM303D_L3GD20H: 283 | // set MSB of register address for auto-increment 284 | readAxes16Bit(L3GD20H_ADDR, L3GD20H_REG_OUT_X_L | (1 << 7), g); 285 | return; 286 | 287 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 288 | // assumes register address auto-increment is enabled (IF_INC in CTRL3_C) 289 | readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_G, g); 290 | return; 291 | 292 | default: 293 | return; 294 | } 295 | } 296 | 297 | // Reads the 3 magnetometer channels and stores them in vector m 298 | void Zumo32U4IMU::readMag() 299 | { 300 | switch (type) 301 | { 302 | case Zumo32U4IMUType::LSM303D_L3GD20H: 303 | // set MSB of register address for auto-increment 304 | readAxes16Bit(LSM303D_ADDR, LSM303D_REG_OUT_X_L_M | (1 << 7), m); 305 | return; 306 | 307 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 308 | // set MSB of register address for auto-increment 309 | readAxes16Bit(LIS3MDL_ADDR, LIS3MDL_REG_OUT_X_L | (1 << 7), m); 310 | return; 311 | 312 | default: 313 | return; 314 | } 315 | } 316 | 317 | // Reads all 9 accelerometer, gyro, and magnetometer channels and stores them 318 | // in the respective vectors 319 | void Zumo32U4IMU::read() 320 | { 321 | readAcc(); 322 | if (lastError) { return; } 323 | readGyro(); 324 | if (lastError) { return; } 325 | readMag(); 326 | } 327 | 328 | bool Zumo32U4IMU::accDataReady() 329 | { 330 | switch (type) 331 | { 332 | case Zumo32U4IMUType::LSM303D_L3GD20H: 333 | return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_A) & 0x08; 334 | 335 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 336 | return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x01; 337 | 338 | default: 339 | return false; 340 | } 341 | } 342 | 343 | bool Zumo32U4IMU::gyroDataReady() 344 | { 345 | switch (type) 346 | { 347 | case Zumo32U4IMUType::LSM303D_L3GD20H: 348 | return readReg(L3GD20H_ADDR, L3GD20H_REG_STATUS) & 0x08; 349 | 350 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 351 | return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x02; 352 | 353 | default: 354 | return false; 355 | } 356 | } 357 | 358 | bool Zumo32U4IMU::magDataReady() 359 | { 360 | switch (type) 361 | { 362 | case Zumo32U4IMUType::LSM303D_L3GD20H: 363 | return readReg(LSM303D_ADDR, LSM303D_REG_STATUS_M) & 0x08; 364 | 365 | case Zumo32U4IMUType::LSM6DS33_LIS3MDL: 366 | return readReg(LIS3MDL_ADDR, LIS3MDL_REG_STATUS_REG) & 0x08; 367 | 368 | default: 369 | return false; 370 | } 371 | } 372 | 373 | int16_t Zumo32U4IMU::testReg(uint8_t addr, uint8_t reg) 374 | { 375 | Wire.beginTransmission(addr); 376 | Wire.write(reg); 377 | if (Wire.endTransmission() != 0) 378 | { 379 | return TEST_REG_ERROR; 380 | } 381 | 382 | uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1); 383 | if (byteCount != 1) 384 | { 385 | return TEST_REG_ERROR; 386 | } 387 | return Wire.read(); 388 | } 389 | 390 | void Zumo32U4IMU::readAxes16Bit(uint8_t addr, uint8_t firstReg, vector & v) 391 | { 392 | Wire.beginTransmission(addr); 393 | Wire.write(firstReg); 394 | lastError = Wire.endTransmission(); 395 | if (lastError) { return; } 396 | 397 | uint8_t byteCount = (Wire.requestFrom(addr, (uint8_t)6)); 398 | if (byteCount != 6) 399 | { 400 | lastError = 50; 401 | return; 402 | } 403 | uint8_t xl = Wire.read(); 404 | uint8_t xh = Wire.read(); 405 | uint8_t yl = Wire.read(); 406 | uint8_t yh = Wire.read(); 407 | uint8_t zl = Wire.read(); 408 | uint8_t zh = Wire.read(); 409 | 410 | // combine high and low bytes 411 | v.x = (int16_t)(xh << 8 | xl); 412 | v.y = (int16_t)(yh << 8 | yl); 413 | v.z = (int16_t)(zh << 8 | zl); 414 | } -------------------------------------------------------------------------------- /src/Zumo32U4IMU.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4IMU.h */ 4 | 5 | #pragma once 6 | 7 | /*! \anchor device_addresses 8 | * 9 | * \name Device Addresses 10 | * @{ 11 | */ 12 | #define LSM303D_ADDR 0b0011101 13 | #define L3GD20H_ADDR 0b1101011 14 | #define LSM6DS33_ADDR 0b1101011 15 | #define LIS3MDL_ADDR 0b0011110 16 | /*! @} */ 17 | 18 | /*! \anchor register_addresses 19 | * 20 | * \name Register Addresses 21 | * @{ 22 | */ 23 | #define LSM303D_REG_STATUS_M 0x07 24 | #define LSM303D_REG_OUT_X_L_M 0x08 25 | #define LSM303D_REG_WHO_AM_I 0x0F 26 | #define LSM303D_REG_CTRL1 0x20 27 | #define LSM303D_REG_CTRL2 0x21 28 | #define LSM303D_REG_CTRL5 0x24 29 | #define LSM303D_REG_CTRL6 0x25 30 | #define LSM303D_REG_CTRL7 0x26 31 | #define LSM303D_REG_STATUS_A 0x27 32 | #define LSM303D_REG_OUT_X_L_A 0x28 33 | 34 | #define L3GD20H_REG_WHO_AM_I 0x0F 35 | #define L3GD20H_REG_CTRL1 0x20 36 | #define L3GD20H_REG_CTRL4 0x23 37 | #define L3GD20H_REG_STATUS 0x27 38 | #define L3GD20H_REG_OUT_X_L 0x28 39 | 40 | #define LSM6DS33_REG_WHO_AM_I 0x0F 41 | #define LSM6DS33_REG_CTRL1_XL 0x10 42 | #define LSM6DS33_REG_CTRL2_G 0x11 43 | #define LSM6DS33_REG_CTRL3_C 0x12 44 | #define LSM6DS33_REG_STATUS_REG 0x1E 45 | #define LSM6DS33_REG_OUTX_L_G 0x22 46 | #define LSM6DS33_REG_OUTX_L_XL 0x28 47 | 48 | #define LIS3MDL_REG_WHO_AM_I 0x0F 49 | #define LIS3MDL_REG_CTRL_REG1 0x20 50 | #define LIS3MDL_REG_CTRL_REG2 0x21 51 | #define LIS3MDL_REG_CTRL_REG3 0x22 52 | #define LIS3MDL_REG_CTRL_REG4 0x23 53 | #define LIS3MDL_REG_STATUS_REG 0x27 54 | #define LIS3MDL_REG_OUT_X_L 0x28 55 | /*! @} */ 56 | 57 | /*! \brief The type of the inertial sensors. */ 58 | enum class Zumo32U4IMUType : uint8_t { 59 | /*! Unknown or unrecognized */ 60 | Unknown, 61 | /*! LSM303D accelerometer + magnetometer, L3GD20H gyro */ 62 | LSM303D_L3GD20H, 63 | /*! LSM6DS33 gyro + accelerometer, LIS3MDL magnetometer */ 64 | LSM6DS33_LIS3MDL 65 | }; 66 | 67 | /*! \brief Interfaces with the inertial sensors on the Zumo 32U4. 68 | * 69 | * This class allows you to configure and get readings from the I2C sensors that 70 | * make up the Zumo 32U4's inertial measurement unit (IMU): gyro, accelerometer, 71 | * and magnetometer. 72 | * 73 | * You must call `Wire.start()` before using any of this library's functions 74 | * that access the sensors. */ 75 | class Zumo32U4IMU 76 | { 77 | public: 78 | 79 | /*! \brief Represents a 3-dimensional vector with x, y, and z 80 | * components. */ 81 | template struct vector 82 | { 83 | T x, y, z; 84 | }; 85 | 86 | /*! \brief Raw accelerometer readings. */ 87 | vector a = {0, 0, 0}; 88 | 89 | /*! \brief Raw gyro readings. */ 90 | vector g = {0, 0, 0}; 91 | 92 | /*! \brief Raw magnetometer readings. */ 93 | vector m = {0, 0, 0}; 94 | 95 | /*! \brief Returns 0 if the last I2C communication with the IMU was 96 | * successful, or a non-zero status code if there was an error. */ 97 | uint8_t getLastError() { return lastError; } 98 | 99 | /*! \brief Initializes the inertial sensors and detects their type. 100 | * 101 | * \return True if the sensor type was detected succesfully; false otherwise. 102 | */ 103 | bool init(); 104 | 105 | /*! \brief Returns the type of the inertial sensors on the Zumo 32U4. 106 | * 107 | * \return The sensor type as a member of the Zumo32U4IMUType enum. If the 108 | * type is not known (e.g. if init() has not been called yet), this will be 109 | * Zumo32U4IMUType::Unknown. */ 110 | Zumo32U4IMUType getType() { return type; } 111 | 112 | /*! \brief Enables all of the inertial sensors with a default configuration. 113 | */ 114 | void enableDefault(); 115 | 116 | /*! \brief Configures the sensors with settings optimized for turn sensing. */ 117 | void configureForTurnSensing(); 118 | 119 | /*! \brief Configures the sensors with settings optimized for balancing. */ 120 | void configureForBalancing(); 121 | 122 | /*! \brief Configures the sensors with settings optimized for the FaceUphill 123 | * example program. */ 124 | void configureForFaceUphill(); 125 | 126 | /*! \brief Writes an 8-bit sensor register. 127 | * 128 | * \param addr Device address. 129 | * \param reg Register address. 130 | * \param value 8-bit register value to be written. */ 131 | void writeReg(uint8_t addr, uint8_t reg, uint8_t value); 132 | 133 | /*! \brief Reads an 8-bit sensor register. 134 | * 135 | * \param addr Device address. 136 | * \param reg Register address. 137 | * 138 | * \return 8-bit register value read from the device. */ 139 | uint8_t readReg(uint8_t addr, uint8_t reg); 140 | 141 | /*! \brief Takes a reading from the accelerometer and makes the measurements 142 | * available in #a. */ 143 | void readAcc(); 144 | 145 | /*! \brief Takes a reading from the gyro and makes the measurements available 146 | * in #g. */ 147 | void readGyro(); 148 | 149 | /*! \brief Takes a reading from the magnetometer and makes the measurements 150 | * available in #m. */ 151 | void readMag(); 152 | 153 | /*! \brief Takes a reading from all three sensors (accelerometer, gyro, and 154 | * magnetometer) and makes their measurements available in the respective 155 | * vectors. */ 156 | void read(); 157 | 158 | /*! \brief Indicates whether the accelerometer has new measurement data ready. 159 | * 160 | * \return True if there is new accelerometer data available; false otherwise. 161 | */ 162 | bool accDataReady(); 163 | 164 | /*! \brief Indicates whether the gyro has new measurement data ready. 165 | * 166 | * \return True if there is new gyro data available; false otherwise. 167 | */ 168 | bool gyroDataReady(); 169 | 170 | /*! \brief Indicates whether the magnetometer has new measurement data ready. 171 | * 172 | * \return True if there is new magnetometer data available; false otherwise. 173 | */ 174 | bool magDataReady(); 175 | 176 | private: 177 | 178 | uint8_t lastError = 0; 179 | Zumo32U4IMUType type = Zumo32U4IMUType::Unknown; 180 | 181 | int16_t testReg(uint8_t addr, uint8_t reg); 182 | void readAxes16Bit(uint8_t addr, uint8_t firstReg, vector & v); 183 | }; -------------------------------------------------------------------------------- /src/Zumo32U4IRPulses.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | void Zumo32U4IRPulses::start(Direction direction, uint16_t brightness, uint16_t period) 8 | { 9 | // Disable Timer 3's interrupts. This should be done first because another 10 | // library might be using the timer and its ISR might be modifying timer 11 | // registers. 12 | TIMSK3 = 0; 13 | 14 | // Make sure brightness is not larger than period because then the compare 15 | // match would never happen and the pulse count would always be zero. 16 | if (brightness > period) 17 | { 18 | brightness = period; 19 | } 20 | 21 | // Set the PWM pin to be an input temporarily. Otherwise, when we configure 22 | // the COM3A<1:0> bits, the OC03A signal might be high from previous 23 | // activity of the timer and result in a glitch on the pin. 24 | PORTC &= ~(1 << 6); 25 | DDRC &= ~(1 << 6); 26 | 27 | // Put the timer into a known state that should not cause any trouble while 28 | // we are reconfiguring it. 29 | // COM3A<1:0> = 10 : Clear OC3A on match, set at top. 30 | TCCR3A = (1 << COM3A1); 31 | TCCR3B = 0; 32 | 33 | // Simulate a compare match, which makes the OC3A PWM signal (which is not 34 | // connected to the I/O pin yet) go low. We must do this after configuring 35 | // COM3A<1:0>. 36 | TCCR3C = (1 << FOC3A); 37 | 38 | // Make the PWM pin be an output. The OC03A signal will control its value. 39 | DDRC |= (1 << 6); 40 | 41 | // Drive PF6/A1 high or low to select which LEDs to use. 42 | if (direction) 43 | { 44 | // Right 45 | PORTF |= (1 << 6); 46 | } 47 | else 48 | { 49 | // Left 50 | PORTF &= ~(1 << 6); 51 | } 52 | DDRF |= (1 << 6); 53 | 54 | // Set frequency/period. 55 | ICR3 = period; 56 | 57 | // Set the count to be one less than ICR3 so that the new duty cycle 58 | // will take effect very soon. 59 | TCNT3 = period - 1; 60 | 61 | // Set the duty cycle. 62 | OCR3A = brightness; 63 | 64 | // Start the timer. It will start running once the clock source bits 65 | // in TCCR3B are set. 66 | // 67 | // COM3A<1:0> = 10 : Set OC3A on match, clear at top. 68 | // WGM3<3:0> = 1110 : Fast PWM, with ICR3 as the TOP. 69 | // CS3<3:0> = 001 : Internal clock with no prescaler 70 | TCCR3A = (1 << COM3A1) | (1 << WGM31); 71 | TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS30); 72 | } 73 | 74 | void Zumo32U4IRPulses::stop() 75 | { 76 | // Prepare the PWM pin to drive low. We don't want to just set it as an 77 | // input because then it might decay from high to low gradually and the 78 | // LEDs would not turn off immediately. 79 | PORTC &= ~(1 << 6); 80 | DDRC |= (1 << 6); 81 | 82 | // Disconnect the PWM signal from the pin, causing it to drive low. We must 83 | // do this before stopping the timer to avoid glitches. 84 | TCCR3A = (1 << WGM31); 85 | 86 | // Turn off the timer. 87 | TCCR3B = 0; 88 | 89 | // Restore the timer's default settings to help avoid compatibilty issues 90 | // with other libraries. 91 | TIMSK3 = 0; 92 | TCCR3A = 0; 93 | OCR3A = 0; 94 | ICR3 = 0; 95 | TCNT3 = 0; 96 | 97 | // Change the IR LED direction pin (A1) back to an input so it 98 | // can be used for measuring the battery level. 99 | DDRF &= ~(1 << 6); 100 | PORTF &= ~(1 << 6); 101 | } 102 | -------------------------------------------------------------------------------- /src/Zumo32U4IRPulses.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /** \file Zumo32U4IRPulses.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /** \brief Emits pulses of infrared (IR) light using the IR LEDs on the Zumo 10 | 32U4 Main Board. 11 | 12 | Timer 3 is used to generate a PWM signal, so this library might conflict with 13 | other libraries that use Timer 3. When the pulses are stopped, Timer 3 can be 14 | used for other purposes. 15 | 16 | Pin A1 (PF6) is used to select which set of LEDs to turn on: the left-side LEDs 17 | or the right-side LEDs. 18 | 19 | Pin 5 (PC6/OC3A) is used as a PWM output to turn the LEDs on and off. 20 | 21 | This class does not do anything with the IR LEDs or detectors on the Zumo 32U4 22 | Front Sensor Array. 23 | */ 24 | class Zumo32U4IRPulses 25 | { 26 | public: 27 | 28 | /** This enum defines the two different sets of LEDs that can be used to 29 | emit pulses. */ 30 | enum Direction 31 | { 32 | /** The LEDs on the left side of the robot. */ 33 | Left = 0, 34 | 35 | /** The LEDs on the right side of the robot. */ 36 | Right = 1 37 | }; 38 | 39 | /** The default frequency is 16000000 / (420 + 1) = 38.005 kHz */ 40 | static const uint16_t defaultPeriod = 420; 41 | 42 | /** \brief Starts emitting IR pulses. 43 | * 44 | * \param direction Specifies which set of LEDs to turn on. 45 | * \param brightness A number that specifies 46 | * how long each pulse is. The pulse length will be 47 | * (1 + brightness) / (16 MHz). 48 | * If \c brightness is greater than or equal to \c period, then the LEDs 49 | * will just be on constantly. 50 | * \param period A number that specifies the frequency of the pulses. 51 | * The interval between consecutive the rising edges of pulses will be 52 | * (1 + period) / (16 MHz). 53 | * The default value is 420, which results in a period very close to 54 | * 38 kHz. */ 55 | static void start(Direction direction, uint16_t brightness, 56 | uint16_t period = defaultPeriod); 57 | 58 | /** \brief Stops emitting IR pulses. 59 | * 60 | * Timer 3 can be used for other purposes after calling this 61 | * function. */ 62 | static void stop(); 63 | }; 64 | -------------------------------------------------------------------------------- /src/Zumo32U4LCD.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4LCD.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | /*! \brief Writes data to the LCD on the Zumo 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 Zumo 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 Zumo32U4LCD : public PololuHD44780Base 34 | { 35 | // Pin assignments 36 | static const uint8_t rs = 0, e = 1, 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 | -------------------------------------------------------------------------------- /src/Zumo32U4LineSensors.h: -------------------------------------------------------------------------------- 1 | /** \file Zumo32U4LineSensors.h **/ 2 | 3 | // Doxygen needs this to recognize that Zumo32U4LineSensors::init() overrides 4 | // QTRSensorsRC::init(). 5 | #ifdef DOXYGEN 6 | typedef unsigned char uint8_t; 7 | typedef unsigned int uint16_t; 8 | #endif 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | /** \brief The pin number for the standard pin that is used to read line sensor 15 | * 1, the left-most sensor. */ 16 | static const uint8_t SENSOR_DOWN1 = 18; 17 | 18 | /** \brief The pin number for the standard pin that is used to read line sensor 19 | * 2, the sensor second from the left. */ 20 | static const uint8_t SENSOR_DOWN2 = 20; 21 | 22 | /** \brief The pin number for standard pin that is used to read line sensor 3, 23 | * the middle sensor. */ 24 | static const uint8_t SENSOR_DOWN3 = 21; 25 | 26 | /** \brief The pin number for the standard pin that is used to read line sensor 27 | * 4, the sensor second from the right. */ 28 | static const uint8_t SENSOR_DOWN4 = 4; 29 | 30 | /** \brief The pin number for the standard pin that is used to read line sensor 31 | * 5, the right-most sensor. */ 32 | static const uint8_t SENSOR_DOWN5 = 12; 33 | 34 | /** \brief The pin number for the standard pin that is used to enable or disable 35 | * the IR emitters of the line sensors. */ 36 | static const uint8_t SENSOR_LEDON = 11; 37 | 38 | /** \brief Gets readings from the five down-facing line sensors on the front 39 | * sensor array. 40 | * 41 | * The functions that this class inherits from QTRSensorsRC, such as read() and 42 | * calibrate(), are also documented in the 43 | * [user's guide for the %QTRSensors library](https://www.pololu.com/docs/0J19). 44 | * %QTRSensors is a separate Arduino library with a [separate GitHub 45 | * repository](https://github.com/pololu/qtr-sensors-arduino), but we include a 46 | * copy of it in the Zumo32U4 library since it is needed for the 47 | * Zumo32U4LineSensors class. (Note that the included copy of QTRSensors is an 48 | * older version with some differences from the latest standalone QTRSensors 49 | * library.) 50 | */ 51 | class Zumo32U4LineSensors : public QTRSensorsRC 52 | { 53 | public: 54 | 55 | /** \brief Minimal constructor. 56 | * 57 | * If you use this (i.e. by not providing any arguments when you create the 58 | * Zumo32U4ProximitySensors object), then you will have to call 59 | * initThreeSensors(), initFiveSensors(), or init() before using the 60 | * functions in this class. */ 61 | Zumo32U4LineSensors() { } 62 | 63 | /** \brief Constructor that takes pin arguments. 64 | * 65 | * This constructor calls init() with the specified arguments. */ 66 | Zumo32U4LineSensors(uint8_t * pins, uint8_t numSensors, 67 | uint8_t emitterPin = SENSOR_LEDON) 68 | { 69 | init(pins, numSensors, emitterPin); 70 | } 71 | 72 | /** \brief Configures this object to use just three line sensors. 73 | * 74 | * This function configures this object to just use line sensors 1, 3, and 75 | * 5. */ 76 | void initThreeSensors(uint8_t emitterPin = SENSOR_LEDON) 77 | { 78 | uint8_t pins[] = { SENSOR_DOWN1, SENSOR_DOWN3, SENSOR_DOWN5 }; 79 | init(pins, sizeof(pins), 2000, emitterPin); 80 | } 81 | 82 | /** \brief Configures this object to use all five line sensors. 83 | * 84 | * For this configuration to work, jumpers on the front sensor array must be 85 | * installed in order to connect pin 20 to DN2 and connect pin 4 to DN4. */ 86 | void initFiveSensors(uint8_t emitterPin = SENSOR_LEDON) 87 | { 88 | uint8_t pins[] = { SENSOR_DOWN1, SENSOR_DOWN2, SENSOR_DOWN3, 89 | SENSOR_DOWN4, SENSOR_DOWN5 }; 90 | init(pins, sizeof(pins), 2000, emitterPin); 91 | } 92 | 93 | /** \brief Configures this object to use a custom set of pins. 94 | * 95 | * \param pins A pointer to an array with the pin numbers for the sensors. 96 | * \param numSensors The number of sensors. 97 | * \param timeout Specifies the length of time in microseconds beyond 98 | * which you consider the sensor reading completely black. 99 | * \param emitterPin The number of the pin that controls the 100 | * emitters for the line sensors. You can specify a value of 101 | * QTR_NO_EMITTER_PIN for this parameter if you want this object to not do 102 | * anything to the emitters. */ 103 | void init(uint8_t * pins, uint8_t numSensors, 104 | uint16_t timeout = 2000, uint8_t emitterPin = SENSOR_LEDON) 105 | { 106 | QTRSensorsRC::init(pins, numSensors, timeout, emitterPin); 107 | } 108 | }; 109 | 110 | -------------------------------------------------------------------------------- /src/Zumo32U4Motors.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 | static bool flipLeft = false; 13 | static bool flipRight = false; 14 | 15 | // initialize timer1 to generate the proper PWM outputs to the motor drivers 16 | void Zumo32U4Motors::init2() 17 | { 18 | FastGPIO::Pin::setOutputLow(); 19 | FastGPIO::Pin::setOutputLow(); 20 | FastGPIO::Pin::setOutputLow(); 21 | FastGPIO::Pin::setOutputLow(); 22 | 23 | // Timer 1 configuration 24 | // prescaler: clockI/O / 1 25 | // outputs enabled 26 | // phase-correct PWM 27 | // top of 400 28 | // 29 | // PWM frequency calculation 30 | // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz 31 | TCCR1A = 0b10100000; 32 | TCCR1B = 0b00010001; 33 | ICR1 = 400; 34 | OCR1A = 0; 35 | OCR1B = 0; 36 | } 37 | 38 | // enable/disable flipping of left motor 39 | void Zumo32U4Motors::flipLeftMotor(bool flip) 40 | { 41 | flipLeft = flip; 42 | } 43 | 44 | // enable/disable flipping of right motor 45 | void Zumo32U4Motors::flipRightMotor(bool flip) 46 | { 47 | flipRight = flip; 48 | } 49 | 50 | // set speed for left motor; speed is a number between -400 and 400 51 | void Zumo32U4Motors::setLeftSpeed(int16_t speed) 52 | { 53 | init(); 54 | 55 | bool reverse = 0; 56 | 57 | if (speed < 0) 58 | { 59 | speed = -speed; // Make speed a positive quantity. 60 | reverse = 1; // Preserve the direction. 61 | } 62 | if (speed > 400) // Max PWM duty cycle. 63 | { 64 | speed = 400; 65 | } 66 | 67 | OCR1B = speed; 68 | 69 | FastGPIO::Pin::setOutput(reverse ^ flipLeft); 70 | } 71 | 72 | // set speed for right motor; speed is a number between -400 and 400 73 | void Zumo32U4Motors::setRightSpeed(int16_t speed) 74 | { 75 | init(); 76 | 77 | bool reverse = 0; 78 | 79 | if (speed < 0) 80 | { 81 | speed = -speed; // Make speed a positive quantity. 82 | reverse = 1; // Preserve the direction. 83 | } 84 | if (speed > 400) // Max PWM duty cycle. 85 | { 86 | speed = 400; 87 | } 88 | 89 | OCR1A = speed; 90 | 91 | FastGPIO::Pin::setOutput(reverse ^ flipRight); 92 | } 93 | 94 | // set speed for both motors 95 | void Zumo32U4Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) 96 | { 97 | setLeftSpeed(leftSpeed); 98 | setRightSpeed(rightSpeed); 99 | } 100 | -------------------------------------------------------------------------------- /src/Zumo32U4Motors.h: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | /*! \file Zumo32U4Motors.h */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | /*! \brief Controls motor speed and direction on the Zumo 32U4. 10 | * 11 | * This library uses Timer 1, so it will conflict with any other libraries using 12 | * that timer. */ 13 | class Zumo32U4Motors 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 Zumo was not installed 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 Zumo was not installed 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 -400 to 400 representing the speed and 43 | * direction of the left motor. Values of -400 or less result in full speed 44 | * reverse, and values of 400 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 -400 to 400 representing the speed and 50 | * direction of the right motor. Values of -400 or less result in full speed 51 | * reverse, and values of 400 or more result in full speed forward. */ 52 | static void setRightSpeed(int16_t speed); 53 | 54 | /** \brief Sets the speeds for both motors. 55 | * 56 | * \param leftSpeed A number from -400 to 400 representing the speed and 57 | * direction of the right motor. Values of -400 or less result in full speed 58 | * reverse, and values of 400 or more result in full speed forward. 59 | * \param rightSpeed A number from -400 to 400 representing the speed and 60 | * direction of the right motor. Values of -400 or less result in full speed 61 | * reverse, and values of 400 or more result in full speed forward. */ 62 | static void setSpeeds(int16_t leftSpeed, int16_t rightSpeed); 63 | 64 | private: 65 | 66 | static inline void init() 67 | { 68 | static bool initialized = false; 69 | 70 | if (!initialized) 71 | { 72 | initialized = true; 73 | init2(); 74 | } 75 | } 76 | 77 | static void init2(); 78 | }; 79 | -------------------------------------------------------------------------------- /src/Zumo32U4OLED.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // This asm inside the macro below is an optimized version of 12 | // FastGPIO::Pin::setOutputValue(d >> b & 1); 13 | // It prevents the compiler from using slow branches to implement the 14 | // conditional logic, and also ensures that the writing speed doesn't depend 15 | // on the data. 16 | #define _P3PP_OLED_SEND_BIT(b) \ 17 | FastGPIO::Pin::setOutputValueLow(); \ 18 | asm volatile( \ 19 | "sbrc %2, %3\n" "sbi %0, %1\n" \ 20 | "sbrs %2, %3\n" "cbi %0, %1\n" \ 21 | : : \ 22 | "I" (FastGPIO::pinStructs[mosPin].portAddr - __SFR_OFFSET), \ 23 | "I" (FastGPIO::pinStructs[mosPin].bit), \ 24 | "r" (d), \ 25 | "I" (b)); \ 26 | FastGPIO::Pin::setOutputValueHigh(); 27 | 28 | /// @brief Low-level functions for writing data to the SH1106 OLED on the 29 | /// Pololu Zumo 32U4 OLED robot. 30 | class Zumo32U4OLEDCore 31 | { 32 | // Pin assignments 33 | static const uint8_t clkPin = 1, mosPin = IO_D5, resPin = 0, dcPin = 17; 34 | 35 | public: 36 | void initPins() 37 | { 38 | FastGPIO::Pin::setOutputLow(); 39 | } 40 | 41 | void reset() 42 | { 43 | FastGPIO::Pin::setOutputLow(); 44 | _delay_us(10); 45 | FastGPIO::Pin::setOutputHigh(); 46 | _delay_us(10); 47 | } 48 | 49 | void sh1106TransferStart() 50 | { 51 | // From https://github.com/pololu/usb-pause-arduino/blob/master/USBPause.h: 52 | // Disables USB interrupts because the Arduino USB interrupts use some of 53 | // the OLED pins. 54 | savedUDIEN = UDIEN; 55 | UDIEN = 0; 56 | savedUENUM = UENUM; 57 | UENUM = 0; 58 | savedUEIENX0 = UEIENX; 59 | UEIENX = 0; 60 | 61 | savedStateMosi = FastGPIO::Pin::getState(); 62 | savedStateDc = FastGPIO::Pin::getState(); 63 | 64 | FastGPIO::Pin::setOutputLow(); 65 | } 66 | 67 | void sh1106TransferEnd() 68 | { 69 | FastGPIO::Pin::setState(savedStateMosi); 70 | FastGPIO::Pin::setState(savedStateDc); 71 | 72 | // From https://github.com/pololu/usb-pause-arduino/blob/master/USBPause.h 73 | UENUM = 0; 74 | UEIENX = savedUEIENX0; 75 | UENUM = savedUENUM; 76 | UDIEN = savedUDIEN; 77 | } 78 | 79 | void sh1106CommandMode() 80 | { 81 | FastGPIO::Pin::setOutputLow(); 82 | } 83 | 84 | void sh1106DataMode() 85 | { 86 | FastGPIO::Pin::setOutputHigh(); 87 | } 88 | 89 | void sh1106Write(uint8_t d) 90 | { 91 | _P3PP_OLED_SEND_BIT(7); 92 | _P3PP_OLED_SEND_BIT(6); 93 | _P3PP_OLED_SEND_BIT(5); 94 | _P3PP_OLED_SEND_BIT(4); 95 | _P3PP_OLED_SEND_BIT(3); 96 | _P3PP_OLED_SEND_BIT(2); 97 | _P3PP_OLED_SEND_BIT(1); 98 | _P3PP_OLED_SEND_BIT(0); 99 | } 100 | 101 | private: 102 | uint8_t savedStateMosi, savedStateDc; 103 | uint8_t savedUDIEN, savedUENUM, savedUEIENX0; 104 | }; 105 | 106 | /// @brief Makes it easy to show text and graphics on the SH1106 OLED of 107 | /// the Pololu Zumo 32U4 OLED robot. 108 | /// 109 | /// This class inherits from the PololuSH1106Main class in the PololuOLED 110 | /// library, which provides almost all of its functionality. See the 111 | /// [PololuOLED library documentation](https://pololu.github.io/pololu-oled-arduino/) 112 | /// for more information about how to use this class. 113 | /// 114 | /// This class also inherits from the Arduino Print class 115 | /// (via PololuSH1106Main), so you can call the `print()` function on it with a 116 | /// variety of arguments. See the 117 | /// [Arduino print() documentation](http://arduino.cc/en/Serial/Print) for 118 | /// more information. 119 | class Zumo32U4OLED : public PololuSH1106Main 120 | { 121 | }; 122 | -------------------------------------------------------------------------------- /src/Zumo32U4ProximitySensors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright Pololu Corporation. For more information, see http://www.pololu.com/ 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // The Arduino's digitalRead, digitalWrite, and pinMode functions will have 10 | // unexpected behavior if the pin number argument happens to be greater than 11 | // NUM_DIGITAL_PINS. We protect users of our library from that. 12 | static bool digitalReadSafe(uint8_t pin, bool defaultValue = 1) 13 | { 14 | if (pin < NUM_DIGITAL_PINS) 15 | { 16 | return digitalRead(pin); 17 | } 18 | else 19 | { 20 | return defaultValue; 21 | } 22 | } 23 | 24 | static void pinModeSafe(uint8_t pin, uint8_t mode) 25 | { 26 | if (pin < NUM_DIGITAL_PINS) 27 | { 28 | pinMode(pin, mode); 29 | } 30 | } 31 | 32 | void Zumo32U4ProximitySensors::clearAll() 33 | { 34 | dataArray = NULL; 35 | numSensors = 0; 36 | 37 | lineSensorEmitterPin = SENSOR_NO_PIN; 38 | 39 | levelsArray = NULL; 40 | numLevels = 0; 41 | 42 | pulseOnTimeUs = defaultPulseOnTimeUs; 43 | pulseOffTimeUs = defaultPulseOffTimeUs; 44 | period = defaultPeriod; 45 | } 46 | 47 | void Zumo32U4ProximitySensors::init(uint8_t * pins, uint8_t numSensors, 48 | uint8_t lineSensorEmitterPin) 49 | { 50 | this->lineSensorEmitterPin = lineSensorEmitterPin; 51 | 52 | // Allocate memory for the new dataArray. If there is an error, return 53 | // before modifying the existing dataArray or numSensors so that we 54 | // don't leave the object in an invalid state. 55 | const size_t size = numSensors * sizeof(SensorData); 56 | void * newArray = realloc(dataArray, size); 57 | if (newArray == NULL) { return; } 58 | 59 | // Store the pointer to the new dataArray. 60 | this->dataArray = (SensorData *)newArray; 61 | 62 | // Store the number of sensors. 63 | this->numSensors = numSensors; 64 | 65 | for (uint8_t i = 0; i < numSensors; i++) 66 | { 67 | // Store the pin numbers specified by the user. 68 | dataArray[i].pin = pins[i]; 69 | } 70 | } 71 | 72 | 73 | void Zumo32U4ProximitySensors::setBrightnessLevels(uint16_t * levels, uint8_t numLevels) 74 | { 75 | const size_t size = numLevels * sizeof(uint16_t); 76 | 77 | void * newArray = realloc(levelsArray, size); 78 | if (newArray == NULL) 79 | { 80 | // There was an error allocating memory so just leave the 81 | // state of the object the same as it was before. 82 | return; 83 | } 84 | 85 | memcpy(newArray, levels, size); 86 | levelsArray = (uint16_t *)newArray; 87 | this->numLevels = numLevels; 88 | } 89 | 90 | void Zumo32U4ProximitySensors::prepareToRead() 91 | { 92 | pullupsOn(); 93 | 94 | lineSensorEmittersOff(); 95 | 96 | if (levelsArray == NULL) 97 | { 98 | uint16_t defaultBrightnessLevels[] = { 4, 15, 32, 55, 85, 120 }; 99 | setBrightnessLevels(defaultBrightnessLevels, 6); 100 | } 101 | } 102 | 103 | void Zumo32U4ProximitySensors::pullupsOn() 104 | { 105 | // Set all the sensor pins to be pulled-up inputs so that they 106 | // are high whenever the sensor outputs are not active. 107 | for (uint8_t i = 0; i < numSensors; i++) 108 | { 109 | pinModeSafe(dataArray[i].pin, INPUT_PULLUP); 110 | } 111 | } 112 | 113 | // Turn off the down-facing IR LEDs because the proximity 114 | // sensors tend to detect the IR coming from them. 115 | void Zumo32U4ProximitySensors::lineSensorEmittersOff() 116 | { 117 | if (lineSensorEmitterPin < NUM_DIGITAL_PINS) 118 | { 119 | digitalWrite(lineSensorEmitterPin, LOW); 120 | pinMode(lineSensorEmitterPin, OUTPUT); 121 | delayMicroseconds(pulseOffTimeUs); 122 | } 123 | } 124 | 125 | /* It is not feasible to turn off the pulses before checking the output of 126 | * the sensor because an interrupt might fire and cause the sensor check to 127 | * happen too late. 128 | */ 129 | void Zumo32U4ProximitySensors::read() 130 | { 131 | prepareToRead(); 132 | 133 | for (uint8_t i = 0; i < numSensors; i++) 134 | { 135 | dataArray[i].withRightLeds = 0; 136 | dataArray[i].withLeftLeds = 0; 137 | } 138 | 139 | for (uint8_t i = 0; i < numLevels; i++) 140 | { 141 | uint16_t brightness = levelsArray[i]; 142 | 143 | Zumo32U4IRPulses::start(Zumo32U4IRPulses::Left, brightness, period); 144 | delayMicroseconds(pulseOnTimeUs); 145 | for (uint8_t i = 0; i < numSensors; i++) 146 | { 147 | if (!digitalReadSafe(dataArray[i].pin, 1)) 148 | { 149 | dataArray[i].withLeftLeds++; 150 | } 151 | } 152 | Zumo32U4IRPulses::stop(); 153 | delayMicroseconds(pulseOffTimeUs); 154 | 155 | Zumo32U4IRPulses::start(Zumo32U4IRPulses::Right, brightness, period); 156 | delayMicroseconds(pulseOnTimeUs); 157 | for (uint8_t i = 0; i < numSensors; i++) 158 | { 159 | if (!digitalReadSafe(dataArray[i].pin, 1)) 160 | { 161 | dataArray[i].withRightLeds++; 162 | } 163 | } 164 | Zumo32U4IRPulses::stop(); 165 | delayMicroseconds(pulseOffTimeUs); 166 | } 167 | } 168 | 169 | bool Zumo32U4ProximitySensors::readBasic(uint8_t sensorNumber) 170 | { 171 | if (sensorNumber >= numSensors) { return 0; } 172 | return !digitalReadSafe(dataArray[sensorNumber].pin, 1); 173 | } 174 | 175 | uint8_t Zumo32U4ProximitySensors::countsWithLeftLeds(uint8_t sensorNumber) const 176 | { 177 | if (sensorNumber >= numSensors) { return 0; } 178 | return dataArray[sensorNumber].withLeftLeds; 179 | } 180 | 181 | uint8_t Zumo32U4ProximitySensors::countsWithRightLeds(uint8_t sensorNumber) const 182 | { 183 | if (sensorNumber >= numSensors) { return 0; } 184 | return dataArray[sensorNumber].withRightLeds; 185 | } 186 | 187 | uint8_t Zumo32U4ProximitySensors::findIndexForPin(uint8_t pin) const 188 | { 189 | for (uint8_t i = 0; i < numSensors; i++) 190 | { 191 | if (dataArray[i].pin == pin) 192 | { 193 | return i; 194 | } 195 | } 196 | 197 | // The specified pin is not being used as a sensor, so return 255. This can 198 | // be safely passed to countsWithLeftLeds, countsWithRightLeds, or 199 | // readBasic, and those functions will return 0. 200 | return 255; 201 | } 202 | --------------------------------------------------------------------------------