├── .arduino-ci.yaml ├── .github ├── ISSUE_TEMPLATE │ ├── bug-report-or-feature-request.md │ └── config.yml └── workflows │ └── ci.yaml ├── .gitignore ├── .gitlab-ci.yml ├── Doxyfile ├── LICENSE.txt ├── README.md ├── examples ├── BlinkLEDs │ └── BlinkLEDs.ino ├── BumpSensorTest │ └── BumpSensorTest.ino ├── DemoForLCDVersion │ └── DemoForLCDVersion.ino ├── DemoForOLEDVersion │ ├── DemoForOLEDVersion.ino │ ├── extended_lcd_font.h │ ├── font.cpp │ ├── pololu_3pi_plus_splash.h │ ├── splash.cpp │ └── splash.h ├── Encoders │ └── Encoders.ino ├── FaceUphill │ └── FaceUphill.ino ├── InertialSensors │ └── InertialSensors.ino ├── LineFollowerPID │ ├── LineFollowerPID.ino │ ├── extended_lcd_font.h │ └── font.cpp ├── LineFollowerSimple │ ├── LineFollowerSimple.ino │ ├── extended_lcd_font.h │ └── font.cpp ├── LineSensorTest │ └── LineSensorTest.ino ├── RotationResist │ ├── RotationResist.ino │ └── TurnSensor.h ├── TurnWithCompass │ └── TurnWithCompass.ino └── WallBumper │ └── WallBumper.ino ├── keywords.txt ├── library.properties └── src ├── Pololu3piPlus32U4.h ├── Pololu3piPlus32U4BumpSensors.cpp ├── Pololu3piPlus32U4BumpSensors.h ├── Pololu3piPlus32U4Buttons.h ├── Pololu3piPlus32U4Buzzer.h ├── Pololu3piPlus32U4Encoders.cpp ├── Pololu3piPlus32U4Encoders.h ├── Pololu3piPlus32U4IMU.cpp ├── Pololu3piPlus32U4IMU.h ├── Pololu3piPlus32U4IMU_declaration.h ├── Pololu3piPlus32U4LCD.h ├── Pololu3piPlus32U4LineSensors.cpp ├── Pololu3piPlus32U4LineSensors.h ├── Pololu3piPlus32U4Motors.cpp ├── Pololu3piPlus32U4Motors.h └── Pololu3piPlus32U4OLED.h /.arduino-ci.yaml: -------------------------------------------------------------------------------- 1 | only_boards: 2 | - arduino:avr:leonardo 3 | library_archives: 4 | - PololuMenu=https://github.com/pololu/pololu-menu-arduino/archive/2.0.0.tar.gz=08p86134yki2z15zq9jwbqld4zvgjg1cp0799mrwx6qjnbsjbssk 5 | -------------------------------------------------------------------------------- /.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 = "Pololu3piPlus32U4 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 | SHOW_NAMESPACES = NO 19 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020-2022 Pololu Corporation (www.pololu.com) 2 | 3 | Permission is hereby granted, free of charge, to any person 4 | obtaining a copy of this software and associated documentation 5 | files (the "Software"), to deal in the Software without 6 | restriction, including without limitation the rights to use, 7 | copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the 9 | Software is furnished to do so, subject to the following 10 | conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 17 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Pololu3piPlus32U4 library 2 | 3 | [www.pololu.com](https://www.pololu.com/) 4 | 5 | ## Summary 6 | 7 | 8 | 9 | This is a C++ library for the Arduino IDE that helps access the on-board hardware of the [Pololu 3pi+ 32U4 Robot](https://www.pololu.com/category/280/3pi-plus-32u4-oled-robot) (both the [newer OLED version](https://www.pololu.com/category/280/3pi-plus-32u4-oled-robot) and the [original LCD version](https://www.pololu.com/category/285/original-3pi-plus-32u4-robot)). 10 | 11 | The 3pi+ 32U4 robot is a complete, high-performance mobile platform based on the ATmega32U4 microcontroller. It has integrated motor drivers, encoders, a display screen (graphical OLED or LCD), a buzzer, buttons, line sensors, front bump sensors, an LSM6DS33 accelerometer and gyro, and an LIS3MDL compass. See the [3pi+ 32U4 user's guide](https://www.pololu.com/docs/0J83) for more information. 12 | 13 | ## Installing the library 14 | 15 | Use the Library Manager in version 1.8.10 or later of the Arduino software (IDE) to install this library: 16 | 17 | 1. In the Arduino IDE, open the "Tools" menu and select "Manage Libraries...". 18 | 2. Search for "3pi+". 19 | 3. Click the Pololu3piPlus32U4 entry in the list. 20 | 4. Click "Install". 21 | 5. If you see a prompt asking to install missing dependencies, click "Install all". 22 | 23 | ## Usage 24 | 25 | To access most of features of this library, you just need a single include statement. For convenience, we recommend using the Pololu3piPlus32U4 namespace and declaring all of the objects you want to use as global variables, as shown below: 26 | 27 | ```cpp 28 | #include 29 | 30 | using namespace Pololu3piPlus32U4; 31 | 32 | OLED display; 33 | Buzzer buzzer; 34 | ButtonA buttonA; 35 | ButtonB buttonB; 36 | ButtonC buttonC; 37 | LineSensors lineSensors; 38 | BumpSensors bumpSensors; 39 | Motors motors; 40 | Encoders encoders; 41 | ``` 42 | 43 | The IMU is not fully enabled by default since it depends on the Wire library, which uses about 1400 bytes of additional code space and defines an interrupt service routine (ISR) that might be incompatible with some applications. 44 | 45 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to enable IMU functionality: 46 | 47 | ```cpp 48 | #include 49 | 50 | IMU imu; 51 | ``` 52 | 53 | ## Examples 54 | 55 | 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 "Pololu3piPlus32U4". If you cannot find these examples, the library was probably installed incorrectly and you should retry the installation instructions above. 56 | 57 | ## Classes and functions 58 | 59 | The contents of the library are contained in the Pololu3piPlus32U4 namespace. The main classes and functions provided by the library are listed below: 60 | 61 | * Pololu3piPlus32U4::ButtonA 62 | * Pololu3piPlus32U4::ButtonB 63 | * Pololu3piPlus32U4::ButtonC 64 | * Pololu3piPlus32U4::Buzzer 65 | * Pololu3piPlus32U4::Encoders 66 | * Pololu3piPlus32U4::OLED 67 | * Pololu3piPlus32U4::LCD 68 | * Pololu3piPlus32U4::Motors 69 | * Pololu3piPlus32U4::LineSensors 70 | * Pololu3piPlus32U4::BumpSensors 71 | * Pololu3piPlus32U4::IMU 72 | * Pololu3piPlus32U4::ledRed() 73 | * Pololu3piPlus32U4::ledGreen() 74 | * Pololu3piPlus32U4::ledYellow() 75 | * Pololu3piPlus32U4::usbPowerPresent() 76 | * Pololu3piPlus32U4::readBatteryMillivolts() 77 | 78 | ## Dependencies 79 | 80 | This library also references several other Arduino libraries which are used to help implement the classes and functions above. 81 | 82 | * [FastGPIO](https://github.com/pololu/fastgpio-arduino) 83 | * [PololuBuzzer](https://github.com/pololu/pololu-buzzer-arduino) 84 | * [PololuHD44780](https://github.com/pololu/pololu-hd44780-arduino) 85 | * [PololuMenu](https://github.com/pololu/pololu-menu-arduino) 86 | * [PololuOLED](https://github.com/pololu/pololu-oled-arduino) 87 | * [Pushbutton](https://github.com/pololu/pushbutton-arduino) 88 | * [USBPause](https://github.com/pololu/usb-pause-arduino) 89 | 90 | ## Version history 91 | 92 | * 1.1.3 (2022-09-06): Fixed a bug in the Encoders demo that could prevent encoder errors from being shown properly on the display. 93 | * 1.1.2 (2021-10-15): Fixed a bug that prevented the OLED display from working if the MOS pin was previously changed to be an input. 94 | * 1.1.1 (2021-08-11): Fixed a typo in the RotationResist demo. 95 | * 1.1.0 (2021-08-10): Added support for the 3pi+ 32U4 OLED. 96 | * 1.0.1 (2020-12-03): Moved code into `src/` folder; continuous integration; fixed some warnings; added missing library dependence on PololuMenu. 97 | * 1.0.0 (2020-12-01): Customized example sketches for different 3pi+ editions. Reimplemented menus using the PololuMenu class. Added flipEncoders() method to the Encoders class to work better with the Hyper edition robot. 98 | * 0.2.0 (2020-11-25): Initial public release; motor demos only support Standard Edition. 99 | * 0.1.0 (2020-11-25): Initial release for production. 100 | -------------------------------------------------------------------------------- /examples/BlinkLEDs/BlinkLEDs.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to blink the three user LEDs on the 2 | // 3pi+ 32U4. 3 | 4 | #include 5 | 6 | using namespace Pololu3piPlus32U4; 7 | 8 | void setup() 9 | { 10 | 11 | } 12 | 13 | void loop() 14 | { 15 | // Turn the LEDs on. 16 | ledRed(1); 17 | ledYellow(1); 18 | ledGreen(1); 19 | 20 | // Wait for a second. 21 | delay(1000); 22 | 23 | // Turn the LEDs off. 24 | ledRed(0); 25 | ledYellow(0); 26 | ledGreen(0); 27 | 28 | // Wait for a second. 29 | delay(1000); 30 | } 31 | -------------------------------------------------------------------------------- /examples/BumpSensorTest/BumpSensorTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace Pololu3piPlus32U4; 4 | BumpSensors bumpSensors; 5 | Buzzer buzzer; 6 | 7 | // Change next line to this if you are using the older 3pi+ 8 | // with a black and green LCD display: 9 | // LCD display; 10 | OLED display; 11 | 12 | void setup() 13 | { 14 | bumpSensors.calibrate(); 15 | display.clear(); 16 | display.gotoXY(0, 1); 17 | display.print("Bump me!"); 18 | } 19 | 20 | void loop() 21 | { 22 | bumpSensors.read(); 23 | 24 | if (bumpSensors.leftChanged()) 25 | { 26 | ledYellow(bumpSensors.leftIsPressed()); 27 | if (bumpSensors.leftIsPressed()) 28 | { 29 | // Left bump sensor was just pressed. 30 | buzzer.play("a32"); 31 | display.gotoXY(0, 0); 32 | display.print('L'); 33 | } 34 | else 35 | { 36 | // Left bump sensor was just released. 37 | buzzer.play("b32"); 38 | display.gotoXY(0, 0); 39 | display.print(' '); 40 | } 41 | } 42 | 43 | if (bumpSensors.rightChanged()) 44 | { 45 | ledRed(bumpSensors.rightIsPressed()); 46 | if (bumpSensors.rightIsPressed()) 47 | { 48 | // Right bump sensor was just pressed. 49 | buzzer.play("e32"); 50 | display.gotoXY(7, 0); 51 | display.print('R'); 52 | } 53 | else 54 | { 55 | // Right bump sensor was just released. 56 | buzzer.play("f32"); 57 | display.gotoXY(7, 0); 58 | display.print(' '); 59 | } 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /examples/DemoForLCDVersion/DemoForLCDVersion.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This is a demo program for the older 3pi+ 32U4 with a black and green 3 | LCD display. 4 | 5 | It uses the buttons, display, and buzzer to provide a user 6 | interface. It presents a menu to the user that lets the user 7 | select from several different demos. 8 | 9 | To use this demo program, you will need to have the LCD connected to 10 | the 3pi+. If you cannot see any text on the LCD, try rotating the 11 | contrast potentiometer. 12 | 13 | If you have an newer 3pi+ with a blue and black OLED display, use the 14 | other demo, DemoForOLEDVersion. 15 | */ 16 | 17 | #include 18 | #include 19 | 20 | /* The IMU is not fully enabled by default since it depends on the 21 | Wire library, which uses about 1400 bytes of additional code space 22 | and defines an interrupt service routine (ISR) that might be 23 | incompatible with some applications (such as our TWISlave example). 24 | 25 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to 26 | enable IMU functionality. 27 | */ 28 | #include 29 | 30 | #include 31 | 32 | using namespace Pololu3piPlus32U4; 33 | 34 | LCD display; 35 | 36 | Buzzer buzzer; 37 | ButtonA buttonA; 38 | ButtonB buttonB; 39 | ButtonC buttonC; 40 | LineSensors lineSensors; 41 | BumpSensors bumpSensors; 42 | IMU imu; 43 | Motors motors; 44 | Encoders encoders; 45 | 46 | PololuMenu mainMenu; 47 | 48 | bool launchSelfTest = false; 49 | 50 | // A couple of simple tunes, stored in program space. 51 | const char beepBrownout[] PROGMEM = "= 500) 174 | { 175 | lastUpdateTime = millis(); 176 | state = state + 1; 177 | if (state >= 4) { state = 0; } 178 | 179 | switch (state) 180 | { 181 | case 0: 182 | buzzer.play("c32"); 183 | display.gotoXY(0, 0); 184 | display.print(F("Red ")); 185 | ledRed(1); 186 | ledGreen(0); 187 | ledYellow(0); 188 | break; 189 | 190 | case 1: 191 | buzzer.play("e32"); 192 | display.gotoXY(0, 0); 193 | display.print(F("Green")); 194 | ledRed(0); 195 | ledGreen(1); 196 | ledYellow(0); 197 | break; 198 | 199 | case 2: 200 | buzzer.play("g32"); 201 | display.gotoXY(0, 0); 202 | display.print(F("Yellow")); 203 | ledRed(0); 204 | ledGreen(0); 205 | ledYellow(1); 206 | break; 207 | } 208 | } 209 | } 210 | 211 | ledRed(0); 212 | ledYellow(0); 213 | ledGreen(0); 214 | } 215 | 216 | void printBar(uint8_t height) 217 | { 218 | if (height > 8) { height = 8; } 219 | static const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 220 | display.print(barChars[height]); 221 | } 222 | 223 | void selfTestWaitShowingVBat() 224 | { 225 | ledYellow(0); 226 | ledGreen(0); 227 | ledRed(0); 228 | while(!mainMenu.buttonMonitor()) 229 | { 230 | display.gotoXY(0,0); 231 | display.print(' '); 232 | display.print(readBatteryMillivolts()); 233 | display.print(F(" mV")); 234 | delay(100); 235 | } 236 | } 237 | 238 | void selfTestFail() 239 | { 240 | display.gotoXY(0, 1); 241 | display.print(F("FAIL")); 242 | buzzer.playFromProgramSpace(beepFail); 243 | while(!mainMenu.buttonMonitor()); 244 | } 245 | 246 | void selfTest() 247 | { 248 | display.clear(); 249 | display.print(F("3\xf7+ 32U4")); 250 | display.gotoXY(0, 1); 251 | display.print(F("SelfTest")); 252 | delay(1000); 253 | 254 | bumpSensors.calibrate(); 255 | 256 | display.clear(); 257 | display.print(F("Press")); 258 | display.gotoXY(0, 1); 259 | display.print(F("bumpers")); 260 | do 261 | { 262 | bumpSensors.read(); 263 | } 264 | while(!bumpSensors.leftIsPressed() || !bumpSensors.rightIsPressed()); 265 | 266 | buzzer.play("!c32"); 267 | display.gotoXY(0, 1); 268 | display.print(F(" ")); 269 | 270 | // test some voltages and IMU presence 271 | display.gotoXY(0, 0); 272 | display.print(F("USB ")); 273 | if(usbPowerPresent()) 274 | { 275 | display.print(F("on")); 276 | selfTestFail(); 277 | return; 278 | } 279 | else 280 | { 281 | display.print(F("off")); 282 | } 283 | ledYellow(1); 284 | delay(500); 285 | 286 | display.gotoXY(0, 0); 287 | display.print(F("VBAT ")); 288 | int v = readBatteryMillivolts(); 289 | display.gotoXY(4, 0); 290 | display.print(v); 291 | if(v < 4000 || v > 7000) 292 | { 293 | selfTestFail(); 294 | return; 295 | } 296 | ledGreen(1); 297 | delay(500); 298 | 299 | display.gotoXY(0, 0); 300 | display.print(F("IMU ")); 301 | display.gotoXY(4, 0); 302 | if(!imu.init()) 303 | { 304 | selfTestFail(); 305 | return; 306 | } 307 | display.print(F("OK")); 308 | ledRed(1); 309 | delay(500); 310 | 311 | // test motor speed, direction, and encoders 312 | display.gotoXY(0, 0); 313 | display.print(F("Motors ")); 314 | ledYellow(1); 315 | ledGreen(1); 316 | ledRed(1); 317 | imu.configureForTurnSensing(); 318 | 319 | encoders.getCountsAndResetLeft(); 320 | encoders.getCountsAndResetRight(); 321 | motors.setSpeeds(90, -90); 322 | delay(250); 323 | 324 | // check rotation speed 325 | imu.read(); 326 | int16_t gyroReading = imu.g.z; 327 | 328 | motors.setSpeeds(0, 0); 329 | delay(100); 330 | int left = encoders.getCountsAndResetLeft(); 331 | int right = encoders.getCountsAndResetRight(); 332 | display.clear(); 333 | if(gyroReading > -7000 && gyroReading < -5000 && 334 | left > 212 && left < 288 && right > -288 && right < -212) 335 | { 336 | display.print(F("Standrd?")); 337 | } 338 | else if(gyroReading > -1800 && gyroReading < -1200 && 339 | left > 140 && left < 200 && right > -200 && right < -140) 340 | { 341 | display.print(F("Turtle?")); 342 | } 343 | else if(gyroReading > 9500 && gyroReading < 17000 && 344 | left > 130 && left < 370 && right > -370 && right < -130) 345 | { 346 | display.print(F("Hyper?")); 347 | } 348 | else 349 | { 350 | display.clear(); 351 | display.print(left); 352 | display.gotoXY(4, 0); 353 | display.print(right); 354 | 355 | display.gotoXY(4, 1); 356 | display.print(gyroReading/100); 357 | selfTestFail(); 358 | return; 359 | } 360 | 361 | display.gotoXY(0,1); 362 | display.print(F("A=? B=Y")); 363 | while(true) 364 | { 365 | char button = mainMenu.buttonMonitor(); 366 | if(button == 'A') 367 | { 368 | display.clear(); 369 | display.print(left); 370 | display.gotoXY(4, 0); 371 | display.print(right); 372 | 373 | display.gotoXY(0, 1); 374 | display.print(gyroReading); 375 | } 376 | if(button == 'B') 377 | { 378 | break; 379 | } 380 | if(button == 'C') 381 | { 382 | selfTestFail(); 383 | return; 384 | } 385 | } 386 | 387 | // Passed all tests! 388 | display.gotoXY(0, 1); 389 | display.print(F("PASS ")); 390 | delay(250); // finish the button beep 391 | buzzer.playFromProgramSpace(beepPass); 392 | selfTestWaitShowingVBat(); 393 | } 394 | 395 | // Display line sensor readings. Holding button C turns off 396 | // the IR emitters. 397 | void lineSensorDemo() 398 | { 399 | loadCustomCharactersBarGraph(); 400 | displayBackArrow(); 401 | display.gotoXY(6, 1); 402 | display.print('C'); 403 | 404 | uint16_t lineSensorValues[5]; 405 | 406 | while (mainMenu.buttonMonitor() != 'B') 407 | { 408 | bool emittersOff = buttonC.isPressed(); 409 | 410 | lineSensors.read(lineSensorValues, emittersOff ? LineSensorsReadMode::Off : LineSensorsReadMode::On); 411 | 412 | display.gotoXY(1, 0); 413 | for (uint8_t i = 0; i < 5; i++) 414 | { 415 | uint8_t barHeight = map(lineSensorValues[i], 0, 2000, 0, 8); 416 | printBar(barHeight); 417 | } 418 | 419 | // Display an indicator of whether emitters are on or 420 | // off. 421 | display.gotoXY(7, 1); 422 | if (emittersOff) 423 | { 424 | display.print('\xa5'); // centered dot 425 | } 426 | else 427 | { 428 | display.print('*'); 429 | } 430 | } 431 | } 432 | 433 | void bumpSensorDemo() 434 | { 435 | bumpSensors.calibrate(); 436 | displayBackArrow(); 437 | 438 | while (mainMenu.buttonMonitor() != 'B') 439 | { 440 | bumpSensors.read(); 441 | 442 | if (bumpSensors.leftChanged()) 443 | { 444 | ledYellow(bumpSensors.leftIsPressed()); 445 | if (bumpSensors.leftIsPressed()) 446 | { 447 | // Left bump sensor was just pressed. 448 | buzzer.play("a32"); 449 | display.gotoXY(0, 0); 450 | display.print('L'); 451 | } 452 | else 453 | { 454 | // Left bump sensor was just released. 455 | buzzer.play("b32"); 456 | display.gotoXY(0, 0); 457 | display.print(' '); 458 | } 459 | } 460 | 461 | if (bumpSensors.rightChanged()) 462 | { 463 | ledRed(bumpSensors.rightIsPressed()); 464 | if (bumpSensors.rightIsPressed()) 465 | { 466 | // Right bump sensor was just pressed. 467 | buzzer.play("e32"); 468 | display.gotoXY(7, 0); 469 | display.print('R'); 470 | } 471 | else 472 | { 473 | // Right bump sensor was just released. 474 | buzzer.play("f32"); 475 | display.gotoXY(7, 0); 476 | display.print(' '); 477 | } 478 | } 479 | } 480 | } 481 | 482 | // Starts I2C and initializes the inertial sensors. 483 | void initInertialSensors() 484 | { 485 | Wire.begin(); 486 | imu.init(); 487 | imu.enableDefault(); 488 | } 489 | 490 | // Given 3 readings for axes x, y, and z, prints the sign 491 | // and axis of the largest reading unless it is below the 492 | // given threshold. 493 | void printLargestAxis(int16_t x, int16_t y, int16_t z, uint16_t threshold) 494 | { 495 | int16_t largest = x; 496 | char axis = 'X'; 497 | 498 | if (abs(y) > abs(largest)) 499 | { 500 | largest = y; 501 | axis = 'Y'; 502 | } 503 | if (abs(z) > abs(largest)) 504 | { 505 | largest = z; 506 | axis = 'Z'; 507 | } 508 | 509 | if (abs(largest) < threshold) 510 | { 511 | display.print(" "); 512 | } 513 | else 514 | { 515 | bool positive = (largest > 0); 516 | display.print(positive ? '+' : '-'); 517 | display.print(axis); 518 | } 519 | } 520 | 521 | // Print the direction of the largest rotation rate measured 522 | // by the gyro and the up direction based on the 523 | // accelerometer's measurement of gravitational acceleration 524 | // (assuming gravity is the dominant force acting on the 3pi+). 525 | void inertialDemo() 526 | { 527 | displayBackArrow(); 528 | 529 | display.gotoXY(3, 0); 530 | display.print(F("Rot")); 531 | display.gotoXY(4, 1); 532 | display.print(F("Up")); 533 | 534 | while (mainMenu.buttonMonitor() != 'B') 535 | { 536 | imu.read(); 537 | 538 | display.gotoXY(6, 0); 539 | printLargestAxis(imu.g.x, imu.g.y, imu.g.z, 2000); 540 | display.gotoXY(6, 1); 541 | printLargestAxis(imu.a.x, imu.a.y, imu.a.z, 200); 542 | } 543 | } 544 | 545 | // Demonstrate the built-in magnetometer with a simple compass 546 | // application. This demo constantly calibrates by keeping 547 | // track of min and max values in all axes, and it displays an 548 | // estimated heading ("S", "NE", etc.) by comparing the current 549 | // values to these values. 550 | void compassDemo() 551 | { 552 | IMU::vector magMax; 553 | IMU::vector magMin; 554 | IMU::vector m; 555 | bool showReadings = false; 556 | bool firstReading = true; 557 | 558 | while (true) 559 | { 560 | display.noAutoDisplay(); 561 | display.clear(); 562 | displayBackArrow(); 563 | display.gotoXY(7,1); 564 | display.print('C'); 565 | imu.readMag(); 566 | 567 | if (firstReading) 568 | { 569 | magMax = imu.m; 570 | magMin = imu.m; 571 | firstReading = false; 572 | } 573 | 574 | // Update min/max calibration values. 575 | magMax.x = max(magMax.x, imu.m.x); 576 | magMax.y = max(magMax.y, imu.m.y); 577 | magMax.z = max(magMax.z, imu.m.z); 578 | magMin.x = min(magMin.x, imu.m.x); 579 | magMin.y = min(magMin.y, imu.m.y); 580 | magMin.z = min(magMin.z, imu.m.z); 581 | 582 | // Subtract the average of min and max to get a vector 583 | // centered at an approximation of true zero. 584 | m.x = imu.m.x - (magMax.x+magMin.x)/2; 585 | m.y = imu.m.y - (magMax.y+magMin.y)/2; 586 | m.z = imu.m.z - (magMax.z+magMin.z)/2; 587 | 588 | if (showReadings) 589 | { 590 | display.gotoXY(0, 0); 591 | display.print(m.x/100); 592 | display.gotoXY(4, 0); 593 | display.print(m.y/100); 594 | 595 | display.gotoXY(3, 1); 596 | display.print(m.z/100); 597 | } 598 | else if (magMax.x - 1000 < magMin.x || magMax.y - 1000 < magMin.y) 599 | { 600 | display.gotoXY(0, 0); 601 | display.print(F("Turn me!")); 602 | } 603 | else { 604 | // Estimate the direction by checking which vector is the closest 605 | // match. 606 | int n = m.x; 607 | int s = -n; 608 | int e = m.y; 609 | int w = -e; 610 | int ne = (n+e)*10/14; 611 | int nw = (n+w)*10/14; 612 | int se = (s+e)*10/14; 613 | int sw = (s+w)*10/14; 614 | 615 | const __FlashStringHelper* dir = F("??"); 616 | int max_value=0; 617 | if (n > max_value) { max_value = n; dir = F("N"); } 618 | if (s > max_value) { max_value = s; dir = F("S"); } 619 | if (e > max_value) { max_value = e; dir = F("E"); } 620 | if (w > max_value) { max_value = w; dir = F("W"); } 621 | if (ne > max_value) { max_value = ne; dir = F("NE"); } 622 | if (se > max_value) { max_value = se; dir = F("SE"); } 623 | if (nw > max_value) { max_value = nw; dir = F("NW"); } 624 | if (sw > max_value) { max_value = sw; dir = F("SW"); } 625 | 626 | display.gotoXY(3,0); 627 | display.print(dir); 628 | } 629 | display.display(); 630 | 631 | switch (mainMenu.buttonMonitor()) 632 | { 633 | case 'B': 634 | return; 635 | case 'C': 636 | showReadings = !showReadings; 637 | break; 638 | } 639 | 640 | delay(50); 641 | } 642 | 643 | } 644 | 645 | // Provides an interface to test the motors. Holding button A or C 646 | // causes the left or right motor to accelerate; releasing the 647 | // button causes the motor to decelerate. Tapping the button while 648 | // the motor is not running reverses the direction it runs. 649 | // 650 | // If the showEncoders argument is true, encoder counts are 651 | // displayed on the first line of the LCD; otherwise, an 652 | // instructional message is shown. 653 | void motorDemoHelper(bool showEncoders) 654 | { 655 | loadCustomCharactersMotorDirs(); 656 | display.clear(); 657 | display.gotoXY(1, 1); 658 | display.print(F("A \7B C")); 659 | 660 | int16_t leftSpeed = 0, rightSpeed = 0; 661 | int8_t leftDir = 1, rightDir = 1; 662 | uint16_t lastUpdateTime = millis() - 100; 663 | uint8_t btnCountA = 0, btnCountC = 0, instructCount = 0; 664 | 665 | int16_t encCountsLeft = 0, encCountsRight = 0; 666 | char buf[4]; 667 | 668 | while (mainMenu.buttonMonitor() != 'B') 669 | { 670 | encCountsLeft += encoders.getCountsAndResetLeft(); 671 | if (encCountsLeft < 0) { encCountsLeft += 1000; } 672 | if (encCountsLeft > 999) { encCountsLeft -= 1000; } 673 | 674 | encCountsRight += encoders.getCountsAndResetRight(); 675 | if (encCountsRight < 0) { encCountsRight += 1000; } 676 | if (encCountsRight > 999) { encCountsRight -= 1000; } 677 | 678 | // Update the LCD and motors every 50 ms. 679 | if ((uint16_t)(millis() - lastUpdateTime) > 50) 680 | { 681 | lastUpdateTime = millis(); 682 | 683 | display.gotoXY(0, 0); 684 | if (showEncoders) 685 | { 686 | sprintf(buf, "%03d", encCountsLeft); 687 | display.print(buf); 688 | display.gotoXY(5, 0); 689 | sprintf(buf, "%03d", encCountsRight); 690 | display.print(buf); 691 | } 692 | else 693 | { 694 | // Cycle the instructions every 2 seconds. 695 | if (instructCount == 0) 696 | { 697 | display.print("Hold=run"); 698 | } 699 | else if (instructCount == 40) 700 | { 701 | display.print("Tap=flip"); 702 | } 703 | if (++instructCount == 80) { instructCount = 0; } 704 | } 705 | 706 | if (buttonA.isPressed()) 707 | { 708 | if (btnCountA < 4) 709 | { 710 | btnCountA++; 711 | } 712 | else 713 | { 714 | // Button has been held for more than 200 ms, so 715 | // start running the motor. 716 | leftSpeed += 15; 717 | } 718 | } 719 | else 720 | { 721 | if (leftSpeed == 0 && btnCountA > 0 && btnCountA < 4) 722 | { 723 | // Motor isn't running and button was pressed for 724 | // 200 ms or less, so flip the motor direction. 725 | leftDir = -leftDir; 726 | } 727 | btnCountA = 0; 728 | leftSpeed -= 30; 729 | } 730 | 731 | if (buttonC.isPressed()) 732 | { 733 | if (btnCountC < 4) 734 | { 735 | btnCountC++; 736 | } 737 | else 738 | { 739 | // Button has been held for more than 200 ms, so 740 | // start running the motor. 741 | rightSpeed += 15; 742 | } 743 | } 744 | else 745 | { 746 | if (rightSpeed == 0 && btnCountC > 0 && btnCountC < 4) 747 | { 748 | // Motor isn't running and button was pressed for 749 | // 200 ms or less, so flip the motor direction. 750 | rightDir = -rightDir; 751 | } 752 | btnCountC = 0; 753 | rightSpeed -= 30; 754 | } 755 | 756 | leftSpeed = constrain(leftSpeed, 0, 400); 757 | rightSpeed = constrain(rightSpeed, 0, 400); 758 | 759 | motors.setSpeeds(leftSpeed * leftDir, rightSpeed * rightDir); 760 | 761 | // Display arrows pointing the appropriate direction 762 | // (solid if the motor is running, chevrons if not). 763 | display.gotoXY(0, 1); 764 | if (leftSpeed == 0) 765 | { 766 | display.print((leftDir > 0) ? '\0' : '\1'); 767 | } 768 | else 769 | { 770 | display.print((leftDir > 0) ? '\2' : '\3'); 771 | } 772 | display.gotoXY(7, 1); 773 | if (rightSpeed == 0) 774 | { 775 | display.print((rightDir > 0) ? '\0' : '\1'); 776 | } 777 | else 778 | { 779 | display.print((rightDir > 0) ? '\2' : '\3'); 780 | } 781 | } 782 | } 783 | motors.setSpeeds(0, 0); 784 | } 785 | 786 | 787 | // Motor demo with instructions. 788 | void motorDemo() 789 | { 790 | motorDemoHelper(false); 791 | } 792 | 793 | // Motor demo with encoder counts. 794 | void encoderDemo() 795 | { 796 | motorDemoHelper(true); 797 | } 798 | 799 | // Spin in place 800 | void spinDemo() 801 | { 802 | display.clear(); 803 | displayBackArrow(); 804 | display.print(F("Spinning")); 805 | display.gotoXY(5,1); 806 | display.print(F("...")); 807 | delay(200); 808 | buzzer.playFromProgramSpace(beepReadySetGo); 809 | while(buzzer.isPlaying()) 810 | { 811 | if(mainMenu.buttonMonitor() == 'B') 812 | return; 813 | } 814 | spinDemoInternal(); 815 | motors.setSpeeds(0, 0); 816 | } 817 | 818 | void spinDemoInternal() 819 | { 820 | // Spin right 821 | for(int i = 0; i < 40; i++) 822 | { 823 | motors.setSpeeds(i * 10, -i * 10); 824 | delay(50); 825 | if(mainMenu.buttonMonitor() == 'B') 826 | return; 827 | } 828 | for(int i = 40; i >= 0; i--) 829 | { 830 | motors.setSpeeds(i * 10, -i * 10); 831 | delay(50); 832 | if(mainMenu.buttonMonitor() == 'B') 833 | return; 834 | } 835 | 836 | // Spin left 837 | for(int i = 0; i < 40; i++) 838 | { 839 | motors.setSpeeds(-i * 10, i * 10); 840 | delay(50); 841 | if(mainMenu.buttonMonitor() == 'B') 842 | return; 843 | } 844 | for(int i = 40; i >= 0; i--) 845 | { 846 | motors.setSpeeds(-i * 10, i * 10); 847 | delay(50); 848 | if(mainMenu.buttonMonitor() == 'B') 849 | return; 850 | } 851 | } 852 | 853 | const char fugue[] PROGMEM = 854 | "! T120O5L16agafaea 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" 862 | "e>d>c#>db>d>c#b >c#agaegfe fO6dc#dfdc# 250) 879 | { 880 | lastShiftTime = millis(); 881 | 882 | display.gotoXY(0, 0); 883 | for (uint8_t i = 0; i < 8; i++) 884 | { 885 | char c = pgm_read_byte(fugueTitle + fugueTitlePos + i); 886 | display.print(c); 887 | } 888 | fugueTitlePos++; 889 | 890 | if (fugueTitlePos + 8 >= strlen(fugueTitle)) 891 | { 892 | fugueTitlePos = 0; 893 | } 894 | } 895 | 896 | if (!buzzer.isPlaying()) 897 | { 898 | buzzer.playFromProgramSpace(fugue); 899 | } 900 | } 901 | } 902 | 903 | // Display the the battery (VIN) voltage and indicate whether USB 904 | // power is detected. 905 | void powerDemo() 906 | { 907 | displayBackArrow(); 908 | 909 | uint16_t lastDisplayTime = millis() - 2000; 910 | char buf[6]; 911 | 912 | while (mainMenu.buttonMonitor() != 'B') 913 | { 914 | if ((uint16_t)(millis() - lastDisplayTime) > 250) 915 | { 916 | bool usbPower = usbPowerPresent(); 917 | 918 | uint16_t batteryLevel = readBatteryMillivolts(); 919 | 920 | lastDisplayTime = millis(); 921 | display.gotoXY(0, 0); 922 | sprintf(buf, "%5d", batteryLevel); 923 | display.print(buf); 924 | display.print(F(" mV")); 925 | display.gotoXY(3, 1); 926 | display.print(F("USB=")); 927 | display.print(usbPower ? 'Y' : 'N'); 928 | } 929 | } 930 | } 931 | 932 | // This demo shows all characters that the LCD can display. Press C to 933 | // advance to the the next page of 8 characters, or A to go back a page. 934 | // 935 | // The demo starts on character 0x20 (space). 936 | // 937 | // Note that characters 0-8 are the custom characters. They are 938 | // initially set to some strange random-looking shapes, but if you run 939 | // other demos that set custom characters then return here, you will 940 | // see what they loaded. 941 | void displayDemo() { 942 | displayBackArrow(); 943 | display.gotoXY(7,1); 944 | display.print('C'); 945 | 946 | // The first four pages are boring/weird, so start at 0x20, which 947 | // will show these characters, starting with a space: 948 | // !"#$%&' 949 | uint8_t startCharacter = 4 * 8; 950 | 951 | while (true) 952 | { 953 | display.gotoXY(0,0); 954 | for(uint8_t i = 0; i < 8; i++) 955 | { 956 | display.print((char)(startCharacter + i)); 957 | } 958 | display.gotoXY(3,1); 959 | 960 | char buf[4]; 961 | sprintf(buf, "x%02x", startCharacter); 962 | display.print(buf); 963 | 964 | char b = mainMenu.buttonMonitor(); 965 | if ('B' == b) break; 966 | if ('A' == b) startCharacter -= 8; 967 | if ('C' == b) startCharacter += 8; 968 | } 969 | } 970 | 971 | const char aboutText[] PROGMEM = 972 | " Pololu 3pi+ 32U4 Robot - more info at pololu.com/3pi+ "; 973 | 974 | void aboutDemo() { 975 | size_t textPos = 0; 976 | uint16_t lastShiftTime = millis() - 2000; 977 | displayBackArrow(); 978 | 979 | while (mainMenu.buttonMonitor() != 'B') 980 | { 981 | // Shift the text to the left every 250 ms. 982 | if ((uint16_t)(millis() - lastShiftTime) > 250) 983 | { 984 | lastShiftTime = millis(); 985 | 986 | display.gotoXY(0, 0); 987 | for (uint8_t i = 0; i < 8; i++) 988 | { 989 | char c = pgm_read_byte(aboutText + textPos + i); 990 | display.print(c); 991 | } 992 | textPos++; 993 | 994 | if (textPos + 8 >= strlen(aboutText)) 995 | { 996 | textPos = 0; 997 | } 998 | } 999 | } 1000 | } 1001 | 1002 | void setup() 1003 | { 1004 | static const PololuMenuItem mainMenuItems[] = { 1005 | { F("Power"), powerDemo }, 1006 | { F("LineSens"), lineSensorDemo }, 1007 | { F("BumpSens"), bumpSensorDemo }, 1008 | { F("Inertial"), inertialDemo }, 1009 | { F("Compass"), compassDemo }, 1010 | { F("Motors"), motorDemo }, 1011 | { F("Encoders"), encoderDemo }, 1012 | { F("Spin"), spinDemo }, 1013 | { F("LEDs"), ledDemo }, 1014 | { F("LCD"), displayDemo }, 1015 | { F("Music"), musicDemo }, 1016 | { F("About"), aboutDemo }, 1017 | }; 1018 | mainMenu.setItems(mainMenuItems, sizeof(mainMenuItems)/sizeof(mainMenuItems[0])); 1019 | mainMenu.setDisplay(display); 1020 | mainMenu.setBuzzer(buzzer); 1021 | mainMenu.setButtons(buttonA, buttonB, buttonC); 1022 | mainMenu.setSecondLine(F("\x7f" "A \xa5" "B C\x7e")); 1023 | 1024 | initInertialSensors(); 1025 | 1026 | loadCustomCharacters(); 1027 | 1028 | // The brownout threshold on the ATmega32U4 is set to 4.3 1029 | // V. If VCC drops below this, a brownout reset will 1030 | // occur, preventing the AVR from operating out of spec. 1031 | // 1032 | // Note: Brownout resets usually do not happen on the 3pi+ 1033 | // 32U4 because the voltage regulator goes straight from 5 1034 | // V to 0 V when VIN drops too low. 1035 | // 1036 | // The bootloader is designed so that you can detect 1037 | // brownout resets from your sketch using the following 1038 | // code: 1039 | bool brownout = MCUSR >> BORF & 1; 1040 | MCUSR = 0; 1041 | 1042 | if (brownout) 1043 | { 1044 | // The board was reset by a brownout reset 1045 | // (VCC dropped below 4.3 V). 1046 | // Play a special sound and display a note to the user. 1047 | 1048 | buzzer.playFromProgramSpace(beepBrownout); 1049 | display.clear(); 1050 | display.print(F("Brownout")); 1051 | display.gotoXY(0, 1); 1052 | display.print(F(" reset! ")); 1053 | delay(1000); 1054 | } 1055 | else 1056 | { 1057 | buzzer.playFromProgramSpace(beepWelcome); 1058 | } 1059 | 1060 | // allow skipping quickly to the menu by holding button C 1061 | if (buttonC.isPressed()) 1062 | { 1063 | selfTest(); 1064 | return; 1065 | } 1066 | 1067 | display.clear(); 1068 | display.print(F("3\xf7+ 32U4")); 1069 | display.gotoXY(2, 1); 1070 | display.print(F("Demo")); 1071 | 1072 | uint16_t blinkStart = millis(); 1073 | while((uint16_t)(millis() - blinkStart) < 1000) 1074 | { 1075 | // keep setting the LEDs on for 1s 1076 | // the Green/Red LEDs might turn off during USB communication 1077 | ledYellow(1); 1078 | ledGreen(1); 1079 | ledRed(1); 1080 | } 1081 | 1082 | display.clear(); 1083 | display.print(F("Use B to")); 1084 | display.gotoXY(0, 1); 1085 | display.print(F("select.")); 1086 | 1087 | while((uint16_t)(millis() - blinkStart) < 2000) 1088 | { 1089 | // keep the LEDs off for 1s 1090 | ledYellow(0); 1091 | ledGreen(0); 1092 | ledRed(0); 1093 | } 1094 | 1095 | display.clear(); 1096 | display.print(F("Press B")); 1097 | display.gotoXY(0, 1); 1098 | display.print(F("-try it!")); 1099 | 1100 | // Keep blinking the yellow LED while waiting for the 1101 | // user to press button B. 1102 | blinkStart = millis(); 1103 | while (mainMenu.buttonMonitor() != 'B') 1104 | { 1105 | uint16_t blinkPhase = millis() - blinkStart; 1106 | ledGreen(blinkPhase < 1000); 1107 | if (blinkPhase >= 2000) { blinkStart += 2000; } 1108 | } 1109 | ledGreen(0); 1110 | 1111 | buzzer.playFromProgramSpace(beepThankYou); 1112 | display.clear(); 1113 | display.print(F(" Thank")); 1114 | display.gotoXY(0, 1); 1115 | display.print(F(" you!")); 1116 | delay(1000); 1117 | mainMenuWelcome(); 1118 | } 1119 | 1120 | // Clear LEDs and show a message about the main menu. 1121 | void mainMenuWelcome() 1122 | { 1123 | ledYellow(false); 1124 | ledGreen(false); 1125 | ledRed(false); 1126 | display.clear(); 1127 | display.print(F(" Main")); 1128 | display.gotoXY(0, 1); 1129 | display.print(F(" Menu")); 1130 | delay(1000); 1131 | } 1132 | 1133 | void loop() 1134 | { 1135 | if(mainMenu.select()) 1136 | { 1137 | // a menu item ran; show "Main Menu" again and repeat 1138 | mainMenuWelcome(); 1139 | } 1140 | } 1141 | -------------------------------------------------------------------------------- /examples/DemoForOLEDVersion/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/DemoForOLEDVersion/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // For the 3pi+ demo, replaces the default OLED font with an extended font 4 | // that incorporates several non-ASCII glyphs from the original HD44780 5 | // character set: the 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. In this example, we use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f; non-ASCII characters above 0x8f 21 | // are excluded. 22 | // 23 | // You can open original_font.xbm as a starting 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/DemoForOLEDVersion/pololu_3pi_plus_splash.h: -------------------------------------------------------------------------------- 1 | #define pololu_3pi_plus_splash_width 64 2 | #define pololu_3pi_plus_splash_height 128 3 | static unsigned char pololu_3pi_plus_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, 0x00, 0x00, 0x00, 8 | 0xff, 0xf1, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 0xff, 0xf1, 0xfc, 0x03, 9 | 0x00, 0x00, 0x00, 0x00, 0xff, 0x21, 0xfe, 0x01, 0x00, 0x00, 0x00, 0x00, 10 | 0xff, 0x00, 0xfe, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0xff, 0x01, 11 | 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 12 | 0x0f, 0x00, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 13 | 0x00, 0x00, 0x00, 0x00, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 14 | 0xff, 0x80, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x80, 0xff, 0x01, 15 | 0x00, 0x00, 0x00, 0x00, 0xff, 0x08, 0xff, 0x01, 0x00, 0x00, 0x00, 0x3c, 16 | 0xff, 0x18, 0xff, 0x03, 0x00, 0x02, 0x00, 0x7e, 0xff, 0x7d, 0xfe, 0x07, 17 | 0x00, 0x03, 0x00, 0x7e, 0xff, 0xfd, 0xff, 0x07, 0x80, 0x01, 0x00, 0xfc, 18 | 0xff, 0xff, 0xff, 0x07, 0xc0, 0x00, 0x00, 0xf8, 0xff, 0xff, 0xff, 0x07, 19 | 0xc0, 0x00, 0x01, 0xf0, 0xff, 0xff, 0xff, 0x07, 0xe0, 0x00, 0x03, 0xe0, 20 | 0xff, 0xff, 0xff, 0x07, 0xe0, 0x80, 0x03, 0xe0, 0x00, 0x00, 0x00, 0x00, 21 | 0xe0, 0x81, 0x07, 0xe0, 0x00, 0x00, 0x00, 0x00, 0xe0, 0xe3, 0x0f, 0xf0, 22 | 0x00, 0x00, 0x00, 0x00, 0xe0, 0xff, 0x3f, 0x78, 0x00, 0x00, 0x00, 0x00, 23 | 0xe0, 0xbf, 0xff, 0x7f, 0x00, 0x00, 0x00, 0x00, 0xc0, 0xbf, 0xff, 0x3f, 24 | 0xff, 0xff, 0xff, 0x07, 0xc0, 0x1f, 0xff, 0x1f, 0xff, 0xff, 0xff, 0x07, 25 | 0x80, 0x0f, 0xff, 0x0f, 0xff, 0xff, 0xff, 0x07, 0x00, 0x07, 0xfe, 0x07, 26 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0xf8, 0x01, 0xff, 0xff, 0xff, 0x07, 27 | 0x00, 0x00, 0x00, 0x00, 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 28 | 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x00, 0x08, 0x00, 0x1f, 0xe0, 0x03, 0x00, 29 | 0x00, 0x00, 0x0e, 0x40, 0x1f, 0xe0, 0x03, 0x00, 0x00, 0x80, 0x07, 0xf0, 30 | 0x1f, 0xe0, 0x03, 0x00, 0x00, 0xc0, 0x07, 0xf8, 0x1f, 0xe0, 0x03, 0x00, 31 | 0x00, 0xc0, 0x03, 0xfe, 0x3f, 0xf0, 0x03, 0x00, 0x00, 0xc0, 0x83, 0xff, 32 | 0x3e, 0xf0, 0x01, 0x00, 0x00, 0xc0, 0xfb, 0x7f, 0xfe, 0xfc, 0x01, 0x00, 33 | 0x00, 0xc0, 0xff, 0x1f, 0xfc, 0xff, 0x00, 0x00, 0x00, 0xc0, 0x3f, 0x00, 34 | 0xfc, 0xff, 0x00, 0x00, 0x00, 0xc0, 0x03, 0x00, 0xf8, 0x7f, 0x00, 0x00, 35 | 0x00, 0xc0, 0x03, 0x00, 0xf0, 0x3f, 0x00, 0x00, 0x00, 0xc0, 0x03, 0x00, 36 | 0xc0, 0x0f, 0x00, 0x00, 0x00, 0xc0, 0xe3, 0x7f, 0x00, 0x00, 0x00, 0x00, 37 | 0x00, 0xc0, 0xff, 0xff, 0x00, 0x80, 0x3f, 0x00, 0x00, 0xc0, 0xff, 0xff, 38 | 0x00, 0xe0, 0xff, 0x00, 0x00, 0xc0, 0xff, 0xff, 0x00, 0xf0, 0xff, 0x01, 39 | 0x00, 0xc0, 0x0f, 0xf0, 0x00, 0xf8, 0xff, 0x03, 0x00, 0xc0, 0x03, 0xe0, 40 | 0x00, 0xf8, 0xff, 0x03, 0x00, 0xc0, 0x03, 0x60, 0x00, 0xfc, 0xe0, 0x07, 41 | 0x00, 0xc0, 0x03, 0x38, 0x00, 0x7c, 0xc0, 0x07, 0x00, 0xc0, 0x03, 0x00, 42 | 0x00, 0x3c, 0x80, 0x07, 0xc0, 0x01, 0x00, 0x00, 0x00, 0x3c, 0x80, 0x07, 43 | 0xc0, 0x01, 0x00, 0x00, 0x00, 0x3c, 0x80, 0x07, 0xc0, 0x01, 0x00, 0x00, 44 | 0x00, 0x3c, 0x80, 0x07, 0xc0, 0x01, 0x00, 0x00, 0x00, 0x7c, 0xc0, 0x07, 45 | 0xc0, 0x01, 0x00, 0x00, 0x00, 0xfc, 0xe0, 0x07, 0xfe, 0x3f, 0x00, 0x00, 46 | 0x00, 0xf8, 0xff, 0x03, 0xfe, 0x3f, 0x00, 0x00, 0x00, 0xf8, 0xff, 0x03, 47 | 0xfe, 0x3f, 0x00, 0x00, 0x00, 0xf0, 0xff, 0x01, 0xc0, 0x01, 0x00, 0x00, 48 | 0x00, 0xe0, 0xff, 0x00, 0xc0, 0x01, 0x00, 0x00, 0x00, 0x80, 0x3f, 0x00, 49 | 0xc0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x01, 0x00, 0x00, 50 | 0x00, 0x00, 0x00, 0x00, 0xc0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 52 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 53 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 54 | 0xff, 0xff, 0xff, 0x07, 0x00, 0x00, 0x04, 0x3e, 0xff, 0xff, 0xff, 0x07, 55 | 0x00, 0x18, 0x0e, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x8c, 0xe3, 56 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xd8, 0x8c, 0xc1, 0x00, 0x00, 0x00, 0x00, 57 | 0x00, 0xf8, 0x8c, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb8, 0x8f, 0xc1, 58 | 0x00, 0x80, 0x3f, 0x00, 0x00, 0x18, 0x87, 0xe3, 0x00, 0xe0, 0xff, 0x00, 59 | 0x00, 0x00, 0x00, 0x7f, 0x00, 0xf0, 0xff, 0x01, 0x00, 0x00, 0x00, 0x3e, 60 | 0x00, 0xf8, 0xff, 0x03, 0x00, 0x10, 0x0c, 0x00, 0x00, 0xf8, 0xff, 0x03, 61 | 0x00, 0x38, 0x0e, 0x00, 0x00, 0xfc, 0xe0, 0x07, 0x00, 0x18, 0x8f, 0xff, 62 | 0x00, 0x7c, 0xc0, 0x07, 0x00, 0x98, 0x8f, 0xff, 0x00, 0x3c, 0x80, 0x07, 63 | 0x00, 0xd8, 0x0d, 0xc0, 0x00, 0x3c, 0x80, 0x07, 0x00, 0xf8, 0x0c, 0xc0, 64 | 0x00, 0x3c, 0x80, 0x07, 0x00, 0x70, 0x0c, 0xc0, 0x00, 0x3c, 0x80, 0x07, 65 | 0x00, 0x00, 0x00, 0xc0, 0x00, 0x7c, 0xc0, 0x07, 0x00, 0x00, 0x00, 0x00, 66 | 0x00, 0xfc, 0xe0, 0x07, 0x00, 0xf8, 0x03, 0x00, 0x00, 0xf8, 0xff, 0x03, 67 | 0x00, 0xf8, 0x87, 0xff, 0x00, 0xf8, 0xff, 0x03, 0x00, 0x00, 0x8e, 0xff, 68 | 0x00, 0xf0, 0xff, 0x01, 0x00, 0x00, 0x8c, 0xcd, 0x00, 0xe0, 0xff, 0x00, 69 | 0x00, 0x00, 0x8c, 0xcd, 0x00, 0x80, 0x3f, 0x00, 0x00, 0x00, 0x8e, 0xcd, 70 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x87, 0xcd, 0x00, 0x00, 0x00, 0x00, 71 | 0x00, 0xf8, 0x83, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 72 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x07, 73 | 0x00, 0x00, 0x83, 0xff, 0xff, 0xff, 0xff, 0x07, 0x00, 0x80, 0x83, 0xff, 74 | 0xff, 0xff, 0xff, 0x07, 0x00, 0xc0, 0x83, 0xc1, 0xff, 0xff, 0xff, 0x07, 75 | 0x00, 0x60, 0x83, 0xc1, 0xff, 0xff, 0xff, 0x07, 0x00, 0x30, 0x83, 0xc1, 76 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xf8, 0x8f, 0xe3, 0x00, 0x00, 0x00, 0x00, 77 | 0x00, 0xf8, 0x0f, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x3e, 78 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0x7f, 0x00, 79 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 80 | 0x00, 0xfc, 0xff, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x03, 81 | 0x00, 0x00, 0x00, 0x00, 0x00, 0xfc, 0xff, 0x07, 0x00, 0x00, 0x00, 0x00, 82 | 0x00, 0x00, 0xc0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x07, 83 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 84 | 0x00, 0x00, 0x80, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x07, 85 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc0, 0x07, 0x00, 0x00, 0x00, 0x00, 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/DemoForOLEDVersion/splash.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define pololu_3pi_plus_splash_bits unused_placeholder; extern const uint8_t PROGMEM pololu3PiPlusSplash 4 | #include "pololu_3pi_plus_splash.h" 5 | -------------------------------------------------------------------------------- /examples/DemoForOLEDVersion/splash.h: -------------------------------------------------------------------------------- 1 | extern const uint8_t PROGMEM pololu3PiPlusSplash[1024]; 2 | 3 | -------------------------------------------------------------------------------- /examples/Encoders/Encoders.ino: -------------------------------------------------------------------------------- 1 | // This program shows how to read the encoders on the 3pi+ 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 3pi+ to drive both motors 6 | // forward at full speed. You can press button C to drive both 7 | // motors in reverse at full speed. 8 | // 9 | // Encoder counts are printed to the LCD/OLED screen and to the 10 | // serial monitor. 11 | // 12 | // On the screen, the top line shows the counts from the left 13 | // encoder, and the bottom line shows the counts from the right 14 | // encoder. Encoder errors should not happen, but if one does 15 | // happen then the buzzer will beep and an exclamation mark will 16 | // appear temporarily on the screen. 17 | // 18 | // In the serial monitor, the first and second numbers represent 19 | // counts from the left and right encoders, respectively. The 20 | // third and fourth numbers represent errors from the left and 21 | // right encoders, respectively. 22 | 23 | #include 24 | 25 | using namespace Pololu3piPlus32U4; 26 | 27 | Encoders encoders; 28 | Buzzer buzzer; 29 | Motors motors; 30 | ButtonA buttonA; 31 | ButtonC buttonC; 32 | 33 | // Change next line to this if you are using the older 3pi+ 34 | // with a black and green LCD display: 35 | // LCD display; 36 | OLED display; 37 | 38 | const char encoderErrorLeft[] PROGMEM = "!= 100) 59 | { 60 | lastDisplayTime = millis(); 61 | 62 | int16_t countsLeft = encoders.getCountsLeft(); 63 | int16_t countsRight = encoders.getCountsRight(); 64 | 65 | bool errorLeft = encoders.checkErrorLeft(); 66 | bool errorRight = encoders.checkErrorRight(); 67 | 68 | if (errorLeft) 69 | { 70 | // An error occurred on the left encoder channel. 71 | // Display it for the next 10 iterations and also beep. 72 | displayErrorLeftCountdown = 10; 73 | buzzer.playFromProgramSpace(encoderErrorLeft); 74 | } 75 | 76 | if (errorRight) 77 | { 78 | // An error occurred on the right encoder channel. 79 | // Display for the next 10 iterations and also beep. 80 | displayErrorRightCountdown = 10; 81 | buzzer.playFromProgramSpace(encoderErrorRight); 82 | } 83 | 84 | // Update the screen with encoder counts and error info. 85 | display.noAutoDisplay(); 86 | display.clear(); 87 | display.print(countsLeft); 88 | display.gotoXY(0, 1); 89 | display.print(countsRight); 90 | if (displayErrorLeftCountdown) 91 | { 92 | // Show an exclamation point on the first line to 93 | // indicate an error from the left encoder. 94 | display.gotoXY(7, 0); 95 | display.print('!'); 96 | displayErrorLeftCountdown--; 97 | } 98 | if (displayErrorRightCountdown) 99 | { 100 | // Show an exclamation point on the second line to 101 | // indicate an error from the left encoder. 102 | display.gotoXY(7, 1); 103 | display.print('!'); 104 | displayErrorRightCountdown--; 105 | } 106 | display.display(); 107 | 108 | // Send the information to the serial monitor also. 109 | snprintf_P(report, sizeof(report), 110 | PSTR("%6d %6d %1d %1d"), 111 | countsLeft, countsRight, errorLeft, errorRight); 112 | Serial.println(report); 113 | } 114 | 115 | if (buttonA.isPressed()) 116 | { 117 | motors.setSpeeds(400, 400); 118 | } 119 | else if (buttonC.isPressed()) 120 | { 121 | motors.setSpeeds(-400, -400); 122 | } 123 | else 124 | { 125 | motors.setSpeeds(0, 0); 126 | } 127 | } 128 | -------------------------------------------------------------------------------- /examples/FaceUphill/FaceUphill.ino: -------------------------------------------------------------------------------- 1 | /* This demo uses the 3pi+ 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 3pi+, so if you 10 | change the code for controlling the motors, you might also affect 11 | the accelerometer readings. 12 | 13 | Also, if the robot is pointing directly downhill, it might not 14 | move, because the y component of the acceleration would be close 15 | to 0. */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | /* The IMU is not fully enabled by default since it depends on the 22 | Wire library, which uses about 1400 bytes of additional code space 23 | and defines an interrupt service routine (ISR) that might be 24 | incompatible with some applications (such as our TWISlave example). 25 | 26 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to 27 | enable IMU functionality. 28 | */ 29 | #include 30 | 31 | using namespace Pololu3piPlus32U4; 32 | 33 | // Change next line to this if you are using the older 3pi+ 34 | // with a black and green LCD display: 35 | // LCD display; 36 | OLED display; 37 | 38 | IMU imu; 39 | Motors motors; 40 | Buzzer buzzer; 41 | ButtonA buttonA; 42 | ButtonB buttonB; 43 | ButtonC buttonC; 44 | Encoders encoders; 45 | 46 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 47 | Hyper versions of 3pi+ have different motor configurations, requiring 48 | the demo to be configured with different parameters for proper 49 | operation. The following functions set up these parameters using a 50 | menu that runs at the beginning of the program. To bypass the menu, 51 | you can replace the call to selectEdition() in setup() with one of the 52 | specific functions. 53 | */ 54 | int16_t maxSpeed; 55 | 56 | void selectHyper() 57 | { 58 | motors.flipLeftMotor(true); 59 | motors.flipRightMotor(true); 60 | encoders.flipEncoders(true); 61 | maxSpeed = 75; 62 | } 63 | 64 | void selectStandard() 65 | { 66 | maxSpeed = 100; 67 | } 68 | 69 | void selectTurtle() 70 | { 71 | maxSpeed = 200; 72 | } 73 | 74 | PololuMenu menu; 75 | 76 | void selectEdition() 77 | { 78 | display.clear(); 79 | display.print(F("Select")); 80 | display.gotoXY(0,1); 81 | display.print(F("edition")); 82 | delay(1000); 83 | 84 | static const PololuMenuItem items[] = { 85 | { F("Standard"), selectStandard }, 86 | { F("Turtle"), selectTurtle }, 87 | { F("Hyper"), selectHyper }, 88 | }; 89 | 90 | menu.setItems(items, 3); 91 | menu.setDisplay(display); 92 | menu.setBuzzer(buzzer); 93 | menu.setButtons(buttonA, buttonB, buttonC); 94 | 95 | while(!menu.select()); 96 | 97 | display.gotoXY(0,1); 98 | display.print("OK! ..."); 99 | } 100 | 101 | void setup() 102 | { 103 | // Start I2C and initialize the IMU sensors. 104 | Wire.begin(); 105 | imu.init(); 106 | imu.enableDefault(); 107 | imu.configureForFaceUphill(); 108 | 109 | // To bypass the menu, replace this function with 110 | // selectHyper(), selectStandard(), or selectTurtle(). 111 | selectEdition(); 112 | 113 | // Delay before running motors 114 | delay(1000); 115 | } 116 | 117 | void loop() 118 | { 119 | // Read the accelerometer. 120 | // A value of 16384 corresponds to approximately 1 g. 121 | imu.readAcc(); 122 | int16_t x = imu.a.x; 123 | int16_t y = imu.a.y; 124 | int32_t magnitudeSquared = (int32_t)x * x + (int32_t)y * y; 125 | 126 | // Display the X and Y acceleration values on the LCD 127 | // every 150 ms. 128 | static uint8_t lastDisplayTime; 129 | if ((uint8_t)(millis() - lastDisplayTime) > 150) 130 | { 131 | lastDisplayTime = millis(); 132 | display.gotoXY(0, 0); 133 | display.print(x); 134 | display.print(F(" ")); 135 | display.gotoXY(0, 1); 136 | display.print(y); 137 | display.print(F(" ")); 138 | } 139 | 140 | // Use the encoders to see how much we should drive forward. 141 | // If the robot rolls downhill, the encoder counts will become 142 | // negative, resulting in a positive forwardSpeed to counteract 143 | // the rolling. 144 | int16_t forwardSpeed = -(encoders.getCountsLeft() + encoders.getCountsRight()); 145 | forwardSpeed = constrain(forwardSpeed, -maxSpeed, maxSpeed); 146 | 147 | // See if we are actually on an incline. 148 | // 16384 * sin(5 deg) = 1427 149 | int16_t turnSpeed; 150 | if (magnitudeSquared > (int32_t)1427 * 1427) 151 | { 152 | // We are on an incline of more than 5 degrees, so 153 | // try to face uphill using a feedback algorithm. 154 | 155 | if (x < 0) 156 | { 157 | if (y < 0) 158 | { 159 | turnSpeed = -maxSpeed; 160 | } 161 | else 162 | { 163 | turnSpeed = maxSpeed; 164 | } 165 | } 166 | else 167 | { 168 | turnSpeed = y / 40; 169 | } 170 | 171 | ledYellow(1); 172 | } 173 | else 174 | { 175 | // We not on a noticeable incline, so don't turn. 176 | turnSpeed = 0; 177 | ledYellow(0); 178 | } 179 | 180 | // To face uphill, we need to turn so that the X acceleration 181 | // is negative and the Y acceleration is 0. Therefore, when 182 | // the Y acceleration is positive, we want to turn to the 183 | // left (counter-clockwise). 184 | int16_t leftSpeed = forwardSpeed - turnSpeed; 185 | int16_t rightSpeed = forwardSpeed + turnSpeed; 186 | 187 | // Constrain the speeds to be between -maxSpeed and maxSpeed. 188 | leftSpeed = constrain(leftSpeed, -maxSpeed, maxSpeed); 189 | rightSpeed = constrain(rightSpeed, -maxSpeed, maxSpeed); 190 | 191 | motors.setSpeeds(leftSpeed, rightSpeed); 192 | } 193 | -------------------------------------------------------------------------------- /examples/InertialSensors/InertialSensors.ino: -------------------------------------------------------------------------------- 1 | /* This example reads the raw values from the accelerometer, 2 | magnetometer, and gyro on the 3pi+ 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 | /* The IMU is not fully enabled by default since it depends on the 24 | Wire library, which uses about 1400 bytes of additional code space 25 | and defines an interrupt service routine (ISR) that might be 26 | incompatible with some applications (such as our TWISlave example). 27 | 28 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to 29 | enable IMU functionality. 30 | */ 31 | #include 32 | 33 | using namespace Pololu3piPlus32U4; 34 | 35 | IMU imu; 36 | 37 | char report[120]; 38 | 39 | void setup() 40 | { 41 | Wire.begin(); 42 | 43 | if (!imu.init()) 44 | { 45 | // Failed to detect the compass. 46 | ledRed(1); 47 | while(1) 48 | { 49 | Serial.println(F("Failed to initialize IMU sensors.")); 50 | delay(100); 51 | } 52 | } 53 | 54 | imu.enableDefault(); 55 | } 56 | 57 | void loop() 58 | { 59 | imu.read(); 60 | 61 | snprintf_P(report, sizeof(report), 62 | PSTR("A: %6d %6d %6d M: %6d %6d %6d G: %6d %6d %6d"), 63 | imu.a.x, imu.a.y, imu.a.z, 64 | imu.m.x, imu.m.y, imu.m.z, 65 | imu.g.x, imu.g.y, imu.g.z); 66 | Serial.println(report); 67 | 68 | delay(100); 69 | } 70 | -------------------------------------------------------------------------------- /examples/LineFollowerPID/LineFollowerPID.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the line sensors on the 3pi+ 32U4 to follow 2 | a black line on a white background, using a PID-based algorithm. 3 | It works well on courses with smooth, 6" radius curves and can 4 | even work with tighter turns, including sharp 90 degree corners. 5 | This example has been tested with robots using 30:1 MP motors. 6 | Modifications might be required for it to work well on different 7 | courses or with different motors. */ 8 | 9 | #include 10 | #include 11 | 12 | using namespace Pololu3piPlus32U4; 13 | 14 | // Change next line to this if you are using the older 3pi+ 15 | // with a black and green LCD display: 16 | // LCD display; 17 | OLED display; 18 | 19 | Buzzer buzzer; 20 | LineSensors lineSensors; 21 | Motors motors; 22 | ButtonA buttonA; 23 | ButtonB buttonB; 24 | ButtonC buttonC; 25 | 26 | int16_t lastError = 0; 27 | 28 | #define NUM_SENSORS 5 29 | unsigned int lineSensorValues[NUM_SENSORS]; 30 | 31 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 32 | Hyper versions of 3pi+ have different motor configurations, requiring 33 | the demo to be configured with different parameters for proper 34 | operation. The following functions set up these parameters using a 35 | menu that runs at the beginning of the program. To bypass the menu, 36 | you can replace the call to selectEdition() in setup() with one of the 37 | specific functions. 38 | */ 39 | 40 | // This is the maximum speed the motors will be allowed to turn. 41 | // A maxSpeed of 400 would let the motors go at top speed, but 42 | // the value of 200 here imposes a speed limit of 50%. 43 | 44 | // Note that making the 3pi+ go faster on a line following course 45 | // might involve more than just increasing this number; you will 46 | // often have to adjust the PID constants too for it to work well. 47 | uint16_t maxSpeed; 48 | int16_t minSpeed; 49 | 50 | // This is the speed the motors will run when centered on the line. 51 | // Set to zero and set minSpeed to -maxSpeed to test the robot 52 | // without. 53 | uint16_t baseSpeed; 54 | 55 | uint16_t calibrationSpeed; 56 | 57 | // PID configuration: This example is configured for a default 58 | // proportional constant of 1/4 and a derivative constant of 1, which 59 | // seems to work well at low speeds for all of our 3pi+ editions. You 60 | // will probably want to use trial and error to tune these constants 61 | // for your particular 3pi+ and line course, especially if you 62 | // increase the speed. 63 | 64 | uint16_t proportional; // coefficient of the P term * 256 65 | uint16_t derivative; // coefficient of the D term * 256 66 | 67 | void selectHyper() 68 | { 69 | motors.flipLeftMotor(true); 70 | motors.flipRightMotor(true); 71 | // Encoders are not used in this example. 72 | // encoders.flipEncoders(true); 73 | maxSpeed = 100; 74 | minSpeed = 0; 75 | baseSpeed = maxSpeed; 76 | calibrationSpeed = 50; 77 | proportional = 64; // P coefficient = 1/4 78 | derivative = 256; // D coefficient = 1 79 | } 80 | 81 | void selectStandard() 82 | { 83 | maxSpeed = 200; 84 | minSpeed = 0; 85 | baseSpeed = maxSpeed; 86 | calibrationSpeed = 60; 87 | proportional = 64; // P coefficient = 1/4 88 | derivative = 256; // D coefficient = 1 89 | } 90 | 91 | void selectTurtle() 92 | { 93 | maxSpeed = 400; 94 | minSpeed = 0; 95 | baseSpeed = 400; 96 | calibrationSpeed = 120; 97 | proportional = 64; // P coefficient = 1/4 98 | derivative = 256; // D coefficient = 1 99 | } 100 | 101 | PololuMenu menu; 102 | 103 | void selectEdition() 104 | { 105 | display.clear(); 106 | display.print(F("Select")); 107 | display.gotoXY(0,1); 108 | display.print(F("edition")); 109 | delay(1000); 110 | 111 | static const PololuMenuItem items[] = { 112 | { F("Standard"), selectStandard }, 113 | { F("Turtle"), selectTurtle }, 114 | { F("Hyper"), selectHyper }, 115 | }; 116 | 117 | menu.setItems(items, 3); 118 | menu.setDisplay(display); 119 | menu.setBuzzer(buzzer); 120 | menu.setButtons(buttonA, buttonB, buttonC); 121 | 122 | while(!menu.select()); 123 | 124 | display.gotoXY(0,1); 125 | display.print("OK! ..."); 126 | } 127 | 128 | // Sets up special characters in the LCD so that we can display 129 | // bar graphs. 130 | void loadCustomCharacters() 131 | { 132 | static const char levels[] PROGMEM = { 133 | 0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63 134 | }; 135 | display.loadCustomCharacter(levels + 0, 0); // 1 bar 136 | display.loadCustomCharacter(levels + 1, 1); // 2 bars 137 | display.loadCustomCharacter(levels + 2, 2); // 3 bars 138 | display.loadCustomCharacter(levels + 3, 3); // 4 bars 139 | display.loadCustomCharacter(levels + 4, 4); // 5 bars 140 | display.loadCustomCharacter(levels + 5, 5); // 6 bars 141 | display.loadCustomCharacter(levels + 6, 6); // 7 bars 142 | } 143 | 144 | void printBar(uint8_t height) 145 | { 146 | if (height > 8) { height = 8; } 147 | const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 148 | display.print(barChars[height]); 149 | } 150 | 151 | void calibrateSensors() 152 | { 153 | display.clear(); 154 | 155 | // Wait 1 second and then begin automatic sensor calibration 156 | // by rotating in place to sweep the sensors over the line 157 | delay(1000); 158 | for(uint16_t i = 0; i < 80; i++) 159 | { 160 | if (i > 20 && i <= 60) 161 | { 162 | motors.setSpeeds(-(int16_t)calibrationSpeed, calibrationSpeed); 163 | } 164 | else 165 | { 166 | motors.setSpeeds(calibrationSpeed, -(int16_t)calibrationSpeed); 167 | } 168 | 169 | lineSensors.calibrate(); 170 | } 171 | motors.setSpeeds(0, 0); 172 | } 173 | 174 | // Displays the estimated line position and a bar graph of sensor 175 | // readings on the LCD. Returns after the user presses B. 176 | void showReadings() 177 | { 178 | display.clear(); 179 | 180 | while(!buttonB.getSingleDebouncedPress()) 181 | { 182 | uint16_t position = lineSensors.readLineBlack(lineSensorValues); 183 | 184 | display.gotoXY(0, 0); 185 | display.print(position); 186 | display.print(" "); 187 | display.gotoXY(0, 1); 188 | for (uint8_t i = 0; i < NUM_SENSORS; i++) 189 | { 190 | uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8); 191 | printBar(barHeight); 192 | } 193 | 194 | delay(50); 195 | } 196 | } 197 | 198 | void setup() 199 | { 200 | // Uncomment if necessary to correct motor directions: 201 | //motors.flipLeftMotor(true); 202 | //motors.flipRightMotor(true); 203 | 204 | loadCustomCharacters(); 205 | 206 | // Play a little welcome song 207 | buzzer.play(">g32>>c32"); 208 | 209 | // To bypass the menu, replace this function with 210 | // selectHyper(), selectStandard(), or selectTurtle(). 211 | selectEdition(); 212 | 213 | // Wait for button B to be pressed and released. 214 | display.clear(); 215 | display.print(F("Press B")); 216 | display.gotoXY(0, 1); 217 | display.print(F("to calib")); 218 | while(!buttonB.getSingleDebouncedPress()); 219 | 220 | calibrateSensors(); 221 | 222 | showReadings(); 223 | 224 | // Play music and wait for it to finish before we start driving. 225 | display.clear(); 226 | display.print(F("Go!")); 227 | buzzer.play("L16 cdegreg4"); 228 | while(buzzer.isPlaying()); 229 | } 230 | 231 | void loop() 232 | { 233 | // Get the position of the line. Note that we *must* provide 234 | // the "lineSensorValues" argument to readLineBlack() here, even 235 | // though we are not interested in the individual sensor 236 | // readings. 237 | int16_t position = lineSensors.readLineBlack(lineSensorValues); 238 | 239 | // Our "error" is how far we are away from the center of the 240 | // line, which corresponds to position 2000. 241 | int16_t error = position - 2000; 242 | 243 | // Get motor speed difference using proportional and derivative 244 | // PID terms (the integral term is generally not very useful 245 | // for line following). 246 | int16_t speedDifference = error * (int32_t)proportional / 256 + (error - lastError) * (int32_t)derivative / 256; 247 | 248 | lastError = error; 249 | 250 | // Get individual motor speeds. The sign of speedDifference 251 | // determines if the robot turns left or right. 252 | int16_t leftSpeed = (int16_t)baseSpeed + speedDifference; 253 | int16_t rightSpeed = (int16_t)baseSpeed - speedDifference; 254 | 255 | // Constrain our motor speeds to be between 0 and maxSpeed. 256 | // One motor will always be turning at maxSpeed, and the other 257 | // will be at maxSpeed-|speedDifference| if that is positive, 258 | // else it will be stationary. For some applications, you 259 | // might want to allow the motor speed to go negative so that 260 | // it can spin in reverse. 261 | leftSpeed = constrain(leftSpeed, minSpeed, (int16_t)maxSpeed); 262 | rightSpeed = constrain(rightSpeed, minSpeed, (int16_t)maxSpeed); 263 | 264 | motors.setSpeeds(leftSpeed, rightSpeed); 265 | } 266 | -------------------------------------------------------------------------------- /examples/LineFollowerPID/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/LineFollowerPID/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // For the 3pi+ demo, replaces the default OLED font with an extended font 4 | // that incorporates several non-ASCII glyphs from the original HD44780 5 | // character set: the 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. In this example, we use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f; non-ASCII characters above 0x8f 21 | // are excluded. 22 | // 23 | // You can open original_font.xbm as a starting 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/LineFollowerSimple/LineFollowerSimple.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the line sensors on the 3pi+ 32U4 to follow 2 | a black line on a white background, using a simple conditional- 3 | based algorithm. */ 4 | 5 | #include 6 | #include 7 | 8 | using namespace Pololu3piPlus32U4; 9 | 10 | // Change next line to this if you are using the older 3pi+ 11 | // with a black and green LCD display: 12 | // LCD display; 13 | OLED display; 14 | 15 | Buzzer buzzer; 16 | LineSensors lineSensors; 17 | Motors motors; 18 | ButtonA buttonA; 19 | ButtonB buttonB; 20 | ButtonC buttonC; 21 | 22 | #define NUM_SENSORS 5 23 | unsigned int lineSensorValues[NUM_SENSORS]; 24 | 25 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 26 | Hyper versions of 3pi+ have different motor configurations, requiring 27 | the demo to be configured with different parameters for proper 28 | operation. The following functions set up these parameters using a 29 | menu that runs at the beginning of the program. To bypass the menu, 30 | you can replace the call to selectEdition() in setup() with one of the 31 | specific functions. 32 | */ 33 | 34 | // This is the maximum speed the motors will be allowed to turn. 35 | // A maxSpeed of 400 lets the motors go at top speed. Decrease 36 | // this value to impose a speed limit. 37 | uint16_t maxSpeed; 38 | 39 | uint16_t calibrationSpeed; 40 | 41 | void selectHyper() 42 | { 43 | motors.flipLeftMotor(true); 44 | motors.flipRightMotor(true); 45 | // Encoders are not used in this example. 46 | // encoders.flipEncoders(true); 47 | maxSpeed = 75; 48 | calibrationSpeed = 50; 49 | } 50 | 51 | void selectStandard() 52 | { 53 | maxSpeed = 100; 54 | calibrationSpeed = 60; 55 | } 56 | 57 | void selectTurtle() 58 | { 59 | maxSpeed = 200; 60 | calibrationSpeed = 120; 61 | } 62 | 63 | PololuMenu menu; 64 | 65 | void selectEdition() 66 | { 67 | display.clear(); 68 | display.print(F("Select")); 69 | display.gotoXY(0,1); 70 | display.print(F("edition")); 71 | delay(1000); 72 | 73 | static const PololuMenuItem items[] = { 74 | { F("Standard"), selectStandard }, 75 | { F("Turtle"), selectTurtle }, 76 | { F("Hyper"), selectHyper }, 77 | }; 78 | 79 | menu.setItems(items, 3); 80 | menu.setDisplay(display); 81 | menu.setBuzzer(buzzer); 82 | menu.setButtons(buttonA, buttonB, buttonC); 83 | 84 | while(!menu.select()); 85 | 86 | display.gotoXY(0,1); 87 | display.print("OK! ..."); 88 | } 89 | 90 | // Sets up special characters in the LCD so that we can display 91 | // bar graphs. 92 | void loadCustomCharacters() 93 | { 94 | static const char levels[] PROGMEM = { 95 | 0, 0, 0, 0, 0, 0, 0, 63, 63, 63, 63, 63, 63, 63 96 | }; 97 | display.loadCustomCharacter(levels + 0, 0); // 1 bar 98 | display.loadCustomCharacter(levels + 1, 1); // 2 bars 99 | display.loadCustomCharacter(levels + 2, 2); // 3 bars 100 | display.loadCustomCharacter(levels + 3, 3); // 4 bars 101 | display.loadCustomCharacter(levels + 4, 4); // 5 bars 102 | display.loadCustomCharacter(levels + 5, 5); // 6 bars 103 | display.loadCustomCharacter(levels + 6, 6); // 7 bars 104 | } 105 | 106 | void printBar(uint8_t height) 107 | { 108 | if (height > 8) { height = 8; } 109 | const char barChars[] = {' ', 0, 1, 2, 3, 4, 5, 6, (char)255}; 110 | display.print(barChars[height]); 111 | } 112 | 113 | void calibrateSensors() 114 | { 115 | display.clear(); 116 | 117 | // Wait 1 second and then begin automatic sensor calibration 118 | // by rotating in place to sweep the sensors over the line 119 | delay(1000); 120 | for(uint16_t i = 0; i < 80; i++) 121 | { 122 | if (i > 20 && i <= 60) 123 | { 124 | motors.setSpeeds(-(int16_t)calibrationSpeed, calibrationSpeed); 125 | } 126 | else 127 | { 128 | motors.setSpeeds(calibrationSpeed, -(int16_t)calibrationSpeed); 129 | } 130 | 131 | lineSensors.calibrate(); 132 | } 133 | motors.setSpeeds(0, 0); 134 | } 135 | 136 | // Displays the estimated line position and a bar graph of sensor readings on 137 | // the LCD. Returns after the user presses B. 138 | void showReadings() 139 | { 140 | display.clear(); 141 | 142 | while(!buttonB.getSingleDebouncedPress()) 143 | { 144 | uint16_t position = lineSensors.readLineBlack(lineSensorValues); 145 | 146 | display.gotoXY(0, 0); 147 | display.print(position); 148 | display.print(" "); 149 | display.gotoXY(0, 1); 150 | for (uint8_t i = 0; i < NUM_SENSORS; i++) 151 | { 152 | uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8); 153 | printBar(barHeight); 154 | } 155 | 156 | delay(50); 157 | } 158 | } 159 | 160 | void setup() 161 | { 162 | // Uncomment if necessary to correct motor directions: 163 | //motors.flipLeftMotor(true); 164 | //motors.flipRightMotor(true); 165 | 166 | loadCustomCharacters(); 167 | 168 | // Play a little welcome song 169 | buzzer.play(">g32>>c32"); 170 | 171 | // To bypass the menu, replace this function with 172 | // selectHyper(), selectStandard(), or selectTurtle(). 173 | selectEdition(); 174 | 175 | // Wait for button B to be pressed and released. 176 | display.clear(); 177 | display.print(F("Press B")); 178 | display.gotoXY(0, 1); 179 | display.print(F("to calib")); 180 | while(!buttonB.getSingleDebouncedPress()); 181 | 182 | calibrateSensors(); 183 | 184 | showReadings(); 185 | 186 | // Play music and wait for it to finish before we start driving. 187 | display.clear(); 188 | display.print(F("Go!")); 189 | buzzer.play("L16 cdegreg4"); 190 | while(buzzer.isPlaying()); 191 | } 192 | 193 | void loop() 194 | { 195 | // Get the position of the line. Note that we *must* provide 196 | // the "lineSensorValues" argument to readLineBlack() here, even 197 | // though we are not interested in the individual sensor 198 | // readings. 199 | int16_t position = lineSensors.readLineBlack(lineSensorValues); 200 | 201 | if (position < 1000) 202 | { 203 | // We are far to the right of the line: turn left. 204 | 205 | // Set the right motor to 100 and the left motor to zero, 206 | // to do a sharp turn to the left. Note that the maximum 207 | // value of either motor speed is 400, so we are driving 208 | // it at just about 25% of the max. 209 | motors.setSpeeds(0, maxSpeed); 210 | 211 | // Just for fun, indicate the direction we are turning on 212 | // the LEDs. 213 | ledYellow(1); 214 | ledRed(0); 215 | } 216 | else if (position < 3000) 217 | { 218 | // We are somewhat close to being centered on the line: 219 | // drive straight. 220 | motors.setSpeeds(maxSpeed, maxSpeed); 221 | ledYellow(1); 222 | ledRed(1); 223 | } 224 | else 225 | { 226 | // We are far to the left of the line: turn right. 227 | motors.setSpeeds(maxSpeed, 0); 228 | ledYellow(0); 229 | ledRed(1); 230 | } 231 | } 232 | -------------------------------------------------------------------------------- /examples/LineFollowerSimple/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/LineFollowerSimple/font.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // For the 3pi+ demo, replaces the default OLED font with an extended font 4 | // that incorporates several non-ASCII glyphs from the original HD44780 5 | // character set: the 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. In this example, we use a font file that defines 20 | // only the 96 characters from 0x20 to 0x8f; non-ASCII characters above 0x8f 21 | // are excluded. 22 | // 23 | // You can open original_font.xbm as a starting 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 line sensors on 2 | // the 3pi+ 32U4. This example is useful if you are having trouble with the 3 | // sensors or just want to characterize their behavior. 4 | // 5 | // The raw (uncalibrated) values are reported to the serial monitor. You can 6 | // press the "C" button to toggle whether the IR emitters are on during the 7 | // reading. 8 | 9 | #include 10 | 11 | using namespace Pololu3piPlus32U4; 12 | 13 | ButtonC buttonC; 14 | LineSensors lineSensors; 15 | 16 | const uint8_t SensorCount = 5; 17 | uint16_t sensorValues[SensorCount]; 18 | 19 | bool useEmitters = true; 20 | 21 | void setup() 22 | { 23 | 24 | } 25 | 26 | // Prints a line with all the sensor readings to the serial 27 | // monitor. 28 | void printReadingsToSerial() 29 | { 30 | char buffer[80]; 31 | sprintf(buffer, "%4d %4d %4d %4d %4d %c\n", 32 | sensorValues[0], 33 | sensorValues[1], 34 | sensorValues[2], 35 | sensorValues[3], 36 | sensorValues[4], 37 | useEmitters ? 'E' : 'e' 38 | ); 39 | Serial.print(buffer); 40 | } 41 | 42 | void loop() 43 | { 44 | static uint16_t lastSampleTime = 0; 45 | 46 | if ((uint16_t)(millis() - lastSampleTime) >= 100) 47 | { 48 | lastSampleTime = millis(); 49 | 50 | // Read the line sensors. 51 | lineSensors.read(sensorValues, useEmitters ? LineSensorsReadMode::On : LineSensorsReadMode::Off); 52 | 53 | // Send the results to the LCD and to the serial monitor. 54 | printReadingsToSerial(); 55 | } 56 | 57 | // If button C is pressed, toggle the state of the emitters. 58 | if (buttonC.getSingleDebouncedPress()) 59 | { 60 | useEmitters = !useEmitters; 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /examples/RotationResist/RotationResist.ino: -------------------------------------------------------------------------------- 1 | /* This demo shows how the 3pi+ can use its gyroscope to detect 2 | when it is being rotated, and use the motors to resist that 3 | rotation. 4 | 5 | Be careful to not move the robot for a few seconds after starting 6 | it while the gyro is being calibrated. During the gyro 7 | calibration, the yellow LED is on and the words "Gyro cal" are 8 | displayed on the display. 9 | 10 | After the gyro calibration is done, press button A to start the 11 | demo. If you try to turn the 3pi+, or put it on a surface that 12 | is turning, it will drive its motors to counteract the turning. 13 | 14 | This demo only uses the Z axis of the gyro, so it is possible to 15 | pick up the 3pi+, rotate it about its X and Y axes, and then put 16 | it down facing in a new position. */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | /* The IMU is not fully enabled by default since it depends on the 23 | Wire library, which uses about 1400 bytes of additional code space 24 | and defines an interrupt service routine (ISR) that might be 25 | incompatible with some applications (such as our TWISlave example). 26 | 27 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to 28 | enable IMU functionality. 29 | */ 30 | #include 31 | 32 | using namespace Pololu3piPlus32U4; 33 | 34 | // Change next line to this if you are using the older 3pi+ 35 | // with a black and green LCD display: 36 | // LCD display; 37 | OLED display; 38 | 39 | Buzzer buzzer; 40 | ButtonA buttonA; 41 | ButtonB buttonB; 42 | ButtonC buttonC; 43 | Motors motors; 44 | IMU imu; 45 | 46 | #include "TurnSensor.h" 47 | 48 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 49 | Hyper versions of 3pi+ have different motor configurations, requiring 50 | the demo to be configured with different parameters for proper 51 | operation. The following functions set up these parameters using a 52 | menu that runs at the beginning of the program. To bypass the menu, 53 | you can replace the call to selectEdition() in setup() with one of the 54 | specific functions. 55 | */ 56 | 57 | // This is the maximum speed the motors will be allowed to turn. 58 | // A maxSpeed of 400 lets the motors go at top speed. Decrease 59 | // this value to impose a speed limit. 60 | int16_t maxSpeed; 61 | 62 | void selectHyper() 63 | { 64 | motors.flipLeftMotor(true); 65 | motors.flipRightMotor(true); 66 | // Encoders are not used in this example. 67 | // encoders.flipEncoders(true); 68 | maxSpeed = 100; 69 | } 70 | 71 | void selectStandard() 72 | { 73 | maxSpeed = 200; 74 | } 75 | 76 | void selectTurtle() 77 | { 78 | maxSpeed = 400; 79 | } 80 | 81 | PololuMenu menu; 82 | 83 | void selectEdition() 84 | { 85 | display.clear(); 86 | display.print(F("Select")); 87 | display.gotoXY(0,1); 88 | display.print(F("edition")); 89 | delay(1000); 90 | 91 | static const PololuMenuItem items[] = { 92 | { F("Standard"), selectStandard }, 93 | { F("Turtle"), selectTurtle }, 94 | { F("Hyper"), selectHyper }, 95 | }; 96 | 97 | menu.setItems(items, 3); 98 | menu.setDisplay(display); 99 | menu.setBuzzer(buzzer); 100 | menu.setButtons(buttonA, buttonB, buttonC); 101 | 102 | while(!menu.select()); 103 | 104 | display.gotoXY(0,1); 105 | display.print("OK! ..."); 106 | } 107 | 108 | void setup() 109 | { 110 | // To bypass the menu, replace this function with 111 | // selectHyper(), selectStandard(), or selectTurtle(). 112 | selectEdition(); 113 | 114 | // Delay before calibrating the gyro. 115 | delay(1000); 116 | 117 | turnSensorSetup(); 118 | turnSensorReset(); 119 | 120 | display.clear(); 121 | display.print(F("Try to")); 122 | display.gotoXY(0, 1); 123 | display.print(F("turn me!")); 124 | } 125 | 126 | void loop() 127 | { 128 | // Read the gyro to update turnAngle, the estimation of how far 129 | // the robot has turned, and turnRate, the estimation of how 130 | // fast it is turning. 131 | turnSensorUpdate(); 132 | 133 | // Calculate the motor turn speed using proportional and 134 | // derivative PID terms. Here we are a using a proportional 135 | // constant of 28 and a derivative constant of 1/40. 136 | int32_t turnSpeed = -(int32_t)turnAngle / (turnAngle1 / 28) 137 | - turnRate / 40; 138 | 139 | // Constrain our motor speeds to be between 140 | // -maxSpeed and maxSpeed. 141 | turnSpeed = constrain(turnSpeed, -maxSpeed, maxSpeed); 142 | 143 | motors.setSpeeds(-turnSpeed, turnSpeed); 144 | } 145 | -------------------------------------------------------------------------------- /examples/RotationResist/TurnSensor.h: -------------------------------------------------------------------------------- 1 | // Turnsensor.h provides functions for configuring the 2 | // 3pi+ 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 | #include 10 | 11 | // This constant represents a turn of 45 degrees. 12 | const int32_t turnAngle45 = 0x20000000; 13 | 14 | // This constant represents a turn of 90 degrees. 15 | const int32_t turnAngle90 = turnAngle45 * 2; 16 | 17 | // This constant represents a turn of approximately 1 degree. 18 | const int32_t turnAngle1 = (turnAngle45 + 22) / 45; 19 | 20 | /* turnAngle is a 32-bit unsigned integer representing the amount 21 | the robot has turned since the last time turnSensorReset was 22 | called. This is computed solely using the Z axis of the gyro, so 23 | it could be inaccurate if the robot is rotated about the X or Y 24 | axes. 25 | 26 | Our convention is that a value of 0x20000000 represents a 45 27 | degree counter-clockwise rotation. This means that a uint32_t 28 | can represent any angle between 0 degrees and 360 degrees. If 29 | you cast it to a signed 32-bit integer by writing 30 | (int32_t)turnAngle, that integer can represent any angle between 31 | -180 degrees and 180 degrees. */ 32 | uint32_t turnAngle = 0; 33 | 34 | // turnRate is the current angular rate of the gyro, in units of 35 | // 0.07 degrees per second. 36 | int16_t turnRate; 37 | 38 | // This is the average reading obtained from the gyro's Z axis 39 | // during calibration. 40 | int16_t gyroOffset; 41 | 42 | // This variable helps us keep track of how much time has passed 43 | // between readings of the gyro. 44 | uint16_t gyroLastUpdate = 0; 45 | 46 | 47 | // This should be called to set the starting point for measuring 48 | // a turn. After calling this, turnAngle will be 0. 49 | void turnSensorReset() 50 | { 51 | gyroLastUpdate = micros(); 52 | turnAngle = 0; 53 | } 54 | 55 | // Read the gyro and update the angle. This should be called as 56 | // frequently as possible while using the gyro to do turns. 57 | void turnSensorUpdate() 58 | { 59 | // Read the measurements from the gyro. 60 | imu.readGyro(); 61 | turnRate = imu.g.z - gyroOffset; 62 | 63 | // Figure out how much time has passed since the last update (dt) 64 | uint16_t m = micros(); 65 | uint16_t dt = m - gyroLastUpdate; 66 | gyroLastUpdate = m; 67 | 68 | // Multiply dt by turnRate in order to get an estimation of how 69 | // much the robot has turned since the last update. 70 | // (angular change = angular velocity * time) 71 | int32_t d = (int32_t)turnRate * dt; 72 | 73 | // The units of d are gyro digits times microseconds. We need 74 | // to convert those to the units of turnAngle, where 2^29 units 75 | // represents 45 degrees. The conversion from gyro digits to 76 | // degrees per second (dps) is determined by the sensitivity of 77 | // the gyro: 0.07 degrees per second per digit. 78 | // 79 | // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree) 80 | // = 14680064/17578125 unit/(digit*us) 81 | turnAngle += (int64_t)d * 14680064 / 17578125; 82 | } 83 | 84 | /* This should be called in setup() to enable and calibrate the 85 | gyro. It uses the display, yellow LED, and button A. While the 86 | display shows "Gyro cal", you should be careful to hold the robot 87 | still. 88 | 89 | The digital zero-rate level of the gyro can be as high as 90 | 25 degrees per second, and this calibration helps us correct for 91 | that. */ 92 | void turnSensorSetup() 93 | { 94 | Wire.begin(); 95 | imu.init(); 96 | imu.enableDefault(); 97 | imu.configureForTurnSensing(); 98 | 99 | display.clear(); 100 | display.print(F("Gyro cal")); 101 | 102 | // Turn on the yellow LED in case the display is not available. 103 | ledYellow(1); 104 | 105 | // Delay to give the user time to remove their finger. 106 | delay(500); 107 | 108 | // Calibrate the gyro. 109 | int32_t total = 0; 110 | for (uint16_t i = 0; i < 1024; i++) 111 | { 112 | // Wait for new data to be available, then read it. 113 | while(!imu.gyroDataReady()) {} 114 | imu.readGyro(); 115 | 116 | // Add the Z axis reading to the total. 117 | total += imu.g.z; 118 | } 119 | ledYellow(0); 120 | gyroOffset = total / 1024; 121 | 122 | // Display the angle (in degrees from -180 to 180) until the 123 | // user presses A. 124 | display.clear(); 125 | turnSensorReset(); 126 | while (!buttonA.getSingleDebouncedRelease()) 127 | { 128 | turnSensorUpdate(); 129 | display.gotoXY(0, 0); 130 | display.print((((int32_t)turnAngle >> 16) * 360) >> 16); 131 | display.print(F(" ")); 132 | } 133 | display.clear(); 134 | } 135 | -------------------------------------------------------------------------------- /examples/TurnWithCompass/TurnWithCompass.ino: -------------------------------------------------------------------------------- 1 | /* This example uses the 3pi+ 32U4 robot's onboard magnetometer to 2 | * help the make precise 90-degree turns and drive in squares. 3 | * 4 | * This program first calibrates the compass to account for offsets in 5 | * its output. Calibration is accomplished in setup(). 6 | * 7 | * In loop(), The driving angle then changes its offset by 90 degrees 8 | * from the heading every second. Essentially, this navigates the 9 | * 3pi+ to drive in square patterns. 10 | * 11 | * It is important to note that stray magnetic fields from electric 12 | * current (including from the robot's own motors) and the environment 13 | * (for example, steel rebar in a concrete floor) might adversely 14 | * affect readings from the compass and make them less reliable. 15 | * 16 | * Note that the Hyper Edition does not drive very straight at the low 17 | * speeds used in this example, so it might not make a very accurate 18 | * square. 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | /* The IMU is not fully enabled by default since it depends on the 25 | Wire library, which uses about 1400 bytes of additional code space 26 | and defines an interrupt service routine (ISR) that might be 27 | incompatible with some applications (such as our TWISlave example). 28 | 29 | Include Pololu3piPlus32U4IMU.h in one of your cpp/ino files to enable IMU 30 | functionality. 31 | */ 32 | #include 33 | 34 | #define CALIBRATION_SAMPLES 70 // Number of compass readings to take when calibrating 35 | 36 | // Allowed deviation (in degrees) relative to target angle that must be achieved before driving straight 37 | #define DEVIATION_THRESHOLD 5 38 | 39 | using namespace Pololu3piPlus32U4; 40 | 41 | // Change next line to this if you are using the older 3pi+ 42 | // with a black and green LCD display: 43 | // LCD display; 44 | OLED display; 45 | 46 | Motors motors; 47 | Buzzer buzzer; 48 | ButtonA buttonA; 49 | ButtonB buttonB; 50 | ButtonC buttonC; 51 | IMU imu; 52 | 53 | IMU::vector m_max; // maximum magnetometer values, used for calibration 54 | IMU::vector m_min; // minimum magnetometer values, used for calibration 55 | 56 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 57 | Hyper versions of 3pi+ have different motor configurations, requiring 58 | the demo to be configured with different parameters for proper 59 | operation. The following functions set up these parameters using a 60 | menu that runs at the beginning of the program. To bypass the menu, 61 | you can replace the call to selectEdition() in setup() with one of the 62 | specific functions. 63 | */ 64 | 65 | uint16_t speedStraightLeft; // Maximum motor speed when going straight; variable speed when turning 66 | uint16_t speedStraightRight; 67 | uint16_t turnBaseSpeed; // Base speed when turning (added to variable speed) 68 | uint16_t driveTime; // Time to drive straight, in milliseconds 69 | 70 | void selectHyper() 71 | { 72 | motors.flipLeftMotor(true); 73 | motors.flipRightMotor(true); 74 | // Encoders are not used in this example. 75 | // encoders.flipEncoders(true); 76 | 77 | // The right motor tends to be a bit slower on the Hyper edition. 78 | // Speed it up to drive straighter. 79 | // You might need to adjust this value for your particular unit. 80 | speedStraightLeft = 70; 81 | speedStraightRight = 80; 82 | turnBaseSpeed = 20; 83 | driveTime = 500; 84 | } 85 | 86 | void selectStandard() 87 | { 88 | speedStraightLeft = 100; 89 | speedStraightRight = speedStraightLeft; 90 | turnBaseSpeed = 20; 91 | driveTime = 1000; 92 | } 93 | 94 | void selectTurtle() 95 | { 96 | speedStraightLeft = 200; 97 | speedStraightRight = speedStraightLeft; 98 | turnBaseSpeed = 40; 99 | driveTime = 2000; 100 | } 101 | 102 | PololuMenu menu; 103 | 104 | void selectEdition() 105 | { 106 | display.clear(); 107 | display.print(F("Select")); 108 | display.gotoXY(0,1); 109 | display.print(F("edition")); 110 | delay(1000); 111 | 112 | static const PololuMenuItem items[] = { 113 | { F("Standard"), selectStandard }, 114 | { F("Turtle"), selectTurtle }, 115 | { F("Hyper"), selectHyper }, 116 | }; 117 | 118 | menu.setItems(items, 3); 119 | menu.setDisplay(display); 120 | menu.setBuzzer(buzzer); 121 | menu.setButtons(buttonA, buttonB, buttonC); 122 | 123 | while(!menu.select()); 124 | 125 | display.gotoXY(0,1); 126 | display.print("OK! ..."); 127 | } 128 | 129 | // Setup will calibrate our compass by finding maximum/minimum magnetic readings 130 | void setup() 131 | { 132 | // The highest possible magnetic value to read in any direction is 32767 133 | // The lowest possible magnetic value to read in any direction is -32767 134 | IMU::vector running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767}; 135 | unsigned char index; 136 | 137 | Serial.begin(9600); 138 | 139 | // Initialize the Wire library and join the I2C bus as a master 140 | Wire.begin(); 141 | 142 | // Initialize IMU 143 | imu.init(); 144 | 145 | // Enables accelerometer and magnetometer 146 | imu.enableDefault(); 147 | 148 | imu.configureForCompassHeading(); 149 | 150 | // To bypass the menu, replace this function with 151 | // selectHyper(), selectStandard(), or selectTurtle(). 152 | selectEdition(); 153 | 154 | delay(1000); 155 | 156 | display.clear(); 157 | display.print("starting"); 158 | display.gotoXY(0,1); 159 | display.print("calib"); 160 | Serial.println("starting calibration"); 161 | 162 | // To calibrate the magnetometer, the 3pi+ spins to find the max/min 163 | // magnetic vectors. This information is used to correct for offsets 164 | // in the magnetometer data. 165 | motors.setLeftSpeed(speedStraightLeft); 166 | motors.setRightSpeed(-speedStraightRight); 167 | 168 | for(index = 0; index < CALIBRATION_SAMPLES; index ++) 169 | { 170 | // Take a reading of the magnetic vector and store it in compass.m 171 | imu.readMag(); 172 | 173 | running_min.x = min(running_min.x, imu.m.x); 174 | running_min.y = min(running_min.y, imu.m.y); 175 | 176 | running_max.x = max(running_max.x, imu.m.x); 177 | running_max.y = max(running_max.y, imu.m.y); 178 | 179 | Serial.println(index); 180 | 181 | delay(50); 182 | } 183 | 184 | motors.setLeftSpeed(0); 185 | motors.setRightSpeed(0); 186 | 187 | Serial.print("max.x "); 188 | Serial.print(running_max.x); 189 | Serial.println(); 190 | Serial.print("max.y "); 191 | Serial.print(running_max.y); 192 | Serial.println(); 193 | Serial.print("min.x "); 194 | Serial.print(running_min.x); 195 | Serial.println(); 196 | Serial.print("min.y "); 197 | Serial.print(running_min.y); 198 | Serial.println(); 199 | 200 | // Store calibrated values in m_max and m_min 201 | m_max.x = running_max.x; 202 | m_max.y = running_max.y; 203 | m_min.x = running_min.x; 204 | m_min.y = running_min.y; 205 | 206 | display.clear(); 207 | display.print("Press A"); 208 | buttonA.waitForButton(); 209 | } 210 | 211 | void loop() 212 | { 213 | float heading, relative_heading; 214 | int speed; 215 | static float target_heading = 0; 216 | 217 | // Heading is given in degrees away from the magnetic vector, increasing clockwise 218 | heading = averageHeading(); 219 | 220 | // This gives us the relative heading with respect to the target angle 221 | relative_heading = relativeHeading(heading, target_heading); 222 | 223 | Serial.print("Target heading: "); 224 | Serial.print(target_heading); 225 | Serial.print(" Actual heading: "); 226 | Serial.print(heading); 227 | Serial.print(" Difference: "); 228 | Serial.print(relative_heading); 229 | 230 | // If the 3pi+ has turned to the direction it wants to be pointing, go straight and then do another turn 231 | if(abs(relative_heading) < DEVIATION_THRESHOLD) 232 | { 233 | motors.setSpeeds(speedStraightLeft, speedStraightRight); 234 | 235 | Serial.print(" Straight"); 236 | 237 | delay(driveTime); 238 | 239 | // Turn off motors and wait a short time to stop smoothly. 240 | motors.setSpeeds(0, 0); 241 | delay(100); 242 | 243 | // Turn 90 degrees. 244 | target_heading = target_heading + 90; 245 | if(target_heading >= 360) 246 | target_heading -= 360; 247 | } 248 | else 249 | { 250 | // To avoid overshooting, the closer the 3pi+ gets to the target 251 | // heading, the slower it should turn. Set the motor speeds to a 252 | // minimum base amount plus an additional variable amount based 253 | // on the heading difference. 254 | 255 | speed = speedStraightLeft*relative_heading/180; 256 | 257 | if (speed < 0) 258 | speed -= turnBaseSpeed; 259 | else 260 | speed += turnBaseSpeed; 261 | 262 | motors.setSpeeds(speed, -speed); 263 | 264 | Serial.print(" Turn"); 265 | } 266 | Serial.println(); 267 | } 268 | 269 | // Converts x and y components of a vector to a heading in degrees. 270 | // This calculation assumes that the 3pi+ is always level. 271 | template float heading(IMU::vector v) 272 | { 273 | float x_scaled = 2.0*(float)(v.x - m_min.x) / (m_max.x - m_min.x) - 1.0; 274 | float y_scaled = 2.0*(float)(v.y - m_min.y) / (m_max.y - m_min.y) - 1.0; 275 | 276 | float angle = atan2(y_scaled, x_scaled)*180 / M_PI; 277 | if (angle < 0) 278 | angle += 360; 279 | return angle; 280 | } 281 | 282 | // Yields the angle difference in degrees between two headings 283 | float relativeHeading(float heading_from, float heading_to) 284 | { 285 | float relative_heading = heading_to - heading_from; 286 | 287 | // constrain to -180 to 180 degree range 288 | if (relative_heading > 180) 289 | relative_heading -= 360; 290 | if (relative_heading < -180) 291 | relative_heading += 360; 292 | 293 | return relative_heading; 294 | } 295 | 296 | // Average 10 vectors to get a better measurement and help smooth out 297 | // the motors' magnetic interference. 298 | float averageHeading() 299 | { 300 | IMU::vector avg = {0, 0, 0}; 301 | 302 | for(int i = 0; i < 10; i ++) 303 | { 304 | imu.readMag(); 305 | avg.x += imu.m.x; 306 | avg.y += imu.m.y; 307 | } 308 | avg.x /= 10.0; 309 | avg.y /= 10.0; 310 | 311 | // avg is the average measure of the magnetic vector. 312 | return heading(avg); 313 | } 314 | -------------------------------------------------------------------------------- /examples/WallBumper/WallBumper.ino: -------------------------------------------------------------------------------- 1 | /* Load this example to make the 3pi+ 32U4 drive forward until it hits 2 | a wall, detect the collision with its bumpers, then reverse, turn, and 3 | keep driving. 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | using namespace Pololu3piPlus32U4; 10 | 11 | // Change next line to this if you are using the older 3pi+ 12 | // with a black and green LCD display: 13 | // LCD display; 14 | OLED display; 15 | 16 | BumpSensors bumpSensors; 17 | Buzzer buzzer; 18 | Motors motors; 19 | ButtonA buttonA; 20 | ButtonB buttonB; 21 | ButtonC buttonC; 22 | 23 | /* Configuration for specific 3pi+ editions: the Standard, Turtle, and 24 | Hyper versions of 3pi+ have different motor configurations, requiring 25 | the demo to be configured with different parameters for proper 26 | operation. The following functions set up these parameters using a 27 | menu that runs at the beginning of the program. To bypass the menu, 28 | you can replace the call to selectEdition() in setup() with one of the 29 | specific functions. 30 | */ 31 | 32 | // This is the maximum speed the motors will be allowed to turn. 33 | // A maxSpeed of 400 lets the motors go at top speed. Decrease 34 | // this value to impose a speed limit. 35 | int16_t maxSpeed; 36 | 37 | int16_t turnTime; 38 | 39 | void selectHyper() 40 | { 41 | motors.flipLeftMotor(true); 42 | motors.flipRightMotor(true); 43 | // Encoders are not used in this example. 44 | // encoders.flipEncoders(true); 45 | maxSpeed = 75; 46 | turnTime = 150; 47 | } 48 | 49 | void selectStandard() 50 | { 51 | maxSpeed = 100; 52 | turnTime = 250; 53 | } 54 | 55 | void selectTurtle() 56 | { 57 | maxSpeed = 200; 58 | turnTime = 500; 59 | } 60 | 61 | PololuMenu menu; 62 | 63 | void selectEdition() 64 | { 65 | display.clear(); 66 | display.print(F("Select")); 67 | display.gotoXY(0,1); 68 | display.print(F("edition")); 69 | delay(1000); 70 | 71 | static const PololuMenuItem items[] = { 72 | { F("Standard"), selectStandard }, 73 | { F("Turtle"), selectTurtle }, 74 | { F("Hyper"), selectHyper }, 75 | }; 76 | 77 | menu.setItems(items, 3); 78 | menu.setDisplay(display); 79 | menu.setBuzzer(buzzer); 80 | menu.setButtons(buttonA, buttonB, buttonC); 81 | 82 | while(!menu.select()); 83 | 84 | display.gotoXY(0,1); 85 | display.print("OK! ..."); 86 | } 87 | 88 | void setup() 89 | { 90 | // To bypass the menu, replace this function with 91 | // selectHyper(), selectStandard(), or selectTurtle(). 92 | selectEdition(); 93 | 94 | bumpSensors.calibrate(); 95 | delay(1000); 96 | display.clear(); 97 | } 98 | 99 | void loop() 100 | { 101 | motors.setSpeeds(maxSpeed, maxSpeed); 102 | bumpSensors.read(); 103 | 104 | if (bumpSensors.leftIsPressed()) 105 | { 106 | // Left bump sensor is pressed. 107 | ledYellow(true); 108 | motors.setSpeeds(0, 0); 109 | buzzer.play("a32"); 110 | display.gotoXY(0, 0); 111 | display.print('L'); 112 | 113 | motors.setSpeeds(maxSpeed, -maxSpeed); 114 | delay(turnTime); 115 | 116 | motors.setSpeeds(0, 0); 117 | buzzer.play("b32"); 118 | ledYellow(false); 119 | display.gotoXY(0, 0); 120 | display.print(' '); 121 | } 122 | else if (bumpSensors.rightIsPressed()) 123 | { 124 | // Right bump sensor is pressed. 125 | ledRed(true); 126 | motors.setSpeeds(0, 0); 127 | buzzer.play("e32"); 128 | display.gotoXY(7, 0); 129 | display.print('R'); 130 | 131 | motors.setSpeeds(-maxSpeed, maxSpeed); 132 | delay(turnTime); 133 | 134 | motors.setSpeeds(0, 0); 135 | buzzer.play("f32"); 136 | ledRed(false); 137 | display.gotoXY(7, 0); 138 | display.print(' '); 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | Pololu3piPlus32U4 KEYWORD1 2 | 3 | ledRed KEYWORD2 4 | ledYellow KEYWORD2 5 | ledGreen KEYWORD2 6 | usbPowerPresent KEYWORD2 7 | readBatteryMillivolts KEYWORD2 8 | 9 | ############################################## 10 | 11 | BumpSide KEYWORD1 12 | 13 | BumpLeft LITERAL1 14 | BumpRight LITERAL1 15 | 16 | BumpSensors KEYWORD1 17 | 18 | calibrate KEYWORD2 19 | read KEYWORD2 20 | leftChanged KEYWORD2 21 | leftIsPressed KEYWORD2 22 | rightChanged KEYWORD2 23 | rightIsPressed KEYWORD2 24 | 25 | ############################################## 26 | 27 | ButtonA KEYWORD1 28 | ButtonB KEYWORD1 29 | ButtonC KEYWORD1 30 | 31 | ############################################## 32 | 33 | Buzzer KEYWORD1 34 | 35 | ############################################## 36 | 37 | Encoders KEYWORD1 38 | init KEYWORD2 39 | getCountsLeft KEYWORD2 40 | getCountsRight KEYWORD2 41 | getCountsAndResetLeft KEYWORD2 42 | getCountsAndResetRight KEYWORD2 43 | checkErrorLeft KEYWORD2 44 | checkErrorRight KEYWORD2 45 | 46 | ############################################## 47 | 48 | IMU KEYWORD1 49 | IMUType KEYWORD1 50 | 51 | LSM6DS33_ADDR LITERAL1 52 | LIS3MDL_ADDR LITERAL1 53 | LSM6DS33_REG_WHO_AM_I LITERAL1 54 | LSM6DS33_REG_CTRL1_XL LITERAL1 55 | LSM6DS33_REG_CTRL2_G LITERAL1 56 | LSM6DS33_REG_CTRL3_C LITERAL1 57 | LSM6DS33_REG_STATUS_REG LITERAL1 58 | LSM6DS33_REG_OUTX_L_G LITERAL1 59 | LSM6DS33_REG_OUTX_L_XL LITERAL1 60 | LIS3MDL_REG_WHO_AM_I LITERAL1 61 | LIS3MDL_REG_CTRL_REG1 LITERAL1 62 | LIS3MDL_REG_CTRL_REG2 LITERAL1 63 | LIS3MDL_REG_CTRL_REG3 LITERAL1 64 | LIS3MDL_REG_CTRL_REG4 LITERAL1 65 | LIS3MDL_REG_STATUS_REG LITERAL1 66 | LIS3MDL_REG_OUT_X_L LITERAL1 67 | 68 | getLastError KEYWORD2 69 | init KEYWORD2 70 | getType KEYWORD2 71 | enableDefault KEYWORD2 72 | configureForTurnSensing KEYWORD2 73 | configureForFaceUphill KEYWORD2 74 | configureForCompassHeading KEYWORD2 75 | writeReg KEYWORD2 76 | readReg KEYWORD2 77 | readAcc KEYWORD2 78 | readGyro KEYWORD2 79 | readMag KEYWORD2 80 | read KEYWORD2 81 | accDataReady KEYWORD2 82 | gyroDataReady KEYWORD2 83 | magDataReady KEYWORD2 84 | 85 | ############################################## 86 | 87 | LCD KEYWORD1 88 | 89 | ############################################## 90 | 91 | LineSensorsReadMode KEYWORD1 92 | Off LITERAL1 93 | On LITERAL1 94 | Manual LITERAL1 95 | 96 | LineSensors KEYWORD1 97 | 98 | setTimeout KEYWORD2 99 | getTimeout KEYWORD2 100 | calibrate KEYWORD2 101 | resetCalibration KEYWORD2 102 | read KEYWORD2 103 | readCalibrated KEYWORD2 104 | readLineBlack KEYWORD2 105 | readLineWhite KEYWORD2 106 | 107 | CalibrationData KEYWORD1 108 | 109 | emittersOn KEYWORD2 110 | emittersOff KEYWORD2 111 | 112 | ############################################## 113 | 114 | Motors KEYWORD1 115 | 116 | flipLeftMotor KEYWORD2 117 | flipRightMotor KEYWORD2 118 | setLeftSpeed KEYWORD2 119 | setRightSpeed KEYWORD2 120 | setSpeeds KEYWORD2 121 | 122 | ############################################## 123 | 124 | OLED KEYWORD1 125 | 126 | ############################################## -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Pololu3piPlus32U4 2 | version=1.1.3 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Arduino library for the Pololu 3pi+ 32u4 Robot 6 | paragraph=This is a library for the Arduino IDE that helps interface with the on-board hardware on the Pololu 3pi+ 32U4 Robot. 7 | category=Device Control 8 | url=https://github.com/pololu/pololu-3pi-plus-32u4-arduino-library 9 | architectures=avr 10 | depends=FastGPIO,USBPause,Pushbutton,PololuBuzzer,PololuHD44780,PololuOLED,PololuMenu 11 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4.h 4 | /// 5 | /// \brief Main header file for the Pololu 3pi+ 32U4 library. 6 | /// 7 | /// This file includes all the other headers files provided by the library. 8 | 9 | #pragma once 10 | 11 | #ifndef __AVR_ATmega32U4__ 12 | #error "This library only supports the ATmega32U4. Try selecting A-Star 32U4 in the Boards menu." 13 | #endif 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | /// Top-level namespace for the Pololu3piPlus32U4 library. 27 | namespace Pololu3piPlus32U4 28 | { 29 | 30 | // TODO: servo support 31 | 32 | /// \brief Turns the red user LED (RX) on or off. 33 | /// 34 | /// \param on A value of 1 turns on the LED; 0 turns it off. 35 | /// 36 | /// The red user LED is on pin 17, which is also known as PB0, SS, and RXLED. 37 | /// The Arduino core code uses this LED to indicate when it receives data over 38 | /// USB, so it might be hard to control this LED when USB is connected. 39 | inline void ledRed(bool on) 40 | { 41 | FastGPIO::Pin<17>::setOutput(!on); 42 | } 43 | 44 | /// \brief Turns the yellow user LED on pin 13 on or off. 45 | /// 46 | /// \param on A value of 1 turns on the LED; 0 turns it off. 47 | inline void ledYellow(bool on) 48 | { 49 | FastGPIO::Pin<13>::setOutput(on); 50 | } 51 | 52 | /// \brief Turns the green user LED (TX) on or off. 53 | /// 54 | /// \param on A value of 1 turns on the LED; 0 turns it off. 55 | /// 56 | /// The green user LED is pin PD5, which is also known as TXLED. The Arduino 57 | /// core code uses this LED to indicate when it receives data over USB, so it 58 | /// might be hard to control this LED when USB is connected. 59 | inline void ledGreen(bool on) 60 | { 61 | FastGPIO::Pin::setOutput(!on); 62 | } 63 | 64 | /// \brief Returns true if USB power is detected. 65 | /// 66 | /// This function returns true if power is detected on the board's USB port and 67 | /// returns false otherwise. It uses the ATmega32U4's VBUS line, which is 68 | /// directly connected to the power pin of the USB connector. 69 | /// 70 | /// \sa A method for detecting whether the board's virtual COM port is open: 71 | /// http://arduino.cc/en/Serial/IfSerial 72 | inline bool usbPowerPresent() 73 | { 74 | return USBSTA >> VBUS & 1; 75 | } 76 | 77 | /// Reads the battery voltage and returns it in millivolts. 78 | inline uint16_t readBatteryMillivolts() 79 | { 80 | const uint8_t sampleCount = 8; 81 | uint16_t sum = 0; 82 | for (uint8_t i = 0; i < sampleCount; i++) 83 | { 84 | sum += analogRead(A1); 85 | } 86 | 87 | // VBAT = 3 * millivolt reading = 3 * raw * 5000/1024 88 | // = raw * 1875 / 128 89 | // The correction term below makes it so that we round to the 90 | // nearest whole number instead of always rounding down. 91 | const uint32_t correction = 64 * sampleCount - 1; 92 | return ((uint32_t)sum * 1875 + correction) / (128 * sampleCount); 93 | } 94 | 95 | } 96 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4BumpSensors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | #include 4 | #include 5 | 6 | namespace Pololu3piPlus32U4 7 | { 8 | 9 | void BumpSensors::readRaw() 10 | { 11 | FastGPIO::Pin::setOutputLow(); // Turn on the emitters. 12 | 13 | FastGPIO::Pin::setOutputHigh(); 14 | FastGPIO::Pin::setOutputHigh(); 15 | _delay_us(10); 16 | 17 | sensorValues[0] = timeout; 18 | sensorValues[1] = timeout; 19 | 20 | noInterrupts(); 21 | uint16_t startTime = micros(); 22 | FastGPIO::Pin::setInput(); 23 | FastGPIO::Pin::setInput(); 24 | interrupts(); 25 | 26 | while (true) 27 | { 28 | noInterrupts(); 29 | uint16_t time = micros() - startTime; 30 | if (time >= timeout) 31 | { 32 | interrupts(); 33 | break; 34 | } 35 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[0]) { sensorValues[0] = time; } 36 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[1]) { sensorValues[1] = time; } 37 | interrupts(); 38 | __builtin_avr_delay_cycles(4); // allow interrupts to run 39 | } 40 | 41 | FastGPIO::Pin::setInput(); // turn off the emitters 42 | } 43 | 44 | void BumpSensors::calibrate(uint8_t count) 45 | { 46 | uint32_t sum[2] = {0, 0}; 47 | 48 | for (uint8_t i = 0; i < count; i++) 49 | { 50 | readRaw(); 51 | sum[BumpLeft] += sensorValues[BumpLeft]; 52 | sum[BumpRight] += sensorValues[BumpRight]; 53 | } 54 | 55 | for (uint8_t s = BumpLeft; s <= BumpRight; s++) 56 | { 57 | baseline[s] = (sum[s] + count / 2) / count; 58 | 59 | // Calculate threshold to compare readings to by adding margin to baseline, 60 | // but make sure it is no larger than the QTR sensor timeout (i.e. if the 61 | // reading timed out, consider the bump sensor pressed). 62 | threshold[s] = baseline[s] + baseline[s] * (uint32_t)marginPercentage / 100; 63 | if (threshold[s] > timeout) { threshold[s] = timeout; } 64 | } 65 | } 66 | 67 | uint8_t BumpSensors::read() 68 | { 69 | readRaw(); 70 | 71 | uint8_t bitField = 0; 72 | for (uint8_t s = BumpLeft; s <= BumpRight; s++) 73 | { 74 | last[s] = pressed[s]; 75 | pressed[s] = (sensorValues[s] >= threshold[s]); 76 | bitField |= pressed[s] << s; 77 | } 78 | return bitField; 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4BumpSensors.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4BumpSensors.h 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace Pololu3piPlus32U4 10 | { 11 | 12 | /// Bump sensor sides. 13 | enum BumpSide { 14 | /// Left bump sensor 15 | BumpLeft = 0, 16 | 17 | /// Right bump sensor 18 | BumpRight = 1 19 | }; 20 | 21 | /// \brief Gets readings from the two bump sensors on the front of the 3pi+ 22 | /// 32U4. 23 | class BumpSensors 24 | { 25 | public: 26 | /// Default timeout for RC sensors (in microseconds). 27 | static const uint16_t defaultTimeout = 4000; 28 | 29 | static const uint8_t emitterPin = 11; 30 | static const uint8_t bumpLeftPin = 4; 31 | static const uint8_t bumpRightPin = 5; 32 | 33 | /// \brief Calibrates the bump sensors. 34 | /// 35 | /// \param count The number of times to read the sensors during calibration. 36 | /// The default is 50. 37 | /// 38 | /// This method reads the bump sensors a number of times for calibration. 39 | /// You should call it while the bump sensors are not pressed and before 40 | /// using them in your program. 41 | /// 42 | /// Calling this function obtains a set of baseline readings that should 43 | /// should represent the raw sensor readings while the bump sensors are not 44 | /// pressed. The library uses these baseline values and an additional margin 45 | /// (#marginPercentage, defined as a percentage of the baselines) to derive 46 | /// thresholds that the sensors must exceed to register as pressed: 47 | /// 48 | /// \f[ 49 | /// {\text{threshold} = \text{baseline} \times \frac{100 + \text{marginPercentage}}{100}} 50 | /// \f] 51 | void calibrate(uint8_t count = 50); 52 | 53 | /// \brief Reads both bump sensors. 54 | /// 55 | /// \return A bit field indicating whether each bump sensor is pressed. The 56 | /// bits representing each sensor are defined by the ::BumpSide enum. 57 | /// 58 | /// For example, a return value of 2 (0b10 in binary) indicates: 59 | /// * The right bump sensor is pressed, since bit 1 (BumpRight) is set. 60 | /// * The left bump sensor is not pressed, since bit 0 (BumpLeft) is 61 | /// cleared. 62 | /// 63 | /// Instead of checking the return value of this method, you can instead 64 | /// call read() and then use the functions leftChanged(), rightChanged(), 65 | /// leftIsPressed(), and rightIsPressed() to get information about the bump 66 | /// sensors. 67 | uint8_t read(); 68 | 69 | /// \brief Indicates whether the left bump sensor's state has changed. 70 | /// 71 | /// \return True if the left bump sensor's state has changed between the 72 | /// second-to-last and the last call to read(); false otherwise. 73 | bool leftChanged() { return (pressed[BumpLeft] ^ last[BumpLeft]) != 0; } 74 | 75 | 76 | /// \brief Indicates whether the right bump sensor's state has changed. 77 | /// 78 | /// \return True if the right bump sensor's state has changed between the 79 | /// second-to-last and the last call to read(); false otherwise. 80 | bool rightChanged() { return (pressed[BumpRight] ^ last[BumpRight]) != 0; } 81 | 82 | /// \brief Indicates whether the left bump sensor is pressed. 83 | /// 84 | /// \return True if the left bump sensor was pressed during the most recent 85 | /// call to read(); false otherwise. 86 | bool leftIsPressed() { return pressed[BumpLeft]; } 87 | 88 | /// \brief Indicates whether the right bump sensor is pressed. 89 | /// 90 | /// \return True if the right bump sensor was pressed during the most recent 91 | /// call to read(); false otherwise. 92 | bool rightIsPressed() { return pressed[BumpRight]; } 93 | 94 | /// \brief The amount, as a percentage, that will be added to the measured 95 | /// baseline to get the threshold. 96 | /// 97 | /// You must calibrate after changing this. 98 | /// 99 | /// \sa calibrate() 100 | uint16_t marginPercentage = 50; 101 | 102 | /// Baseline readings obtained from calibration. 103 | uint16_t baseline[2]; 104 | 105 | /// Thresholds for bump sensor press detection. 106 | uint16_t threshold[2]; 107 | 108 | /// Raw reflectance sensor readings. 109 | uint16_t sensorValues[2]; 110 | 111 | /// Timeout for bump sensor readings (in microseconds). 112 | uint16_t timeout = defaultTimeout; 113 | 114 | private: 115 | void readRaw(); 116 | uint8_t pressed[2]; 117 | uint8_t last[2]; 118 | }; 119 | 120 | } 121 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Buttons.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4Buttons.h 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace Pololu3piPlus32U4 13 | { 14 | 15 | /// \brief Interfaces with button A on the 3pi+ 32U4. 16 | class ButtonA : public Pushbutton 17 | { 18 | public: 19 | /// The pin number for the pin connected to button A on the 3pi+ 32U4. 20 | static const uint8_t buttonAPin = 14; 21 | 22 | ButtonA() : Pushbutton(buttonAPin) 23 | { 24 | } 25 | }; 26 | 27 | /// \brief Interfaces with button B on the 3pi+ 32U4. 28 | /// 29 | /// The pin used for button B is also used for the TX LED. 30 | /// 31 | /// This class temporarily disables USB interrupts because the Arduino core code 32 | /// has USB interrupts enabled that sometimes write to the pin this button is 33 | /// on. 34 | /// 35 | /// This class temporarily sets the pin to be an input without a pull-up 36 | /// resistor. The pull-up resistor is not needed because of the resistors on 37 | /// the board. 38 | class ButtonB : public PushbuttonBase 39 | { 40 | public: 41 | /// The pin number for the pin connected to button B on the 3pi+ 32U4. 42 | static const uint8_t buttonBPin = IO_D5; 43 | 44 | virtual bool isPressed() 45 | { 46 | USBPause usbPause; 47 | FastGPIO::PinLoan loan; 48 | FastGPIO::Pin::setInputPulledUp(); 49 | _delay_us(3); 50 | return !FastGPIO::Pin::isInputHigh(); 51 | } 52 | }; 53 | 54 | /// \brief Interfaces with button C on the 3pi+ 32U4. 55 | /// 56 | /// The pin used for button C is also used for the RX LED. 57 | /// 58 | /// This class temporarily disables USB interrupts because the Arduino core code 59 | /// has USB interrupts enabled that sometimes write to the pin this button is 60 | /// on. 61 | /// 62 | /// This class temporarily sets the pin to be an input without a pull-up 63 | /// resistor. The pull-up resistor is not needed because of the resistors on 64 | /// the board. 65 | class ButtonC : public PushbuttonBase 66 | { 67 | public: 68 | /// The pin number for the pin conencted to button C on the 3pi+ 32U4. 69 | static const uint8_t buttonCPin = 17; 70 | 71 | virtual bool isPressed() 72 | { 73 | USBPause usbPause; 74 | FastGPIO::PinLoan loan; 75 | FastGPIO::Pin::setInputPulledUp(); 76 | _delay_us(3); 77 | return !FastGPIO::Pin::isInputHigh(); 78 | } 79 | }; 80 | 81 | } 82 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Buzzer.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4Buzzer.h 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace Pololu3piPlus32U4 10 | { 11 | 12 | /// \brief Plays beeps and music on the buzzer on the 3pi+ 32U4. 13 | /// 14 | /// This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on 15 | /// the 3pi+ 32U4 buzzer. 16 | /// 17 | /// Note durations are timed using a timer overflow interrupt (`TIMER4_OVF`), 18 | /// which will briefly interrupt execution of your main program at the frequency 19 | /// of the sound being played. In most cases, the interrupt-handling routine is 20 | /// very short (several microseconds). However, when playing a sequence of notes 21 | /// in `PLAY_AUTOMATIC` mode (the default mode) with the `play()` command, this 22 | /// interrupt takes much longer than normal (perhaps several hundred 23 | /// microseconds) every time it starts a new note. It is important to take this 24 | /// into account when writing timing-critical code. 25 | class Buzzer : public PololuBuzzer 26 | { 27 | // This is a trivial subclass of PololuBuzzer; it exists because we wanted 28 | // the class names to be consistent and we didn't just use a typedef 29 | // to define it because that would make the Doxygen documentation harder to 30 | // understand. 31 | }; 32 | 33 | } 34 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Encoders.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 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 | namespace Pololu3piPlus32U4 14 | { 15 | 16 | static volatile bool lastLeftA; 17 | static volatile bool lastLeftB; 18 | static volatile bool lastRightA; 19 | static volatile bool lastRightB; 20 | 21 | static volatile bool errorLeft; 22 | static volatile bool errorRight; 23 | 24 | // These count variables are uint16_t instead of int16_t because 25 | // signed integer overflow is undefined behavior in C++. 26 | static volatile uint16_t countLeft; 27 | static volatile uint16_t countRight; 28 | 29 | ISR(PCINT0_vect) 30 | { 31 | bool newLeftB = FastGPIO::Pin::isInputHigh(); 32 | bool newLeftA = FastGPIO::Pin::isInputHigh() ^ newLeftB; 33 | 34 | countLeft += (newLeftA ^ lastLeftB) - (lastLeftA ^ newLeftB); 35 | 36 | if((lastLeftA ^ newLeftA) & (lastLeftB ^ newLeftB)) 37 | { 38 | errorLeft = true; 39 | } 40 | 41 | lastLeftA = newLeftA; 42 | lastLeftB = newLeftB; 43 | } 44 | 45 | static void rightISR() 46 | { 47 | bool newRightB = FastGPIO::Pin::isInputHigh(); 48 | bool newRightA = FastGPIO::Pin::isInputHigh() ^ newRightB; 49 | 50 | countRight += (newRightA ^ lastRightB) - (lastRightA ^ newRightB); 51 | 52 | if((lastRightA ^ newRightA) & (lastRightB ^ newRightB)) 53 | { 54 | errorRight = true; 55 | } 56 | 57 | lastRightA = newRightA; 58 | lastRightB = newRightB; 59 | } 60 | 61 | void Encoders::init2() 62 | { 63 | // Set the pins as pulled-up inputs. 64 | FastGPIO::Pin::setInputPulledUp(); 65 | FastGPIO::Pin::setInputPulledUp(); 66 | FastGPIO::Pin::setInputPulledUp(); 67 | FastGPIO::Pin::setInputPulledUp(); 68 | 69 | // Enable pin-change interrupt on PB4 for left encoder, and disable other 70 | // pin-change interrupts. 71 | PCICR = (1 << PCIE0); 72 | PCMSK0 = (1 << PCINT4); 73 | PCIFR = (1 << PCIF0); // Clear its interrupt flag by writing a 1. 74 | 75 | // Enable interrupt on PE6 for the right encoder. We use attachInterrupt 76 | // instead of defining ISR(INT6_vect) ourselves so that this class will be 77 | // compatible with other code that uses attachInterrupt. 78 | attachInterrupt(4, rightISR, CHANGE); 79 | 80 | // Initialize the variables. It's good to do this after enabling the 81 | // interrupts in case the interrupts fired by accident as we were enabling 82 | // them. 83 | lastLeftB = FastGPIO::Pin::isInputHigh(); 84 | lastLeftA = FastGPIO::Pin::isInputHigh() ^ lastLeftB; 85 | countLeft = 0; 86 | errorLeft = 0; 87 | 88 | lastRightB = FastGPIO::Pin::isInputHigh(); 89 | lastRightA = FastGPIO::Pin::isInputHigh() ^ lastRightB; 90 | countRight = 0; 91 | errorRight = 0; 92 | } 93 | 94 | bool Encoders::flip; 95 | 96 | void Encoders::flipEncoders(bool f) 97 | { 98 | flip = f; 99 | } 100 | 101 | int16_t Encoders::getCountsLeft() 102 | { 103 | init(); 104 | 105 | cli(); 106 | int16_t counts = countLeft; 107 | sei(); 108 | return flip ? -counts : counts; 109 | } 110 | 111 | int16_t Encoders::getCountsRight() 112 | { 113 | init(); 114 | 115 | cli(); 116 | int16_t counts = countRight; 117 | sei(); 118 | return flip ? -counts : counts; 119 | } 120 | 121 | int16_t Encoders::getCountsAndResetLeft() 122 | { 123 | init(); 124 | 125 | cli(); 126 | int16_t counts = countLeft; 127 | countLeft = 0; 128 | sei(); 129 | return flip ? -counts : counts; 130 | } 131 | 132 | int16_t Encoders::getCountsAndResetRight() 133 | { 134 | init(); 135 | 136 | cli(); 137 | int16_t counts = countRight; 138 | countRight = 0; 139 | sei(); 140 | return flip ? -counts : counts; 141 | } 142 | 143 | bool Encoders::checkErrorLeft() 144 | { 145 | init(); 146 | 147 | bool error = errorLeft; 148 | errorLeft = 0; 149 | return error; 150 | } 151 | 152 | bool Encoders::checkErrorRight() 153 | { 154 | init(); 155 | 156 | bool error = errorRight; 157 | errorRight = 0; 158 | return error; 159 | } 160 | 161 | } 162 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Encoders.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4Encoders.h 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace Pololu3piPlus32U4 10 | { 11 | 12 | /// \brief Reads counts from the encoders on the 3pi+ 32U4. 13 | /// 14 | /// This class allows you to read counts from the encoders on the 3pi+ 32U4, 15 | /// which lets you tell how much each motor has turned and in what direction. 16 | /// 17 | /// The encoders are monitored in the background using interrupts, so your code 18 | /// can perform other tasks without missing encoder counts. 19 | /// 20 | /// To read the left encoder, this class uses an interrupt service routine (ISR) 21 | /// for PCINT0_vect, so there will be a compile-time conflict with any other 22 | /// code that defines a pin-change ISR. 23 | /// 24 | /// To read the right encoder, this class calls 25 | /// [attachInterrupt()](http://arduino.cc/en/Reference/attachInterrupt), so 26 | /// there will be a compile-time conflict with any other code that defines an 27 | /// ISR for an external interrupt directly instead of using attachInterrupt(). 28 | class Encoders 29 | { 30 | 31 | public: 32 | 33 | /// \brief Flips the direction of the encoders. 34 | /// 35 | /// This is useful if you have to flip the direction of the motors 36 | /// due to a non-standard gearbox. 37 | /// 38 | /// \param flip If true, the direction of counting will be 39 | /// reversed relative to the standard 3pi+ 32U4. 40 | static void flipEncoders(bool flip); 41 | 42 | /// \brief Initializes the encoders (called automatically). 43 | /// 44 | /// This function initializes the encoders if they have not been initialized 45 | /// already and starts listening for counts. This function is called 46 | /// automatically whenever you call any other function in this class, so you 47 | /// should not normally need to call it in your code. 48 | static void init() 49 | { 50 | static bool initialized = 0; 51 | if (!initialized) 52 | { 53 | initialized = true; 54 | init2(); 55 | } 56 | } 57 | 58 | /// \brief Returns the number of counts that have been detected from the 59 | /// left-side encoder. 60 | /// 61 | /// The count starts at 0. Positive counts correspond to forward movement 62 | /// of the left side of the 3pi+, while negative counts correspond to 63 | /// backwards movement. 64 | /// 65 | /// The count is returned as a signed 16-bit integer. When the count goes 66 | /// over 32767, it will overflow down to -32768. When the count goes below 67 | /// -32768, it will overflow up to 32767. 68 | static int16_t getCountsLeft(); 69 | 70 | /// \brief Returns the number of counts that have been detected from the 71 | /// right-side encoder. 72 | /// 73 | /// \sa getCountsLeft() 74 | static int16_t getCountsRight(); 75 | 76 | /// \brief Returns the number of counts that have been detected from the 77 | /// left-side encoder and clears the counts. 78 | /// 79 | /// This function is just like getCountsLeft() except it also clears the 80 | /// counts before returning. If you call this frequently enough, you will 81 | /// not have to worry about the count overflowing. 82 | static int16_t getCountsAndResetLeft(); 83 | 84 | /// \brief Returns the number of counts that have been detected from the 85 | /// left-side encoder and clears the counts. 86 | /// 87 | /// \sa getCountsAndResetLeft() 88 | static int16_t getCountsAndResetRight(); 89 | 90 | /// \brief Returns true if an error was detected on the left-side encoder. 91 | /// 92 | /// This function resets the error flag automatically, so it will only 93 | /// return true if an error was detected since the last time 94 | /// checkErrorLeft() was called. 95 | /// 96 | /// If an error happens, it means that both of the encoder outputs changed 97 | /// at the same time from the perspective of the ISR, so the ISR was unable 98 | /// to tell what direction the motor was moving, and the encoder count could 99 | /// be inaccurate. The most likely cause for an error is that the interrupt 100 | /// service routine for the encoders could not be started soon enough. If 101 | /// you get encoder errors, make sure you are not disabling interrupts for 102 | /// extended periods of time in your code. 103 | static bool checkErrorLeft(); 104 | 105 | /// \brief Returns true if an error was detected on the right-side encoder. 106 | /// 107 | /// \sa checkErrorLeft() 108 | static bool checkErrorRight(); 109 | 110 | private: 111 | 112 | static void init2(); 113 | static bool flip; 114 | }; 115 | 116 | } 117 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4IMU.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | #include 4 | 5 | #define LSM6DS33_WHO_ID 0x69 6 | #define LIS3MDL_WHO_ID 0x3D 7 | 8 | namespace Pololu3piPlus32U4 9 | { 10 | 11 | bool IMU::init() 12 | { 13 | if (testReg(LSM6DS33_ADDR, LSM6DS33_REG_WHO_AM_I) == LSM6DS33_WHO_ID && 14 | testReg( LIS3MDL_ADDR, LIS3MDL_REG_WHO_AM_I) == LIS3MDL_WHO_ID) 15 | { 16 | type = IMUType::LSM6DS33_LIS3MDL; 17 | return true; 18 | } 19 | else 20 | { 21 | return false; 22 | } 23 | } 24 | 25 | void IMU::enableDefault() 26 | { 27 | switch (type) 28 | { 29 | case IMUType::LSM6DS33_LIS3MDL: 30 | 31 | // Accelerometer 32 | 33 | // 0x30 = 0b00110000 34 | // ODR = 0011 (52 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale) 35 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x30); 36 | if (lastError) { return; } 37 | 38 | // Gyro 39 | 40 | // 0x50 = 0b01010000 41 | // ODR = 0101 (208 Hz (high performance)); FS_G = 00 (+/- 245 dps full scale) 42 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x50); 43 | if (lastError) { return; } 44 | 45 | // Accelerometer + Gyro 46 | 47 | // 0x04 = 0b00000100 48 | // IF_INC = 1 (automatically increment register address) 49 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL3_C, 0x04); 50 | if (lastError) { return; } 51 | 52 | // Magnetometer 53 | 54 | // 0x70 = 0b01110000 55 | // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR) 56 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG1, 0x70); 57 | if (lastError) { return; } 58 | 59 | // 0x00 = 0b00000000 60 | // FS = 00 (+/- 4 gauss full scale) 61 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG2, 0x00); 62 | if (lastError) { return; } 63 | 64 | // 0x00 = 0b00000000 65 | // MD = 00 (continuous-conversion mode) 66 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG3, 0x00); 67 | if (lastError) { return; } 68 | 69 | // 0x0C = 0b00001100 70 | // OMZ = 11 (ultra-high-performance mode for Z) 71 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG4, 0x0C); 72 | return; 73 | default: 74 | return; 75 | } 76 | } 77 | 78 | void IMU::configureForTurnSensing() 79 | { 80 | switch (type) 81 | { 82 | case IMUType::LSM6DS33_LIS3MDL: 83 | 84 | // Gyro 85 | 86 | // 0x7C = 0b01111100 87 | // ODR = 0111 (833 Hz (high performance)); FS_G = 11 (+/- 2000 dps full scale) 88 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL2_G, 0x7C); 89 | return; 90 | default: 91 | return; 92 | } 93 | } 94 | 95 | void IMU::configureForFaceUphill() 96 | { 97 | switch (type) 98 | { 99 | case IMUType::LSM6DS33_LIS3MDL: 100 | 101 | // Accelerometer 102 | 103 | // 0x10 = 0b00010000 104 | // ODR = 0001 (13 Hz (high performance)); FS_XL = 00 (+/- 2 g full scale) 105 | writeReg(LSM6DS33_ADDR, LSM6DS33_REG_CTRL1_XL, 0x10); 106 | return; 107 | default: 108 | return; 109 | } 110 | } 111 | 112 | void IMU::configureForCompassHeading() 113 | { 114 | switch (type) 115 | { 116 | case IMUType::LSM6DS33_LIS3MDL: 117 | 118 | // Magnetometer 119 | 120 | // 0x7C = 0b01111100 121 | // OM = 11 (ultra-high-performance mode for X and Y); DO = 111 (80 Hz ODR) 122 | writeReg(LIS3MDL_ADDR, LIS3MDL_REG_CTRL_REG1, 0x7C); 123 | return; 124 | default: 125 | return; 126 | } 127 | } 128 | 129 | // Reads the 3 accelerometer channels and stores them in vector a 130 | void IMU::readAcc(void) 131 | { 132 | switch (type) 133 | { 134 | case IMUType::LSM6DS33_LIS3MDL: 135 | // assumes register address auto-increment is enabled (IF_INC in CTRL3_C) 136 | readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_XL, a); 137 | return; 138 | default: 139 | return; 140 | } 141 | } 142 | 143 | // Reads the 3 gyro channels and stores them in vector g 144 | void IMU::readGyro() 145 | { 146 | switch (type) 147 | { 148 | case IMUType::LSM6DS33_LIS3MDL: 149 | // assumes register address auto-increment is enabled (IF_INC in CTRL3_C) 150 | readAxes16Bit(LSM6DS33_ADDR, LSM6DS33_REG_OUTX_L_G, g); 151 | return; 152 | default: 153 | return; 154 | } 155 | } 156 | 157 | // Reads the 3 magnetometer channels and stores them in vector m 158 | void IMU::readMag() 159 | { 160 | switch (type) 161 | { 162 | case IMUType::LSM6DS33_LIS3MDL: 163 | // set MSB of register address for auto-increment 164 | readAxes16Bit(LIS3MDL_ADDR, LIS3MDL_REG_OUT_X_L | (1 << 7), m); 165 | return; 166 | default: 167 | return; 168 | } 169 | } 170 | 171 | // Reads all 9 accelerometer, gyro, and magnetometer channels and stores them 172 | // in the respective vectors 173 | void IMU::read() 174 | { 175 | readAcc(); 176 | if (lastError) { return; } 177 | readGyro(); 178 | if (lastError) { return; } 179 | readMag(); 180 | } 181 | 182 | bool IMU::accDataReady() 183 | { 184 | switch (type) 185 | { 186 | case IMUType::LSM6DS33_LIS3MDL: 187 | return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x01; 188 | default: 189 | return false; 190 | } 191 | } 192 | 193 | bool IMU::gyroDataReady() 194 | { 195 | switch (type) 196 | { 197 | case IMUType::LSM6DS33_LIS3MDL: 198 | return readReg(LSM6DS33_ADDR, LSM6DS33_REG_STATUS_REG) & 0x02; 199 | default: 200 | return false; 201 | } 202 | } 203 | 204 | bool IMU::magDataReady() 205 | { 206 | switch (type) 207 | { 208 | case IMUType::LSM6DS33_LIS3MDL: 209 | return readReg(LIS3MDL_ADDR, LIS3MDL_REG_STATUS_REG) & 0x08; 210 | default: 211 | return false; 212 | } 213 | } 214 | 215 | } 216 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4IMU.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4IMU.h 4 | /// 5 | /// \brief Include this file in one of your cpp/ino files for IMU 6 | /// functionality. 7 | /// 8 | /// The IMU is not fully enabled by default since it depends on the 9 | /// Wire library, which uses about 1400 bytes of additional code space 10 | /// and defines an interrupt service routine (ISR) that might be 11 | /// incompatible with some applications (such as our TWISlave example). 12 | 13 | #pragma once 14 | #include 15 | #include 16 | 17 | namespace Pololu3piPlus32U4 18 | { 19 | 20 | void IMU::writeReg(uint8_t addr, uint8_t reg, uint8_t value) 21 | { 22 | Wire.beginTransmission(addr); 23 | Wire.write(reg); 24 | Wire.write(value); 25 | lastError = Wire.endTransmission(); 26 | } 27 | 28 | uint8_t IMU::readReg(uint8_t addr, uint8_t reg) 29 | { 30 | Wire.beginTransmission(addr); 31 | Wire.write(reg); 32 | lastError = Wire.endTransmission(); 33 | if (lastError) { return 0; } 34 | 35 | uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1); 36 | if (byteCount != 1) 37 | { 38 | lastError = 50; 39 | return 0; 40 | } 41 | return Wire.read(); 42 | } 43 | 44 | int16_t IMU::testReg(uint8_t addr, uint8_t reg) 45 | { 46 | Wire.beginTransmission(addr); 47 | Wire.write(reg); 48 | if (Wire.endTransmission() != 0) 49 | { 50 | return -1; 51 | } 52 | 53 | uint8_t byteCount = Wire.requestFrom(addr, (uint8_t)1); 54 | if (byteCount != 1) 55 | { 56 | return -1; 57 | } 58 | return Wire.read(); 59 | } 60 | 61 | void IMU::readAxes16Bit(uint8_t addr, uint8_t firstReg, vector & v) 62 | { 63 | Wire.beginTransmission(addr); 64 | Wire.write(firstReg); 65 | lastError = Wire.endTransmission(); 66 | if (lastError) { return; } 67 | 68 | uint8_t byteCount = (Wire.requestFrom(addr, (uint8_t)6)); 69 | if (byteCount != 6) 70 | { 71 | lastError = 50; 72 | return; 73 | } 74 | uint8_t xl = Wire.read(); 75 | uint8_t xh = Wire.read(); 76 | uint8_t yl = Wire.read(); 77 | uint8_t yh = Wire.read(); 78 | uint8_t zl = Wire.read(); 79 | uint8_t zh = Wire.read(); 80 | 81 | // combine high and low bytes 82 | v.x = (int16_t)(xh << 8 | xl); 83 | v.y = (int16_t)(yh << 8 | yl); 84 | v.z = (int16_t)(zh << 8 | zl); 85 | } 86 | 87 | } 88 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4IMU_declaration.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4IMU_declaration.h 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace Pololu3piPlus32U4 10 | { 11 | 12 | 13 | /// \anchor device_addresses 14 | /// 15 | /// \name Device Addresses 16 | /// \{ 17 | #define LSM6DS33_ADDR 0b1101011 18 | #define LIS3MDL_ADDR 0b0011110 19 | /// \} 20 | 21 | /// \anchor register_addresses 22 | /// 23 | /// \name Register Addresses 24 | /// \{ 25 | #define LSM6DS33_REG_WHO_AM_I 0x0F 26 | #define LSM6DS33_REG_CTRL1_XL 0x10 27 | #define LSM6DS33_REG_CTRL2_G 0x11 28 | #define LSM6DS33_REG_CTRL3_C 0x12 29 | #define LSM6DS33_REG_STATUS_REG 0x1E 30 | #define LSM6DS33_REG_OUTX_L_G 0x22 31 | #define LSM6DS33_REG_OUTX_L_XL 0x28 32 | 33 | #define LIS3MDL_REG_WHO_AM_I 0x0F 34 | #define LIS3MDL_REG_CTRL_REG1 0x20 35 | #define LIS3MDL_REG_CTRL_REG2 0x21 36 | #define LIS3MDL_REG_CTRL_REG3 0x22 37 | #define LIS3MDL_REG_CTRL_REG4 0x23 38 | #define LIS3MDL_REG_STATUS_REG 0x27 39 | #define LIS3MDL_REG_OUT_X_L 0x28 40 | /// \} 41 | 42 | /// \brief The type of the inertial sensors. 43 | enum class IMUType : uint8_t { 44 | /// Unknown or unrecognized 45 | Unknown, 46 | /// LSM6DS33 gyro + accelerometer, LIS3MDL magnetometer 47 | LSM6DS33_LIS3MDL 48 | }; 49 | 50 | /// \brief Interfaces with the inertial sensors on the 3pi+ 32U4. 51 | /// 52 | /// This class allows you to configure and get readings from the I2C sensors 53 | /// that make up the 3pi+ 32U4's inertial measurement unit (IMU): gyro, 54 | /// accelerometer, and magnetometer. 55 | /// 56 | /// The IMU is not fully enabled by default since it depends on the Wire 57 | /// library, which uses about 1400 bytes of additional code space and defines an 58 | /// interrupt service routine (ISR) that might be incompatible with some 59 | /// applications (such as our TWISlave example). To use the IMU, include the 60 | /// file Pololu3piPlus32U4IMU.h in one of your cpp/ino files for IMU 61 | /// functionality (**not** Pololu3piPlus32U4IMU_declaration.h, where this class 62 | /// is declared). 63 | /// 64 | /// You must call `Wire.start()` before using any of this library's functions 65 | /// that access the sensors. 66 | class IMU 67 | { 68 | public: 69 | 70 | /// Represents a 3-dimensional vector with x, y, and z components. 71 | template struct vector 72 | { 73 | T x, y, z; 74 | }; 75 | 76 | /// Raw accelerometer readings. 77 | vector a = {0, 0, 0}; 78 | 79 | /// Raw gyro readings. 80 | vector g = {0, 0, 0}; 81 | 82 | /// Raw magnetometer readings. 83 | vector m = {0, 0, 0}; 84 | 85 | /// \brief Returns 0 if the last I2C communication with the IMU was 86 | /// successful, or a non-zero status code if there was an error. 87 | uint8_t getLastError() { return lastError; } 88 | 89 | /// \brief Initializes the inertial sensors and detects their type. 90 | /// 91 | /// \return True if the sensor type was detected succesfully; false otherwise. 92 | bool init(); 93 | 94 | /// \brief Returns the type of the inertial sensors on the 3pi+ 32U4. 95 | /// 96 | /// \return The sensor type as a member of the IMUType enum. If the 97 | /// type is not known (e.g. if init() has not been called yet), this will be 98 | /// IMUType::Unknown. 99 | IMUType getType() { return type; } 100 | 101 | /// \brief Enables all of the inertial sensors with a default configuration. 102 | void enableDefault(); 103 | 104 | /// \brief Configures the sensors with settings optimized for turn sensing. 105 | void configureForTurnSensing(); 106 | 107 | /// \brief Configures the sensors with settings optimized for the FaceUphill 108 | /// example program. 109 | void configureForFaceUphill(); 110 | 111 | /// \brief Configures the sensors with settings optimized for determining a 112 | /// compass heading with the magnetometer. 113 | void configureForCompassHeading(); 114 | 115 | /// \brief Writes an 8-bit sensor register. 116 | /// 117 | /// \param addr Device address. 118 | /// \param reg Register address. 119 | /// \param value The 8-bit register value to be written. 120 | void writeReg(uint8_t addr, uint8_t reg, uint8_t value); 121 | 122 | /// \brief Reads an 8-bit sensor register. 123 | /// 124 | /// \param addr Device address. 125 | /// \param reg Register address. 126 | /// 127 | /// \return The 8-bit register value read from the device. 128 | uint8_t readReg(uint8_t addr, uint8_t reg); 129 | 130 | /// \brief Takes a reading from the accelerometer and makes the measurements 131 | /// available in #a. 132 | void readAcc(); 133 | 134 | /// \brief Takes a reading from the gyro and makes the measurements available 135 | /// in #g. 136 | void readGyro(); 137 | 138 | /// \brief Takes a reading from the magnetometer and makes the measurements 139 | /// available in #m. 140 | void readMag(); 141 | 142 | /// \brief Takes a reading from all three sensors (accelerometer, gyro, and 143 | /// magnetometer) and makes their measurements available in the respective 144 | /// vectors. 145 | void read(); 146 | 147 | /// \brief Indicates whether the accelerometer has new measurement data ready. 148 | /// 149 | /// \return True if there is new accelerometer data available; false 150 | /// otherwise. 151 | bool accDataReady(); 152 | 153 | /// \brief Indicates whether the gyro has new measurement data ready. 154 | /// 155 | /// \return True if there is new gyro data available; false otherwise. 156 | bool gyroDataReady(); 157 | 158 | /// \brief Indicates whether the magnetometer has new measurement data ready. 159 | /// 160 | /// \return True if there is new magnetometer data available; false otherwise. 161 | bool magDataReady(); 162 | 163 | private: 164 | 165 | uint8_t lastError = 0; 166 | IMUType type = IMUType::Unknown; 167 | 168 | int16_t testReg(uint8_t addr, uint8_t reg); 169 | void readAxes16Bit(uint8_t addr, uint8_t firstReg, vector & v); 170 | }; 171 | 172 | } 173 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4LCD.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4LCD.h 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace Pololu3piPlus32U4 12 | { 13 | 14 | /// \brief Writes data to the LCD on the 3pi+ 32U4. 15 | /// 16 | /// This library is similar to the Arduino 17 | /// [LiquidCrystal library](http://arduino.cc/en/Reference/LiquidCrystal), 18 | /// but it has some extra features needed on the 3pi+ 32U4: 19 | /// 20 | /// * This class disables USB interrupts temporarily while writing to the LCD so 21 | /// that the USB interrupts will not change the RXLED and TXLED pins, which 22 | /// double as LCD data lines. 23 | /// * This class restores the RS, DB4, DB5, DB6, and DB7 pins to their previous 24 | /// states when it is done using them so that those pins can also be used for 25 | /// other purposes such as controlling LEDs. 26 | /// 27 | /// This class inherits from the Arduino Print class, so you can call the 28 | /// `print()` function on it with a variety of arguments. See the 29 | /// [Arduino print() documentation](http://arduino.cc/en/Serial/Print) 30 | /// for more information. 31 | /// 32 | /// For detailed information about HD44780 LCD interface, including what 33 | /// characters can be displayed, see the 34 | /// [HD44780 datasheet](http://www.pololu.com/file/0J72/HD44780.pdf). 35 | class LCD : public PololuHD44780Base 36 | { 37 | // Pin assignments 38 | static const uint8_t rs = 0, e = 1, db4 = 14, db5 = 17, db6 = 13, db7 = IO_D5; 39 | 40 | public: 41 | 42 | virtual void initPins() 43 | { 44 | FastGPIO::Pin::setOutputLow(); 45 | } 46 | 47 | virtual void send(uint8_t data, bool rsValue, bool only4bits) 48 | { 49 | // Temporarily disable USB interrupts because they write some pins 50 | // we are using as LCD pins. 51 | USBPause usbPause; 52 | 53 | // Save the state of the RS and data pins. The state automatically 54 | // gets restored before this function returns. 55 | FastGPIO::PinLoan loanRS; 56 | FastGPIO::PinLoan loanDB4; 57 | FastGPIO::PinLoan loanDB5; 58 | FastGPIO::PinLoan loanDB6; 59 | FastGPIO::PinLoan loanDB7; 60 | 61 | // Drive the RS pin high or low. 62 | FastGPIO::Pin::setOutput(rsValue); 63 | 64 | // Send the data. 65 | if (!only4bits) { sendNibble(data >> 4); } 66 | sendNibble(data & 0x0F); 67 | } 68 | 69 | /// This does nothing, but it is here for compatibility with 70 | /// the OLED class. It lets us write examples that work with 71 | /// either class. 72 | void noAutoDisplay() {} 73 | 74 | private: 75 | 76 | void sendNibble(uint8_t data) 77 | { 78 | FastGPIO::Pin::setOutput(data >> 0 & 1); 79 | FastGPIO::Pin::setOutput(data >> 1 & 1); 80 | FastGPIO::Pin::setOutput(data >> 2 & 1); 81 | FastGPIO::Pin::setOutput(data >> 3 & 1); 82 | 83 | FastGPIO::Pin::setOutputHigh(); 84 | _delay_us(1); // Must be at least 450 ns. 85 | FastGPIO::Pin::setOutputLow(); 86 | _delay_us(1); // Must be at least 550 ns. 87 | } 88 | }; 89 | 90 | } 91 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4LineSensors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | #include 4 | 5 | namespace Pololu3piPlus32U4 6 | { 7 | 8 | void LineSensors::setTimeout(uint16_t timeout) 9 | { 10 | if (timeout > 32767) { timeout = 32767; } 11 | _timeout = timeout; 12 | _maxValue = timeout; 13 | } 14 | 15 | void LineSensors::resetCalibration() 16 | { 17 | for (uint8_t i = 0; i < _sensorCount; i++) 18 | { 19 | if (calibrationOn.maximum) { calibrationOn.maximum[i] = 0; } 20 | if (calibrationOff.maximum) { calibrationOff.maximum[i] = 0; } 21 | if (calibrationOn.minimum) { calibrationOn.minimum[i] = _maxValue; } 22 | if (calibrationOff.minimum) { calibrationOff.minimum[i] = _maxValue; } 23 | } 24 | } 25 | 26 | void LineSensors::calibrate(LineSensorsReadMode mode) 27 | { 28 | // manual emitter control is not supported 29 | if (mode == LineSensorsReadMode::Manual) { return; } 30 | 31 | if (mode == LineSensorsReadMode::On) 32 | { 33 | calibrateOnOrOff(calibrationOn, LineSensorsReadMode::On); 34 | } 35 | 36 | if (mode == LineSensorsReadMode::Off) 37 | { 38 | calibrateOnOrOff(calibrationOff, LineSensorsReadMode::Off); 39 | } 40 | } 41 | 42 | void LineSensors::calibrateOnOrOff(CalibrationData & calibration, LineSensorsReadMode mode) 43 | { 44 | uint16_t sensorValues[_sensorCount]; 45 | uint16_t maxSensorValues[_sensorCount]; 46 | uint16_t minSensorValues[_sensorCount]; 47 | 48 | // (Re)allocate and initialize the arrays if necessary. 49 | if (!calibration.initialized) 50 | { 51 | uint16_t * oldMaximum = calibration.maximum; 52 | calibration.maximum = (uint16_t *)realloc(calibration.maximum, 53 | sizeof(uint16_t) * _sensorCount); 54 | if (calibration.maximum == nullptr) 55 | { 56 | // Memory allocation failed; don't continue. 57 | free(oldMaximum); // deallocate any memory used by old array 58 | return; 59 | } 60 | 61 | uint16_t * oldMinimum = calibration.minimum; 62 | calibration.minimum = (uint16_t *)realloc(calibration.minimum, 63 | sizeof(uint16_t) * _sensorCount); 64 | if (calibration.minimum == nullptr) 65 | { 66 | // Memory allocation failed; don't continue. 67 | free(oldMinimum); // deallocate any memory used by old array 68 | return; 69 | } 70 | 71 | // Initialize the max and min calibrated values to values that 72 | // will cause the first reading to update them. 73 | for (uint8_t i = 0; i < _sensorCount; i++) 74 | { 75 | calibration.maximum[i] = 0; 76 | calibration.minimum[i] = _maxValue; 77 | } 78 | 79 | calibration.initialized = true; 80 | } 81 | 82 | for (uint8_t j = 0; j < 10; j++) 83 | { 84 | read(sensorValues, mode); 85 | 86 | for (uint8_t i = 0; i < _sensorCount; i++) 87 | { 88 | // set the max we found THIS time 89 | if ((j == 0) || (sensorValues[i] > maxSensorValues[i])) 90 | { 91 | maxSensorValues[i] = sensorValues[i]; 92 | } 93 | 94 | // set the min we found THIS time 95 | if ((j == 0) || (sensorValues[i] < minSensorValues[i])) 96 | { 97 | minSensorValues[i] = sensorValues[i]; 98 | } 99 | } 100 | } 101 | 102 | // record the min and max calibration values 103 | for (uint8_t i = 0; i < _sensorCount; i++) 104 | { 105 | // Update maximum only if the min of 10 readings was still higher than it 106 | // (we got 10 readings in a row higher than the existing maximum). 107 | if (minSensorValues[i] > calibration.maximum[i]) 108 | { 109 | calibration.maximum[i] = minSensorValues[i]; 110 | } 111 | 112 | // Update minimum only if the max of 10 readings was still lower than it 113 | // (we got 10 readings in a row lower than the existing minimum). 114 | if (maxSensorValues[i] < calibration.minimum[i]) 115 | { 116 | calibration.minimum[i] = maxSensorValues[i]; 117 | } 118 | } 119 | } 120 | 121 | void LineSensors::read(uint16_t * sensorValues, LineSensorsReadMode mode) 122 | { 123 | switch (mode) 124 | { 125 | case LineSensorsReadMode::Off: 126 | emittersOff(); 127 | readPrivate(sensorValues); 128 | return; 129 | 130 | case LineSensorsReadMode::Manual: 131 | readPrivate(sensorValues); 132 | return; 133 | 134 | case LineSensorsReadMode::On: 135 | emittersOn(); 136 | readPrivate(sensorValues); 137 | emittersOff(); 138 | return; 139 | 140 | default: // invalid - do nothing 141 | return; 142 | } 143 | } 144 | 145 | void LineSensors::readCalibrated(uint16_t * sensorValues, LineSensorsReadMode mode) 146 | { 147 | // manual emitter control is not supported 148 | if (mode == LineSensorsReadMode::Manual) { return; } 149 | 150 | // if not calibrated, do nothing 151 | 152 | if (mode == LineSensorsReadMode::On) 153 | { 154 | if (!calibrationOn.initialized) 155 | { 156 | return; 157 | } 158 | } 159 | 160 | if (mode == LineSensorsReadMode::Off) 161 | { 162 | if (!calibrationOff.initialized) 163 | { 164 | return; 165 | } 166 | } 167 | 168 | // read the needed values 169 | read(sensorValues, mode); 170 | 171 | for (uint8_t i = 0; i < _sensorCount; i++) 172 | { 173 | uint16_t calmin, calmax; 174 | 175 | // find the correct calibration 176 | if (mode == LineSensorsReadMode::On) 177 | { 178 | calmax = calibrationOn.maximum[i]; 179 | calmin = calibrationOn.minimum[i]; 180 | } 181 | else if (mode == LineSensorsReadMode::Off) 182 | { 183 | calmax = calibrationOff.maximum[i]; 184 | calmin = calibrationOff.minimum[i]; 185 | } 186 | 187 | uint16_t denominator = calmax - calmin; 188 | int16_t value = 0; 189 | 190 | if (denominator != 0) 191 | { 192 | value = (((int32_t)sensorValues[i]) - calmin) * 1000 / denominator; 193 | } 194 | 195 | if (value < 0) { value = 0; } 196 | else if (value > 1000) { value = 1000; } 197 | 198 | sensorValues[i] = value; 199 | } 200 | } 201 | 202 | uint16_t LineSensors::readLinePrivate(uint16_t * sensorValues, LineSensorsReadMode mode, 203 | bool invertReadings) 204 | { 205 | bool onLine = false; 206 | uint32_t avg = 0; // this is for the weighted total 207 | uint16_t sum = 0; // this is for the denominator, which is <= 64000 208 | 209 | // manual emitter control is not supported 210 | if (mode == LineSensorsReadMode::Manual) { return 0; } 211 | 212 | readCalibrated(sensorValues, mode); 213 | 214 | for (uint8_t i = 0; i < _sensorCount; i++) 215 | { 216 | uint16_t value = sensorValues[i]; 217 | if (invertReadings) { value = 1000 - value; } 218 | 219 | // keep track of whether we see the line at all 220 | if (value > 200) { onLine = true; } 221 | 222 | // only average in values that are above a noise threshold 223 | if (value > 50) 224 | { 225 | avg += (uint32_t)value * (i * 1000); 226 | sum += value; 227 | } 228 | } 229 | 230 | if (!onLine) 231 | { 232 | // If it last read to the left of center, return 0. 233 | if (_lastPosition < (_sensorCount - 1) * 1000 / 2) 234 | { 235 | return 0; 236 | } 237 | // If it last read to the right of center, return the max. 238 | else 239 | { 240 | return (_sensorCount - 1) * 1000; 241 | } 242 | } 243 | 244 | _lastPosition = avg / sum; 245 | return _lastPosition; 246 | } 247 | 248 | // the destructor frees up allocated memory 249 | LineSensors::~LineSensors() 250 | { 251 | if (calibrationOn.maximum) { free(calibrationOn.maximum); } 252 | if (calibrationOff.maximum) { free(calibrationOff.maximum); } 253 | if (calibrationOn.minimum) { free(calibrationOn.minimum); } 254 | if (calibrationOff.minimum) { free(calibrationOff.minimum); } 255 | } 256 | 257 | void LineSensors::readPrivate(uint16_t * sensorValues) 258 | { 259 | FastGPIO::Pin::setOutputHigh(); 260 | FastGPIO::Pin::setOutputHigh(); 261 | FastGPIO::Pin::setOutputHigh(); 262 | FastGPIO::Pin::setOutputHigh(); 263 | FastGPIO::Pin::setOutputHigh(); 264 | _delay_us(10); 265 | 266 | sensorValues[0] = _timeout; 267 | sensorValues[1] = _timeout; 268 | sensorValues[2] = _timeout; 269 | sensorValues[3] = _timeout; 270 | sensorValues[4] = _timeout; 271 | 272 | noInterrupts(); 273 | uint16_t startTime = micros(); 274 | FastGPIO::Pin::setInput(); 275 | FastGPIO::Pin::setInput(); 276 | FastGPIO::Pin::setInput(); 277 | FastGPIO::Pin::setInput(); 278 | FastGPIO::Pin::setInput(); 279 | interrupts(); 280 | 281 | uint16_t time = 0; 282 | while (true) 283 | { 284 | noInterrupts(); 285 | time = micros() - startTime; 286 | if (time >= _timeout) 287 | { 288 | interrupts(); 289 | break; 290 | } 291 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[0]) { sensorValues[0] = time; } 292 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[1]) { sensorValues[1] = time; } 293 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[2]) { sensorValues[2] = time; } 294 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[3]) { sensorValues[3] = time; } 295 | if (!FastGPIO::Pin::isInputHigh() && time < sensorValues[4]) { sensorValues[4] = time; } 296 | interrupts(); 297 | __builtin_avr_delay_cycles(4); // allow interrupts to run 298 | } 299 | } 300 | 301 | } 302 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4LineSensors.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4LineSensors.h 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace Pololu3piPlus32U4 11 | { 12 | 13 | /// \brief Emitter behavior when taking readings. 14 | enum class LineSensorsReadMode : uint8_t { 15 | /// Each reading is made without turning on the infrared (IR) emitters. The 16 | /// reading represents ambient light levels near the sensor. 17 | Off, 18 | 19 | /// Each reading is made with the emitters on. The reading is a measure of 20 | /// reflectance. 21 | On, 22 | 23 | /// Calling read() with this mode prevents it from automatically controlling 24 | /// the emitters: they are left in their existing states, which allows manual 25 | /// control of the emitters for testing and advanced use. Calibrating and 26 | /// obtaining calibrated readings are not supported with this mode. 27 | Manual 28 | }; 29 | 30 | /// \brief Gets readings from the five reflectance sensors on the bottom of the 31 | /// 3pi+ 32U4. 32 | /// 33 | /// The readLineBlack() and readLineWhite() methods will always return values 34 | /// that increase from left to right, with 0 corresponding to the leftmost 35 | /// sensor and 4000 corresponding to the rightmost sensor. 36 | class LineSensors 37 | { 38 | public: 39 | /// The 3pi+ 32U4 has 5 line sensors. 40 | static const uint8_t _sensorCount = 5; 41 | 42 | /// Default timeout for RC sensors (in microseconds). 43 | static const uint16_t defaultTimeout = 4000; 44 | 45 | static const uint8_t emitterPin = 11; 46 | static const uint8_t line0Pin = 12; 47 | static const uint8_t line1Pin = A0; 48 | static const uint8_t line2Pin = A2; 49 | static const uint8_t line3Pin = A3; 50 | static const uint8_t line4Pin = A4; 51 | 52 | ~LineSensors(); 53 | 54 | /// \brief Sets the timeout for RC sensors. 55 | /// 56 | /// \param timeout The length of time, in microseconds, beyond which you 57 | /// consider the sensor reading completely black. 58 | /// 59 | /// If the pulse length for a pin exceeds \p timeout, pulse timing will 60 | /// stop and the reading for that pin will be considered full black. It is 61 | /// recommended that you set \p timeout to be between 1000 and 3000 62 | /// µs, depending on factors like the height of your sensors and 63 | /// ambient lighting. This allows you to shorten the duration of a 64 | /// sensor-reading cycle while maintaining useful measurements of 65 | /// reflectance. The default timeout is 4000 µs. 66 | /// 67 | /// The maximum allowed timeout is 32767. 68 | void setTimeout(uint16_t timeout); 69 | 70 | /// \brief Returns the timeout. 71 | /// 72 | /// \return The RC sensor timeout in microseconds. 73 | /// 74 | /// See also setTimeout(). 75 | uint16_t getTimeout() { return _timeout; } 76 | 77 | /// \brief Reads the sensors for calibration. 78 | /// 79 | /// \param mode The emitter behavior during calibration, as a member of 80 | /// the ::LineSensorsReadMode enum. The default is LineSensorsReadMode::On. 81 | /// Manual emitter control with LineSensorsReadMode::Manual is not supported. 82 | /// 83 | /// This method reads the sensors 10 times and uses the results for 84 | /// calibration. The sensor values are not returned; instead, the maximum 85 | /// and minimum values found over time are stored in #calibrationOn and/or 86 | /// #calibrationOff for use by the readCalibrated() method. 87 | /// 88 | /// If the storage for the calibration values has not been initialized, 89 | /// this function will (re)allocate the arrays and initialize the maximum 90 | /// and minimum values to 0 and the maximum possible sensor reading, 91 | /// respectively, so that the very first calibration sensor reading will 92 | /// update both of them. 93 | /// 94 | /// Note that the `minimum` and `maximum` pointers in the CalibrationData 95 | /// structs will point to arrays of length five, 96 | /// and they will only be allocated when calibrate() is 97 | /// called. If you only calibrate with the emitters on, the calibration 98 | /// arrays that hold the off values will not be allocated (and vice versa). 99 | /// 100 | /// \if usage 101 | /// See \ref md_usage for more information and example code. 102 | /// \endif 103 | void calibrate(LineSensorsReadMode mode = LineSensorsReadMode::On); 104 | 105 | /// \brief Resets all calibration that has been done. 106 | void resetCalibration(); 107 | 108 | /// \brief Reads the raw sensor values into an array. 109 | /// 110 | /// \param[out] sensorValues A pointer to an array in which to store the 111 | /// raw sensor readings. There **MUST** be space in the array for five 112 | /// readings. 113 | /// 114 | /// \param mode The emitter behavior during the read, as a member of the 115 | /// ::LineSensorsReadMode enum. The default is LineSensorsReadMode::On. 116 | /// 117 | /// Example usage: 118 | /// ~~~{.cpp} 119 | /// uint16_t sensorValues[5]; 120 | /// lineSensors.read(sensorValues); 121 | /// ~~~ 122 | /// 123 | /// The values returned are a measure of the reflectance in abstract units, 124 | /// with higher values corresponding to lower reflectance (e.g. a black 125 | /// surface or a void). 126 | /// 127 | /// Analog sensors will return a raw value between 0 and 1023 (like 128 | /// Arduino's `analogRead()` function). 129 | /// 130 | /// RC sensors will return a raw value in microseconds between 0 and the 131 | /// timeout setting configured with setTimeout() (the default timeout is 132 | /// 2500 µs). 133 | /// 134 | /// \if usage 135 | /// See \ref md_usage for more information and example code. 136 | /// \endif 137 | void read(uint16_t * sensorValues, LineSensorsReadMode mode = LineSensorsReadMode::On); 138 | 139 | /// \brief Reads the sensors and provides calibrated values between 0 and 140 | /// 1000. 141 | /// 142 | /// \param[out] sensorValues A pointer to an array in which to store the 143 | /// calibrated sensor readings. There **MUST** be space in the array for 144 | /// all five values. 145 | /// 146 | /// \param mode The emitter behavior during the read, as a member of the 147 | /// ::LineSensorsReadMode enum. The default is LineSensorsReadMode::On. Manual 148 | /// emitter control with LineSensorsReadMode::Manual is not supported. 149 | /// 150 | /// 0 corresponds to the minimum value stored in #calibrationOn or 151 | /// #calibrationOff, depending on \p mode, and 1000 corresponds to the 152 | /// maximum value. Calibration values are typically obtained by calling 153 | /// calibrate(), and they are stored separately for each sensor, so that 154 | /// differences in the sensors are accounted for automatically. 155 | /// 156 | /// \if usage 157 | /// See \ref md_usage for more information and example code. 158 | /// \endif 159 | void readCalibrated(uint16_t * sensorValues, LineSensorsReadMode mode = LineSensorsReadMode::On); 160 | 161 | /// \brief Reads the sensors, provides calibrated values, and returns an 162 | /// estimated black line position. 163 | /// 164 | /// \param[out] sensorValues A pointer to an array in which to store the 165 | /// calibrated sensor readings. There **MUST** be space in the array for 166 | /// five values. 167 | /// 168 | /// \param mode The emitter behavior during the read, as a member of the 169 | /// ::LineSensorsReadMode enum. The default is LineSensorsReadMode::On. Manual 170 | /// emitter control with LineSensorsReadMode::Manual is not supported. 171 | /// 172 | /// \return An estimate of the position of a black line under the sensors. 173 | /// 174 | /// The estimate is made using a weighted average of the sensor indices 175 | /// multiplied by 1000, so that a return value of 0 indicates that the line 176 | /// is directly below sensor 0, a return value of 1000 indicates that the 177 | /// line is directly below sensor 1, 2000 indicates that it's below sensor 178 | /// 2000, etc. Intermediate values indicate that the line is between two 179 | /// sensors. The formula is (where \f$v_0\f$ represents the value from the 180 | /// first sensor): 181 | /// 182 | /// \f[ 183 | /// {(0 \times v_0) + (1000 \times v_1) + (2000 \times v_2) + \cdots 184 | /// \over 185 | /// v_0 + v_1 + v_2 + \cdots} 186 | /// \f] 187 | /// 188 | /// As long as your sensors aren’t spaced too far apart relative to the 189 | /// line, this returned value is designed to be monotonic, which makes it 190 | /// great for use in closed-loop PID control. Additionally, this method 191 | /// remembers where it last saw the line, so if you ever lose the line to 192 | /// the left or the right, its line position will continue to indicate the 193 | /// direction you need to go to reacquire the line. For example, if sensor 194 | /// 4 is your rightmost sensor and you end up completely off the line to 195 | /// the left, this function will continue to return 4000. 196 | /// 197 | /// This function is intended to detect a black (or dark-colored) line on a 198 | /// white (or light-colored) background. For a white line, see 199 | /// readLineWhite(). 200 | /// 201 | /// \if usage 202 | /// See \ref md_usage for more information and example code. 203 | /// \endif 204 | uint16_t readLineBlack(uint16_t * sensorValues, LineSensorsReadMode mode = LineSensorsReadMode::On) 205 | { 206 | return readLinePrivate(sensorValues, mode, false); 207 | } 208 | 209 | /// \brief Reads the sensors, provides calibrated values, and returns an 210 | /// estimated white line position. 211 | /// 212 | /// \param[out] sensorValues A pointer to an array in which to store the 213 | /// calibrated sensor readings. There **MUST** be space in the array for 214 | /// five values. 215 | /// 216 | /// \param mode The emitter behavior during the read, as a member of the 217 | /// ::LineSensorsReadMode enum. The default is LineSensorsReadMode::On. Manual 218 | /// emitter control with LineSensorsReadMode::Manual is not supported. 219 | /// 220 | /// \return An estimate of the position of a white line under the sensors. 221 | /// 222 | /// This function is intended to detect a white (or light-colored) line on 223 | /// a black (or dark-colored) background. For a black line, see 224 | /// readLineBlack(). 225 | /// 226 | /// \if usage 227 | /// See \ref md_usage for more information and example code. 228 | /// \endif 229 | uint16_t readLineWhite(uint16_t * sensorValues, LineSensorsReadMode mode = LineSensorsReadMode::On) 230 | { 231 | return readLinePrivate(sensorValues, mode, true); 232 | } 233 | 234 | 235 | /// \brief Stores sensor calibration data. 236 | /// 237 | /// See calibrate() and readCalibrated() for details. 238 | struct CalibrationData 239 | { 240 | /// Whether array pointers have been allocated and initialized. 241 | bool initialized = false; 242 | /// Lowest readings seen during calibration. 243 | uint16_t * minimum = nullptr; 244 | /// Highest readings seen during calibration. 245 | uint16_t * maximum = nullptr; 246 | }; 247 | 248 | /// \name Calibration data 249 | /// 250 | /// See calibrate() and readCalibrated() for details. 251 | /// 252 | /// These variables are made public so that you can use them for your own 253 | /// calculations and do things like saving the values to EEPROM, performing 254 | /// sanity checking, etc. 255 | /// \{ 256 | 257 | /// Data from calibrating with emitters on. 258 | CalibrationData calibrationOn; 259 | 260 | /// Data from calibrating with emitters off. 261 | CalibrationData calibrationOff; 262 | 263 | /// \} 264 | 265 | /// \brief Turns the IR LEDs on. 266 | void emittersOn() 267 | { 268 | FastGPIO::Pin::setOutputHigh(); 269 | } 270 | 271 | /// \brief Turns the IR LEDs off. 272 | void emittersOff() 273 | { 274 | FastGPIO::Pin::setInput(); 275 | } 276 | 277 | private: 278 | 279 | // Handles the actual calibration, including (re)allocating and 280 | // initializing the storage for the calibration values if necessary. 281 | void calibrateOnOrOff(CalibrationData & calibration, LineSensorsReadMode mode); 282 | 283 | void readPrivate(uint16_t * sensorValues); 284 | 285 | uint16_t readLinePrivate(uint16_t * sensorValues, LineSensorsReadMode mode, bool invertReadings); 286 | 287 | uint16_t _timeout = defaultTimeout; 288 | uint16_t _maxValue = defaultTimeout; // the maximum value returned by readPrivate() 289 | uint16_t _lastPosition = 0; 290 | }; 291 | 292 | } 293 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Motors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace Pololu3piPlus32U4 8 | { 9 | 10 | #define PWM_L 10 11 | #define PWM_R 9 12 | #define DIR_L 16 13 | #define DIR_R 15 14 | 15 | bool Motors::flipLeft = false; 16 | bool Motors::flipRight = false; 17 | 18 | // initialize timer1 to generate the proper PWM outputs to the motor drivers 19 | void Motors::init2() 20 | { 21 | FastGPIO::Pin::setOutputLow(); 22 | FastGPIO::Pin::setOutputLow(); 23 | FastGPIO::Pin::setOutputLow(); 24 | FastGPIO::Pin::setOutputLow(); 25 | 26 | // Timer 1 configuration 27 | // prescaler: clockI/O / 1 28 | // outputs enabled 29 | // phase-correct PWM 30 | // top of 400 31 | // 32 | // PWM frequency calculation 33 | // 16MHz / 1 (prescaler) / 2 (phase-correct) / 400 (top) = 20kHz 34 | TCCR1A = 0b10100000; 35 | TCCR1B = 0b00010001; 36 | ICR1 = 400; 37 | OCR1A = 0; 38 | OCR1B = 0; 39 | } 40 | 41 | void Motors::flipLeftMotor(bool flip) 42 | { 43 | flipLeft = flip; 44 | } 45 | 46 | void Motors::flipRightMotor(bool flip) 47 | { 48 | flipRight = flip; 49 | } 50 | 51 | void Motors::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) 63 | { 64 | speed = 400; 65 | } 66 | 67 | OCR1B = speed; 68 | 69 | FastGPIO::Pin::setOutput(reverse ^ flipLeft); 70 | } 71 | 72 | void Motors::setRightSpeed(int16_t speed) 73 | { 74 | init(); 75 | 76 | bool reverse = 0; 77 | 78 | if (speed < 0) 79 | { 80 | speed = -speed; // Make speed a positive quantity. 81 | reverse = 1; // Preserve the direction. 82 | } 83 | if (speed > 400) 84 | { 85 | speed = 400; 86 | } 87 | 88 | OCR1A = speed; 89 | 90 | FastGPIO::Pin::setOutput(reverse ^ flipRight); 91 | } 92 | 93 | void Motors::setSpeeds(int16_t leftSpeed, int16_t rightSpeed) 94 | { 95 | setLeftSpeed(leftSpeed); 96 | setRightSpeed(rightSpeed); 97 | } 98 | 99 | } 100 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4Motors.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See www.pololu.com for details. 2 | 3 | /// \file Pololu3piPlus32U4Motors.h 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace Pololu3piPlus32U4 10 | { 11 | 12 | /// \brief Controls motor speed and direction on the 3pi+ 32U4. 13 | /// 14 | /// This library uses Timer 1, so it will conflict with any other libraries 15 | /// using that timer. 16 | class Motors 17 | { 18 | public: 19 | 20 | /// \brief Flips the direction of the left motor. 21 | /// 22 | /// You can call this function with an argument of \c true if the left motor 23 | /// of your 3pi+ was not wired in the standard way and you want a 24 | /// positive speed argument to correspond to forward movement. 25 | /// 26 | /// \param flip If true, then positive motor speeds will correspond to the 27 | /// direction pin being high. If false, then positive motor speeds will 28 | /// correspond to the direction pin being low. 29 | /// 30 | static void flipLeftMotor(bool flip); 31 | 32 | /// \brief Flips the direction of the right motor. 33 | /// 34 | /// You can call this function with an argument of \c true if the right 35 | /// motor of your 3pi+ was not wired in the standard way and you want a 36 | /// positive speed argument to correspond to forward movement. 37 | /// 38 | /// \param flip If true, then positive motor speeds will correspond to the 39 | /// direction pin being high. If false, then positive motor speeds will 40 | /// correspond to the direction pin being low. 41 | static void flipRightMotor(bool flip); 42 | 43 | /// \brief Sets the speed for the left motor. 44 | /// 45 | /// \param speed A number from -400 to 400 representing the speed and 46 | /// direction of the left motor. Values of -400 or less result in full 47 | /// speed reverse, and values of 400 or more result in full speed forward. 48 | static void setLeftSpeed(int16_t speed); 49 | 50 | /// \brief Sets the speed for the right motor. 51 | /// 52 | /// \param speed A number from -400 to 400 representing the speed and 53 | /// direction of the right motor. Values of -400 or less result in full 54 | /// speed reverse, and values of 400 or more result in full speed forward. 55 | static void setRightSpeed(int16_t speed); 56 | 57 | /// \brief Sets the speeds for both motors. 58 | /// 59 | /// \param leftSpeed A number from -400 to 400 representing the speed and 60 | /// direction of the right motor. Values of -400 or less result in full 61 | /// speed reverse, and values of 400 or more result in full speed forward. 62 | /// \param rightSpeed A number from -400 to 400 representing the speed and 63 | /// direction of the right motor. Values of -400 or less result in full 64 | /// speed reverse, and values of 400 or more result in full speed forward. 65 | static void setSpeeds(int16_t leftSpeed, int16_t rightSpeed); 66 | 67 | private: 68 | 69 | static inline void init() 70 | { 71 | static bool initialized = false; 72 | 73 | if (!initialized) 74 | { 75 | initialized = true; 76 | init2(); 77 | } 78 | } 79 | 80 | static void init2(); 81 | 82 | static bool flipLeft; 83 | static bool flipRight; 84 | }; 85 | 86 | } 87 | -------------------------------------------------------------------------------- /src/Pololu3piPlus32U4OLED.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 | namespace Pololu3piPlus32U4 29 | { 30 | 31 | /// @brief Low-level functions for writing data to the SH1106 OLED on the 32 | /// Pololu 3pi+ 32U4 OLED robot. 33 | class OLEDCore 34 | { 35 | // Pin assignments 36 | static const uint8_t clkPin = 1, mosPin = IO_D5, resPin = 0, dcPin = 17; 37 | 38 | public: 39 | void initPins() 40 | { 41 | FastGPIO::Pin::setOutputLow(); 42 | } 43 | 44 | void reset() 45 | { 46 | FastGPIO::Pin::setOutputLow(); 47 | _delay_us(10); 48 | FastGPIO::Pin::setOutputHigh(); 49 | _delay_us(10); 50 | } 51 | 52 | void sh1106TransferStart() 53 | { 54 | // From https://github.com/pololu/usb-pause-arduino/blob/master/USBPause.h: 55 | // Disables USB interrupts because the Arduino USB interrupts use some of 56 | // the OLED pins. 57 | savedUDIEN = UDIEN; 58 | UDIEN = 0; 59 | savedUENUM = UENUM; 60 | UENUM = 0; 61 | savedUEIENX0 = UEIENX; 62 | UEIENX = 0; 63 | 64 | savedStateMosi = FastGPIO::Pin::getState(); 65 | savedStateDc = FastGPIO::Pin::getState(); 66 | 67 | FastGPIO::Pin::setOutputLow(); 68 | } 69 | 70 | void sh1106TransferEnd() 71 | { 72 | FastGPIO::Pin::setState(savedStateMosi); 73 | FastGPIO::Pin::setState(savedStateDc); 74 | 75 | // From https://github.com/pololu/usb-pause-arduino/blob/master/USBPause.h 76 | UENUM = 0; 77 | UEIENX = savedUEIENX0; 78 | UENUM = savedUENUM; 79 | UDIEN = savedUDIEN; 80 | } 81 | 82 | void sh1106CommandMode() 83 | { 84 | FastGPIO::Pin::setOutputLow(); 85 | } 86 | 87 | void sh1106DataMode() 88 | { 89 | FastGPIO::Pin::setOutputHigh(); 90 | } 91 | 92 | void sh1106Write(uint8_t d) 93 | { 94 | _P3PP_OLED_SEND_BIT(7); 95 | _P3PP_OLED_SEND_BIT(6); 96 | _P3PP_OLED_SEND_BIT(5); 97 | _P3PP_OLED_SEND_BIT(4); 98 | _P3PP_OLED_SEND_BIT(3); 99 | _P3PP_OLED_SEND_BIT(2); 100 | _P3PP_OLED_SEND_BIT(1); 101 | _P3PP_OLED_SEND_BIT(0); 102 | } 103 | 104 | private: 105 | uint8_t savedStateMosi, savedStateDc; 106 | uint8_t savedUDIEN, savedUENUM, savedUEIENX0; 107 | }; 108 | 109 | /// @brief Makes it easy to show text and graphics on the SH1106 OLED of 110 | /// the Pololu 3pi+ 32U4 OLED robot. 111 | /// 112 | /// This class inherits from the PololuSH1106Main class in the PololuOLED 113 | /// library, which provides almost all of its functionality. See the 114 | /// [PololuOLED library documentation](https://pololu.github.io/pololu-oled-arduino/) 115 | /// for more information about how to use this class. 116 | /// 117 | /// This class also inherits from the Arduino Print class 118 | /// (via PololuSH1106Main), so you can call the `print()` function on it with a 119 | /// variety of arguments. See the 120 | /// [Arduino print() documentation](http://arduino.cc/en/Serial/Print) for 121 | /// more information. 122 | class OLED : public PololuSH1106Main 123 | { 124 | }; 125 | 126 | } 127 | --------------------------------------------------------------------------------