Digital IO support (#4)

* Add initial Digital Input support * Add support for Digital Output (LED) --------- Co-authored-by: Zhiquan Yeo <[email protected]>
wpilibsuite · Aug 5, 2023 · df20e3c · df20e3c
1 parent cf1167e
commit df20e3c
Show file tree

Hide file tree

Showing 6 changed files with 122 additions and 20 deletions.
diff --git a/README.md b/README.md
@@ -1 +1,36 @@
-# xrp-wpilib-lite
+# WPILib HAL Simulation - XRP Edition (Lite)
+## Introduction
+This repository contains a reference implementation of a XRP robot that can be controlled via the WPILib HALSim WebSocket extension.
+
+The firmware implements (a subset) of the [WPILib Robot Hardware Interface WebSockets API spec](https://github.com/wpilibsuite/allwpilib/blob/main/simulation/halsim_ws_core/doc/hardware_ws_api.md). It behaves similarly to the [WPILib Romi Project](https://github.com/wpilibsuite/wpilib-ws-robot-romi) (with some functionality removed due to being an embedded system vs a Linux machine).
+
+## Built-in IO Mapping
+
+### Digital I/O Map
+| DIO Port # | Function          |
+|------------|-------------------|
+| 0          | XRP User Button   |
+| 1          | XRP Onboard LED   |
+| 2          | RESERVED          |
+| 3          | RESERVED          |
+| 4          | Left Encoder A    |
+| 5          | Left Encoder B    |
+| 6          | Right Encoder A   |
+| 7          | Right Encoder B   |
+| 8          | Motor 3 Encoder A |
+| 9          | Motor 3 Encoder B |
+| 10         | Motor 4 Encoder A |
+| 11         | Motor 4 Encoder B |
+
+### Motor and Servo Map
+
+Instead of pure PWM channels, the XRP uses SimDevices, specifically the `XRPMotor` and `XRPServo` devices. 
+
+| Device  # | Device Type | Function    |
+|-----------|-------------|-------------|
+| 0         | XRPMotor    | Left Motor  |
+| 1         | XRPMotor    | Right Motor |
+| 2         | XRPMotor    | Motor 3     |
+| 3         | XRPMotor    | Motor 4     |
+| 4         | XRPServo    | Servo 1     |
+| 5         | XRPServo    | Servo 2     |
diff --git a/include/robot.h b/include/robot.h
@@ -43,11 +43,14 @@
 #define WPILIB_CH_PWM_SERVO_1 4
 #define WPILIB_CH_PWM_SERVO_2 5
 
+#define XRP_DATA_ENCODER 0x01
+#define XRP_DATA_DIO 0x02
+
 namespace xrp {
 
 void robotInit();
 bool robotInitialized();
-bool robotPeriodic();
+uint8_t robotPeriodic();
 
 // Robot control
 void setEnabled(bool enabled);
@@ -61,4 +64,8 @@ std::vector<std::pair<int,int> > getActiveEncoderValues();
 // PWM Related
 void setPwmValue(int wpilibChannel, double value);
 
+// DIO Related
+bool isUserButtonPressed();
+void setDigitalOutput(int channel, bool value);
+
 } // namespace xrp
diff --git a/include/wpilibws.h b/include/wpilibws.h
@@ -10,5 +10,6 @@ void processWSMessage(JsonDocument& jsonMsg);
 
 // Message Encoders
 std::string makeEncoderMessage(int deviceId, int count);
+std::string makeDIOMessage(int deviceId, bool value);
 
 } // namespace wpilibws
diff --git a/src/main.cpp b/src/main.cpp
@@ -157,14 +157,19 @@ void loop() {
     checkAndSendMessages();
 
     // Read sensor data
-    if (xrp::robotPeriodic()) {
-      // TODO Got new data, send it up
-      // Send Encoder data if present
+    auto updatedData = xrp::robotPeriodic();
+    if (updatedData & XRP_DATA_ENCODER) {
       auto encValues = xrp::getActiveEncoderValues();
       for (auto encData : encValues) {
         sendMessage(wpilibws::makeEncoderMessage(encData.first, encData.second));
       }
     }
+
+    if (updatedData & XRP_DATA_DIO) {
+      // User button is on DIO 0
+      sendMessage(wpilibws::makeDIOMessage(0, xrp::isUserButtonPressed()));
+    }
+
   }
 
   updateLoopTime(loopStartTime);

diff --git a/src/robot.cpp b/src/robot.cpp
@@ -13,6 +13,9 @@ bool _robotInitialized = false;
 bool _robotEnabled = false;
 unsigned long _lastRobotPeriodicCall = 0;
 
+// Digital IO
+bool _lastUserButtonState = false;
+
 // Servo Outputs
 Servo servo1;
 Servo servo2;
@@ -29,6 +32,8 @@ std::vector<std::pair<int, int> > _encoderPins = {
   {0, 1},
   {8, 9}
 };
+
+int _encoderValuesLast[4] = {0, 0, 0, 0};
 int _encoderValues[4] = {0, 0, 0, 0};
 int _encoderStateMachineIdx[4] = {-1, -1, -1, -1};
 PIO _encoderPioInstance[4] = {nullptr, nullptr, nullptr, nullptr};
@@ -65,18 +70,26 @@ bool _initEncoders() {
   // quadrature_program_init(_encoderPio, ENC_SM_IDX_MOTOR_4, offset0, 8, 9);
 }
 
-unsigned long _readEncodersInternal() {
+bool _readEncodersInternal() {
   unsigned long _start = millis();
+  bool hasChange = false;
   for (int i = 0; i < 4; i++) {
     PIO _pio = _encoderPioInstance[i];
     uint _smIdx = _encoderStateMachineIdx[i];
 
     if (_pio != nullptr) {
       pio_sm_exec_wait_blocking(_pio, _smIdx, pio_encode_in(pio_x, 32));
       _encoderValues[i] = pio_sm_get_blocking(_pio, _smIdx);
+
+      if (_encoderValues[i] != _encoderValuesLast[i]) {
+        hasChange = true;
+      }
+
+      _encoderValuesLast[i] = _encoderValues[i];
     }
   }
-  return millis() - _start;
+
+  return hasChange;
 }
 
 void _initMotors() {
@@ -176,6 +189,8 @@ void _pwmShutoff() {
 
 void robotInit() {
   Serial.println("[XRP] Initializing XRP Onboards");
+  pinMode(XRP_BUILTIN_LED, OUTPUT);
+  pinMode(XRP_BUILTIN_BUTTON, INPUT_PULLUP);
 
   // Set up the encoder state machines
   Serial.println("[XRP] Initializing Encoders");
@@ -193,10 +208,6 @@ void robotInit() {
     Serial.println("  - ERROR");
   }
 
-  // Set up on-board hardware
-  pinMode(XRP_BUILTIN_LED, OUTPUT);
-  pinMode(XRP_BUILTIN_BUTTON, INPUT_PULLUP);
-
   _robotInitialized = true;
 }
 
@@ -205,18 +216,37 @@ bool robotInitialized() {
 }
 
 // Return true if this actually ran
-bool robotPeriodic() {
+uint8_t robotPeriodic() {
+  uint8_t ret = 0;
+
   // Kill PWM if the watchdog is dead
   // We want this to run as quickly as possible
   if (!wpilibws::dsWatchdogActive()) {
     _pwmShutoff();
   }
 
-  if (millis() - _lastRobotPeriodicCall < 50) return false;
+  if (millis() - _lastRobotPeriodicCall < 50) return ret;
+
+  // Check for encoder updates
+  bool hasEncUpdate = _readEncodersInternal();
+  if (hasEncUpdate) {
+    ret |= XRP_DATA_ENCODER;
+  }
+
+  // Check for DIO (button) updates
+  bool currButtonState = isUserButtonPressed();
+  if (currButtonState != _lastUserButtonState) {
+    ret |= XRP_DATA_DIO;
+    _lastUserButtonState = currButtonState;
+  }
 
-  unsigned long encoderReadTime = _readEncodersInternal();
   _lastRobotPeriodicCall = millis();
-  return true;
+  return ret;
+}
+
+bool isUserButtonPressed() {
+  // This is a pull up circuit, so when pressed, the pin is low
+  return digitalRead(XRP_BUILTIN_BUTTON) == LOW;
 }
 
 void setEnabled(bool enabled) {
@@ -285,5 +315,12 @@ void setPwmValue(int wpilibChannel, double value) {
   _setPwmValueInternal(wpilibChannel, value, false);
 }
 
+void setDigitalOutput(int channel, bool value) {
+  if (channel == 1) {
+    // LED
+    digitalWrite(XRP_BUILTIN_LED, value ? HIGH : LOW);
+  }
+}
+
 
 } // namespace xrp
diff --git a/src/wpilibws.cpp b/src/wpilibws.cpp
@@ -20,10 +20,6 @@ std::map<std::string, int> _xrpServoMap = {
   {"servo2", 5}
 };
 
-void _handleDIOMessage(JsonDocument& dioMsg) {
-
-}
-
 void _handleDSMessage(JsonDocument& dsMsg) {
   _dsWatchdog.feed();
 
@@ -55,6 +51,16 @@ void _handleEncoderMessage(JsonDocument& encMsg) {
   }
 }
 
+void _handleDIOMessage(JsonDocument& dioMsg) {
+  int deviceNum = atoi(dioMsg["device"].as<const char*>());
+  auto data = dioMsg["data"];
+
+  if (data.containsKey("<>value")) {
+    bool state = data["<>value"];
+    xrp::setDigitalOutput(deviceNum, state);
+  }
+}
+
 void _handleGyroMessage(JsonDocument& gyroMsg) {
 
 }
@@ -130,7 +136,7 @@ void processWSMessage(JsonDocument& jsonMsg) {
       _handleEncoderMessage(jsonMsg);
     }
     else if (jsonMsg["type"] == "DIO") {
-      // TODO DIO
+      _handleDIOMessage(jsonMsg);
     }
     else if (jsonMsg["type"] == "Gyro") {
       // TODO Gyro
@@ -160,4 +166,15 @@ std::string makeEncoderMessage(int deviceId, int count) {
   return ret;
 }
 
+std::string makeDIOMessage(int deviceId, bool value) {
+  StaticJsonDocument<256> msg;
+  msg["type"] = "DIO";
+  msg["device"] = std::to_string(deviceId);
+  msg["data"]["<>value"] = value;
+
+  std::string ret;
+  serializeJson(msg, ret);
+  return ret;
+}
+
 } // namespace wpilibws