diff --git a/examples/custom_sensor/custom_sensor.ino b/examples/custom_sensor/custom_sensor.ino new file mode 100644 index 0000000..2148703 --- /dev/null +++ b/examples/custom_sensor/custom_sensor.ino @@ -0,0 +1,63 @@ +/* + * MyOwnBricks is a library for the emulation of PoweredUp sensors on microcontrollers + * Copyright (C) 2021-2023 Ysard - + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#define SWITCH_GPIO 23 + +int8_t sensorSwitch; +bool connection_status; + +CustomSensor myOwnSwitch(& sensorSwitch); + +void setup() { + pinMode(SWITCH_GPIO, INPUT_PULLUP); +#if (defined(INFO) || defined(DEBUG)) + Serial.begin(115200); // USB CDC + + while (!Serial) { + // Wait for serial port to connect. + } +#endif + + connection_status = false; +} + + +void loop() { + // Get data from GPIO pin + if( digitalRead(SWITCH_GPIO) != HIGH) { + sensorSwitch = 100; + } + else { + sensorSwitch = 0; + } + + myOwnSwitch.process(); + + if (myOwnSwitch.isConnected()) { + // Already connected ? + if (!connection_status) { + INFO_PRINTLN(F("Connected !")); + connection_status = true; + } + } else { + INFO_PRINTLN(F("Not Connected !")); + connection_status = false; + } +} diff --git a/examples/custom_sensor_output/custom_sensor_output.ino b/examples/custom_sensor_output/custom_sensor_output.ino new file mode 100644 index 0000000..084411d --- /dev/null +++ b/examples/custom_sensor_output/custom_sensor_output.ino @@ -0,0 +1,118 @@ +/* + * MyOwnBricks is a library for the emulation of PoweredUp sensors on microcontrollers + * Copyright (C) 2021-2023 Ysard - + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + + +#include + +#include +Servo myservo; +#define pinServo 29 // Rc Servo signal + +#define pinGreen 26 // Green LED anode - High to ON +#define pinYellow 27 // Yellow LED anode - High to ON +#define pinRed 28 // Red LED anode - High to ON + +#define SWITCH_GPIO 23 + +int8_t sensorSwitch; + +bool connection_status; + +CustomSensor myOwnSwitch(& sensorSwitch); + +void ledSignal(bool green, bool yellow, bool red){ + digitalWrite(pinGreen, (green == true) ? HIGH : LOW); + digitalWrite(pinYellow, (yellow == true) ? HIGH : LOW); + digitalWrite(pinRed, (red == true) ? HIGH : LOW); +} + +void setup() { + pinMode(SWITCH_GPIO, INPUT_PULLUP); + + // initialize the RGB LED pins as an output: + pinMode(pinGreen, OUTPUT); + pinMode(pinYellow, OUTPUT); + pinMode(pinRed, OUTPUT); + // turn all LEDs off + ledSignal(LOW, LOW, LOW); + + // initialize RC servo - Tower Pro SG92R + // with custom MIN and MAX timings + myservo.attach(pinServo, 400, 2500); + +#if (defined(INFO) || defined(DEBUG)) + Serial.begin(115200); // USB CDC + + while (!Serial) { + // Wait for serial port to connect. + } +#endif + + connection_status = false; +} + + +void loop() { + // Get data from GPIO pin + if( digitalRead(SWITCH_GPIO) != HIGH) { + sensorSwitch = 100; + } + else { + sensorSwitch = 0; + } + + myOwnSwitch.process(); + + if (myOwnSwitch.isConnected()) { + // Already connected ? + if (!connection_status) { + INFO_PRINTLN(F("Connected !")); + connection_status = true; + } + } else { + INFO_PRINTLN(F("Not Connected !")); + connection_status = false; + } + + switch(myOwnSwitch.m_sensorOutput){ + case 0: + ledSignal(false, false, false); // all off + break; + case 1: + ledSignal(true, false, false); // green on + break; + case 2: + ledSignal(false, true, false); // yellow on + break; + case 3: + ledSignal(false, false, true); // red on + break; + case 4: + ledSignal(true, true, true); // all on + break; + case 5: + myservo.write(0); // servo to min + break; + case 6: + myservo.write(90); // servo to mid + break; + case 7: + myservo.write(180); // servo to max + break; + } +} \ No newline at end of file diff --git a/examples/tilt_sensor/tilt_sensor.ino b/examples/tilt_sensor/tilt_sensor.ino index cd639e8..f51ce61 100644 --- a/examples/tilt_sensor/tilt_sensor.ino +++ b/examples/tilt_sensor/tilt_sensor.ino @@ -46,7 +46,7 @@ void loop() { sensorX = map(arbitraryValue, 0, 1023, -45, 45); sensorY = map(arbitraryValue, 0, 1023, -45, 45); - myOwnTilt.Process(); + myOwnTilt.process(); if (myOwnTilt.isConnected()) { // Already connected ? diff --git a/src/BaseSensor.cpp b/src/BaseSensor.cpp index 38a3807..04651bf 100644 --- a/src/BaseSensor.cpp +++ b/src/BaseSensor.cpp @@ -20,8 +20,14 @@ #include "BaseSensor.h" BaseSensor::BaseSensor() : - m_connSerialRX_pin(0), - m_connSerialTX_pin(1), +#if defined(ESP32) + m_connSerialRX_pin(21), + m_connSerialTX_pin(19), +#else + // this works for Raspberry Pi Pico and XIAO RP2040 + m_connSerialRX_pin(1), + m_connSerialTX_pin(0), +#endif m_lastAckTick(0), m_connected(false) {} @@ -114,7 +120,11 @@ void BaseSensor::connectToHub() { unsigned char dat = SerialTTL.read(); if (dat == 0x04) { // ACK //DEBUG_PRINTLN("Connection established !"); +#if defined(ESP32) + SerialTTL.begin(115200, SERIAL_8N1, RXD1, TXD1); +#else SerialTTL.begin(115200); +#endif m_connected = true; m_lastAckTick = millis(); break; diff --git a/src/CustomSensor.cpp b/src/CustomSensor.cpp new file mode 100644 index 0000000..65fef7e --- /dev/null +++ b/src/CustomSensor.cpp @@ -0,0 +1,173 @@ +/* + * A library for the emulation of PoweredUp sensors on microcontrollers + * Copyright (C) 2021-2023 Ysard - + * + * Based on the original work of Ahmed Jouirou - + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include "CustomSensor.h" + +CustomSensor::CustomSensor(){ + m_sensorSwitch = nullptr; +} + +CustomSensor::CustomSensor(int8_t *pSensorSwitch){ + m_sensorSwitch = pSensorSwitch; +} + +void CustomSensor::setSensorSwitch(int8_t *pData){ + m_sensorSwitch = pData; +} + + + +/** + * @brief Send initialization sequences for the current sensor. + * @see https://github.com/pybricks/technical-info/blob/master/uart-protocol.md + */ +void CustomSensor::commSendInitSequence(){ + + // Initialize uart +#if defined(ESP32) + SerialTTL.begin(2400, SERIAL_8N1, RXD1, TXD1); +#else + SerialTTL.begin(2400); +#endif + + SerialTTL.write("\x40\x24\x9B", 3); // CMD_TYPE, Type ID: 0x24 = 36 OK = N/A + + // CustomSensor sensor has just 1 mode (mode 0, MYOWNSWITCH) + SerialTTL.write("\x49\x00\x00\xB6", 4); // CMD_MODES: 1 mode, 1 view, 0 Ext.Modes/Ext.Views + + SerialTTL.write("\x52\x00\xC2\x01\x00\x6E", 6); // CMD_SPEED: 115200 + + SerialTTL.write("\x5F\x00\x00\x00\x10\x00\x00\x00\x10\xA0", 10); // CMD_VERSION: fw-version: 1.0.0.0, hw-version: 1.0.0.0 + SerialTTL.flush(); + + // describe Mode 0 + SerialTTL.write("\xA0\x00\x4D\x59\x4F\x57\x4E\x53\x57\x49\x54\x43\x48\x00\x00\x00\x00\x00\x0F", 19); // Name: "MYOWNSWITCH" + + // next 5 defs copied from TiltSensor mode 0 - need to adapt + SerialTTL.write("\x98\x01\x00\x00\x34\xC2\x00\x00\x34\x42\xE6", 11); // Range: -45.0 to 45.0 + SerialTTL.write("\x98\x02\x00\x00\xC8\xC2\x00\x00\xC8\x42\xE5", 11); // PCT Range: -100.0% to 100.0% + SerialTTL.write("\x98\x03\x00\x00\x34\xC2\x00\x00\x34\x42\xE4", 11); // Si Range: -45.0 to 45.0 + SerialTTL.write("\x90\x04\x44\x45\x47\x00\x2D", 7); // Si Symbol: DEG + + // added output_flags + SerialTTL.write("\x88\x05\x10\x10\x72", 5); // input_flags: Absolute, output_flags: Absolute + + // Mode 0 just sends 1 byte (DATA8 for 8-bit signed integer = int8) + SerialTTL.write("\x90\x80\x01\x00\x03\x00\xED", 7); // Format: DATA8, 3 figures, 0 decimals + SerialTTL.flush(); + + // end of CustomSensor description + + SerialTTL.write("\x04", 1); // ACK + SerialTTL.flush(); + delay(5); +} + + +/** + * @brief Handle the protocol queries & responses from/to the hub. + * Queries can be read/write according to the requested mode. + * @warning In the situation where the processing of the responses to the + * queries from the hub takes longer than 200ms, a disconnection + * will be performed here. + */ +void CustomSensor::handleModes(){ + if (SerialTTL.available() == 0) + return; + + unsigned char mode; + unsigned char header = SerialTTL.read(); + + if (header == 0x02) { // NACK + m_lastAckTick = millis(); + + // Send default mode: 0 (only mode in fact) + this->sensorSwitchMode_0(); + } else if (header == 0x43) { + // "Get value" commands (3 bytes message: header, mode, checksum) + size_t ret = SerialTTL.readBytes(m_rxBuf, 2); + if (ret < 2) { + // check if all expected bytes are received without timeout + DEBUG_PRINT("incomplete 0x43 message"); + return; + } + + switch (m_rxBuf[0]) { + + case CustomSensor::LEGO_DEVICE_MODE_PUP_CUSTOM_SENSOR__MYOWNSWITCH: + this->sensorSwitchMode_0(); + break; + + default: + break; + } + + // for output: added check for extra header - copied from ColorDistanceSensor.cpp + // with just a few modifications + } else if (header == 0x46) { + // "Set value" commands + // The message has 2 parts (each with header, value and checksum): + // - The EXT_MODE status as value + // - The LUMP_MSG_TYPE_DATA itself with its data as value + + // Get data1, checksum1, header2 (header of the next message) + size_t ret = SerialTTL.readBytes(m_rxBuf, 3); + if (ret < 3) + // check if all expected bytes are received without timeout + return; + + this->m_currentExtMode = m_rxBuf[0]; + + // Get mode and size of the message from the header + uint8_t msg_size; + parseHeader(m_rxBuf[2], mode, msg_size); + // TODO: avoid buffer overflow: check msg size <= size rx buffer + + // Read the remaining bytes after the header (cheksum included) + // Data will be in the indexes [0;msg_size-2] + ret = SerialTTL.readBytes(m_rxBuf, msg_size - 1); + if (_(signed)(ret) != msg_size - 1) + return; + + switch(mode) { + // if Hub is writing to Mode MYOWNSWITCH + // store data in member variable + case CustomSensor::LEGO_DEVICE_MODE_PUP_CUSTOM_SENSOR__MYOWNSWITCH: + this->m_sensorOutput = m_rxBuf[0]; + break; + default: + INFO_PRINT(F("unknown W mode: ")); + INFO_PRINTLN(mode, HEX); + break; + } + } + +} + + +/** + * @brief Mode 0 response (read): Send 1 byte + */ + + +void CustomSensor::sensorSwitchMode_0(){ + m_txBuf[0] = 0xC0; // MESSAGE_CMD | LENGHT_1 | MODE_0 + m_txBuf[1] = _(uint8_t)(*m_sensorSwitch); + sendUARTBuffer(1); +} diff --git a/src/CustomSensor.h b/src/CustomSensor.h new file mode 100644 index 0000000..03c4f39 --- /dev/null +++ b/src/CustomSensor.h @@ -0,0 +1,59 @@ +/* + * MyOwnBricks is a library for the emulation of PoweredUp sensors on microcontrollers + * Copyright (C) 2021-2023 Ysard - + * + * Based on the original work of Ahmed Jouirou - + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef CUSTOMSENSOR_H +#define CUSTOMSENSOR_H + +#include "BaseSensor.h" + + +/** + * @brief Handle the LegoUART protocol and define modes of the + * Custom sensor. + * + * @param m_sensorSwitch int8 (DATA8) + + */ +class CustomSensor : public BaseSensor { + + enum { + LEGO_DEVICE_MODE_PUP_CUSTOM_SENSOR__MYOWNSWITCH = 0, // read 1x int18_t + }; + +public: + CustomSensor(); + CustomSensor(int8_t *pSensorSwitch); + + void setSensorSwitch(int8_t *pData); + void sensorSwitchMode_0(); + + int8_t m_sensorOutput; + +private: + // Process queries from/to hub + virtual void handleModes(); + virtual void commSendInitSequence(); + + // UART protocol + uint8_t m_currentExtMode = 0; + + int8_t *m_sensorSwitch; +}; + +#endif diff --git a/src/TiltSensor.cpp b/src/TiltSensor.cpp index 52bf67a..e3adb90 100644 --- a/src/TiltSensor.cpp +++ b/src/TiltSensor.cpp @@ -63,7 +63,11 @@ void TiltSensor::setSensorTiltY(int8_t *pData){ */ void TiltSensor::commSendInitSequence(){ // Initialize uart +#if defined(ESP32) + SerialTTL.begin(2400, SERIAL_8N1, RXD1, TXD1); +#else SerialTTL.begin(2400); +#endif SerialTTL.write("\x40\x22\x9D", 3); // Type ID: 0x22 SerialTTL.write("\x49\x03\x02\xB7", 4); // CMD_MODES: modes: 4, views: 3, Ext. Modes: 0 modes, 0 views diff --git a/src/TiltSensor.h b/src/TiltSensor.h index a8fc9f8..616feeb 100644 --- a/src/TiltSensor.h +++ b/src/TiltSensor.h @@ -34,7 +34,7 @@ * also called pitch/tangage. * Continuous values ??...?? */ -class TiltSensor : BaseSensor { +class TiltSensor : public BaseSensor { // LEGO POWERED UP WEDO 2.0 Tilt sensor modes // https://github.com/pybricks/pybricks-micropython/blob/master/pybricks/util_pb/pb_device.h enum { diff --git a/src/global.h b/src/global.h index be1c581..bf957ff 100644 --- a/src/global.h +++ b/src/global.h @@ -26,6 +26,11 @@ #if defined(ARDUINO_AVR_PROMICRO) #define SerialTTL Serial1 #define DbgSerial Serial +#elif defined(ESP32) +#define TXD1 19 // GPIO 19 = D19 +#define RXD1 21 // GPIO 21 = D21 +#elif defined(ARDUINO_ARCH_RP2040) +#define SerialTTL Serial1 #else #define SerialTTL Serial #endif diff --git a/tilt_sensor.ino b/tilt_sensor.ino new file mode 100644 index 0000000..f51ce61 --- /dev/null +++ b/tilt_sensor.ino @@ -0,0 +1,61 @@ +/* + * MyOwnBricks is a library for the emulation of PoweredUp sensors on microcontrollers + * Copyright (C) 2021-2023 Ysard - + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#include + +int8_t sensorX; +int8_t sensorY; +bool connection_status; + +TiltSensor myOwnTilt(& sensorX, & sensorY); + + +void setup() { +#if (defined(INFO) || defined(DEBUG)) + Serial.begin(115200); // USB CDC + while (!Serial) { + // Wait for serial port to connect. + } +#endif + + // Init roll & pitch values if not made via the constructor + //myOwnTilt.setSensorTiltX(&sensorX); + //myOwnTilt.setSensorTiltY(&sensorY); + connection_status = false; +} + + +void loop() { + // Get data from orientation sensor + int arbitraryValue = 512; + + sensorX = map(arbitraryValue, 0, 1023, -45, 45); + sensorY = map(arbitraryValue, 0, 1023, -45, 45); + + myOwnTilt.process(); + + if (myOwnTilt.isConnected()) { + // Already connected ? + if (!connection_status) { + INFO_PRINTLN(F("Connected !")); + connection_status = true; + } + } else { + INFO_PRINTLN(F("Not Connected !")); + connection_status = false; + } +}