Add support for Reflectance sensors and Rangefinder (#22)

* Initial support for reflectance sensor

* Add rangefinder support

---------

Co-authored-by: Zhiquan Yeo <[email protected]>

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. 

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

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

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
+    arduino-libraries/Madgwick@^1.2.0
+    martinsos/HCSR04@^2.0.0

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);
+}

src/robot.cpp

@@ -6,6 +6,10 @@
 #include <vector>
 
 #include <Servo.h>
+#include <HCSR04.h>
+
+#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<int, int> _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

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