diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..ba034ff Binary files /dev/null and b/.DS_Store differ diff --git a/Archive/ADXL375 Test/.gitignore b/Archive/ADXL375 Test/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Archive/ADXL375 Test/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Archive/ADXL375 Test/.vscode/extensions.json b/Archive/ADXL375 Test/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/Archive/ADXL375 Test/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/Archive/ADXL375 Test/include/README b/Archive/ADXL375 Test/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/Archive/ADXL375 Test/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Archive/ADXL375 Test/lib/README b/Archive/ADXL375 Test/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/Archive/ADXL375 Test/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Archive/ADXL375 Test/platformio.ini b/Archive/ADXL375 Test/platformio.ini new file mode 100644 index 0000000..c784fac --- /dev/null +++ b/Archive/ADXL375 Test/platformio.ini @@ -0,0 +1,15 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-s3-devkitc-1] +platform = espressif32 +board = esp32-s3-devkitc-1 +framework = arduino +lib_deps = adafruit/Adafruit ADXL375@^1.1.2 diff --git a/Archive/ADXL375 Test/src/main.cpp b/Archive/ADXL375 Test/src/main.cpp new file mode 100644 index 0000000..b8d2dd7 --- /dev/null +++ b/Archive/ADXL375 Test/src/main.cpp @@ -0,0 +1,137 @@ +#include +#include +#include + +#define ADXL375_SCK 13 +#define ADXL375_MISO 12 +#define ADXL375_MOSI 11 +#define ADXL375_CS 10 + +/* Assign a unique ID to this sensor at the same time */ +/* Uncomment following line for default Wire bus */ +Adafruit_ADXL375 accel = Adafruit_ADXL375(12345); + +/* Uncomment for software SPI */ +//Adafruit_ADXL375 accel = Adafruit_ADXL375(ADXL375_SCK, ADXL375_MISO, ADXL375_MOSI, ADXL375_CS, 12345); + +/* Uncomment for hardware SPI */ +//Adafruit_ADXL375 accel = Adafruit_ADXL375(ADXL375_CS, &SPI, 12345); + +void displayDataRate(void) +{ + USBSerial.print ("Data Rate: "); + + switch(accel.getDataRate()) + { + case ADXL343_DATARATE_3200_HZ: + USBSerial.print ("3200 "); + break; + case ADXL343_DATARATE_1600_HZ: + USBSerial.print ("1600 "); + break; + case ADXL343_DATARATE_800_HZ: + USBSerial.print ("800 "); + break; + case ADXL343_DATARATE_400_HZ: + USBSerial.print ("400 "); + break; + case ADXL343_DATARATE_200_HZ: + USBSerial.print ("200 "); + break; + case ADXL343_DATARATE_100_HZ: + USBSerial.print ("100 "); + break; + case ADXL343_DATARATE_50_HZ: + USBSerial.print ("50 "); + break; + case ADXL343_DATARATE_25_HZ: + USBSerial.print ("25 "); + break; + case ADXL343_DATARATE_12_5_HZ: + USBSerial.print ("12.5 "); + break; + case ADXL343_DATARATE_6_25HZ: + USBSerial.print ("6.25 "); + break; + case ADXL343_DATARATE_3_13_HZ: + USBSerial.print ("3.13 "); + break; + case ADXL343_DATARATE_1_56_HZ: + USBSerial.print ("1.56 "); + break; + case ADXL343_DATARATE_0_78_HZ: + USBSerial.print ("0.78 "); + break; + case ADXL343_DATARATE_0_39_HZ: + USBSerial.print ("0.39 "); + break; + case ADXL343_DATARATE_0_20_HZ: + USBSerial.print ("0.20 "); + break; + case ADXL343_DATARATE_0_10_HZ: + USBSerial.print ("0.10 "); + break; + default: + USBSerial.print ("???? "); + break; + } + USBSerial.println(" Hz"); +} + +void setup(void) +{ + USBSerial.begin(115200); + while (!USBSerial); + delay(1000); + USBSerial.println("ADXL375 Accelerometer Test"); USBSerial.println(""); + Wire.begin(5,6); + /* Initialise the sensor */ + while(!accel.begin()) + { + /* There was a problem detecting the ADXL375 ... check your connections */ + USBSerial.println("Ooops, no ADXL375 detected ... Check your wiring!"); + delay(100); + } + + // Range is fixed at +-200g + + /* Display some basic information on this sensor */ + accel.setTrimOffsets(0, 0, 0); + + USBSerial.println("Hold accelerometer flat to set offsets to 0, 0, and -1g..."); + delay(1000); + int16_t x, y, z; + x = accel.getX(); + y = accel.getY(); + z = accel.getZ(); + USBSerial.print("Raw X: "); USBSerial.print(x); USBSerial.print(" "); + USBSerial.print("Y: "); USBSerial.print(y); USBSerial.print(" "); + USBSerial.print("Z: "); USBSerial.print(z); USBSerial.print(" ");USBSerial.println(" counts"); + + // the trim offsets are in 'multiples' of 4, we want to round, so we add 2 + accel.setTrimOffsets(-(x+2)/4, + -(y+2)/4, + -(z-20+2)/4); // Z should be '20' at 1g (49mg per bit) + + int8_t x_offset, y_offset, z_offset; + accel.getTrimOffsets(&x_offset, &y_offset, &z_offset); + USBSerial.print("Current trim offsets: "); + USBSerial.print(x_offset); USBSerial.print(", "); + USBSerial.print(y_offset); USBSerial.print(", "); + USBSerial.println(z_offset); + + USBSerial.println(); +} + +void loop(void) +{ + /* Get a new sensor event */ + sensors_event_t event; + accel.getEvent(&event); + + /* Display the results (acceleration is measured in m/s^2) */ + USBSerial.print("X: "); USBSerial.print(event.acceleration.x); USBSerial.print(" "); + USBSerial.print("Y: "); USBSerial.print(event.acceleration.y); USBSerial.print(" "); + USBSerial.print("Z: "); USBSerial.print(event.acceleration.z); USBSerial.print(" ");USBSerial.println("m/s^2 "); + delay(250); +} \ No newline at end of file diff --git a/Archive/ADXL375 Test/test/README b/Archive/ADXL375 Test/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Archive/ADXL375 Test/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Archive/AlipayTelemetry.py b/Archive/AlipayTelemetry.py new file mode 100644 index 0000000..5109e67 --- /dev/null +++ b/Archive/AlipayTelemetry.py @@ -0,0 +1,34 @@ +import socket + +class Alipay: + def __init__(self, targetIP, PORT, name): + self.targetIP = targetIP + self.receiveBufferSize = 128 + self.PORT = PORT + self.name = name + + def get_laptop_ip(self): + laptop_ip = "" + while (laptop_ip == ""): + laptop_ip = socket.gethostbyname_ex(socket.gethostname())[-1] + return laptop_ip + + def udp_receiver(self): + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind(("0.0.0.0", self.PORT)) + + while True: + data, addr = sock.recvfrom(64) # This code is blocking!! Implement whether connected / disconnected from Alipay?? + try: + data_str = data.decode("utf-8") + print(f"[{self.name}] {data_str}") + except: + pass + + def send_udp_packet(self, data): + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, self.receiveBufferSize) + try: + sock.sendto(data.encode(), (self.targetIP, 12345)) # Fixed port because each esp32 has different IP address + except: + pass \ No newline at end of file diff --git a/IR Beacon/.DS_Store b/IR Beacon/.DS_Store new file mode 100644 index 0000000..e147f99 Binary files /dev/null and b/IR Beacon/.DS_Store differ diff --git a/IR Beacon/.gitignore b/IR Beacon/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/IR Beacon/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/IR Beacon/.vscode/extensions.json b/IR Beacon/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/IR Beacon/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/IR Beacon/.vscode/settings.json b/IR Beacon/.vscode/settings.json new file mode 100644 index 0000000..2018d0a --- /dev/null +++ b/IR Beacon/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "system_error": "cpp" + } +} \ No newline at end of file diff --git a/IR Beacon/include/README b/IR Beacon/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/IR Beacon/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/IR Beacon/lib/.DS_Store b/IR Beacon/lib/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/IR Beacon/lib/.DS_Store differ diff --git a/IR Beacon/lib/README b/IR Beacon/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/IR Beacon/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/IR Beacon/platformio.ini b/IR Beacon/platformio.ini new file mode 100644 index 0000000..2a790f4 --- /dev/null +++ b/IR Beacon/platformio.ini @@ -0,0 +1,26 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-c3-devkitc-1] +platform = espressif32 +board = esp32-c3-devkitm-1 +framework = arduino +upload_speed = 921600 +monitor_speed = 115200 +build_flags = + -D IR_BEACON=1 + -D ARDUINO_USB_CDC_ON_BOOT=1 + -D ARDUINO_USB_MODE=1 +lib_deps = + fastled/FastLED@^3.6.0 + h2zero/NimBLE-Arduino@^1.4.1 + freenove/Freenove WS2812 Lib for ESP32@^1.0.6 +lib_extra_dirs = + /Users/mingweiyeoh/Documents/GitHub/Oreo/Robot Code/lib diff --git a/IR Beacon/src/main.cpp b/IR Beacon/src/main.cpp new file mode 100644 index 0000000..64d3735 --- /dev/null +++ b/IR Beacon/src/main.cpp @@ -0,0 +1,85 @@ +#include +#include +#include +#include +#include + +#define TARGETCMD '2' // Change based on which IR_Beacon working on + +#define IRLEDPIN 3 +#define BAT_PIN 1 + +const int packSize = 6; +char laptop_packetBuffer[packSize] = {'0', '0', '0', '0', '0', '0'}; + +const int freq = 38000; +const int ledChannel = 1; +const int resolution = 8; + +int dutycycle = 30; +const int maxdutycycle = 100; + +BLE_Uart laptop = BLE_Uart(laptop_packetBuffer, packSize); + +void setup(){ + init_led(); + setLeds(ORANGE); + Serial.begin(115200); + ledcSetup(ledChannel, freq, resolution); + ledcAttachPin(IRLEDPIN, ledChannel); + laptop.init_ble("IR Beacon"); + setLeds(BLACK); +} + +void loop(){ + if (laptop.isConnected()) { + switch (laptop_packetBuffer[0]) { + case '0': // Disabled + toggleLeds(RED, GREEN, 500); + ledcWrite(ledChannel, 0); + EVERY_N_SECONDS(10) { + laptop.send("SOC: " + String(get1sSOC(BAT_PIN)) + " %"); + } + break; + case '1': // Enable one of the IR Beacons + if (laptop_packetBuffer[1] == TARGETCMD) { + toggleLeds(BLUE, PURPLE, 500); + ledcWrite(ledChannel, dutycycle); + + EVERY_N_MILLIS(100) { + switch (laptop_packetBuffer[2]) { + case '1': + dutycycle+=5; + if (dutycycle > maxdutycycle) + dutycycle = maxdutycycle; + break; + case '2': + dutycycle-=5; + if (dutycycle < 0) + dutycycle = 0; + break; + } + if (laptop_packetBuffer[2] == '1' || laptop_packetBuffer[2] == '2') { + String msg = "dutycycle : " + String(dutycycle); + laptop.send(msg); + } + } + + } else { + setLeds(BLACK); + ledcWrite(ledChannel, 0); + } + break; + default: // Don't enable current IR Beacon if in another mode + toggleLeds(WHITE, BLACK, 500); + ledcWrite(ledChannel, 0); + break; + } + + + + } else { + toggleLeds(RED, BLACK, 500); + ledcWrite(ledChannel, 0); + } +} \ No newline at end of file diff --git a/IR Beacon/test/README b/IR Beacon/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/IR Beacon/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Motor_Test_DSDHOT/.gitignore b/Motor_Test_DSDHOT/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Motor_Test_DSDHOT/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Motor_Test_DSDHOT/.vscode/extensions.json b/Motor_Test_DSDHOT/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/Motor_Test_DSDHOT/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/Motor_Test_DSDHOT/include/README b/Motor_Test_DSDHOT/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/Motor_Test_DSDHOT/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.cpp b/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.cpp new file mode 100644 index 0000000..ac703ef --- /dev/null +++ b/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.cpp @@ -0,0 +1,277 @@ +// +// Name: DShotRMT.cpp +// Created: 20.03.2021 00:49:15 +// Author: derdoktor667 +// + +#include + +// Constructor that takes gpio and rmtChannel as arguments +DShotRMT::DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel) +{ + // Initialize the dshot_config structure with the arguments passed to the constructor + dshot_config.gpio_num = gpio; + dshot_config.pin_num = static_cast(gpio); + dshot_config.rmt_channel = rmtChannel; + dshot_config.mem_block_num = static_cast(RMT_CHANNEL_MAX - static_cast(rmtChannel)); + + // Create an empty packet using the DSHOT_NULL_PACKET and the buildTxRmtItem function + buildTxRmtItem(DSHOT_NULL_PACKET); +} + +// Constructor that takes pin and channel as arguments +DShotRMT::DShotRMT(uint8_t pin, uint8_t channel) +{ + // Initialize the dshot_config structure with the arguments passed to the constructor + dshot_config.gpio_num = static_cast(pin); + dshot_config.pin_num = pin; + dshot_config.rmt_channel = static_cast(channel); + dshot_config.mem_block_num = RMT_CHANNEL_MAX - channel; + + // Create an empty packet using the DSHOT_NULL_PACKET and the buildTxRmtItem function + buildTxRmtItem(DSHOT_NULL_PACKET); +} + +// ...simplest but only for testing +DShotRMT::DShotRMT(uint8_t pin) +{ + // Initialize the dshot_config structure with the arguments passed to the constructor + dshot_config.gpio_num = static_cast(pin); + dshot_config.pin_num = pin; + dshot_config.rmt_channel = static_cast(RMT_CHANNEL_MAX -1); + dshot_config.mem_block_num = RMT_CHANNEL_MAX - 1; + + // Create an empty packet using the DSHOT_NULL_PACKET and the buildTxRmtItem function + buildTxRmtItem(DSHOT_NULL_PACKET); +} + +DShotRMT::~DShotRMT() +{ + // Uninstall the RMT driver + rmt_driver_uninstall(dshot_config.rmt_channel); +} + +DShotRMT::DShotRMT(DShotRMT const &) +{ + // ...write me +} + +bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional) +{ + // Set DShot configuration parameters based on input parameters + dshot_config.mode = dshot_mode; + dshot_config.clk_div = DSHOT_CLK_DIVIDER; + dshot_config.name_str = dshot_mode_name[dshot_mode]; + dshot_config.is_bidirectional = is_bidirectional; + + // Set timing parameters based on selected DShot mode + switch (dshot_config.mode) + { + case DSHOT150: + dshot_config.ticks_per_bit = 64; + dshot_config.ticks_zero_high = 24; + dshot_config.ticks_one_high = 48; + break; + + case DSHOT300: + dshot_config.ticks_per_bit = 32; + dshot_config.ticks_zero_high = 12; + dshot_config.ticks_one_high = 24; + break; + + case DSHOT600: + dshot_config.ticks_per_bit = 16; + dshot_config.ticks_zero_high = 6; + dshot_config.ticks_one_high = 12; + break; + + case DSHOT1200: + dshot_config.ticks_per_bit = 8; + dshot_config.ticks_zero_high = 3; + dshot_config.ticks_one_high = 6; + break; + + // Default case to handle invalid input + default: + dshot_config.ticks_per_bit = 0; + dshot_config.ticks_zero_high = 0; + dshot_config.ticks_one_high = 0; + break; + } + + // Calculate low signal timing + dshot_config.ticks_zero_low = (dshot_config.ticks_per_bit - dshot_config.ticks_zero_high); + dshot_config.ticks_one_low = (dshot_config.ticks_per_bit - dshot_config.ticks_one_high); + + // Set up RMT configuration for DShot transmission + dshot_tx_rmt_config.rmt_mode = RMT_MODE_TX; + dshot_tx_rmt_config.channel = dshot_config.rmt_channel; + dshot_tx_rmt_config.gpio_num = dshot_config.gpio_num; + dshot_tx_rmt_config.mem_block_num = dshot_config.mem_block_num; + dshot_tx_rmt_config.clk_div = dshot_config.clk_div; + dshot_tx_rmt_config.tx_config.loop_en = false; + dshot_tx_rmt_config.tx_config.carrier_en = false; + dshot_tx_rmt_config.tx_config.idle_output_en = true; + + // Set idle level for RMT transmission based on input parameter + if (dshot_config.is_bidirectional) + { + dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_HIGH; + } + else + { + dshot_tx_rmt_config.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; + } + + // Set up selected DShot mode + rmt_config(&dshot_tx_rmt_config); + + // Install RMT driver and return result + return rmt_driver_install(dshot_tx_rmt_config.channel, 0, 0); +} + +// Define a function to send a DShot command over an RMT interface to control a brushless motor's speed. +void DShotRMT::sendThrottleValue(uint16_t throttle_value) +{ + dshot_packet_t dshot_rmt_packet = {}; + + // Check if the throttle value is less than the minimum allowed value for the DShot protocol. + if (throttle_value < DSHOT_THROTTLE_MIN) + { + throttle_value = DSHOT_THROTTLE_MIN; + } + + // Check if the throttle value is greater than the maximum allowed value for the DShot protocol. + if (throttle_value > DSHOT_THROTTLE_MAX) + { + throttle_value = DSHOT_THROTTLE_MAX; + } + + dshot_rmt_packet.throttle_value = throttle_value; + + // Telemetric using additional pin on the ESC is not supported. + dshot_rmt_packet.telemetric_request = NO_TELEMETRIC; + + // Calculate the checksum for the DShot packet using the calculateCRC function. + dshot_rmt_packet.checksum = calculateCRC(dshot_rmt_packet); + + // Send the DShot packet over the RMT interface to control the motor's speed. + sendRmtPaket(dshot_rmt_packet); +} + +// This method builds the RMT data transmission sequence for the DShot protocol +rmt_item32_t *DShotRMT::buildTxRmtItem(uint16_t parsed_packet) +{ + // Check if DShot is set to bidirectional mode + if (dshot_config.is_bidirectional) + { + // If bidirectional, invert the high/low bits + for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1) + { + if (parsed_packet & 0b1000000000000000) + { + // Set RMT item for a logic high signal + dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_low; + dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_high; + } + else + { + // Set RMT item for a logic low signal + dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_low; + dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_high; + } + + // Set level of RMT item + dshot_tx_rmt_item[i].level0 = 0; + dshot_tx_rmt_item[i].level1 = 1; + } + } + else + { + // If not bidirectional, set the RMT items as usual + for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1) + { + if (parsed_packet & 0b1000000000000000) + { + // Set RMT item for a logic high signal + dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_one_high; + dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_one_low; + } + else + { + // Set RMT item for a logic low signal + dshot_tx_rmt_item[i].duration0 = dshot_config.ticks_zero_high; + dshot_tx_rmt_item[i].duration1 = dshot_config.ticks_zero_low; + } + + // Set level of RMT item + dshot_tx_rmt_item[i].level0 = 1; + dshot_tx_rmt_item[i].level1 = 0; + } + } + + // Set end marker for each frame + if (dshot_config.is_bidirectional) + { + dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = 1; + dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = 0; + } + else + { + dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = 0; + dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = 1; + } + + // Add packet seperator aka DShot Pause. + dshot_tx_rmt_item[DSHOT_PAUSE_BIT].duration1 = DSHOT_PAUSE; + + // Return the rmt_item + return dshot_tx_rmt_item; +} + +// Calculates a CRC value for a DShot digital control signal packet +uint16_t DShotRMT::calculateCRC(const dshot_packet_t &dshot_packet) +{ + uint16_t crc; + + // Combine the throttle value and telemetric request flag into a 16-bit packet value + const uint16_t packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request; + + // Calculate the CRC value using different bitwise operations depending on the DShot configuration + if (dshot_config.is_bidirectional) + { + // Bidirectional configuration: perform a bitwise negation of the result of XORing the packet with its right-shifted values by 4 and 8 bits, + // and then bitwise AND the result with 0x0F + const uint16_t intermediate_result = packet ^ (packet >> 4) ^ (packet >> 8); + crc = (~intermediate_result) & 0x0F; + } + else + { + // Unidirectional configuration: XOR the packet with its right-shifted values by 4 and 8 bits, + // and then bitwise AND the result with 0x0F + crc = (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F; + } + + // Return the calculated CRC value as a 16-bit unsigned integer + return crc; +} + +uint16_t DShotRMT::parseRmtPaket(const dshot_packet_t &dshot_packet) +{ + uint16_t parsedRmtPaket = DSHOT_NULL_PACKET; + uint16_t crc = calculateCRC(dshot_packet); + + // Complete the paket + parsedRmtPaket = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request; + parsedRmtPaket = (parsedRmtPaket << 4) | crc; + + return parsedRmtPaket; +} + +// Output using ESP32 RMT +void DShotRMT::sendRmtPaket(const dshot_packet_t &dshot_packet) +{ + buildTxRmtItem(parseRmtPaket(dshot_packet)); + + rmt_write_items(dshot_tx_rmt_config.channel, dshot_tx_rmt_item, DSHOT_PACKET_LENGTH, false); +} diff --git a/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.h b/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.h new file mode 100644 index 0000000..7713be5 --- /dev/null +++ b/Motor_Test_DSDHOT/lib/DShotRMT/DShotRMT.h @@ -0,0 +1,201 @@ +// +// Name: DShotRMT.h +// Created: 20.03.2021 00:49:15 +// Author: derdoktor667 +// + +#ifndef _DSHOTRMT_h +#define _DSHOTRMT_h + +#include + +// The RMT (Remote Control) module library is used for generating the DShot signal. +#include + +// Defines the library version +constexpr auto DSHOT_LIB_VERSION = "0.2.4"; + +// Constants related to the DShot protocol +constexpr auto DSHOT_CLK_DIVIDER = 8; // Slow down RMT clock to 0.1 microseconds / 100 nanoseconds per cycle +constexpr auto DSHOT_PACKET_LENGTH = 17; // Last pack is the pause +constexpr auto DSHOT_THROTTLE_MIN = 48; +constexpr auto DSHOT_THROTTLE_MAX = 2047; +constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000; +constexpr auto DSHOT_PAUSE = 21; // 21-bit is recommended +constexpr auto DSHOT_PAUSE_BIT = 16; +constexpr auto F_CPU_RMT = APB_CLK_FREQ; +constexpr auto RMT_CYCLES_PER_SEC = (F_CPU_RMT / DSHOT_CLK_DIVIDER); +constexpr auto RMT_CYCLES_PER_ESP_CYCLE = (F_CPU / RMT_CYCLES_PER_SEC); + +// Enumeration for the DShot mode +typedef enum dshot_mode_e +{ + DSHOT_OFF, + DSHOT150, + DSHOT300, + DSHOT600, + DSHOT1200 +} dshot_mode_t; + +// Array of human-readable DShot mode names +static const char *const dshot_mode_name[] = { + "DSHOT_OFF", + "DSHOT150", + "DSHOT300", + "DSHOT600", + "DSHOT1200"}; + +// Enumeration for telemetric request +typedef enum telemetric_request_e +{ + NO_TELEMETRIC, + ENABLE_TELEMETRIC, +} telemetric_request_t; + +// Structure for DShot packets +typedef struct dshot_packet_s +{ + uint16_t throttle_value : 11; + telemetric_request_t telemetric_request : 1; + uint16_t checksum : 4; +} dshot_packet_t; + +// Structure for eRPM packets +typedef struct eRPM_packet_s +{ + uint16_t eRPM_data : 12; + uint8_t checksum : 4; +} eRPM_packet_t; + +// return states of the RPM getting function +typedef enum dshot_erpm_exit_mode_e +{ + DECODE_SUCCESS = 0, + ERR_EMPTY_QUEUE, + ERR_NO_PACKETS, + ERR_CHECKSUM_FAIL, + ERR_BIDIRECTION_DISABLED, + +} dshot_erpm_exit_mode_t; + +// Structure for all settings for the DShot mode +typedef struct dshot_config_s +{ + dshot_mode_t mode; + String name_str; + bool is_bidirectional; + gpio_num_t gpio_num; + uint8_t pin_num; + rmt_channel_t rmt_channel; + uint8_t mem_block_num; + uint16_t ticks_per_bit; + uint8_t clk_div; + uint16_t ticks_zero_high; + uint16_t ticks_zero_low; + uint16_t ticks_one_high; + uint16_t ticks_one_low; +} dshot_config_t; + +// The official DShot Commands +typedef enum dshot_cmd_e +{ + DSHOT_CMD_MOTOR_STOP = 0, // Currently not implemented - STOP Motors + DSHOT_CMD_BEEP1, // Wait at least length of beep (380ms) before next command + DSHOT_CMD_BEEP2, // Wait at least length of beep (380ms) before next command + DSHOT_CMD_BEEP3, // Wait at least length of beep (400ms) before next command + DSHOT_CMD_BEEP4, // Wait at least length of beep (400ms) before next command + DSHOT_CMD_BEEP5, // Wait at least length of beep (400ms) before next command + DSHOT_CMD_ESC_INFO, // Currently not implemented + DSHOT_CMD_SPIN_DIRECTION_1, // Need 6x, no wait required + DSHOT_CMD_SPIN_DIRECTION_2, // Need 6x, no wait required + DSHOT_CMD_3D_MODE_OFF, // Need 6x, no wait required + DSHOT_CMD_3D_MODE_ON, // Need 6x, no wait required + DSHOT_CMD_SETTINGS_REQUEST, // Currently not implemented + DSHOT_CMD_SAVE_SETTINGS, // Need 6x, wait at least 12ms before next command + DSHOT_CMD_SPIN_DIRECTION_NORMAL, // Need 6x, no wait required + DSHOT_CMD_SPIN_DIRECTION_REVERSED, // Need 6x, no wait required + DSHOT_CMD_LED0_ON, // Currently not implemented + DSHOT_CMD_LED1_ON, // Currently not implemented + DSHOT_CMD_LED2_ON, // Currently not implemented + DSHOT_CMD_LED3_ON, // Currently not implemented + DSHOT_CMD_LED0_OFF, // Currently not implemented + DSHOT_CMD_LED1_OFF, // Currently not implemented + DSHOT_CMD_LED2_OFF, // Currently not implemented + DSHOT_CMD_LED3_OFF, // Currently not implemented + DSHOT_CMD_36, // Not yet assigned + DSHOT_CMD_37, // Not yet assigned + DSHOT_CMD_38, // Not yet assigned + DSHOT_CMD_39, // Not yet assigned + DSHOT_CMD_40, // Not yet assigned + DSHOT_CMD_41, // Not yet assigned + DSHOT_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY, // No wait required + DSHOT_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY, // No wait required + DSHOT_CMD_SIGNAL_LINE_CURRENT_TELEMETRY, // No wait required + DSHOT_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY, // No wait required + DSHOT_CMD_SIGNAL_LINE_ERPM_TELEMETRY, // No wait required + DSHOT_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY, // No wait required (also command 47) + DSHOT_CMD_MAX = 47 +} dshot_cmd_t; + +// ...Mapping for GCR +static const unsigned char GCR_encode[16] = + { + 0x19, 0x1B, 0x12, 0x13, + 0x1D, 0x15, 0x16, 0x17, + 0x1A, 0x09, 0x0A, 0x0B, + 0x1E, 0x0D, 0x0E, 0x0F}; + +// ...shifting 5 bits > 4 bits (0xff => invalid) +static const unsigned char GCR_decode[32] = + { + 0xFF, 0xFF, 0xFF, 0xFF, // 0 - 3 + 0xFF, 0xFF, 0xFF, 0xFF, // 4 - 7 + 0xFF, 9, 10, 11, // 8 - 11 + 0xFF, 13, 14, 15, // 12 - 15 + + 0xFF, 0xFF, 2, 3, // 16 - 19 + 0xFF, 5, 6, 7, // 20 - 23 + 0xFF, 0, 8, 1, // 24 - 27 + 0xFF, 4, 12, 0xFF, // 28 - 31 +}; + +// The main DShotRMT class +class DShotRMT +{ +public: + // Constructor for the DShotRMT class + DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel); + DShotRMT(uint8_t pin, uint8_t channel); + DShotRMT(uint8_t pin); + + // Destructor for the DShotRMT class + ~DShotRMT(); + + // Copy constructor for the DShotRMT class + DShotRMT(DShotRMT const &); + + // The begin() function initializes the DShotRMT class with + // a given DShot mode (DSHOT_OFF, DSHOT150, DSHOT300, DSHOT600, DSHOT1200) + // and a bidirectional flag. It returns a boolean value + // indicating whether or not the initialization was successful. + bool begin(dshot_mode_t dshot_mode = DSHOT_OFF, bool is_bidirectional = false); + + // The sendThrottleValue() function sends a DShot packet with a given + // throttle value (between 49 and 2047) and an optional telemetry + // request flag. + // void sendThrottleValue(uint16_t throttle_value, telemetric_request_t telemetric_request = NO_TELEMETRIC); + void sendThrottleValue(uint16_t throttle_value); + +private: + rmt_item32_t dshot_tx_rmt_item[DSHOT_PACKET_LENGTH]; // An array of RMT items used to send a DShot packet. + rmt_config_t dshot_tx_rmt_config; // The RMT configuration used for sending DShot packets. + dshot_config_t dshot_config; // The configuration for the DShot mode. + + rmt_item32_t *buildTxRmtItem(uint16_t parsed_packet); // Constructs an RMT item from a parsed DShot packet. + uint16_t calculateCRC(const dshot_packet_t &dshot_packet); // Calculates the CRC checksum for a DShot packet. + uint16_t parseRmtPaket(const dshot_packet_t &dshot_packet); // Parses an RMT packet to obtain a DShot packet. + + void sendRmtPaket(const dshot_packet_t &dshot_packet); // Sends a DShot packet via RMT. +}; + +#endif diff --git a/Motor_Test_DSDHOT/lib/README b/Motor_Test_DSDHOT/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/Motor_Test_DSDHOT/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Motor_Test_DSDHOT/platformio.ini b/Motor_Test_DSDHOT/platformio.ini new file mode 100644 index 0000000..7720063 --- /dev/null +++ b/Motor_Test_DSDHOT/platformio.ini @@ -0,0 +1,21 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-s3-devkitc-1] +platform = espressif32 +board = esp32-s3-devkitc-1 +framework = arduino +lib_deps = + moddingear/ESP32 ESC@^1.0.0 + fastled/FastLED@^3.6.0 + freenove/Freenove WS2812 Lib for ESP32@^1.0.6 +upload_speed = 921600 +monitor_speed = 115200 +monitor_filters = send_on_enter diff --git a/Motor_Test_DSDHOT/src/main.cpp b/Motor_Test_DSDHOT/src/main.cpp new file mode 100644 index 0000000..e7b129d --- /dev/null +++ b/Motor_Test_DSDHOT/src/main.cpp @@ -0,0 +1,73 @@ +#define LEDS_COUNT 1 +#define LEDS_PIN 48 +#define CHANNEL 0 +#include "Freenove_WS2812_Lib_for_ESP32.h" +#include "DShotESC.h" +#include "/Users/mingweiyeoh/Documents/GitHub/Arduino-Projects/libraries/Custom/USBSerialHandler.h" +SerialHandler myComputer = SerialHandler(); + +Freenove_ESP32_WS2812 strip = Freenove_ESP32_WS2812(LEDS_COUNT, LEDS_PIN, 0, TYPE_GRB); // Channel is always 0 for some reason..... + +DShotESC rmot; + +enum Colors { + RED, + ORANGE, + YELLOW, + LIME, + GREEN, + AQUA, + CYAN, + SKY, + BLUE, + PURPLE, + RED_PURPLE, +}; + +void setLed(int colorWheel) { + strip.setLedColorData(0, strip.hsv2rgb(colorWheel*30, 100, 100)); + strip.show(); +} + + +void setup() +{ + USBSerial.begin(115200); + + + rmot.install(GPIO_NUM_7, RMT_CHANNEL_1); //<-- This is the problem line. Apparently this line has to go before initializing the strip + delay(250); + rmot.init(); + delay(250); + rmot.sendMotorStop(); + delay(250); + // rmot.setReversed(false); + // rmot.set3DMode(true); + rmot.throttleArm(); // <--- Super important!!!; + delay(250); + // rmot.blueJayArm(); + // delay(1000); + // for (int i = 0; i < 10; i++) { + // rmot.sendThrottle3D(0); + // delay(10); + // } + + strip.begin(); + strip.setBrightness(10); + USBSerial.println("Done initializing!!!"); +} + +void loop() { + int throttle = myComputer.getInt(0); + rmot.sendThrottle3D(throttle); // Throttle value from -999 to 999 + // lmot.sendThrottle3D(throttle); + delay(1); // Maybe need to send update faster than 10ms?? + + if (throttle != 0) { + setLed(RED); + } else { + setLed(GREEN); + } + +} + diff --git a/Motor_Test_DSDHOT/test/README b/Motor_Test_DSDHOT/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Motor_Test_DSDHOT/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Motor_Test_PWM/.gitignore b/Motor_Test_PWM/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Motor_Test_PWM/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Motor_Test_PWM/.vscode/extensions.json b/Motor_Test_PWM/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/Motor_Test_PWM/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/Motor_Test_PWM/.vscode/settings.json b/Motor_Test_PWM/.vscode/settings.json new file mode 100644 index 0000000..d963677 --- /dev/null +++ b/Motor_Test_PWM/.vscode/settings.json @@ -0,0 +1,11 @@ +{ + "files.associations": { + "array": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp" + } +} \ No newline at end of file diff --git a/Motor_Test_PWM/include/README b/Motor_Test_PWM/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/Motor_Test_PWM/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Motor_Test_PWM/lib/README b/Motor_Test_PWM/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/Motor_Test_PWM/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Motor_Test_PWM/platformio.ini b/Motor_Test_PWM/platformio.ini new file mode 100644 index 0000000..785d779 --- /dev/null +++ b/Motor_Test_PWM/platformio.ini @@ -0,0 +1,21 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-c3-devkitc-1] +platform = espressif32 +board = esp32-c3-devkitm-1 +build_flags = + -D ARDUINO_USB_CDC_ON_BOOT=1 + -D ARDUINO_USB_MODE=1 +framework = arduino +upload_speed = 921600 +monitor_speed = 115200 +monitor_filters = send_on_enter +lib_deps = madhephaestus/ESP32Servo@^3.0.5 diff --git a/Motor_Test_PWM/src/main.cpp b/Motor_Test_PWM/src/main.cpp new file mode 100644 index 0000000..2533636 --- /dev/null +++ b/Motor_Test_PWM/src/main.cpp @@ -0,0 +1,46 @@ +#include +#include "/Users/mingweiyeoh/Documents/GitHub/Arduino-Projects/libraries/Custom/SerialHandler.h" + +SerialHandler myComputer = SerialHandler(); + +const int motpin = 1; +const int motchannel = 0; +const int neutralVal = 189; + +void autoMotorTune() { + delay(1000); + Serial.println("Start with motor off!!"); + ledcWrite(motchannel, 220); + delay(3000); + Serial.println("Turn motor on!!!"); + delay(5000); + ledcWrite(motchannel, 40); + Serial.println("Tuning low value!!!"); + delay(2500); + ledcWrite(motchannel, neutralVal); + Serial.println("Tuning neutral value!!!"); + delay(2500); + Serial.println("Done!!!"); +} + + +void setup() { + Serial.begin(115200); + ledcSetup(motchannel, 500, 8); + ledcAttachPin(motpin, motchannel); + ledcWrite(motchannel, neutralVal); + + + autoMotorTune(); + + while (myComputer.getInt(0) == 0) { + delay(10); + } +} + +void loop() { + // put your main code here, to run repeatedly: + ledcWrite(motchannel, myComputer.getInt(0)); + delay(10); +} + diff --git a/Motor_Test_PWM/test/README b/Motor_Test_PWM/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Motor_Test_PWM/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/Robot Code/.DS_Store b/Robot Code/.DS_Store new file mode 100644 index 0000000..cd5968c Binary files /dev/null and b/Robot Code/.DS_Store differ diff --git a/Robot Code/.gitignore b/Robot Code/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/Robot Code/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/Robot Code/.vscode/extensions.json b/Robot Code/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/Robot Code/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/Robot Code/.vscode/settings.json b/Robot Code/.vscode/settings.json new file mode 100644 index 0000000..151a080 --- /dev/null +++ b/Robot Code/.vscode/settings.json @@ -0,0 +1,56 @@ +{ + "files.associations": { + "*.sqlbook": "sql", + "*.ndjson": "jsonl", + "*.dbclient-js": "javascript", + "array": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp", + "random": "cpp", + "*.tcc": "cpp", + "memory": "cpp", + "istream": "cpp", + "functional": "cpp", + "tuple": "cpp", + "utility": "cpp", + "list": "cpp", + "atomic": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "system_error": "cpp", + "type_traits": "cpp", + "fstream": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "limits": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/Robot Code/data/motor_settings.txt b/Robot Code/data/motor_settings.txt new file mode 100644 index 0000000..c866e35 --- /dev/null +++ b/Robot Code/data/motor_settings.txt @@ -0,0 +1,2 @@ +{"rot" : "80", "tra" : "80", "per" : "0.5", "boost" : "100"} +{"drive" : "10", "turn" : "10", "boost" : "40"} \ No newline at end of file diff --git a/Robot Code/include/README b/Robot Code/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/Robot Code/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/Robot Code/include/pin_definitions.h b/Robot Code/include/pin_definitions.h new file mode 100644 index 0000000..5e84f0d --- /dev/null +++ b/Robot Code/include/pin_definitions.h @@ -0,0 +1,16 @@ +#define TOP_PHOTO_TRANSISTOR 6 +#define BOTTOM_PHOTO_TRANSISTOR 5 + +#define TOP_IR_PIN 42 +#define BOTTOM_IR_PIN 41 + +#define LEFT_MOTOR_PIN GPIO_NUM_37 +#define RIGHT_MOTOR_PIN GPIO_NUM_1 + +#define LEFT_MOTOR_CHANNEL RMT_CHANNEL_2 +#define RIGHT_MOTOR_CHANNEL RMT_CHANNEL_1 + +#define RED_LED_TOP 40 +#define RED_LED_BOT 35 + +#define BAT_PIN 18 diff --git a/Robot Code/lib/.DS_Store b/Robot Code/lib/.DS_Store new file mode 100644 index 0000000..c474800 Binary files /dev/null and b/Robot Code/lib/.DS_Store differ diff --git a/Robot Code/lib/BLE_Uart/BLE_Uart.cpp b/Robot Code/lib/BLE_Uart/BLE_Uart.cpp new file mode 100644 index 0000000..25a9c29 --- /dev/null +++ b/Robot Code/lib/BLE_Uart/BLE_Uart.cpp @@ -0,0 +1,84 @@ +#include + +BLEServer *pServer = NULL; +BLECharacteristic * pTxCharacteristic; +bool deviceConnected = false; +bool oldDeviceConnected = false; + +char* packetBuffer; +int packSize = 6; + +class MyServerCallbacks: public BLEServerCallbacks { + void onConnect(BLEServer* pServer) { + deviceConnected = true; + } + + void onDisconnect(BLEServer* pServer) { + deviceConnected = false; + } +}; + +class MyCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + std::string rxValue = pCharacteristic->getValue(); + if (rxValue.length() > 0) { + for (int i = 0; i < packSize; i++) { + packetBuffer[i] = rxValue[i]; + // USBSerial.print(rxValue[i]); + } + // USBSerial.println(); + } + } +}; + +BLE_Uart::BLE_Uart(char* _packetBuffer, int _packSize) { + packetBuffer = _packetBuffer; + packSize = _packSize; +} + +bool BLE_Uart::isConnected() { + return deviceConnected; +} + +void BLE_Uart::init_ble(const std::string &name) { + BLEDevice::init(name); + BLEDevice::setDeviceName(name); + + pServer = BLEDevice::createServer(); // Create the BLE Server + pServer->setCallbacks(new MyServerCallbacks()); + + BLEService *pService = pServer->createService(SERVICE_UUID); // Create the BLE Service= + + pTxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_TX,NIMBLE_PROPERTY::NOTIFY); // Create a BLE Characteristic + + BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID_RX,NIMBLE_PROPERTY::WRITE); + + pRxCharacteristic->setCallbacks(new MyCallbacks()); + + // Start the service + pService->start(); + + // Start advertising + pServer->getAdvertising()->start(); +} + +void BLE_Uart::send(String message) { + pTxCharacteristic->setValue(message); + pTxCharacteristic->notify(); +} + +void BLE_Uart::send(float value) + { + char conversionbuffer[5]; + sprintf(conversionbuffer, "%.2f", value); + send(conversionbuffer); + } + +ROBOT_MODES Robot_BLE_Uart::getMode() { + auto map = RobotModeMap.find(packetBuffer[0]); + if (map != RobotModeMap.end()) { + return map->second; + } else { + return ROBOT_MODES::IDLE; + } +} diff --git a/Robot Code/lib/BLE_Uart/BLE_Uart.h b/Robot Code/lib/BLE_Uart/BLE_Uart.h new file mode 100644 index 0000000..ff13c7f --- /dev/null +++ b/Robot Code/lib/BLE_Uart/BLE_Uart.h @@ -0,0 +1,33 @@ +#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID +#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" +#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + +#include +#include +class BLE_Uart { + public: + BLE_Uart(char* _packetBuffer, int _packSize); + void init_ble(const std::string &name); + void send(String message); + void send(float value); + bool isConnected(); + private: +}; + +enum class ROBOT_MODES { + IDLE, + MELTY, + TANK +}; + +static std::unordered_map RobotModeMap{ + {'0', ROBOT_MODES::IDLE}, + {'1', ROBOT_MODES::MELTY}, + {'2', ROBOT_MODES::TANK} +}; + +class Robot_BLE_Uart : public BLE_Uart { + public: + Robot_BLE_Uart(char* _packetBuffer, int _packSize) : BLE_Uart(_packetBuffer, _packSize) {}; + ROBOT_MODES getMode(); +}; \ No newline at end of file diff --git a/Robot Code/lib/Battery_Monitor/Battery_Monitor.h b/Robot Code/lib/Battery_Monitor/Battery_Monitor.h new file mode 100644 index 0000000..b1dd416 --- /dev/null +++ b/Robot Code/lib/Battery_Monitor/Battery_Monitor.h @@ -0,0 +1,24 @@ +#include + +int getPerc(int min, int max, int val) { + if (val < 100) // Switch not on + return -1; + int perc = 100 * (val - min) / double(max - min); + if (perc > 100) + perc = 100; + if (perc < 0) + perc = 0; + return perc; + +} +int get3sSOC(int pin) { + int max = 4096-50; + int min = 3000; + return getPerc(min, max ,analogRead(pin)); +} + +int get1sSOC(int pin) { + int max = 2900-50; + int min = 2048; + return getPerc(min, max ,analogRead(pin)); +} diff --git a/Robot Code/lib/Drive_Motors/Drive_Motors.cpp b/Robot Code/lib/Drive_Motors/Drive_Motors.cpp new file mode 100644 index 0000000..a3bb3b4 --- /dev/null +++ b/Robot Code/lib/Drive_Motors/Drive_Motors.cpp @@ -0,0 +1,67 @@ +#include "Drive_Motors.h" +#include + +Drive_Motors::Drive_Motors(gpio_num_t l_motor_pin, rmt_channel_t l_motor_channel, gpio_num_t r_motor_pin, rmt_channel_t r_motor_channel) : l_motor_pin(l_motor_pin), l_motor_channel(l_motor_channel), r_motor_pin(r_motor_pin), r_motor_channel(r_motor_channel) {} + +void Drive_Motors::init_motors() { + rmot.install(r_motor_pin, r_motor_channel); + rmot.init(); + rmot.setReversed(false); + rmot.set3DMode(true); + rmot.throttleArm(); // <--- Super important!!!; + + lmot.install(l_motor_pin, l_motor_channel); + lmot.init(); + lmot.setReversed(false); + lmot.set3DMode(true); + lmot.throttleArm(); // <--- Super important!!!; +} + +void Drive_Motors::arm_motors() { + for (int i = 0; i < 10; i++) + { + rmot.sendThrottle3D(0); + lmot.sendThrottle3D(0); + delay(10); + } +} + +void Drive_Motors::l_motor_write(int value) { + l_motor_value = value; + if (value != 0) + if (value > 0) + value += kickstart_value; + else + value -= kickstart_value; + EVERY_N_MILLIS(1) // Important or else DSHOT data is sent too fast. + if (!flip_motors) + lmot.sendThrottle3D(value); + else + rmot.sendThrottle3D(value); +} + +void Drive_Motors::r_motor_write(int value) { + r_motor_value = value; + if (value != 0) + if (value > 0) + value += kickstart_value; + else + value -= kickstart_value; + EVERY_N_MILLIS(1) // Important or else DSHOT data is sent too fast. + if (!flip_motors) + rmot.sendThrottle3D(value); + else + lmot.sendThrottle3D(value); +} + +void Drive_Motors::set_both_motors(int value) { + l_motor_write(value); + r_motor_write(value); +} + +bool Drive_Motors::isNeutral() { + if (l_motor_value == 0 && r_motor_value == 0) + return true; + else + return false; +} \ No newline at end of file diff --git a/Robot Code/lib/Drive_Motors/Drive_Motors.h b/Robot Code/lib/Drive_Motors/Drive_Motors.h new file mode 100644 index 0000000..4aead87 --- /dev/null +++ b/Robot Code/lib/Drive_Motors/Drive_Motors.h @@ -0,0 +1,27 @@ +#include "DShotESC.h" +#include "FastLED.h" + +class Drive_Motors { + public: + Drive_Motors(gpio_num_t l_motor_pin, rmt_channel_t l_motor_channel, gpio_num_t r_motor_pin, rmt_channel_t r_motor_channel); + void init_motors(); + void arm_motors(); + void l_motor_write(int value); + void r_motor_write(int value); + void set_both_motors(int value); + bool isNeutral(); + bool flip_motors = 0; + private: + DShotESC rmot; + DShotESC lmot; + + gpio_num_t l_motor_pin = GPIO_NUM_NC; + gpio_num_t r_motor_pin = GPIO_NUM_NC; + + rmt_channel_t l_motor_channel; + rmt_channel_t r_motor_channel; + + int kickstart_value = 8; + int l_motor_value = 0; + int r_motor_value = 0; +}; diff --git a/Robot Code/lib/LEDHandler/LEDHandler.cpp b/Robot Code/lib/LEDHandler/LEDHandler.cpp new file mode 100644 index 0000000..35cf955 --- /dev/null +++ b/Robot Code/lib/LEDHandler/LEDHandler.cpp @@ -0,0 +1,69 @@ +#include "LEDHandler.h" + +bool ledToggleState = 0; +unsigned long lastdelayToggle = millis(); +unsigned long currentDelayToggle = millis(); + +Colors lastColor1; +Colors lastColor2; + +Freenove_ESP32_WS2812 strip = Freenove_ESP32_WS2812(2, LEDPIN, 0, TYPE_GRB); + +int ledmode = BOTH; + +void init_led() { + strip.begin(); + strip.setBrightness(100); +} + +void setLedMode(ledmodes newledmode) { + ledmode = newledmode; +} + +void setLeds(Colors color) { + static unsigned long lastLedShowing = 0; + if (millis() - lastLedShowing > 5) { + if (ledmode == BOTH) { + if (color == BLACK) + strip.setAllLedsColorData(0, 0, 0); + else if (color == WHITE) + strip.setAllLedsColorData(255, 255, 255); + else + strip.setAllLedsColorData(strip.hsv2rgb(color*30, 100, 100)); + } else { // We are only using one of the two leds, not both anymore + strip.setAllLedsColorData(0, 0, 0); + + if (color == BLACK) + strip.setLedColorData(ledmode-1, 0, 0, 0); + else if (color == WHITE) + strip.setLedColorData(ledmode-1, 255, 255, 255); + else + strip.setLedColorData(ledmode-1, strip.hsv2rgb(color*30, 100, 100)); + } + strip.show(); + lastLedShowing = millis(); + } + +} + +void toggleLeds(Colors color1, Colors color2, int delayMS) { + if (color1 != lastColor1 || color2 != lastColor2 || millis() - lastdelayToggle > delayMS + 50) { + lastdelayToggle = millis(); + ledToggleState = 0; + setLeds(color1); + } + currentDelayToggle = millis(); + + if (currentDelayToggle - lastdelayToggle > delayMS) { + ledToggleState = !ledToggleState; + lastdelayToggle = currentDelayToggle; + } + + if (ledToggleState) + setLeds(color1); + else + setLeds(color2); + + lastColor1 = color1; + lastColor2 = color2; +} \ No newline at end of file diff --git a/Robot Code/lib/LEDHandler/LEDHandler.h b/Robot Code/lib/LEDHandler/LEDHandler.h new file mode 100644 index 0000000..da48795 --- /dev/null +++ b/Robot Code/lib/LEDHandler/LEDHandler.h @@ -0,0 +1,36 @@ +#include +#include + +#ifdef IR_BEACON + #define LEDPIN 4 +#else + #define LEDPIN 45 +#endif + +enum Colors { + RED, + ORANGE, + YELLOW, + LIME, + GREEN, + AQUA, + CYAN, + SKY, + BLUE, + PURPLE, + RED_PURPLE, + BLACK, + WHITE +}; + +enum ledmodes{ + BOTH, BOTTOM, TOP +}; + +void setLedMode(ledmodes newledmode); + +void init_led(); + +void setLeds(Colors color); + +void toggleLeds(Colors color1, Colors color2, int delayMS); \ No newline at end of file diff --git a/Robot Code/lib/Melty/melty.cpp b/Robot Code/lib/Melty/melty.cpp new file mode 100644 index 0000000..002cd67 --- /dev/null +++ b/Robot Code/lib/Melty/melty.cpp @@ -0,0 +1,140 @@ +#include +#include + +melty::melty(int top_ir_pin, int bottom_ir_pin) : top_ir_pin(top_ir_pin), bottom_ir_pin(bottom_ir_pin) { + period_micros_calc = ringBuffer(period_micros_calc_array, 5, 1); + time_seen_beacon_calc = ringBuffer(time_seen_beacon_calc_array, TIME_SEEN_BEACON_ARRAY_SIZE, 0.7); +} + +bool melty::update() { + bool curSeenIRLed; + if (useTopIr) + curSeenIRLed = isBeaconSensed(!digitalRead(top_ir_pin)); + else + curSeenIRLed = isBeaconSensed(!digitalRead(bottom_ir_pin)); + + if (curSeenIRLed != lastSeenIRLed) { + if (curSeenIRLed) { // Activates on the rising edge of seeing the IR LED + currentPulse = micros(); + } + else { // Activates on the falling edge of seeing the IR LED + time_seen_beacon = micros() - currentPulse; + + time_seen_beacon_calc.update(time_seen_beacon); + + if (time_seen_beacon_calc.isLegit(time_seen_beacon)) { + unsigned long curMicros = micros(); + period_micros = curMicros - lastPulse; // How long it takes to complete one revolution + period_micros_calc.update(period_micros); + lastPulse = curMicros; + computeTimings(); + } + } + } + + lastSeenIRLed = curSeenIRLed; + return curSeenIRLed; +} + +void melty::computeTimings() { + unsigned long rotation_period; + ledRPM = (us_per_min)/(period_micros_calc.getMinVal()); + rotation_period = period_micros_calc.getMinVal(); + + + + unsigned long center_of_beacon = currentPulse + time_seen_beacon/2; // This should ideally be centered on the beacon + unsigned long centerOfDrivePulse = center_of_beacon + (float(deg)/360)*rotation_period; // Direction that we should be driving towards + unsigned long deltaDriveTiming = (percentageOfRotation * rotation_period)/2; + + unsigned long offset = 0; + if (centerOfDrivePulse - deltaDriveTiming < micros()) { // If we're supposed to start translating before we have calculated those values, offset by one rotation + offset = rotation_period; + } + + for (int i = 0; i < TRANSLATE_TIMINGS_SIZE/2; i++) { + int index = 0; + if (timingToggle) + index = i*2; + else + index = i*2+1; + + startDrive[index] = centerOfDrivePulse - deltaDriveTiming + offset + rotation_period*i; + endDrive[index] = centerOfDrivePulse + deltaDriveTiming + offset + rotation_period*i; + + startDriveInverse[index] = startDrive[index] + rotation_period/2; + endDriveInverse[index] = endDrive[index] + rotation_period/2; + } + + timingToggle = !timingToggle; // Toggle it so that next iteration uses different variables + +} + +int melty::translate() { // Returns percentage that robot should translate + unsigned long currentTime = micros(); + + if (percentageOfRotation != 0) { + for (int i = 0; i < TRANSLATE_TIMINGS_SIZE; i++) { + if (currentTime > startDrive[i] && currentTime < endDrive[i]) { + int delta = endDrive[i] - startDrive[i]; + int timeSinceStart = currentTime - startDrive[i]; + int maxPowerAt = delta/2; + int distanceFromMax = abs(timeSinceStart - maxPowerAt); + + return 100 - (100 * distanceFromMax/maxPowerAt); + } + } + + for (int i = 0; i < TRANSLATE_TIMINGS_SIZE; i++) { + if (currentTime > startDriveInverse[i] && currentTime < endDriveInverse[i]) { + int delta = endDriveInverse[i] - startDriveInverse[i]; + int timeSinceStart = currentTime - startDriveInverse[i]; + int maxPowerAt = delta/2; + int distanceFromMax = abs(timeSinceStart - maxPowerAt); + + return -(100 - (100 * distanceFromMax/maxPowerAt)); + } + } + + } + return 0; +} + +// bool melty::translateInverse() { // Returns whether or not robot should translate now +// unsigned long currentTime = micros(); + +// if (percentageOfRotation != 0) { +// for (int i = 0; i < TRANSLATE_TIMINGS_SIZE; i++) { +// if (currentTime > startDriveInverse[i] && currentTime < endDriveInverse[i]) +// return 1; +// } +// return 0; +// } +// else +// return 0; +// } + +bool melty::isBeaconSensed(bool currentReading) { + // for (int i = 0; i < IRLedDataSize; i++) + // USBSerial.print(IRLedReadings[i]); + // USBSerial.println(); + enum states {seen, not_seen}; + + IRLedReadings[IRLedIndex++] = currentReading; // Add to ring buffer + if (IRLedIndex == IRLedDataSize) + IRLedIndex = 0; + + if (lastIRLedReturnValue == seen) { + for (int i = 0; i < IRLedDataSize; i++) + if (IRLedReadings[i] == lastIRLedReturnValue) // If any of our readings in the ring buffer is the last value, keep the previous state + return seen; + lastIRLedReturnValue = not_seen; + return not_seen; + } else { // This is to cover the not_seen case + for (int i = 0; i < IRLedDataSize; i++) + if (IRLedReadings[i] == lastIRLedReturnValue) + return not_seen; + lastIRLedReturnValue = seen; + return seen; + } +} diff --git a/Robot Code/lib/Melty/melty.h b/Robot Code/lib/Melty/melty.h new file mode 100644 index 0000000..d0506b8 --- /dev/null +++ b/Robot Code/lib/Melty/melty.h @@ -0,0 +1,56 @@ +#include +#include + +#define IRLedDataSize 40 // Size of our Ring Buffer that will hold the IR Led data + +#define TRANSLATE_TIMINGS_SIZE 6 +#define TIME_SEEN_BEACON_ARRAY_SIZE 5 +class melty { + public: + melty(int top_ir_pin, int bottom_ir_pin); + bool update(); + bool isBeaconSensed(bool currentReading); + void computeTimings(); + int translate(); + // bool translateInverse(); + int ledRPM = 0; + unsigned long acccel_period = 1; + int deg = 0; + float percentageOfRotation = 0; + bool useTopIr = 1; + + long period_micros_calc_array[5] = {0}; + ringBuffer period_micros_calc; + + long time_seen_beacon_calc_array[TIME_SEEN_BEACON_ARRAY_SIZE] = {0}; + ringBuffer time_seen_beacon_calc; + + const int us_per_min = 60000000; + + private: + + int top_ir_pin = 0; + int bottom_ir_pin = 0; + + bool lastSeenIRLed = 0; + long period_micros = micros(); + long time_seen_beacon = micros(); + long currentPulse = micros(), lastPulse = micros(); + + bool IRLedReadings[IRLedDataSize] = {0}; + int IRLedIndex = 0; + + bool lastIRLedReturnValue = 0; + + long startDrive[TRANSLATE_TIMINGS_SIZE] = {0}; + long endDrive[TRANSLATE_TIMINGS_SIZE] = {0}; + + long startDriveInverse[TRANSLATE_TIMINGS_SIZE] = {0}; + long endDriveInverse[TRANSLATE_TIMINGS_SIZE] = {0}; + + bool timingToggle = 0; + + +}; + + \ No newline at end of file diff --git a/Robot Code/lib/PID/PID.h b/Robot Code/lib/PID/PID.h new file mode 100644 index 0000000..cf8d9d6 --- /dev/null +++ b/Robot Code/lib/PID/PID.h @@ -0,0 +1,45 @@ +#include + +struct PID { + float kP = .01; + float kI = 0; + float kD = .01; + float maxError = 100; + float maxI = 2; + float maxOut = 2; +} PID; + +int calcTurnPower(float headingError) { + if (headingError > PID.maxError) + headingError = PID.maxError; + if (headingError < -PID.maxError) + headingError = -PID.maxError; + // Static variables to store the previous error and the integral of errors + static float prevHeadingError = 0; + static float integral = 0; + static unsigned long lastCalc = millis(); + float derivative; + float output; + + // PID calculations + integral += headingError; + if (integral > PID.maxI) + integral = PID.maxI; + if ((headingError > 0 && prevHeadingError < 0) || (headingError < 0 && prevHeadingError > 0)) + integral = 0; + + derivative = headingError - prevHeadingError / (millis() - lastCalc); + lastCalc = millis(); + // PID output calculation + output = PID.kP * headingError + PID.kI * integral + PID.kD * derivative; + + if (output > PID.maxOut) + output = PID.maxOut; + else if(output < -PID.maxOut) + output = -PID.maxOut; + + // Update previous error + prevHeadingError = headingError; + + return (int)output; +} \ No newline at end of file diff --git a/Robot Code/lib/Photo_Transistors/Photo_Transistors.h b/Robot Code/lib/Photo_Transistors/Photo_Transistors.h new file mode 100644 index 0000000..6bd857d --- /dev/null +++ b/Robot Code/lib/Photo_Transistors/Photo_Transistors.h @@ -0,0 +1,56 @@ +#include +#include +#define RING_BUF_PHOTOTRANS_SIZE 10 + +class robotOrientation{ + + private: + bool curIsFlipped = 0; + + + bool lastIsFlipped = 0; + unsigned long lastTransition = millis(); + int topPin; + int bottomPin; + + const int delayTimeMS = 100; + + long topPhotoTransVals[RING_BUF_PHOTOTRANS_SIZE] = {0}; + ringBuffer topPhotoRingBuf = ringBuffer(topPhotoTransVals, RING_BUF_PHOTOTRANS_SIZE, 0); + + long bottomPhotoTransVals[RING_BUF_PHOTOTRANS_SIZE] = {0}; + ringBuffer bottomPhotoRingBuf = ringBuffer(bottomPhotoTransVals, RING_BUF_PHOTOTRANS_SIZE, 0); + + public: + bool isFlippedResult = 0; + + robotOrientation(int topPin, int bottomPin) : topPin(topPin), bottomPin(bottomPin) {} + + void printDebugInfo() { + USBSerial.printf("Top : %d Bottom: %d \n", analogRead(topPin), analogRead(bottomPin) ); + } + + bool checkIsFlipped() { + topPhotoRingBuf.update(analogRead(topPin)); + bottomPhotoRingBuf.update(analogRead(bottomPin)); + const int deltaVal = 1; + if (bottomPhotoRingBuf.getMaxVal() - topPhotoRingBuf.getMaxVal() > deltaVal) + curIsFlipped = 1; + if (topPhotoRingBuf.getMaxVal() - bottomPhotoRingBuf.getMaxVal() > deltaVal) + curIsFlipped = 0; + + if (curIsFlipped != lastIsFlipped) + lastTransition = millis(); + + if (curIsFlipped == 1 && millis() - lastTransition > delayTimeMS) + isFlippedResult = 1; + + if (curIsFlipped == 0 && millis() - lastTransition > delayTimeMS) + isFlippedResult = 0; + + lastIsFlipped = curIsFlipped; + return isFlippedResult; + } +}; + + diff --git a/Robot Code/lib/README b/Robot Code/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/Robot Code/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/Robot Code/lib/databasehandler/databasehandler.cpp b/Robot Code/lib/databasehandler/databasehandler.cpp new file mode 100644 index 0000000..150af9b --- /dev/null +++ b/Robot Code/lib/databasehandler/databasehandler.cpp @@ -0,0 +1,50 @@ +#include + +database_handler::database_handler() {} + +void database_handler::updateFromDatabase() { + File databaseFile = SPIFFS.open("/motor_settings.txt", "r"); + + melty_param_string = databaseFile.readStringUntil('\n'); + tankdrive_param_string = databaseFile.readStringUntil('\n'); + + databaseFile.close(); +} + +void database_handler::storeMeltyParameters(int rot, int tra, float per, int boost) { + JsonDocument newSettings; + + newSettings["rot"] = rot; + newSettings["tra"] = tra; + newSettings["per"] = per; + newSettings["boost"] = boost; + + File newFile = SPIFFS.open("/motor_settings.txt", "w"); + + String newSettingsString; + serializeJson(newSettings, newSettingsString); + newFile.println(newSettingsString); + newFile.println(tankdrive_param_string); + + newFile.close(); + newSettings = 1; + updateFromDatabase(); +} + +void database_handler::storeTankParameters(int drive, int turn, int boost) { + JsonDocument newSettings; + + newSettings["drive"] = drive; + newSettings["turn"] = turn; + newSettings["boost"] = boost; + + File newFile = SPIFFS.open("/motor_settings.txt", "w"); + String newSettingsString; + serializeJson(newSettings, newSettingsString); + newFile.println(melty_param_string); + newFile.println(newSettingsString); + + newFile.close(); + newSettings = 1; + updateFromDatabase(); +} \ No newline at end of file diff --git a/Robot Code/lib/databasehandler/databasehandler.h b/Robot Code/lib/databasehandler/databasehandler.h new file mode 100644 index 0000000..9394003 --- /dev/null +++ b/Robot Code/lib/databasehandler/databasehandler.h @@ -0,0 +1,25 @@ +#include +#include +#include +#include + +using namespace std; + +class database_handler { + public: + String melty_param_string; + String tankdrive_param_string; + + bool newSettings = 1; + + database_handler(); + + void updateFromDatabase(); + + void storeMeltyParameters(int rot, int tra, float per, int boost); + + void storeTankParameters(int drive, int turn, int boost); + + private: + +}; \ No newline at end of file diff --git a/Robot Code/lib/ringBuffer/ringBuffer.h b/Robot Code/lib/ringBuffer/ringBuffer.h new file mode 100644 index 0000000..9f33859 --- /dev/null +++ b/Robot Code/lib/ringBuffer/ringBuffer.h @@ -0,0 +1,85 @@ +#include +#ifndef RINGBUFFER_H +#define RINGBUFFER_H +class ringBuffer{ + public: + /** + * @brief Construct a ring buffer object. + * + * @param _criteria How close should the new value match the max value from the ring buffer to be considered legit? + */ + ringBuffer(long *_ringBuf, int _arraysize, float _criteria) { + ringBuf = _ringBuf; + arraySize = _arraysize; + criteria = _criteria; + } + + ringBuffer() {} + + /** + * @brief Adds a value to the ring buffer. + * + * @param val Value to be added to the ring buffer. + */ + void update(long val) { + ringBuf[curIndex++] = val; + if (curIndex > arraySize-1) + curIndex = 0; + } + + /** + * @brief Compares the input value to the max value. + * + * @param newVal New value to check against current max value. + * + * @return Whether new value is some percentage of max value. Based on the criteria variable. + */ + bool isLegit(long newVal) { + long maxVal = getMaxVal(); + if (newVal > maxVal * criteria) + return true; + else + return false; + } + + /** + * @brief Returns maximum value from our ring buffer. + */ + long getMaxVal() { + long maxVal = 0; + for (int i = 0; i < arraySize; i++) + if (maxVal < ringBuf[i]) + maxVal = ringBuf[i]; + return maxVal; + } + + long getMinVal() { + long minVal = ringBuf[0]; + for (int i = 1; i < arraySize; i++) + if (minVal > ringBuf[i]) + minVal = ringBuf[i]; + if (minVal == 0) + minVal = 1; + return minVal; + } + + String returnArray() { // For debug purposes + String msg = ""; + for (int i = 0; i < arraySize; i++) { + msg = msg + String(ringBuf[i]) + " "; + } + return msg; + } + + + + private: + long *ringBuf; + int arraySize; + + int curIndex = 0; + float criteria = 0; + +}; + +#endif \ No newline at end of file diff --git a/Robot Code/platformio.ini b/Robot Code/platformio.ini new file mode 100644 index 0000000..5782b16 --- /dev/null +++ b/Robot Code/platformio.ini @@ -0,0 +1,23 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:esp32-s3-devkitc-1] +platform = espressif32 +board = esp32-s3-devkitc-1 +framework = arduino +upload_speed = 921600 +monitor_speed = 115200 +lib_deps = + fastled/FastLED@^3.6.0 + Wire + h2zero/NimBLE-Arduino@^1.4.1 + freenove/Freenove WS2812 Lib for ESP32@^1.0.6 + moddingear/ESP32 ESC@^1.0.0 + bblanchon/ArduinoJson@^7.0.4 diff --git a/Robot Code/src/main.cpp b/Robot Code/src/main.cpp new file mode 100644 index 0000000..1c552ea --- /dev/null +++ b/Robot Code/src/main.cpp @@ -0,0 +1,283 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "SPIFFS.h" +#include + +#define IS_HOCKEY_PUCK 0 + +const int packSize = 6; +char laptop_packetBuffer[packSize] = {'0', '0', '0', '0', '0', '0'}; +const int headings[] = {0, 45, 90, 135, 180, 225, 270, 315}; +Robot_BLE_Uart transmitter = Robot_BLE_Uart(laptop_packetBuffer, packSize); +Drive_Motors driveMotors = Drive_Motors(LEFT_MOTOR_PIN, LEFT_MOTOR_CHANNEL, RIGHT_MOTOR_PIN, RIGHT_MOTOR_CHANNEL); +robotOrientation myPTs = robotOrientation(TOP_PHOTO_TRANSISTOR, BOTTOM_PHOTO_TRANSISTOR); +database_handler motor_settings = database_handler(); + +melty oreo = melty(TOP_IR_PIN, BOTTOM_IR_PIN); + +struct melty_parameters { + int rot; + int tra; + float per; + int boost; +} melty_parameters; + +struct tank_drive_parameters { + int drive; + int turn; + int boost; +} tank_drive_parameters; + +int tuningValue = 10; + +void setup() +{ + init_led(); + setLeds(ORANGE); + + SPIFFS.begin(true); + + motor_settings.updateFromDatabase(); + + driveMotors.init_motors(); // <- This needs to be init first or else something with RMT doesnt work.... + + pinMode(RED_LED_BOT, OUTPUT); + pinMode(RED_LED_TOP, OUTPUT); + digitalWrite(RED_LED_BOT, LOW); + digitalWrite(RED_LED_TOP, LOW); + + driveMotors.arm_motors(); + transmitter.init_ble("Oreo"); + setLeds(BLACK); + + delay(250); // Power up delay for the Serial + USBSerial.begin(115200); +} + +void loop() +{ + if (!transmitter.isConnected() || laptop_packetBuffer[0] != '1') { // Turn off high powered leds if not meltying + digitalWrite(RED_LED_TOP, LOW); + digitalWrite(RED_LED_BOT, LOW); + } + + if (transmitter.isConnected()) { + EVERY_N_MILLIS(50) { + myPTs.printDebugInfo(); + if (!myPTs.checkIsFlipped()) { // Not flipped + oreo.useTopIr = 1; + driveMotors.flip_motors = 0; + setLedMode(TOP); + } else { + oreo.useTopIr = 0; + driveMotors.flip_motors = 1; + setLedMode(BOTTOM); + } + + if (motor_settings.newSettings == true) { + JsonDocument melty_params_json; + deserializeJson(melty_params_json, motor_settings.melty_param_string); + + melty_parameters.rot = melty_params_json["rot"].as(); + melty_parameters.tra = melty_params_json["tra"].as(); + melty_parameters.per = melty_params_json["per"].as(); + melty_parameters.boost = melty_params_json["boost"].as(); + + JsonDocument tank_params_json; + deserializeJson(tank_params_json, motor_settings.tankdrive_param_string); + + tank_drive_parameters.drive = tank_params_json["drive"].as(); + tank_drive_parameters.turn = tank_params_json["turn"].as(); + tank_drive_parameters.boost = tank_params_json["boost"].as(); + + motor_settings.newSettings = false; + + } + } + + + if (transmitter.getMode() == ROBOT_MODES::MELTY) { // Currently enabled and meltybraining!!! + setLedMode(BOTH); + setLeds(BLACK); + if (oreo.update()) { // If seen the LED + if (oreo.useTopIr) + digitalWrite(RED_LED_TOP, HIGH); + else + digitalWrite(RED_LED_BOT, HIGH); + EVERY_N_MILLIS(250) { + transmitter.send("seen"); + } + + } else { + digitalWrite(RED_LED_TOP, LOW); + digitalWrite(RED_LED_BOT, LOW); + } + + int boostVal = 0; + if (laptop_packetBuffer[3] == '1') + boostVal = melty_parameters.boost; + + int adjRotValue = melty_parameters.rot + boostVal; + int adjTransValue = melty_parameters.tra + boostVal; + float transMultiplier = oreo.translate() / 100.0; + + if (transMultiplier != 0) { + driveMotors.l_motor_write(adjRotValue - adjTransValue * transMultiplier); + driveMotors.r_motor_write(adjRotValue + adjTransValue * transMultiplier); + } else { + driveMotors.set_both_motors((adjRotValue)); + } + + int drivecmd = laptop_packetBuffer[1] - '0'; + if (drivecmd > 0 && drivecmd < 9) { // 1,2,3,4,5,6,7,8 + drivecmd -= 1; // Make it 0 indexed, so now it goes 0 -> 7 + drivecmd = 7 - drivecmd; + + if (drivecmd < 0 || drivecmd > 7) { + drivecmd = (drivecmd % 8 + 8) % 8; // Make sure we always in the bounds of the array + } + + oreo.deg = headings[drivecmd]; + } + + if (laptop_packetBuffer[1] != '0') { // Check drive cmd for setting the "neutral" state + oreo.percentageOfRotation = melty_parameters.per; + } else { + oreo.percentageOfRotation = 0; + } + + EVERY_N_SECONDS(1) { // DEBUGGIN!!!! + String msg = "RPM : " + String(oreo.ledRPM); + transmitter.send(msg); + } + + EVERY_N_MILLIS(100) { + switch (laptop_packetBuffer[2]) { + case '1': + melty_parameters.rot+=tuningValue; + break; + case '2': + melty_parameters.rot-=tuningValue; + break; + case '3': + melty_parameters.tra+=tuningValue; + break; + case '4': + melty_parameters.tra-=tuningValue; + break; + case '5': + melty_parameters.per = melty_parameters.per + .03; + break; + case '6': + melty_parameters.per = melty_parameters.per - .03; + break; + } + + if (laptop_packetBuffer[2] != '0') { + String msg = "rotpwr : " + String(melty_parameters.rot) + " tranpwr : " + String(melty_parameters.tra) + " perc : " + String(melty_parameters.per); + transmitter.send(msg); + + motor_settings.storeMeltyParameters(melty_parameters.rot, melty_parameters.tra, melty_parameters.per, melty_parameters.boost); + } + } + + } else if (transmitter.getMode() == ROBOT_MODES::TANK) { // Tank driving mode! + + int lmotorpwr = 0; + int rmotorpwr = 0; + + int boostVal = 0; + if (laptop_packetBuffer[3] == '1') + boostVal = tank_drive_parameters.boost; + + + switch (laptop_packetBuffer[1]) { // Check the drive cmd + case '0': + lmotorpwr = 0; + rmotorpwr = 0; + break; + case '1': + lmotorpwr = tank_drive_parameters.drive + boostVal; + rmotorpwr = tank_drive_parameters.drive + boostVal; + break; + case '2': + lmotorpwr = tank_drive_parameters.drive + tank_drive_parameters.turn + boostVal; + rmotorpwr = tank_drive_parameters.drive + boostVal; + break; + case '3': + lmotorpwr = tank_drive_parameters.turn + boostVal; + // rmotorpwr = -tank_drive_parameters.turn; + break; + case '4': + lmotorpwr = -tank_drive_parameters.drive - tank_drive_parameters.turn - boostVal; + rmotorpwr = -tank_drive_parameters.drive - boostVal; + break; + case '5': + lmotorpwr = -tank_drive_parameters.drive - boostVal; + rmotorpwr = -tank_drive_parameters.drive - boostVal; + break; + case '6': + lmotorpwr = -tank_drive_parameters.drive - boostVal; + rmotorpwr = -tank_drive_parameters.drive - tank_drive_parameters.turn - boostVal; + break; + case '7': + // lmotorpwr = -tank_drive_parameters.turn; + rmotorpwr = tank_drive_parameters.turn + boostVal; + break; + case '8': + lmotorpwr = tank_drive_parameters.drive + boostVal; + rmotorpwr = tank_drive_parameters.drive + tank_drive_parameters.turn + boostVal; + break; + } + + EVERY_N_MILLIS(100) { + switch (laptop_packetBuffer[2]) { + case '1': + tank_drive_parameters.drive+=tuningValue; + break; + case '2': + tank_drive_parameters.drive-=tuningValue; + break; + case '3': + tank_drive_parameters.turn+=tuningValue; + break; + case '4': + tank_drive_parameters.turn-=tuningValue; + break; + } + + if (laptop_packetBuffer[2] != '0') { + String msg = "drivpwr : " + String(tank_drive_parameters.drive) + " turnpwr : " + String(tank_drive_parameters.turn); + transmitter.send(msg); + + motor_settings.storeTankParameters(tank_drive_parameters.drive, tank_drive_parameters.turn, tank_drive_parameters.boost); + } + } + + driveMotors.l_motor_write(-lmotorpwr); + driveMotors.r_motor_write(rmotorpwr); + toggleLeds(WHITE, BLACK, 500); + + } else if (transmitter.getMode() == ROBOT_MODES::IDLE ){ // Currently DISABLED + toggleLeds(RED, GREEN, 500); + driveMotors.set_both_motors(0); + + EVERY_N_SECONDS(1) { + transmitter.send("SOC: " + String(get3sSOC(BAT_PIN)) + " %"); + } + } + } else { // Currently DISCONNECTED + setLedMode(BOTH); + driveMotors.set_both_motors(0); + toggleLeds(RED, BLACK, 500); + } + +} + +// this is katie hello :)) \ No newline at end of file diff --git a/Robot Code/test/README b/Robot Code/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/Robot Code/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html diff --git a/bluetooth scanner.py b/bluetooth scanner.py new file mode 100644 index 0000000..c08a78d --- /dev/null +++ b/bluetooth scanner.py @@ -0,0 +1,10 @@ +import asyncio +from bleak import BleakScanner + +async def scan(): + devices = await BleakScanner.discover(timeout=2) + for device in devices: + if device.name is not None: + print(device) + +asyncio.run(scan()) \ No newline at end of file diff --git a/bluetooth testing.py b/bluetooth testing.py new file mode 100644 index 0000000..a9b9052 --- /dev/null +++ b/bluetooth testing.py @@ -0,0 +1,69 @@ +import asyncio +import sys + +from bleak import BleakScanner, BleakClient +from bleak.backends.scanner import AdvertisementData +from bleak.backends.device import BLEDevice + +class BLE_UART: + UART_SERVICE_UUID = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" + UART_RX_CHAR_UUID = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" + UART_TX_CHAR_UUID = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + + def __init__(self, peripheral_name='ESP32_BLE'): + self._peripheral_name = peripheral_name + self._rx_queue = asyncio.Queue() + + async def read(self): + msg = await self._rx_queue.get() + return msg + + def getBytes(self, letter): + return int(letter).to_bytes(1) + + async def write(self, msg): + await self._client.write_gatt_char(self.UART_RX_CHAR_UUID, msg.encode()) + + async def connect(self): + self._discovery_queue = asyncio.Queue() + device = None + print(f"scanning for {self._peripheral_name}") + async with BleakScanner(detection_callback=self._find_uart_device): + device: BLEDevice = await self._discovery_queue.get() + print(f"connecting to {self._peripheral_name} ...", end="") + client = self._client = BleakClient(device, disconnected_callback=self._handle_disconnect) + await client.connect() + await client.start_notify(self.UART_TX_CHAR_UUID, self._rx_handler) + print(f" connected") + + async def disconnect(self): + await self._client.disconnect() + + async def __aenter__(self): + return self + + async def __aexit__(self, *args): + await self.disconnect() + + def _rx_handler(self, _: int, data: bytearray): + self._rx_queue.put_nowait(data) + + def _find_uart_device(self, device: BLEDevice, adv: AdvertisementData): + if device.name == self._peripheral_name: + self._discovery_queue.put_nowait(device) + + def _handle_disconnect(self, _: BleakClient): + self._rx_queue.put_nowait(None) + + +async def main(): + async with BLE_UART() as uart: + await uart.connect() + + for i in range(3): + # msg = 123456 + await uart.write("123456") + await asyncio.sleep(1) +# num = "123456" +# print(int(num[0]).to_bytes(1)) +asyncio.run(main()) \ No newline at end of file diff --git a/controller/LaptopKeyboard.py b/controller/LaptopKeyboard.py new file mode 100644 index 0000000..5c39849 --- /dev/null +++ b/controller/LaptopKeyboard.py @@ -0,0 +1,24 @@ +from pynput.keyboard import Key, Listener, KeyCode +from time import sleep +key_state = {} + +# Function to be called when a key is pressed +def on_press(key): + key = str(key).strip("'") + # print(len(key)) + if len(key) == 1 and key.isupper(): + key = key.lower() + key_state[key] = True # Update key state to pressed + +# Function to be called when a key is released +def on_release(key): + key = str(key).strip("'") + if len(key) == 1 and key.isupper(): + key = key.lower() + key_state[key] = False # Update key state to released + +def get_key_state(key): + if (key in key_state and key_state[key]): + return True + else: + return False \ No newline at end of file diff --git a/controller/__pycache__/AlipayTelemetry.cpython-311.pyc b/controller/__pycache__/AlipayTelemetry.cpython-311.pyc new file mode 100644 index 0000000..b7317a9 Binary files /dev/null and b/controller/__pycache__/AlipayTelemetry.cpython-311.pyc differ diff --git a/controller/__pycache__/LaptopKeyboard.cpython-311.pyc b/controller/__pycache__/LaptopKeyboard.cpython-311.pyc new file mode 100644 index 0000000..83ff409 Binary files /dev/null and b/controller/__pycache__/LaptopKeyboard.cpython-311.pyc differ diff --git a/controller/__pycache__/bluetooth.cpython-311.pyc b/controller/__pycache__/bluetooth.cpython-311.pyc new file mode 100644 index 0000000..db5e194 Binary files /dev/null and b/controller/__pycache__/bluetooth.cpython-311.pyc differ diff --git a/controller/__pycache__/streamlit.cpython-311.pyc b/controller/__pycache__/streamlit.cpython-311.pyc new file mode 100644 index 0000000..d184218 Binary files /dev/null and b/controller/__pycache__/streamlit.cpython-311.pyc differ diff --git a/controller/app.py b/controller/app.py new file mode 100644 index 0000000..7fa5a46 --- /dev/null +++ b/controller/app.py @@ -0,0 +1,43 @@ +import streamlit as st +import time +import random + +# Function to simulate checking connection status for each device +def check_connection(device_id): + time.sleep(1) # Simulate a delay for checking each device + return random.choice([True, False]) # Randomly return connected or disconnected status + +# Set up the Streamlit app +st.title("Multi-Device Connection Status Checker") + +# Define the device names +devices = ["Device 1", "Device 2", "Device 3"] + +# Layout the devices in columns +status_columns = st.columns(len(devices)) + +# Function to update the status with a loading bar +def update_status(): + for i, device in enumerate(devices): + with status_columns[i]: + st.subheader(device) + st.spinner("Connecting...") + # Show a progress bar while checking connection + # with st.spinner("Checking..."): + # connection_status = check_connection(device) + # time.sleep(1) # Simulate loading time + + # Display the connection status + # if connection_status: + # st.success("Connected") + # else: + # st.error("Disconnected") + +# Button to manually refresh connection statuses +if st.button("Refresh Connection Status"): + update_status() + +# Auto-refresh every 10 seconds (optional) +while True: + update_status() + time.sleep(10) # Adjust the refresh interval as needed diff --git a/controller/bluetooth.py b/controller/bluetooth.py new file mode 100644 index 0000000..4b18e60 --- /dev/null +++ b/controller/bluetooth.py @@ -0,0 +1,61 @@ +import asyncio + +from bleak import BleakScanner, BleakClient +from bleak.backends.scanner import AdvertisementData +from bleak.backends.device import BLEDevice + +class BLE_UART: + UART_SERVICE_UUID = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" + UART_RX_CHAR_UUID = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" + UART_TX_CHAR_UUID = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + isConnected = False + batteryPerc = 0 + + def __init__(self, peripheral_name="", address = ""): + self._peripheral_name = peripheral_name + self._address = address + self._rx_queue = asyncio.Queue() + + async def setSOC(self, percentage): + self.batteryPerc = percentage + + async def read(self): + try: + msg = await self._rx_queue.get() + return msg.decode() + except Exception as e: + await self.disconnect() + + async def write(self, msg): # Where msg is a String + try: + await self._client.write_gatt_char(self.UART_RX_CHAR_UUID, msg.encode()) + except Exception as e: + await self.disconnect() + + async def connect(self): + self._discovery_queue = asyncio.Queue() + device = None + print(f"Scanning for {self._peripheral_name}") + async with BleakScanner(detection_callback=self._find_uart_device): + device: BLEDevice = await self._discovery_queue.get() + client = self._client = BleakClient(device, disconnected_callback=self._handle_disconnect) + await client.connect() + await client.start_notify(self.UART_TX_CHAR_UUID, self._rx_handler) + print(f"Connected to {self._peripheral_name}!") + self.isConnected = True + + async def disconnect(self): + if (self.isConnected != False): + print(f"Disconnected from {self._peripheral_name}") + self.isConnected = False + await self._client.disconnect() + + def _rx_handler(self, _: int, data: bytearray): + self._rx_queue.put_nowait(data) + + def _find_uart_device(self, device: BLEDevice, adv: AdvertisementData): + if device.address == self._address: + self._discovery_queue.put_nowait(device) + + def _handle_disconnect(self, _: BleakClient): + self._rx_queue.put_nowait(None) diff --git a/controller/controller.py b/controller/controller.py new file mode 100644 index 0000000..ff544ab --- /dev/null +++ b/controller/controller.py @@ -0,0 +1,164 @@ +# `sudo pkill bluetoothd` if bluetooth not working on ming's mac m3 pro +import threading +from LaptopKeyboard import * +from bluetooth import * +import asyncio +import time + +startTime = time.time() + +def millis(): + return round((time.time()-startTime) * 1000) + +ir_beacon_2 = BLE_UART(peripheral_name='Beac 2', address = '9AC0FFF3-446C-1A64-DA89-1376064B2BA1') +ir_beacon_1 = BLE_UART(peripheral_name='Beac 1', address = 'DBA047A7-4143-45D5-E469-FEEA2E354502') +oreo = BLE_UART(peripheral_name='Oreo', address = '07C80925-7C0F-1236-FB54-CFC3912A3B9D') +bt_devices = {ir_beacon_2, ir_beacon_1, oreo} +# hockey_puck = BLE_UART(peripheral_name='Hockey Puck', address = '07C80925-7C0F-1236-FB54-CFC3912A3B9D') + +keyboard_thread = threading.Thread(target=lambda: Listener(on_press=on_press, on_release=on_release).start()) +keyboard_thread.daemon = True +keyboard_thread.start() + +robotcmd = "" +irbeaconcmd = "" +enabled = 0 +activeBeacon = 1 +lastBeaconRead = millis() + +async def bluetooth_receive_handler(BLE_DEVICE): + global lastBeaconRead + while True: + await asyncio.sleep(0.1) + if (BLE_DEVICE.isConnected): + msg = await BLE_DEVICE.read() + if (msg == "seen"): + if (lastBeaconRead < millis()): # Some reason it thinks its "seen" right after enabling + lastBeaconRead = millis() + elif (msg is not None): + print(f"[{BLE_DEVICE._peripheral_name}] {msg}") + +async def bluetooth_comm_handler(BLE_DEVICE, isMainRobot): + await BLE_DEVICE.connect() + while True: + await asyncio.sleep(0.05) + if (BLE_DEVICE.isConnected): + if (isMainRobot): + await BLE_DEVICE.write(robotcmd) + else: + await BLE_DEVICE.write(irbeaconcmd) + else: + await BLE_DEVICE.connect() + + +async def ir_beacon_switcher(): + global enabled + global lastBeaconRead + while True: + await asyncio.sleep(0.1) + if (enabled != 1): + lastBeaconRead = millis() + 2000 # Add some time so the beacon doesnt switch right after enabling melty brain mode + if (enabled == 1 and millis() - lastBeaconRead > 1000 and ir_beacon_2.isConnected and ir_beacon_1.isConnected): + toggleBeacon() + +def toggleBeacon(): + global activeBeacon + global lastBeaconRead + lastBeaconRead = millis() + if activeBeacon == 1: + activeBeacon = 2 + elif activeBeacon == 2: + activeBeacon = 1 + +async def cmd_handler(): + global robotcmd + global irbeaconcmd + global enabled + global activeBeacon + waitForEnableReleased = 0 + lastState = 0 + drivestate = 1 + while True: + x,y,drivecmd,robottuning,irbeacontuning,boost = (0,)*6 + + if get_key_state("Key.up"): + y = y + 1 + if get_key_state("Key.down"): + y = y - 1 + if get_key_state("Key.left"): + x = x - 1 + if get_key_state("Key.right"): + x = x + 1 + + if x == 0 and y == 0: + drivecmd = 0 + elif x == 0 and y == 1: + drivecmd = 1 + elif x == 1 and y == 1: + drivecmd = 2 + elif x == 1 and y == 0: + drivecmd = 3 + elif x == 1 and y == -1: + drivecmd = 4 + elif x == 0 and y == -1: + drivecmd = 5 + elif x == -1 and y == -1: + drivecmd = 6 + elif x == -1 and y == 0: + drivecmd = 7 + elif x == -1 and y == 1: + drivecmd = 8 + + if get_key_state('q'): + robottuning = 1 + elif get_key_state('a'): + robottuning = 2 + elif get_key_state('w'): + robottuning = 3 + elif get_key_state('s'): + robottuning = 4 + elif get_key_state('e'): + robottuning = 5 + elif get_key_state('d'): + robottuning = 6 + + if get_key_state('t'): + irbeacontuning = 1 + elif get_key_state('g'): + irbeacontuning = 2 + + if get_key_state("Key.space"): + if get_key_state("Key.ctrl"): + enabled = drivestate + waitForEnableReleased = 1 + else: + if not waitForEnableReleased: + enabled = 0 + if not (get_key_state("Key.space")) and not (get_key_state("Key.ctrl")): + waitForEnableReleased = 0 + + if (enabled != 0): + if (get_key_state('z') or get_key_state('Z')): + enabled = drivestate = 1 + if (get_key_state('x') or get_key_state('X')): + enabled = drivestate = 2 + + curState = get_key_state('1') + if curState and not lastState: + toggleBeacon() + lastState = curState + + if (get_key_state("Key.shift")): + boost = 1 + + robotcmd = f"{enabled}{drivecmd}{robottuning}{boost}00" + irbeaconcmd = f"{enabled}{activeBeacon}{irbeacontuning}000" + await asyncio.sleep(0.05) + +async def main(): + await asyncio.gather(ir_beacon_switcher(), cmd_handler(), bluetooth_comm_handler(ir_beacon_1, False), bluetooth_comm_handler(oreo, True), bluetooth_receive_handler(ir_beacon_1), bluetooth_receive_handler(oreo), bluetooth_comm_handler(ir_beacon_2, False), bluetooth_receive_handler(ir_beacon_2)) + # await asyncio.gather(ir_beacon_switcher(), cmd_handler(), bluetooth_comm_handler(ir_beacon_1, False), bluetooth_comm_handler(oreo, True), bluetooth_comm_handler(hockey_puck, True), bluetooth_receive_handler(hockey_puck), bluetooth_receive_handler(ir_beacon_1), bluetooth_receive_handler(oreo), bluetooth_comm_handler(ir_beacon_2, False), bluetooth_receive_handler(ir_beacon_2)) + +asyncio.run(main()) + + \ No newline at end of file diff --git a/controller/controller_gui.py b/controller/controller_gui.py new file mode 100644 index 0000000..468f9ab --- /dev/null +++ b/controller/controller_gui.py @@ -0,0 +1,195 @@ +# `sudo pkill bluetoothd` if bluetooth not working on ming's mac m3 pro +import threading +from LaptopKeyboard import * +from bluetooth import * +import asyncio +import time +import streamlit as st + +startTime = time.time() + +def millis(): + return round((time.time()-startTime) * 1000) + +ir_beacon_2 = BLE_UART(peripheral_name='Beac 2', address = '9AC0FFF3-446C-1A64-DA89-1376064B2BA1') +ir_beacon_1 = BLE_UART(peripheral_name='Beac 1', address = 'DBA047A7-4143-45D5-E469-FEEA2E354502') +oreo = BLE_UART(peripheral_name='Oreo', address = '1932D032-A476-F238-07F0-A39D5208BC73') +bt_devices = {ir_beacon_2, ir_beacon_1, oreo} +# hockey_puck = BLE_UART(peripheral_name='Hockey Puck', address = '07C80925-7C0F-1236-FB54-CFC3912A3B9D') + +keyboard_thread = threading.Thread(target=lambda: Listener(on_press=on_press, on_release=on_release).start()) +keyboard_thread.daemon = True +keyboard_thread.start() + +robotcmd = "" +irbeaconcmd = "" +enabled = 0 +activeBeacon = 1 +lastBeaconRead = millis() +calibrate_accel = 0 + +async def bluetooth_receive_handler(BLE_DEVICE): + global lastBeaconRead + while True: + await asyncio.sleep(0.1) + if (BLE_DEVICE.isConnected): + msg = await BLE_DEVICE.read() + if (msg == "seen"): + if (lastBeaconRead < millis()): # Some reason it thinks its "seen" right after enabling + lastBeaconRead = millis() + elif (msg is not None): + print(f"[{BLE_DEVICE._peripheral_name}] {msg}") + if msg.find("SOC") != -1: + await BLE_DEVICE.setSOC(int(msg[5:8])) + +async def bluetooth_comm_handler(BLE_DEVICE, isMainRobot): + await BLE_DEVICE.connect() + while True: + await asyncio.sleep(0.05) + if (BLE_DEVICE.isConnected): + if (isMainRobot): + await BLE_DEVICE.write(robotcmd) + else: + await BLE_DEVICE.write(irbeaconcmd) + else: + await BLE_DEVICE.connect() + + +async def ir_beacon_switcher(): + global enabled + global lastBeaconRead + while True: + await asyncio.sleep(0.1) + if (enabled != 1): + lastBeaconRead = millis() + 2000 # Add some time so the beacon doesnt switch right after enabling melty brain mode + if (enabled == 1 and millis() - lastBeaconRead > 1000 and ir_beacon_2.isConnected and ir_beacon_1.isConnected): + toggleBeacon() + +def toggleBeacon(): + global activeBeacon + global lastBeaconRead + lastBeaconRead = millis() + if activeBeacon == 1: + activeBeacon = 2 + elif activeBeacon == 2: + activeBeacon = 1 + +async def cmd_handler(): + global robotcmd + global irbeaconcmd + global enabled + global activeBeacon + waitForEnableReleased = 0 + lastState = 0 + drivestate = 1 + while True: + x,y,drivecmd,robottuning,irbeacontuning,boost = (0,)*6 + + if get_key_state("Key.up"): + y = y + 1 + if get_key_state("Key.down"): + y = y - 1 + if get_key_state("Key.left"): + x = x - 1 + if get_key_state("Key.right"): + x = x + 1 + + if x == 0 and y == 0: + drivecmd = 0 + elif x == 0 and y == 1: + drivecmd = 1 + elif x == 1 and y == 1: + drivecmd = 2 + elif x == 1 and y == 0: + drivecmd = 3 + elif x == 1 and y == -1: + drivecmd = 4 + elif x == 0 and y == -1: + drivecmd = 5 + elif x == -1 and y == -1: + drivecmd = 6 + elif x == -1 and y == 0: + drivecmd = 7 + elif x == -1 and y == 1: + drivecmd = 8 + + if get_key_state('q'): + robottuning = 1 + elif get_key_state('a'): + robottuning = 2 + elif get_key_state('w'): + robottuning = 3 + elif get_key_state('s'): + robottuning = 4 + elif get_key_state('e'): + robottuning = 5 + elif get_key_state('d'): + robottuning = 6 + + if get_key_state('t'): + irbeacontuning = 1 + elif get_key_state('g'): + irbeacontuning = 2 + + if get_key_state("Key.space"): + if get_key_state("Key.ctrl"): + enabled = drivestate + waitForEnableReleased = 1 + else: + if not waitForEnableReleased: + enabled = 0 + if not (get_key_state("Key.space")) and not (get_key_state("Key.ctrl")): + waitForEnableReleased = 0 + + if (enabled != 0): + if (get_key_state('z') or get_key_state('Z')): + enabled = drivestate = 1 + if (get_key_state('x') or get_key_state('X')): + enabled = drivestate = 2 + + curState = get_key_state('1') + if curState and not lastState: + toggleBeacon() + lastState = curState + + if (get_key_state("Key.shift")): + boost = 1 + + if get_key_state("u"): + calibrate_accel = 1 + elif get_key_state("j"): + calibrate_accel = 2 + else: + calibrate_accel = 0 + + robotcmd = f"{enabled}{drivecmd}{robottuning}{boost}{calibrate_accel}0" + irbeaconcmd = f"{enabled}{activeBeacon}{irbeacontuning}000" + await asyncio.sleep(0.05) + +async def main_async_tasks(): + await asyncio.gather(ir_beacon_switcher(), cmd_handler(), bluetooth_comm_handler(ir_beacon_1, False), bluetooth_comm_handler(oreo, True), bluetooth_receive_handler(ir_beacon_1), bluetooth_receive_handler(oreo), bluetooth_comm_handler(ir_beacon_2, False), bluetooth_receive_handler(ir_beacon_2)) + # await asyncio.gather(ir_beacon_switcher(), cmd_handler(), bluetooth_comm_handler(ir_beacon_1, False), bluetooth_comm_handler(oreo, True), bluetooth_comm_handler(hockey_puck, True), bluetooth_receive_handler(hockey_puck), bluetooth_receive_handler(ir_beacon_1), bluetooth_receive_handler(oreo), bluetooth_comm_handler(ir_beacon_2, False), bluetooth_receive_handler(ir_beacon_2)) + +def start_async_tasks(): + asyncio.run(main_async_tasks()) + + +async_thread = threading.Thread(target=start_async_tasks) +async_thread.start() + +# Use an empty container to update just the status UI +status_container = st.empty() + +while True: + with status_container.container(): # Using st.container() instead of st.empty() + status_cols = st.columns(len(bt_devices)) + for i, bt_device in enumerate(bt_devices): + with status_cols[i]: + st.subheader(bt_device._peripheral_name) + if (bt_device.isConnected): + st.progress(bt_device.batteryPerc / 100) # Normalize battery level (0 to 1 for progress bar) + st.caption(f"Battery: {bt_device.batteryPerc}%") + else: + st.error("Disconnected") + + time.sleep(0.1) # Update interval in seconds \ No newline at end of file