├── .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 |
--------------------------------------------------------------------------------