From df20e3c01107866acb18bc918cc253fc79103c15 Mon Sep 17 00:00:00 2001 From: Zhiquan Yeo Date: Fri, 4 Aug 2023 23:23:41 -0400 Subject: [PATCH] Digital IO support (#4) * Add initial Digital Input support * Add support for Digital Output (LED) --------- Co-authored-by: Zhiquan Yeo --- README.md | 37 +++++++++++++++++++++++++++++- include/robot.h | 9 +++++++- include/wpilibws.h | 1 + src/main.cpp | 11 ++++++--- src/robot.cpp | 57 ++++++++++++++++++++++++++++++++++++++-------- src/wpilibws.cpp | 27 ++++++++++++++++++---- 6 files changed, 122 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 4453b59..11e0336 100644 --- 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 | \ No newline at end of file diff --git a/include/robot.h b/include/robot.h index 125a099..4a828e8 100644 --- 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 > getActiveEncoderValues(); // PWM Related void setPwmValue(int wpilibChannel, double value); +// DIO Related +bool isUserButtonPressed(); +void setDigitalOutput(int channel, bool value); + } // namespace xrp \ No newline at end of file diff --git a/include/wpilibws.h b/include/wpilibws.h index 40a3f91..38307f6 100644 --- 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 index 8c3eef2..870569c 100644 --- 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 index 3051c36..4f25ac9 100644 --- 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 > _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,8 +70,9 @@ 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]; @@ -74,9 +80,16 @@ unsigned long _readEncodersInternal() { 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 index 0de64ef..b8e5b6d 100644 --- a/src/wpilibws.cpp +++ b/src/wpilibws.cpp @@ -20,10 +20,6 @@ std::map _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()); + 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