|
5 | 5 | * Turn the wheels by hand and check the output in the serial monitor. |
6 | 6 | */ |
7 | 7 |
|
| 8 | +// Include unity library |
| 9 | +// For details see https://docs.platformio.org/en/latest/plus/unit-testing.html#api |
8 | 10 | #include <unity.h> |
9 | 11 |
|
| 12 | +#include <ros.h> |
10 | 13 | #include "diffbot_base_config.h" |
11 | | -#include "encoder.h" |
| 14 | +#include "encoder_diffbot.h" |
12 | 15 |
|
13 | 16 |
|
| 17 | +// ROS node handle |
| 18 | +ros::NodeHandle nh; |
| 19 | + |
14 | 20 | // Encoder setup |
15 | 21 | // Change these pin numbers to the pins connected to your encoder. |
16 | 22 | // Best Performance: both pins have interrupt capability |
17 | 23 | // Good Performance: only the first pin has interrupt capability |
18 | 24 | // Low Performance: neither pin has interrupt capability |
19 | | -diffbot::Encoder encoderLeft(ENCODER_LEFT_H1, ENCODER_LEFT_H2); // Default pins 5, 6 |
20 | | -diffbot::Encoder encoderRight(ENCODER_RIGHT_H1, ENCODER_RIGHT_H2); // Default pins 7, 8 |
| 25 | +diffbot::Encoder encoderLeft(nh, ENCODER_LEFT_H1, ENCODER_LEFT_H2, ENCODER_RESOLUTION); // Default pins 5, 6 |
| 26 | +diffbot::Encoder encoderRight(nh, ENCODER_RIGHT_H1, ENCODER_RIGHT_H2, ENCODER_RESOLUTION); // Default pins 7, 8 |
21 | 27 | // avoid using pins with LEDs attached |
22 | 28 |
|
23 | 29 | void setup() { |
24 | 30 | Serial.begin(115200); |
25 | 31 | Serial.println("DiffBot Wheel Encoders:"); |
| 32 | + Serial.println("Trun wheels by hand to check for correct encoder output."); |
26 | 33 | } |
27 | 34 |
|
28 | 35 | long positionLeft = -999; |
29 | 36 | long positionRight = -999; |
30 | 37 |
|
31 | 38 | int test_start_time = millis(); |
32 | | -int test_max_time = 5000; |
| 39 | +int test_max_time = 8000; |
33 | 40 |
|
34 | 41 | void loop() { |
35 | 42 | UNITY_BEGIN(); |
|
0 commit comments