From ef809a8c4b1817986fc0abbb76f41fec4acaf11a Mon Sep 17 00:00:00 2001 From: Zhiquan Yeo Date: Thu, 21 Sep 2023 12:40:55 -0400 Subject: [PATCH] Add support for Reflectance sensors and Rangefinder (#22) * Initial support for reflectance sensor * Add rangefinder support --------- Co-authored-by: Zhiquan Yeo --- README.md | 17 +++++++++++ include/robot.h | 15 +++++++++- include/wpilibudp.h | 1 + platformio.ini | 4 +-- src/main.cpp | 25 ++++++++++++++++ src/robot.cpp | 73 +++++++++++++++++++++++++++++++++++++++++++++ src/wpilibudp.cpp | 11 +++++++ 7 files changed, 143 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 404e87d..5d8dfad 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,23 @@ Also comment out the following lines (this disables the WebSocket plugins which | 10 | Motor 4 Encoder A | | 11 | Motor 4 Encoder B | +### Analog I/O Map +| Analog Port # | Function | +|---------------|-------------------| +| 0 | Left Reflectance | +| 1 | Right Reflectance | +| 2 | Rangefinder | + +NOTE: The analog I/O mapping assumes that the reflectance sensor and rangefinder are plugged into the XRP as directed in the setup instructions. All analog I/O channels return values in the range 0-5V. + +#### Reflectance Sensors +The reflectance sensors return a value from 0.0V (white) to 5.0V (black). + +#### Rangefinder +The maximum range of the rangefinder is 4m, and the minimum detectable range is 2cm. Any values outside of this range will cause the rangefinder to saturate and return the maximum value. + +The rangefinder will return a value between 0.0V (min distance) to 5.0V (4m). + ### Motor and Servo Map Instead of pure PWM channels, the XRP uses SimDevices, specifically the `XRPMotor` and `XRPServo` devices. diff --git a/include/robot.h b/include/robot.h index 8b12956..4cedfc0 100644 --- a/include/robot.h +++ b/include/robot.h @@ -48,7 +48,8 @@ #define XRP_DATA_ENCODER 0x01 #define XRP_DATA_DIO 0x02 -#define XRP_DATA_GENERAL 0x04 +#define XRP_DATA_AIO 0x04 +#define XRP_DATA_GENERAL 0x08 namespace xrp { @@ -73,4 +74,16 @@ void setPwmValue(int wpilibChannel, double value); bool isUserButtonPressed(); void setDigitalOutput(int channel, bool value); +// Line/Reflectance Sensing Related +void reflectanceInit(); +bool reflectanceInitialized(); +float getReflectanceLeft5V(); +float getReflectanceRight5V(); + +// Rangefinder +void rangefinderInit(); +bool rangefinderInitialized(); +float getRangefinderDistance5V(); +void rangefinderPeriodic(); + } // namespace xrp \ No newline at end of file diff --git a/include/wpilibudp.h b/include/wpilibudp.h index f41ff72..bb1b0a7 100644 --- a/include/wpilibudp.h +++ b/include/wpilibudp.h @@ -18,4 +18,5 @@ int writeEncoderData(int deviceId, int count, char* buffer, int offset = 0); int writeDIOData(int deviceId, bool value, char* buffer, int offset = 0); int writeGyroData(float rates[3], float angles[3], char* buffer, int offset = 0); int writeAccelData(float accels[3], char* buffer, int offset = 0); +int writeAnalogData(int deviceId, float voltage, char* buffer, int offset = 0); } // namespace wpilibudp \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 59f8843..c200c61 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,10 +8,10 @@ extra_scripts = pre:extra_script.py [env:rpipicow] board = rpipicow lib_deps = - links2004/WebSockets@^2.4.1 bblanchon/ArduinoJson Wire adafruit/Adafruit LSM6DS@^4.7.0 adafruit/Adafruit BusIO@^1.14.1 adafruit/Adafruit Unified Sensor@^1.1.9 - arduino-libraries/Madgwick@^1.2.0 \ No newline at end of file + arduino-libraries/Madgwick@^1.2.0 + martinsos/HCSR04@^2.0.0 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 75e7eb7..560ed7d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -138,6 +138,15 @@ void sendData() { ptr += wpilibudp::writeAccelData(accels, buffer, ptr); // 1x 14 bytes + if (xrp::reflectanceInitialized()) { + ptr += wpilibudp::writeAnalogData(0, xrp::getReflectanceLeft5V(), buffer, ptr); + ptr += wpilibudp::writeAnalogData(1, xrp::getReflectanceRight5V(), buffer, ptr); + } + + if (xrp::rangefinderInitialized()) { + ptr += wpilibudp::writeAnalogData(2, xrp::getRangefinderDistance5V(), buffer, ptr); + } + // ptr should now point to 1 past the last byte size = ptr; @@ -283,6 +292,14 @@ void setup() { xrp::robotInit(); + // NOTE: For now, we'll force init the reflectance sensor + // TODO Enable this via configuration + xrp::reflectanceInit(); + + // NOTE: For now we'll force init the rangefinder + // TODO enable this via configuration + xrp::rangefinderInit(); + _lastMessageStatusPrint = millis(); _baselineUsedHeap = rp2040.getUsedHeap(); @@ -321,3 +338,11 @@ void loop() { updateLoopTime(loopStartTime); checkPrintStatus(); } + +void loop1() { + if (xrp::rangefinderInitialized()) { + xrp::rangefinderPeriodic(); + } + + delay(20); +} \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp index bec3227..6f9a655 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -6,6 +6,10 @@ #include #include +#include + +#define REFLECT_LEFT_PIN 26 +#define REFLECT_RIGHT_PIN 27 namespace xrp { @@ -39,6 +43,15 @@ int _encoderStateMachineIdx[4] = {-1, -1, -1, -1}; PIO _encoderPioInstance[4] = {nullptr, nullptr, nullptr, nullptr}; std::map _encoderWPILibChannelToNativeMap; +// Reflectance +bool _reflectanceInitialized = false; + +// Rangefinder +bool _rangefinderInitialized = false; +float _rangefinderDistMetres = 0.0f; +const float RANGEFINDER_MAX_DIST_M = 4.0f; +UltraSonicDistanceSensor* _rangefinder = nullptr; + // Internal helper functions bool _initEncoders() { for (int i = 0; i < 4; i++) { @@ -335,5 +348,65 @@ void setDigitalOutput(int channel, bool value) { } } +void reflectanceInit() { + analogReadResolution(12); + + _reflectanceInitialized = true; +} + +bool reflectanceInitialized() { + return _reflectanceInitialized; +} + +/** + * Return a scaled voltage (0 to 1) based off analog pin reading + */ +float _readAnalogPinScaled(uint8_t pin) { + float scaled = (float)analogRead(pin) / 4095.0f; + return scaled; +} + +float getReflectanceLeft5V() { + if (!_reflectanceInitialized) { + return -1.0f; + } + + return _readAnalogPinScaled(REFLECT_LEFT_PIN) * 5.0f; +} + +float getReflectanceRight5V() { + if (!_reflectanceInitialized) { + return -1.0f; + } + + return _readAnalogPinScaled(REFLECT_RIGHT_PIN) * 5.0f; +} + +void rangefinderInit() { + _rangefinder = new UltraSonicDistanceSensor(20, 21); + _rangefinderInitialized = true; +} + +bool rangefinderInitialized() { + return _rangefinderInitialized; +} + +float getRangefinderDistance5V() { + return (_rangefinderDistMetres / RANGEFINDER_MAX_DIST_M) * 5.0f; +} + +void rangefinderPeriodic() { + if (_rangefinder == nullptr) { + return; + } + + float distCM = _rangefinder->measureDistanceCm(); + if (distCM < 0) { + _rangefinderDistMetres = RANGEFINDER_MAX_DIST_M; + } + else { + _rangefinderDistMetres = distCM / 100.0f; + } +} } // namespace xrp diff --git a/src/wpilibudp.cpp b/src/wpilibudp.cpp index 728f67f..65b7da9 100644 --- a/src/wpilibudp.cpp +++ b/src/wpilibudp.cpp @@ -183,4 +183,15 @@ int writeAccelData(float accels[3], char* buffer, int offset) { return 14; // +1 for the size byte } +int writeAnalogData(int deviceId, float voltage, char* buffer, int offset) { + // Analog message is 6 bytes + // tag(1) id(1) value(4) + buffer[offset] = 6; + buffer[offset+1] = XRP_TAG_ANALOG; + buffer[offset+2] = deviceId; + floatToNetwork(voltage, buffer, offset+3); + + return 7; // +1 for size byte +} + } // namespace wpilibudp \ No newline at end of file