From 1bf3177747fd48e5619c78e3a4948791b05f3c1f Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 1 Apr 2023 10:10:11 -0700 Subject: [PATCH 001/138] resetting --- src/software/jetson_nano/services/redis.cpp | 38 +++++++++++++++++++++ src/software/jetson_nano/services/redis.h | 22 ++++++++++++ 2 files changed, 60 insertions(+) create mode 100644 src/software/jetson_nano/services/redis.cpp create mode 100644 src/software/jetson_nano/services/redis.h diff --git a/src/software/jetson_nano/services/redis.cpp b/src/software/jetson_nano/services/redis.cpp new file mode 100644 index 0000000000..831d4e1b60 --- /dev/null +++ b/src/software/jetson_nano/services/redis.cpp @@ -0,0 +1,38 @@ +#include "service.h" +#include "redis.h" + +#include "string" + +#include "software/logger/logger.h" + +// Redis config +static size_t PORT = 6379; + +Redis::Redis() : subscriber() { +} + +void Redis::start() { + subscriber.connect("127.0.0.1", PORT, + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { + // Should be logged instead + if (status == cpp_redis::connect_state::dropped) { + // Should be logged + LOG(WARNING) << "connection dropped"; + std::cout << "client disconnected from " << host << ":" << port << std::endl; + } else if (status == cpp_redis::connect_state::failed) { + LOG(WARNING) << "connection failed"; + } else if (status == cpp_redis::connect_state::ok) { + LOG(INFO) << "connection succesful"; + } + }); +} + +void Redis::stop() { + subscriber.disconnect(false); +} + +void Redis::subscribe(const std::string &channel, + void (*subscribe_callback)(std::string, std::string)) { + subscriber.subscribe(channel, subscribe_callback); +} + diff --git a/src/software/jetson_nano/services/redis.h b/src/software/jetson_nano/services/redis.h new file mode 100644 index 0000000000..da0c83c235 --- /dev/null +++ b/src/software/jetson_nano/services/redis.h @@ -0,0 +1,22 @@ +#pragma once + +#include "cpp_redis/cpp_redis" +#include "cpp_redis/core/client.hpp" + + +class Redis : public Service { +public: + + explicit Redis(); + + void start() override; + + void stop() override; + + void subscribe(const std::string &channel, + void (*subscribe_callback)(std::string, std::string)); + + +private: + cpp_redis::subscriber subscriber; +}; \ No newline at end of file From 803a815e651c9eb8c66963692b0a1732394155f0 Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 14 Jan 2022 18:18:36 -0800 Subject: [PATCH 002/138] Added redis service to thunderloop and some cleanup --- src/software/jetson_nano/services/redis.cpp | 54 ++++++++++----------- src/software/jetson_nano/services/redis.h | 12 +++-- 2 files changed, 34 insertions(+), 32 deletions(-) diff --git a/src/software/jetson_nano/services/redis.cpp b/src/software/jetson_nano/services/redis.cpp index 831d4e1b60..91cbf4b65e 100644 --- a/src/software/jetson_nano/services/redis.cpp +++ b/src/software/jetson_nano/services/redis.cpp @@ -1,38 +1,38 @@ -#include "service.h" #include "redis.h" -#include "string" - +#include "service.h" #include "software/logger/logger.h" -// Redis config -static size_t PORT = 6379; - -Redis::Redis() : subscriber() { +RedisService::RedisService(std::string host, size_t port) : subscriber(), host(host), port(port) {} + +void RedisService::start() +{ + subscriber.connect( + host, port, + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) + { + if (status == cpp_redis::connect_state::dropped) + { + LOG(WARNING) << "Connection dropped"; + } + else if (status == cpp_redis::connect_state::failed) + { + LOG(WARNING) << "Connection failed"; + } + else if (status == cpp_redis::connect_state::ok) + { + LOG(INFO) << "Connection successful"; + } + }); } -void Redis::start() { - subscriber.connect("127.0.0.1", PORT, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { - // Should be logged instead - if (status == cpp_redis::connect_state::dropped) { - // Should be logged - LOG(WARNING) << "connection dropped"; - std::cout << "client disconnected from " << host << ":" << port << std::endl; - } else if (status == cpp_redis::connect_state::failed) { - LOG(WARNING) << "connection failed"; - } else if (status == cpp_redis::connect_state::ok) { - LOG(INFO) << "connection succesful"; - } - }); -} - -void Redis::stop() { +void RedisService::stop() +{ subscriber.disconnect(false); } -void Redis::subscribe(const std::string &channel, - void (*subscribe_callback)(std::string, std::string)) { +void RedisService::subscribe(const std::string &channel, + void (*subscribe_callback)(std::string, std::string)) +{ subscriber.subscribe(channel, subscribe_callback); } - diff --git a/src/software/jetson_nano/services/redis.h b/src/software/jetson_nano/services/redis.h index da0c83c235..a6de63de73 100644 --- a/src/software/jetson_nano/services/redis.h +++ b/src/software/jetson_nano/services/redis.h @@ -1,17 +1,17 @@ #pragma once #include "cpp_redis/cpp_redis" -#include "cpp_redis/core/client.hpp" +#include "string" -class Redis : public Service { +class RedisService : public Service { public: - explicit Redis(); + explicit RedisService(std::string host, size_t port); - void start() override; + virtual void start() override; - void stop() override; + virtual void stop() override; void subscribe(const std::string &channel, void (*subscribe_callback)(std::string, std::string)); @@ -19,4 +19,6 @@ class Redis : public Service { private: cpp_redis::subscriber subscriber; + std::string host; + size_t port; }; \ No newline at end of file From bda285e6643c6bee4b77831aad41da9750fe2bd1 Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 14 Jan 2022 18:35:14 -0800 Subject: [PATCH 003/138] added deps to build file --- src/software/jetson_nano/services/redis.cpp | 11 ++++++----- src/software/jetson_nano/services/redis.h | 10 ++++++---- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/software/jetson_nano/services/redis.cpp b/src/software/jetson_nano/services/redis.cpp index 91cbf4b65e..f090fbc346 100644 --- a/src/software/jetson_nano/services/redis.cpp +++ b/src/software/jetson_nano/services/redis.cpp @@ -1,16 +1,17 @@ #include "redis.h" -#include "service.h" #include "software/logger/logger.h" -RedisService::RedisService(std::string host, size_t port) : subscriber(), host(host), port(port) {} +RedisService::RedisService(std::string host, size_t port) + : subscriber(), host(host), port(port) +{ +} void RedisService::start() { subscriber.connect( host, port, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) - { + [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { if (status == cpp_redis::connect_state::dropped) { LOG(WARNING) << "Connection dropped"; @@ -32,7 +33,7 @@ void RedisService::stop() } void RedisService::subscribe(const std::string &channel, - void (*subscribe_callback)(std::string, std::string)) + void (*subscribe_callback)(std::string, std::string)) { subscriber.subscribe(channel, subscribe_callback); } diff --git a/src/software/jetson_nano/services/redis.h b/src/software/jetson_nano/services/redis.h index a6de63de73..cdd04b0773 100644 --- a/src/software/jetson_nano/services/redis.h +++ b/src/software/jetson_nano/services/redis.h @@ -1,12 +1,14 @@ #pragma once #include "cpp_redis/cpp_redis" +#include "software/jetson_nano/services/service.h" #include "string" -class RedisService : public Service { -public: +class RedisService : public Service +{ + public: explicit RedisService(std::string host, size_t port); virtual void start() override; @@ -17,8 +19,8 @@ class RedisService : public Service { void (*subscribe_callback)(std::string, std::string)); -private: + private: cpp_redis::subscriber subscriber; std::string host; size_t port; -}; \ No newline at end of file +}; From f0c9f29ed4ee91ec048a297c35a73e20c3fee526 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 24 Mar 2022 16:40:04 -0700 Subject: [PATCH 004/138] Merge branch 'master' of https://github.com/UBC-Thunderbots/Software into boris/shoot_or_pass_play_fsm_guards  Conflicts:  src/software/jetson_nano/redis/redis_client_test.cpp --- src/software/jetson_nano/services/redis.cpp | 39 --------------------- src/software/jetson_nano/services/redis.h | 26 -------------- 2 files changed, 65 deletions(-) delete mode 100644 src/software/jetson_nano/services/redis.cpp delete mode 100644 src/software/jetson_nano/services/redis.h diff --git a/src/software/jetson_nano/services/redis.cpp b/src/software/jetson_nano/services/redis.cpp deleted file mode 100644 index f090fbc346..0000000000 --- a/src/software/jetson_nano/services/redis.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "redis.h" - -#include "software/logger/logger.h" - -RedisService::RedisService(std::string host, size_t port) - : subscriber(), host(host), port(port) -{ -} - -void RedisService::start() -{ - subscriber.connect( - host, port, - [](const std::string &host, std::size_t port, cpp_redis::connect_state status) { - if (status == cpp_redis::connect_state::dropped) - { - LOG(WARNING) << "Connection dropped"; - } - else if (status == cpp_redis::connect_state::failed) - { - LOG(WARNING) << "Connection failed"; - } - else if (status == cpp_redis::connect_state::ok) - { - LOG(INFO) << "Connection successful"; - } - }); -} - -void RedisService::stop() -{ - subscriber.disconnect(false); -} - -void RedisService::subscribe(const std::string &channel, - void (*subscribe_callback)(std::string, std::string)) -{ - subscriber.subscribe(channel, subscribe_callback); -} diff --git a/src/software/jetson_nano/services/redis.h b/src/software/jetson_nano/services/redis.h deleted file mode 100644 index cdd04b0773..0000000000 --- a/src/software/jetson_nano/services/redis.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include "cpp_redis/cpp_redis" -#include "software/jetson_nano/services/service.h" -#include "string" - - - -class RedisService : public Service -{ - public: - explicit RedisService(std::string host, size_t port); - - virtual void start() override; - - virtual void stop() override; - - void subscribe(const std::string &channel, - void (*subscribe_callback)(std::string, std::string)); - - - private: - cpp_redis::subscriber subscriber; - std::string host; - size_t port; -}; From 3dfba8311b9c01f4afaefce4e1e016c374d942e9 Mon Sep 17 00:00:00 2001 From: arun Date: Sat, 3 Jun 2023 13:50:27 -0700 Subject: [PATCH 005/138] initial changes compiling --- src/software/jetson_nano/services/motor.cpp | 37 +++++++++++++++++---- src/software/jetson_nano/services/motor.h | 10 ++++-- 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 83f379b901..50b79af859 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -90,7 +90,9 @@ MotorService::MotorService(const RobotConstants_t& robot_constants, robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), motor_fault_detector_(0), - dribbler_ramp_rpm_(0) + dribbler_ramp_rpm_(0), + tracked_motor_fault_start_time_(std::nullopt), + num_tracked_motor_resets_(0) { int ret = 0; @@ -133,6 +135,28 @@ MotorService::~MotorService() {} void MotorService::setup() { + const auto now = std::chrono::system_clock::now(); + if (tracked_motor_fault_start_time_.has_value()) + { + if ((tracked_motor_fault_start_time_.value() - now).count() < MOTOR_FAULT_TIME_THRESHOLD_S) + { + num_tracked_motor_resets_++; + } + + tracked_motor_fault_start_time_ = std::make_optional(now); + num_tracked_motor_resets_ = 1; + } + else + { + tracked_motor_fault_start_time_ = std::make_optional(now); + } + + + if (tracked_motor_fault_start_time_.has_value() && num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD) + { + LOG(FATAL) << "In the last " << (now - tracked_motor_fault_start_time_.value()).count() << "s, the motor board has resetted " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; + } + prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; // reset motor fault cache @@ -151,7 +175,7 @@ void MotorService::setup() for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) { - if (hasMotorReset(motor)) + if (requiresMotorReinit(motor)) { LOG(INFO) << "Clearing RESET for " << MOTOR_NAMES[motor]; tmc6100_writeInt(motor, TMC6100_GSTAT, 0x00000001); @@ -396,7 +420,7 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor // checks if any motor has reset, sends a log message if so for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) { - if (hasMotorReset(motor)) + if (requiresMotorReinit(motor)) { LOG(DEBUG) << "RESET DETECTED FOR MOTOR: " << MOTOR_NAMES[motor]; is_initialized_ = false; @@ -569,12 +593,13 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor return motor_status; } -bool MotorService::hasMotorReset(uint8_t motor) +bool MotorService::requiresMotorReinit(uint8_t motor) { - auto search = + auto reset_search = cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); - return (search != cached_motor_faults_[motor].motor_faults.end()); + return !cached_motor_faults_[motor].drive_enabled + && (reset_search != cached_motor_faults_[motor].motor_faults.end()); } void MotorService::spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, diff --git a/src/software/jetson_nano/services/motor.h b/src/software/jetson_nano/services/motor.h index cba3635e2d..eebeab097c 100644 --- a/src/software/jetson_nano/services/motor.h +++ b/src/software/jetson_nano/services/motor.h @@ -243,13 +243,13 @@ class MotorService double dribbler_rpm); /** - * Returns true if we've detected a RESET in our cached motor faults indicators. + * Returns true if we've detected a RESET in our cached motor faults indicators or if we have a fault that disables drive. * * @param motor chip select to check for RESETs * * @return true if the motor has returned a cached RESET fault, false otherwise */ - bool hasMotorReset(uint8_t motor); + bool requiresMotorReinit(uint8_t motor); // to check if the motors have been calibrated bool is_initialized_ = false; @@ -292,6 +292,9 @@ class MotorService int dribbler_ramp_rpm_; + std::optional> tracked_motor_fault_start_time_; + int num_tracked_motor_resets_; + // SPI Chip Selects static const uint8_t FRONT_LEFT_MOTOR_CHIP_SELECT = 0; static const uint8_t FRONT_RIGHT_MOTOR_CHIP_SELECT = 3; @@ -303,6 +306,9 @@ class MotorService static const uint8_t NUM_DRIVE_MOTORS = 4; static const uint8_t NUM_MOTORS = NUM_DRIVE_MOTORS + 1; + static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; + static const int MOTOR_FAULT_THRESHOLD = 3; + // SPI Trinamic Motor Driver Paths (indexed with chip select above) static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1", "/dev/spidev0.2", "/dev/spidev0.3", From d4a6ce73b0481e9cb97cd8fe2fb7484cbec3930b Mon Sep 17 00:00:00 2001 From: arun Date: Sat, 3 Jun 2023 16:56:47 -0700 Subject: [PATCH 006/138] perform RESET write to clear RESET indicator properly --- src/software/jetson_nano/services/motor.cpp | 22 +++++++-------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 50b79af859..56cff8ea00 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -154,18 +154,11 @@ void MotorService::setup() if (tracked_motor_fault_start_time_.has_value() && num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD) { - LOG(FATAL) << "In the last " << (now - tracked_motor_fault_start_time_.value()).count() << "s, the motor board has resetted " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; + LOG(FATAL) << "In the last " << (now - tracked_motor_fault_start_time_.value()).count() << "s, the motor board has reset " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; } prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; - // reset motor fault cache - for (uint8_t motor = 0; motor < NUM_MOTORS; motor++) - { - cached_motor_faults_[motor] = MotorFaultIndicator(); - encoder_calibrated_[motor] = false; - } - // Clear faults by resetting all the chips on the motor board reset_gpio_.setValue(GpioState::LOW); usleep(MICROSECONDS_PER_MILLISECOND * 100); @@ -175,11 +168,10 @@ void MotorService::setup() for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) { - if (requiresMotorReinit(motor)) - { - LOG(INFO) << "Clearing RESET for " << MOTOR_NAMES[motor]; - tmc6100_writeInt(motor, TMC6100_GSTAT, 0x00000001); - } + LOG(INFO) << "Clearing RESET for " << MOTOR_NAMES[motor]; + tmc6100_writeInt(motor, TMC6100_GSTAT, 0x00000001); + cached_motor_faults_[motor] = MotorFaultIndicator(); + encoder_calibrated_[motor] = false; } // Drive Motor Setup @@ -599,7 +591,7 @@ bool MotorService::requiresMotorReinit(uint8_t motor) cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); return !cached_motor_faults_[motor].drive_enabled - && (reset_search != cached_motor_faults_[motor].motor_faults.end()); + || (reset_search != cached_motor_faults_[motor].motor_faults.end()); } void MotorService::spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, @@ -818,7 +810,7 @@ void MotorService::configureDrivePI(uint8_t motor) writeToControllerOrDieTrying(motor, TMC4671_PID_POSITION_P_POSITION_I, 0); writeToControllerOrDieTrying(motor, TMC4671_PIDOUT_UQ_UD_LIMITS, 32767); - writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 5000); + writeToControllerOrDieTrying(motor, TMC4671_PID_TORQUE_FLUX_LIMITS, 2500); writeToControllerOrDieTrying(motor, TMC4671_PID_ACCELERATION_LIMIT, 1000); writeToControllerOrDieTrying(motor, TMC4671_PID_VELOCITY_LIMIT, 45000); From 3e1ccbb64e5291aaae90afe39cf46eb9aa005124 Mon Sep 17 00:00:00 2001 From: Arun Date: Tue, 6 Jun 2023 00:21:43 -0700 Subject: [PATCH 007/138] formatting --- src/software/jetson_nano/services/motor.cpp | 27 ++++++++++++--------- src/software/jetson_nano/services/motor.h | 8 +++--- 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 56cff8ea00..3e9bfbc9d9 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -138,23 +138,28 @@ void MotorService::setup() const auto now = std::chrono::system_clock::now(); if (tracked_motor_fault_start_time_.has_value()) { - if ((tracked_motor_fault_start_time_.value() - now).count() < MOTOR_FAULT_TIME_THRESHOLD_S) - { - num_tracked_motor_resets_++; - } + if ((tracked_motor_fault_start_time_.value() - now).count() < + MOTOR_FAULT_TIME_THRESHOLD_S) + { + num_tracked_motor_resets_++; + } - tracked_motor_fault_start_time_ = std::make_optional(now); - num_tracked_motor_resets_ = 1; + tracked_motor_fault_start_time_ = std::make_optional(now); + num_tracked_motor_resets_ = 1; } else { - tracked_motor_fault_start_time_ = std::make_optional(now); + tracked_motor_fault_start_time_ = std::make_optional(now); } - if (tracked_motor_fault_start_time_.has_value() && num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD) + if (tracked_motor_fault_start_time_.has_value() && + num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD) { - LOG(FATAL) << "In the last " << (now - tracked_motor_fault_start_time_.value()).count() << "s, the motor board has reset " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; + LOG(FATAL) << "In the last " + << (now - tracked_motor_fault_start_time_.value()).count() + << "s, the motor board has reset " << num_tracked_motor_resets_ + << " times. Thunderloop crashing for safety."; } prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; @@ -590,8 +595,8 @@ bool MotorService::requiresMotorReinit(uint8_t motor) auto reset_search = cached_motor_faults_[motor].motor_faults.find(TbotsProto::MotorFault::RESET); - return !cached_motor_faults_[motor].drive_enabled - || (reset_search != cached_motor_faults_[motor].motor_faults.end()); + return !cached_motor_faults_[motor].drive_enabled || + (reset_search != cached_motor_faults_[motor].motor_faults.end()); } void MotorService::spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, diff --git a/src/software/jetson_nano/services/motor.h b/src/software/jetson_nano/services/motor.h index eebeab097c..3ab6492046 100644 --- a/src/software/jetson_nano/services/motor.h +++ b/src/software/jetson_nano/services/motor.h @@ -243,7 +243,8 @@ class MotorService double dribbler_rpm); /** - * Returns true if we've detected a RESET in our cached motor faults indicators or if we have a fault that disables drive. + * Returns true if we've detected a RESET in our cached motor faults indicators or if + * we have a fault that disables drive. * * @param motor chip select to check for RESETs * @@ -292,7 +293,8 @@ class MotorService int dribbler_ramp_rpm_; - std::optional> tracked_motor_fault_start_time_; + std::optional> + tracked_motor_fault_start_time_; int num_tracked_motor_resets_; // SPI Chip Selects @@ -307,7 +309,7 @@ class MotorService static const uint8_t NUM_MOTORS = NUM_DRIVE_MOTORS + 1; static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static const int MOTOR_FAULT_THRESHOLD = 3; + static const int MOTOR_FAULT_THRESHOLD = 3; // SPI Trinamic Motor Driver Paths (indexed with chip select above) static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1", From a54183af135dfd4ebd90d0ceb6cb381907aaeb19 Mon Sep 17 00:00:00 2001 From: Arun Date: Tue, 6 Jun 2023 00:59:40 -0700 Subject: [PATCH 008/138] cleanup logic --- src/software/jetson_nano/services/motor.cpp | 15 +++++---------- src/software/jetson_nano/services/motor.h | 2 +- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 3e9bfbc9d9..1f3641b9ff 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -136,25 +136,20 @@ MotorService::~MotorService() {} void MotorService::setup() { const auto now = std::chrono::system_clock::now(); - if (tracked_motor_fault_start_time_.has_value()) + if (tracked_motor_fault_start_time_.has_value() + && (tracked_motor_fault_start_time_.value() - now).count() < MOTOR_FAULT_TIME_THRESHOLD_S) { - if ((tracked_motor_fault_start_time_.value() - now).count() < - MOTOR_FAULT_TIME_THRESHOLD_S) - { - num_tracked_motor_resets_++; - } - - tracked_motor_fault_start_time_ = std::make_optional(now); - num_tracked_motor_resets_ = 1; + num_tracked_motor_resets_++; } else { tracked_motor_fault_start_time_ = std::make_optional(now); + num_tracked_motor_resets_ = 1; } if (tracked_motor_fault_start_time_.has_value() && - num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD) + num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) { LOG(FATAL) << "In the last " << (now - tracked_motor_fault_start_time_.value()).count() diff --git a/src/software/jetson_nano/services/motor.h b/src/software/jetson_nano/services/motor.h index 3ab6492046..0cf9f3dfd8 100644 --- a/src/software/jetson_nano/services/motor.h +++ b/src/software/jetson_nano/services/motor.h @@ -309,7 +309,7 @@ class MotorService static const uint8_t NUM_MOTORS = NUM_DRIVE_MOTORS + 1; static const int MOTOR_FAULT_TIME_THRESHOLD_S = 60; - static const int MOTOR_FAULT_THRESHOLD = 3; + static const int MOTOR_FAULT_THRESHOLD_COUNT = 3; // SPI Trinamic Motor Driver Paths (indexed with chip select above) static constexpr const char* SPI_PATHS[] = {"/dev/spidev0.0", "/dev/spidev0.1", From 1caa5736951a49c3019f875d6d3e4db8e24af57e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 6 Jun 2023 08:08:27 +0000 Subject: [PATCH 009/138] [pre-commit.ci lite] apply automatic fixes --- src/software/jetson_nano/services/motor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 1f3641b9ff..bcf0cbb7a9 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -136,8 +136,9 @@ MotorService::~MotorService() {} void MotorService::setup() { const auto now = std::chrono::system_clock::now(); - if (tracked_motor_fault_start_time_.has_value() - && (tracked_motor_fault_start_time_.value() - now).count() < MOTOR_FAULT_TIME_THRESHOLD_S) + if (tracked_motor_fault_start_time_.has_value() && + (tracked_motor_fault_start_time_.value() - now).count() < + MOTOR_FAULT_TIME_THRESHOLD_S) { num_tracked_motor_resets_++; } From b6d612caa97978b6ea2cdd57a0553bacb6bb0aec Mon Sep 17 00:00:00 2001 From: arun Date: Sat, 10 Jun 2023 17:15:34 -0700 Subject: [PATCH 010/138] better time printout --- src/software/jetson_nano/services/motor.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index bcf0cbb7a9..01c4ffe32b 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -136,8 +136,14 @@ MotorService::~MotorService() {} void MotorService::setup() { const auto now = std::chrono::system_clock::now(); + long int total_duration_since_last_fault_s = 0; + if (tracked_motor_fault_start_time_.has_value()) + { + total_duration_since_last_fault_s = std::chrono::duration_cast(now - tracked_motor_fault_start_time_.value()).count(); + } + if (tracked_motor_fault_start_time_.has_value() && - (tracked_motor_fault_start_time_.value() - now).count() < + total_duration_since_last_fault_s < MOTOR_FAULT_TIME_THRESHOLD_S) { num_tracked_motor_resets_++; @@ -153,7 +159,7 @@ void MotorService::setup() num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) { LOG(FATAL) << "In the last " - << (now - tracked_motor_fault_start_time_.value()).count() + << total_duration_since_last_fault_s << "s, the motor board has reset " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; } From 3a7829bce21d2aa833f5c094c050793d05bd1584 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sun, 11 Jun 2023 00:51:54 +0000 Subject: [PATCH 011/138] [pre-commit.ci lite] apply automatic fixes --- src/software/jetson_nano/services/motor.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index 01c4ffe32b..a99f44ecee 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -135,16 +135,18 @@ MotorService::~MotorService() {} void MotorService::setup() { - const auto now = std::chrono::system_clock::now(); + const auto now = std::chrono::system_clock::now(); long int total_duration_since_last_fault_s = 0; if (tracked_motor_fault_start_time_.has_value()) { - total_duration_since_last_fault_s = std::chrono::duration_cast(now - tracked_motor_fault_start_time_.value()).count(); + total_duration_since_last_fault_s = + std::chrono::duration_cast( + now - tracked_motor_fault_start_time_.value()) + .count(); } if (tracked_motor_fault_start_time_.has_value() && - total_duration_since_last_fault_s < - MOTOR_FAULT_TIME_THRESHOLD_S) + total_duration_since_last_fault_s < MOTOR_FAULT_TIME_THRESHOLD_S) { num_tracked_motor_resets_++; } @@ -158,8 +160,7 @@ void MotorService::setup() if (tracked_motor_fault_start_time_.has_value() && num_tracked_motor_resets_ > MOTOR_FAULT_THRESHOLD_COUNT) { - LOG(FATAL) << "In the last " - << total_duration_since_last_fault_s + LOG(FATAL) << "In the last " << total_duration_since_last_fault_s << "s, the motor board has reset " << num_tracked_motor_resets_ << " times. Thunderloop crashing for safety."; } From b5c14007fa3a322e60867c4c58b0a4954bc58d6a Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 25 Nov 2023 16:05:44 -0800 Subject: [PATCH 012/138] merging is difficult --- environment_setup/ubuntu20_requirements.txt | 1 + .../thunderscope/controller_diagnostics.py | 161 ++++++++++++++++++ .../robot__control_input_manager.py | 40 +++++ .../thunderscope/thunderscope_main.py | 10 ++ 4 files changed, 212 insertions(+) create mode 100644 src/software/thunderscope/controller_diagnostics.py create mode 100644 src/software/thunderscope/robot__control_input_manager.py diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 31d0cdaa96..d08c90e739 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -13,3 +13,4 @@ PyOpenGL==3.1.6 PyOpenGL-accelerate==3.1.5 psutil==5.9.0 numpy==1.24.4 +evdev==1.6.1 diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py new file mode 100644 index 0000000000..704aaa0a11 --- /dev/null +++ b/src/software/thunderscope/controller_diagnostics.py @@ -0,0 +1,161 @@ + +from proto.import_all_protos import * +from software.thunderscope.constants import * + +from evdev import InputDevice, categorize, ecodes +from threading import Event, Thread + +XBOX_MAX_RANGE = 32768 +XBOX_BUTTON_MAX_RANGE = 1024 + +DEADZONE_PERCENTAGE = 0.30 + +DRIBBLER_STEPPER = 100 +POWER_STEPPER = 100 + +MAX_LINEAR_SPEED_MPS = 2 + +class ControllerDiagnostics(object): + def __init__(self, input_path, proto_unix_io): + self.controller = InputDevice(input_path) + print("Start input device " + input_path) + self.proto_unix_io = proto_unix_io + + self.stop_event_thread = Event() + self._event_thread = Thread(target=self._event_loop) + self._event_thread.start() + + self.move_x = 0 + self.move_y = 0 + self.ang_vel = 0 + self.power = 1000 + + self.enable_dribbler = 0 + self.dribbler_speed = 0 + + def _event_loop(self): + print("start event loop") + for event in self.controller.read_loop(): + if self.stop_event_thread.isSet(): + return + self.process_event(event) + + def process_event(self, event): + if event.type == ecodes.EV_ABS: + absevent = categorize(event) + event_t = ecodes.bytype[absevent.event.type][absevent.event.code] + print("EVENT: " + event_t) + if event_t == "ABS_X": + self.update_move( + x=None, + y=absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, + ) + elif event_t == "ABS_Y": + self.update_move( + x=-absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, + y=None, + ) + elif event_t == "ABS_RX": + self.update_angular_velocity( + absevent.event.value / XBOX_MAX_RANGE * MAX_ANGULAR_SPEED_RAD_PER_S + ) + elif event_t == "ABS_HAT0Y": + self.update_dribbler( + self.dribbler_speed - absevent.event.value * DRIBBLER_STEPPER + ) + elif event_t == "ABS_HAT0X": + self.update_power(self.power + absevent.event.value * POWER_STEPPER) + elif event_t == "ABS_RZ": + if absevent.event.value > (XBOX_BUTTON_MAX_RANGE / 2): + self.enable_dribbler = 1 + print("dribbler enabled") + elif event_t == "ABS_Z": + if absevent.event.value < (XBOX_BUTTON_MAX_RANGE / 2): + self.enable_dribbler = 0 + print("dribbler enabled") + + + if ( + event_t == "ABS_X" + or event_t == "ABS_Y" + or event_t == "ABS_RX" + or event_t == "ABS_HAT0Y" + or event_t == "ABS_RZ" + ): + self.send_move_primitive() + elif event.type == ecodes.EV_KEY: + print("event code: " + str(event.code)) + if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: + print("kick") + self.kick() + elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: + print("chip") + self.chip() + + def kick(self): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.kick_speed_m_per_s = self.power + + self.proto_unix_io.send_proto(PowerControl, power_control) + print(power_control) + + def chip(self): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.chip_distance_meters = self.power + + self.proto_unix_io.send_proto(PowerControl, power_control) + print(power_control) + + def update_power(self, new_power): + if new_power > MIN_POWER and new_power < MAX_POWER: + self.power = new_power + print("power: " + str(self.power)) + + def update_dribbler(self, new_dribbler_speed): + if ( + new_dribbler_speed < MAX_DRIBBLER_RPM + and new_dribbler_speed > MIN_DRIBBLER_RPM + ): + self.dribbler_speed = new_dribbler_speed + print("dribbler speed: " + str(self.dribbler_speed)) + + def update_move(self, x, y): + if x is not None: + self.move_x = x + if y is not None: + self.move_y = y + + if (abs(self.move_x) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS)) : + self.move_x = 0 + if (abs(self.move_y) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS)) : + self.move_y = 0 + + self.ang_vel = 0 + + def update_angular_velocity(self, ang_vel): + self.ang_vel = ang_vel if ang_vel > (MAX_ANGULAR_SPEED_RAD_PER_S * DEADZONE_PERCENTAGE) else 0 + self.move_x = 0 + self.move_y = 0 + print("new and vel: " + str(self.ang_vel)) + + def send_move_primitive(self): + motor_control = MotorControl() + + motor_control.dribbler_speed_rpm = 0 + if self.enable_dribbler: + motor_control.dribbler_speed_rpm = self.dribbler_speed + + motor_control.direct_velocity_control.velocity.x_component_meters = self.move_x + motor_control.direct_velocity_control.velocity.y_component_meters = self.move_y + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.ang_vel + ) + + print(motor_control) + self.proto_unix_io.send_proto(MotorControl, motor_control) + + def close(self): + self.stop_event_thread.set() + self._event_thread.join() \ No newline at end of file diff --git a/src/software/thunderscope/robot__control_input_manager.py b/src/software/thunderscope/robot__control_input_manager.py new file mode 100644 index 0000000000..c42937f651 --- /dev/null +++ b/src/software/thunderscope/robot__control_input_manager.py @@ -0,0 +1,40 @@ +from enum import IntEnum + +from software.thunderscope.proto_unix_io import ProtoUnixIO + + +class ControlMode(IntEnum): + """ + Enum for the 3 modes representing the source of input controls from the robot + """ + + DIAGNOSTICS = 0 + XBOX = 1 + NONE = 2 + + +class DiagnosticsControlManager: + """ + Sets up a manager that handles the control input source for each robot, + and forwards that to communication + """ + def __init__( + self, + ): + self.diagnostics_input_widget_signal = 0 + self.robot_id_input_source_map: set[(int, ControlMode)] = set() + self.robots_connected_to_fullsystem = set() + + + def get_fullsystem_robots_ids(self): + return + def get_xbox_robot_ids(self): + return + def get_diagnostic_robots_ids(self): + return + def connect_control_mode_signal(self, signal): + return + + def connect_control_mode_signal(self, signal): + return + diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index b56075373b..9eadbc9f08 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -155,6 +155,13 @@ default=False, help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) + parser.add_argument( + "--xbox", + action="store", + type=str, + default='/dev/input/input1', + help="Path to the controller", + ) # Sanity check that an interface was provided args = parser.parse_args() @@ -223,6 +230,9 @@ # We want to run either 1 instance of AI or 1 instance of RobotCommunication or both which will # send/recv packets over the provided multicast channel. + if args.xbox is not None : + controller_diagnostics = ControllerDiagnostics(args.xbox, proto_unix_io) + elif args.run_blue or args.run_yellow or args.run_diagnostics: tscope_config = config.configure_ai_or_diagnostics( args.run_blue, From 87580c9b62f8ba48c39d1b5c144575d708b2bd22 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 30 Nov 2023 04:23:26 -0800 Subject: [PATCH 013/138] added handlers for signals, moved some comm logic out of controller, most of the manager and overall structure done --- environment_setup/ubuntu20_requirements.txt | 2 +- src/software/thunderscope/BUILD | 12 +++ src/software/thunderscope/constants.py | 5 +- .../thunderscope/controller_diagnostics.py | 71 ++++++--------- .../robot__control_input_manager.py | 40 --------- .../thunderscope/robot_communication.py | 83 +++++++++++++++--- .../robot_input_control_manager.py | 86 +++++++++++++++++++ .../thunderscope/thunderscope_config.py | 26 +++--- .../thunderscope/thunderscope_main.py | 37 +++++--- .../thunderscope/thunderscope_types.py | 14 +-- 10 files changed, 245 insertions(+), 131 deletions(-) delete mode 100644 src/software/thunderscope/robot__control_input_manager.py create mode 100644 src/software/thunderscope/robot_input_control_manager.py diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index d08c90e739..389452cefe 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,5 +1,5 @@ protobuf==3.20.2 -pyqtgraph==0.12.4 +pyqtgraph==0.13.3 autoflake==1.4 pyqt6==6.5.0 PyQt6-WebEngine==6.5.0 diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index e6744a7f97..4173a1531d 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -37,6 +37,7 @@ py_library( "//software/thunderscope:dock_style", "//software/thunderscope:proto_unix_io", "//software/thunderscope:robot_communication", + "//software/thunderscope:input_control_manager", requirement("pyqtgraph"), requirement("numpy"), ], @@ -78,6 +79,17 @@ py_library( ], ) +py_library( + name = "input_control_manager", + srcs = [ + "robot_input_control_manager.py", + "controller_diagnostics.py", + ], + deps = [ + "//software/thunderscope:constants", + ], +) + py_library( name = "thunderscope_types", srcs = ["thunderscope_types.py"], diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index f0ee87dcc7..e370b729e0 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -17,7 +17,7 @@ class ProtoUnixIOTypes(Enum): CURRENT = 5 -class TabNames(str, Enum): +class TabKeys(str, Enum): """ Different keys for tabs used in various Thunderscope views """ @@ -113,6 +113,9 @@ class EstopMode(IntEnum): ESTOP_PATH_1 = "/dev/ttyACM0" ESTOP_PATH_2 = "/dev/ttyUSB0" +# Path to check for handheld controller when running diagnostics +HANDHELD_PATH = "/dev/input/input1" + # Mapping between RobotStatus Error Codes and their dialog messages ERROR_CODE_MESSAGES = { ErrorCode.LOW_CAP: "Low Cap", diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py index 704aaa0a11..e2ec6196ef 100644 --- a/src/software/thunderscope/controller_diagnostics.py +++ b/src/software/thunderscope/controller_diagnostics.py @@ -1,6 +1,4 @@ - -from proto.import_all_protos import * -from software.thunderscope.constants import * +from typing import Callable from evdev import InputDevice, categorize, ecodes from threading import Event, Thread @@ -15,11 +13,27 @@ MAX_LINEAR_SPEED_MPS = 2 + class ControllerDiagnostics(object): - def __init__(self, input_path, proto_unix_io): + """ + This class is responsible for reading from an Xbox controller device and + interpreting the inputs into usable inputs for robot. + """ + + # TODO add function descriptions + def __init__( + self, + input_path: str, + perform_move: Callable[[int, int, int, int], None], + perform_kick: Callable[[int], None], + perform_chip: Callable[[int], None] + ): + self.__perform_move = perform_move + self.__perform_kick = perform_kick + self.__perform_chip = perform_chip self.controller = InputDevice(input_path) print("Start input device " + input_path) - self.proto_unix_io = proto_unix_io + # self.proto_unix_io = proto_unix_io self.stop_event_thread = Event() self._event_thread = Thread(target=self._event_loop) @@ -45,6 +59,7 @@ def process_event(self, event): absevent = categorize(event) event_t = ecodes.bytype[absevent.event.type][absevent.event.code] print("EVENT: " + event_t) + # TODO remove as fields and pass as args to handlers if event_t == "ABS_X": self.update_move( x=None, @@ -74,7 +89,6 @@ def process_event(self, event): self.enable_dribbler = 0 print("dribbler enabled") - if ( event_t == "ABS_X" or event_t == "ABS_Y" @@ -82,7 +96,7 @@ def process_event(self, event): or event_t == "ABS_HAT0Y" or event_t == "ABS_RZ" ): - self.send_move_primitive() + self.__perform_move(self.dribbler_speed, self.move_x, self.move_y, self.ang_vel) elif event.type == ecodes.EV_KEY: print("event code: " + str(event.code)) if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: @@ -93,31 +107,18 @@ def process_event(self, event): self.chip() def kick(self): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.kick_speed_m_per_s = self.power - - self.proto_unix_io.send_proto(PowerControl, power_control) - print(power_control) + self.__perform_kick(self.power) def chip(self): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = self.power - - self.proto_unix_io.send_proto(PowerControl, power_control) - print(power_control) + self.__perform_chip(self.power) def update_power(self, new_power): - if new_power > MIN_POWER and new_power < MAX_POWER: + if MIN_POWER < new_power < MAX_POWER: self.power = new_power print("power: " + str(self.power)) def update_dribbler(self, new_dribbler_speed): - if ( - new_dribbler_speed < MAX_DRIBBLER_RPM - and new_dribbler_speed > MIN_DRIBBLER_RPM - ): + if MAX_DRIBBLER_RPM > new_dribbler_speed > MIN_DRIBBLER_RPM: self.dribbler_speed = new_dribbler_speed print("dribbler speed: " + str(self.dribbler_speed)) @@ -127,9 +128,9 @@ def update_move(self, x, y): if y is not None: self.move_y = y - if (abs(self.move_x) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS)) : + if abs(self.move_x) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS): self.move_x = 0 - if (abs(self.move_y) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS)) : + if abs(self.move_y) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS): self.move_y = 0 self.ang_vel = 0 @@ -140,22 +141,6 @@ def update_angular_velocity(self, ang_vel): self.move_y = 0 print("new and vel: " + str(self.ang_vel)) - def send_move_primitive(self): - motor_control = MotorControl() - - motor_control.dribbler_speed_rpm = 0 - if self.enable_dribbler: - motor_control.dribbler_speed_rpm = self.dribbler_speed - - motor_control.direct_velocity_control.velocity.x_component_meters = self.move_x - motor_control.direct_velocity_control.velocity.y_component_meters = self.move_y - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.ang_vel - ) - - print(motor_control) - self.proto_unix_io.send_proto(MotorControl, motor_control) - def close(self): self.stop_event_thread.set() - self._event_thread.join() \ No newline at end of file + self._event_thread.join() diff --git a/src/software/thunderscope/robot__control_input_manager.py b/src/software/thunderscope/robot__control_input_manager.py deleted file mode 100644 index c42937f651..0000000000 --- a/src/software/thunderscope/robot__control_input_manager.py +++ /dev/null @@ -1,40 +0,0 @@ -from enum import IntEnum - -from software.thunderscope.proto_unix_io import ProtoUnixIO - - -class ControlMode(IntEnum): - """ - Enum for the 3 modes representing the source of input controls from the robot - """ - - DIAGNOSTICS = 0 - XBOX = 1 - NONE = 2 - - -class DiagnosticsControlManager: - """ - Sets up a manager that handles the control input source for each robot, - and forwards that to communication - """ - def __init__( - self, - ): - self.diagnostics_input_widget_signal = 0 - self.robot_id_input_source_map: set[(int, ControlMode)] = set() - self.robots_connected_to_fullsystem = set() - - - def get_fullsystem_robots_ids(self): - return - def get_xbox_robot_ids(self): - return - def get_diagnostic_robots_ids(self): - return - def connect_control_mode_signal(self, signal): - return - - def connect_control_mode_signal(self, signal): - return - diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index fb0697a11c..2276114feb 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -1,18 +1,20 @@ -from software.py_constants import * -from software.thunderscope.constants import ROBOT_COMMUNICATIONS_TIMEOUT_S +import threading +import time +import os + +from software.thunderscope.constants import ROBOT_COMMUNICATIONS_TIMEOUT_S, IndividualRobotMode, EstopMode +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode +from software.thunderscope.robot_input_control_manager import RobotInputControlManager from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer -from software.thunderscope.constants import IndividualRobotMode, EstopMode -import software.python_bindings as tbots_cpp from software.thunderscope.proto_unix_io import ProtoUnixIO -from queue import Empty +import software.python_bindings as tbots_cpp +from software.py_constants import * +from google.protobuf.message import Message from proto.import_all_protos import * from pyqtgraph.Qt import QtCore -from software.thunderscope.proto_unix_io import ProtoUnixIO from typing import Type -import threading -import time -import os -from google.protobuf.message import Message +from queue import Empty + class RobotCommunication(object): @@ -25,6 +27,7 @@ def __init__( multicast_channel: str, interface: str, estop_mode: EstopMode, + input_device_path: str = None, estop_path: os.PathLike = None, estop_baudrate: int = 115200, ): @@ -58,9 +61,13 @@ def __init__( self.current_proto_unix_io.register_observer(World, self.world_buffer) - self.robots_connected_to_fullsystem = set() - self.robots_connected_to_manual = set() - self.robots_to_be_disconnected = {} + self.control_manager = RobotInputControlManager( + input_device_path, + self.send_move_command, + self.send_kick_command, + self.end_chip_command, + ) + # TODO initialize callbacks for manager self.current_proto_unix_io.register_observer( PrimitiveSet, self.primitive_buffer @@ -79,6 +86,7 @@ def __init__( target=self.__run_primitive_set, daemon=True ) + # TODO move this to estop helpers # initialising the estop # tries to access a plugged in estop. if not found, throws an exception # if using keyboard estop, skips this step @@ -142,7 +150,7 @@ def toggle_keyboard_estop(self) -> None: ) ) - def toggle_robot_connection(self, mode: IndividualRobotMode, robot_id: int): + def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ Changes the input mode for a robot between None, Manual, or AI If changing from anything to None, add robot to disconnected map @@ -151,6 +159,9 @@ def toggle_robot_connection(self, mode: IndividualRobotMode, robot_id: int): :param mode: the mode of input for this robot's primitives :param robot_id: the id of the robot whose mode we're changing """ + + self.control_manager.toggle_control_mode_for_robot(robot_id, mode) + # TODO remove this self.robots_connected_to_fullsystem.discard(robot_id) self.robots_connected_to_manual.discard(robot_id) self.robots_to_be_disconnected.pop(robot_id, None) @@ -162,6 +173,14 @@ def toggle_robot_connection(self, mode: IndividualRobotMode, robot_id: int): elif mode == IndividualRobotMode.AI: self.robots_connected_to_fullsystem.add(robot_id) + def toggle_input_mode(self, mode: ControlMode): + """ + Changes the diagnostics input mode for all robots between Xbox and Diagnostics. + + :param mode: Control mode to use when sending diagnostics primitives + """ + self.control_manager.toggle_input_mode(mode) + def __send_estop_state(self) -> None: """ Constant loop which sends the current estop status proto if estop is not disabled @@ -220,6 +239,42 @@ def __run_world(self): # send the world proto self.send_world.send_proto(world) + def send_move_command(self, dribbler_speed: int, move_x: int, move_y: int, ang_vel: int): + motor_control = MotorControl() + + motor_control.dribbler_speed_rpm = 0 + if self.enable_dribbler: + motor_control.dribbler_speed_rpm = dribbler_speed + + motor_control.direct_velocity_control.velocity.x_component_meters = move_x + motor_control.direct_velocity_control.velocity.y_component_meters = move_y + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + ang_vel + ) + + print(motor_control) + + self.proto_unix_io.send_proto(MotorControl, motor_control) + + def send_kick_command(self, power: int): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.kick_speed_m_per_s = power + + print(power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) + + def send_chip_command(self, distance: int): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.chip_distance_meters = distance + + print(power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) + + def __run_primitive_set(self) -> None: """Forward PrimitiveSet protos from fullsystem to the robots. diff --git a/src/software/thunderscope/robot_input_control_manager.py b/src/software/thunderscope/robot_input_control_manager.py new file mode 100644 index 0000000000..9f436bb3b1 --- /dev/null +++ b/src/software/thunderscope/robot_input_control_manager.py @@ -0,0 +1,86 @@ +import os +from typing import Callable + +from evdev import util + +from software.thunderscope.constants import IndividualRobotMode, HANDHELD_PATH +from software.thunderscope.controller_diagnostics import ControllerDiagnostics +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode + + +class RobotInputControlManager: + """ + Sets up a manager that handles the control input source for each robot, + and forwards that to communication + + @:param xbox_enabled: If xbox control is enabled + """ + + def __init__( + self, + controller_device_path: str, + perform_move: Callable[[int, int, int, int], None], + perform_kick: Callable[[int], None], + perform_chip: Callable[[int], None] + ): + self.__perform_move = perform_move + self.__perform_kick = perform_kick + self.__perform_chip = perform_chip + self.__input_mode = ControlMode.DIAGNOSTICS + self.__controller_diagnostics = None + + self.setup_controller(controller_device_path) + + self.__robot_id_input_source_map: set[(int, IndividualRobotMode)] = set() + + self.__robots_to_be_disconnected: set[int] = {} + + def setup_controller(self, controller_device_path: str): + if controller_device_path: + if os.path.exists(controller_device_path) and util.is_device(controller_device_path): + self.__input_mode = ControlMode.XBOX + self.__controller_diagnostics = ControllerDiagnostics( + controller_device_path, + self.__perform_move, + self.__perform_kick, + self.__perform_chip + ) + else: + self.__input_mode = ControlMode.DIAGNOSTICS + + def set_full_system_control(self): + self.__robot_id_input_source_map = set(map( + lambda robot_control: (robot_control[0], IndividualRobotMode.AI), + self.__robot_id_input_source_map + )) + + def get_diagnostics_input_mode(self) -> ControlMode: + """ + Get the diagnostics input mode + :return: + """ + return self.__input_mode + + def toggle_input_mode(self, mode: ControlMode): + """ + Process and Qt signal indicating the diagnostics control mode has changed + :param mode: + :return: + """ + if mode == ControlMode.XBOX and self.__controller_diagnostics is not None: + self.__input_mode = mode + else: + self.__input_mode = ControlMode.DIAGNOSTICS + + def toggle_control_mode_for_robot(self, robot_id: int, mode: IndividualRobotMode): + """ + Updates the control mode for the given robot id to the given mode + :param robot_id: The robot id to update + :param mode: The input mode this robot should use - one of AI, MANUAL, or NONE + """ + self.__robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP + self.__robot_id_input_source_map = set(map( + # TODO figure way to destruct params or make callback maybe + lambda robot_control: (robot_id, mode) if robot_id == robot_control[0] else robot_control, + self.__robot_id_input_source_map + )) diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index 1580469d41..a9fe956291 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -1,6 +1,6 @@ from software.thunderscope.widget_setup_functions import * from software.thunderscope.constants import ( - TabNames, + TabKeys, ProtoUnixIOTypes, GAME_CONTROLLER_URL, ) @@ -266,7 +266,7 @@ def configure_two_ai_gamecontroller_view( tabs=[ TScopeQTTab( name="Blue FullSystem", - key=TabNames.BLUE, + key=TabKeys.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -277,7 +277,7 @@ def configure_two_ai_gamecontroller_view( ), TScopeQTTab( name="Yellow FullSystem", - key=TabNames.YELLOW, + key=TabKeys.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -290,7 +290,7 @@ def configure_two_ai_gamecontroller_view( ), TScopeWebTab( name="Gamecontroller", - key=TabNames.GAMECONTROLLER, + key=TabKeys.GAMECONTROLLER, url=GAME_CONTROLLER_URL, ), ], @@ -329,7 +329,7 @@ def configure_simulated_test_view( tabs=[ TScopeQTTab( name="Blue FullSystem", - key=TabNames.BLUE, + key=TabKeys.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -340,7 +340,7 @@ def configure_simulated_test_view( ), TScopeQTTab( name="Yellow FullSystem", - key=TabNames.YELLOW, + key=TabKeys.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -389,7 +389,7 @@ def configure_field_test_view( tabs = [ TScopeQTTab( name="Yellow FullSystem", - key=TabNames.YELLOW, + key=TabKeys.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -405,7 +405,7 @@ def configure_field_test_view( tabs = [ TScopeQTTab( name="Blue FullSystem", - key=TabNames.BLUE, + key=TabKeys.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -446,7 +446,7 @@ def configure_replay_view( tabs.append( TScopeQTTab( name="Blue FullSystem", - key=TabNames.BLUE, + key=TabKeys.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -464,7 +464,7 @@ def configure_replay_view( tabs.append( TScopeQTTab( name="Yellow FullSystem", - key=TabNames.YELLOW, + key=TabKeys.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -526,7 +526,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Blue Fullsystem", - key=TabNames.BLUE, + key=TabKeys.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -546,7 +546,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Yellow Fullsystem", - key=TabNames.YELLOW, + key=TabKeys.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -585,7 +585,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Robot Diagnostics", - key=TabNames.DIAGNOSTICS, + key=TabKeys.DIAGNOSTICS, widgets=configure_base_diagnostics( diagnostics_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.DIAGNOSTICS diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 9eadbc9f08..7970804264 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -1,15 +1,18 @@ import argparse import numpy + +from proto.message_translation import tbots_protobuf +from software.thunderscope.robot_input_control_manager import RobotInputControlManager from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.thunderscope import Thunderscope from software.thunderscope.binary_context_managers import * -from proto.message_translation import tbots_protobuf -import software.python_bindings as tbots_cpp from software.py_constants import * from software.thunderscope.robot_communication import RobotCommunication from software.thunderscope.replay.proto_logger import ProtoLogger -from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes +from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes, TabKeys, HANDHELD_PATH from software.thunderscope.estop_helpers import get_estop_config + +import software.python_bindings as tbots_cpp import software.thunderscope.thunderscope_config as config NUM_ROBOTS = 6 @@ -159,7 +162,7 @@ "--xbox", action="store", type=str, - default='/dev/input/input1', + default=HANDHELD_PATH, help="Path to the controller", ) @@ -230,9 +233,6 @@ # We want to run either 1 instance of AI or 1 instance of RobotCommunication or both which will # send/recv packets over the provided multicast channel. - if args.xbox is not None : - controller_diagnostics = ControllerDiagnostics(args.xbox, proto_unix_io) - elif args.run_blue or args.run_yellow or args.run_diagnostics: tscope_config = config.configure_ai_or_diagnostics( args.run_blue, @@ -265,6 +265,7 @@ with RobotCommunication( current_proto_unix_io=current_proto_unix_io, multicast_channel=getRobotMulticastChannel(args.channel), + input_device_path=args.xbox, interface=args.interface, estop_mode=estop_mode, estop_path=estop_path, @@ -277,16 +278,24 @@ if args.run_diagnostics: for tab in tscope_config.tabs: - if hasattr(tab, "widgets"): - robot_view_widget = tab.find_widget("Robot View") - - if robot_view_widget: - robot_view_widget.control_mode_signal.connect( - lambda mode, robot_id: robot_communication.toggle_robot_connection( - mode, robot_id + # handle the signal emitted from switching + if tab.key == TabKeys.DIAGNOSTICS: + control_input_widget = tab.find_widget("Manual Control Input") + if control_input_widget: + control_input_widget.toggle_controls_signal( + lambda control_mode: robot_communication.toggle_input_mode( + control_mode ) ) + robot_view_widget = tab.find_widget("Robot View") + if robot_view_widget: + robot_view_widget.control_mode_signal.connect( + lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( + robot_id, robot_mode + ) + ) + if args.run_blue or args.run_yellow: robot_communication.setup_for_fullsystem() full_system_runtime_dir = ( diff --git a/src/software/thunderscope/thunderscope_types.py b/src/software/thunderscope/thunderscope_types.py index 88847402cc..a4eef0895b 100644 --- a/src/software/thunderscope/thunderscope_types.py +++ b/src/software/thunderscope/thunderscope_types.py @@ -1,5 +1,5 @@ from typing import Callable, Optional, Sequence, Any, Dict -from software.thunderscope.constants import TabNames +from software.thunderscope.constants import TabKeys import PyQt6 from PyQt6.QtWebEngineWidgets import QWebEngineView @@ -59,16 +59,20 @@ class TScopeTab: """ name: str # name of tab - key: TabNames # key to identify this tab + key: TabKeys # key to identify this tab dock_area: QWidget # Dock Area for this tab - def __init__(self, name: str, key: TabNames) -> None: + def __init__(self, name: str, key: TabKeys) -> None: self.name = name self.key = key def refresh(self) -> None: pass + def find_widget(self, param): + print('i hate python - is this syntax for super or child getting called? why pass tho') + pass + class TScopeQTTab(TScopeTab): """ @@ -83,7 +87,7 @@ class TScopeQTTab(TScopeTab): ] # Mapping of widget names to refresh functions def __init__( - self, name: str, key: TabNames, widgets: Sequence[TScopeWidget] + self, name: str, key: TabKeys, widgets: Sequence[TScopeWidget] ) -> None: super().__init__(name, key) self.widgets = widgets @@ -158,7 +162,7 @@ class TScopeWebTab(TScopeTab): url: str # url of webpage displayed by this tab - def __init__(self, name: str, key: TabNames, url: str) -> None: + def __init__(self, name: str, key: TabKeys, url: str) -> None: super().__init__(name, key) self.url = url From 7c9d0c753eefde218fa40cf2b80c90edd94439e7 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 30 Nov 2023 04:27:30 -0800 Subject: [PATCH 014/138] formatting --- src/software/thunderscope/BUILD | 6 +-- .../thunderscope/controller_diagnostics.py | 30 ++++++++------ .../thunderscope/robot_communication.py | 12 ++++-- .../robot_input_control_manager.py | 40 +++++++++++-------- .../thunderscope/thunderscope_main.py | 7 +++- .../thunderscope/thunderscope_types.py | 5 ++- 6 files changed, 62 insertions(+), 38 deletions(-) diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index 4173a1531d..46c03b7bea 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -35,9 +35,9 @@ py_library( "//software/thunderscope:binary_context_managers", "//software/thunderscope:constants", "//software/thunderscope:dock_style", + "//software/thunderscope:input_control_manager", "//software/thunderscope:proto_unix_io", "//software/thunderscope:robot_communication", - "//software/thunderscope:input_control_manager", requirement("pyqtgraph"), requirement("numpy"), ], @@ -82,9 +82,9 @@ py_library( py_library( name = "input_control_manager", srcs = [ - "robot_input_control_manager.py", "controller_diagnostics.py", - ], + "robot_input_control_manager.py", + ], deps = [ "//software/thunderscope:constants", ], diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py index e2ec6196ef..47435f6bd7 100644 --- a/src/software/thunderscope/controller_diagnostics.py +++ b/src/software/thunderscope/controller_diagnostics.py @@ -22,11 +22,11 @@ class ControllerDiagnostics(object): # TODO add function descriptions def __init__( - self, - input_path: str, - perform_move: Callable[[int, int, int, int], None], - perform_kick: Callable[[int], None], - perform_chip: Callable[[int], None] + self, + input_path: str, + perform_move: Callable[[int, int, int, int], None], + perform_kick: Callable[[int], None], + perform_chip: Callable[[int], None], ): self.__perform_move = perform_move self.__perform_kick = perform_kick @@ -90,13 +90,15 @@ def process_event(self, event): print("dribbler enabled") if ( - event_t == "ABS_X" - or event_t == "ABS_Y" - or event_t == "ABS_RX" - or event_t == "ABS_HAT0Y" - or event_t == "ABS_RZ" + event_t == "ABS_X" + or event_t == "ABS_Y" + or event_t == "ABS_RX" + or event_t == "ABS_HAT0Y" + or event_t == "ABS_RZ" ): - self.__perform_move(self.dribbler_speed, self.move_x, self.move_y, self.ang_vel) + self.__perform_move( + self.dribbler_speed, self.move_x, self.move_y, self.ang_vel + ) elif event.type == ecodes.EV_KEY: print("event code: " + str(event.code)) if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: @@ -136,7 +138,11 @@ def update_move(self, x, y): self.ang_vel = 0 def update_angular_velocity(self, ang_vel): - self.ang_vel = ang_vel if ang_vel > (MAX_ANGULAR_SPEED_RAD_PER_S * DEADZONE_PERCENTAGE) else 0 + self.ang_vel = ( + ang_vel + if ang_vel > (MAX_ANGULAR_SPEED_RAD_PER_S * DEADZONE_PERCENTAGE) + else 0 + ) self.move_x = 0 self.move_y = 0 print("new and vel: " + str(self.ang_vel)) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 2276114feb..e1e88e5e5e 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -2,7 +2,11 @@ import time import os -from software.thunderscope.constants import ROBOT_COMMUNICATIONS_TIMEOUT_S, IndividualRobotMode, EstopMode +from software.thunderscope.constants import ( + ROBOT_COMMUNICATIONS_TIMEOUT_S, + IndividualRobotMode, + EstopMode, +) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.robot_input_control_manager import RobotInputControlManager from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer @@ -16,7 +20,6 @@ from queue import Empty - class RobotCommunication(object): """ Communicate with the robots """ @@ -239,7 +242,9 @@ def __run_world(self): # send the world proto self.send_world.send_proto(world) - def send_move_command(self, dribbler_speed: int, move_x: int, move_y: int, ang_vel: int): + def send_move_command( + self, dribbler_speed: int, move_x: int, move_y: int, ang_vel: int + ): motor_control = MotorControl() motor_control.dribbler_speed_rpm = 0 @@ -274,7 +279,6 @@ def send_chip_command(self, distance: int): self.proto_unix_io.send_proto(PowerControl, power_control) - def __run_primitive_set(self) -> None: """Forward PrimitiveSet protos from fullsystem to the robots. diff --git a/src/software/thunderscope/robot_input_control_manager.py b/src/software/thunderscope/robot_input_control_manager.py index 9f436bb3b1..2daaf24c28 100644 --- a/src/software/thunderscope/robot_input_control_manager.py +++ b/src/software/thunderscope/robot_input_control_manager.py @@ -17,11 +17,11 @@ class RobotInputControlManager: """ def __init__( - self, - controller_device_path: str, - perform_move: Callable[[int, int, int, int], None], - perform_kick: Callable[[int], None], - perform_chip: Callable[[int], None] + self, + controller_device_path: str, + perform_move: Callable[[int, int, int, int], None], + perform_kick: Callable[[int], None], + perform_chip: Callable[[int], None], ): self.__perform_move = perform_move self.__perform_kick = perform_kick @@ -37,22 +37,26 @@ def __init__( def setup_controller(self, controller_device_path: str): if controller_device_path: - if os.path.exists(controller_device_path) and util.is_device(controller_device_path): + if os.path.exists(controller_device_path) and util.is_device( + controller_device_path + ): self.__input_mode = ControlMode.XBOX self.__controller_diagnostics = ControllerDiagnostics( controller_device_path, self.__perform_move, self.__perform_kick, - self.__perform_chip + self.__perform_chip, ) else: self.__input_mode = ControlMode.DIAGNOSTICS def set_full_system_control(self): - self.__robot_id_input_source_map = set(map( - lambda robot_control: (robot_control[0], IndividualRobotMode.AI), - self.__robot_id_input_source_map - )) + self.__robot_id_input_source_map = set( + map( + lambda robot_control: (robot_control[0], IndividualRobotMode.AI), + self.__robot_id_input_source_map, + ) + ) def get_diagnostics_input_mode(self) -> ControlMode: """ @@ -79,8 +83,12 @@ def toggle_control_mode_for_robot(self, robot_id: int, mode: IndividualRobotMode :param mode: The input mode this robot should use - one of AI, MANUAL, or NONE """ self.__robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP - self.__robot_id_input_source_map = set(map( - # TODO figure way to destruct params or make callback maybe - lambda robot_control: (robot_id, mode) if robot_id == robot_control[0] else robot_control, - self.__robot_id_input_source_map - )) + self.__robot_id_input_source_map = set( + map( + # TODO figure way to destruct params or make callback maybe + lambda robot_control: (robot_id, mode) + if robot_id == robot_control[0] + else robot_control, + self.__robot_id_input_source_map, + ) + ) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 7970804264..b24ce5d015 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -9,7 +9,12 @@ from software.py_constants import * from software.thunderscope.robot_communication import RobotCommunication from software.thunderscope.replay.proto_logger import ProtoLogger -from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes, TabKeys, HANDHELD_PATH +from software.thunderscope.constants import ( + EstopMode, + ProtoUnixIOTypes, + TabKeys, + HANDHELD_PATH, +) from software.thunderscope.estop_helpers import get_estop_config import software.python_bindings as tbots_cpp diff --git a/src/software/thunderscope/thunderscope_types.py b/src/software/thunderscope/thunderscope_types.py index a4eef0895b..088cbe18c9 100644 --- a/src/software/thunderscope/thunderscope_types.py +++ b/src/software/thunderscope/thunderscope_types.py @@ -70,8 +70,9 @@ def refresh(self) -> None: pass def find_widget(self, param): - print('i hate python - is this syntax for super or child getting called? why pass tho') - pass + print( + "i hate python - is this syntax for super or child getting called? why pass tho" + ) class TScopeQTTab(TScopeTab): From 83862f1441f6b55db3ab86ebe79a436e50469568 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 13 Jan 2024 13:11:48 -0800 Subject: [PATCH 015/138] wip --- .../thunderscope/controller_diagnostics.py | 13 ++++---- .../thunderscope/robot_communication.py | 32 +++++++++++++++++-- 2 files changed, 35 insertions(+), 10 deletions(-) diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py index 47435f6bd7..bc48a83110 100644 --- a/src/software/thunderscope/controller_diagnostics.py +++ b/src/software/thunderscope/controller_diagnostics.py @@ -3,6 +3,8 @@ from evdev import InputDevice, categorize, ecodes from threading import Event, Thread +from software.thunderscope.proto_unix_io import ProtoUnixIO + XBOX_MAX_RANGE = 32768 XBOX_BUTTON_MAX_RANGE = 1024 @@ -24,16 +26,13 @@ class ControllerDiagnostics(object): def __init__( self, input_path: str, - perform_move: Callable[[int, int, int, int], None], - perform_kick: Callable[[int], None], - perform_chip: Callable[[int], None], + proto_unix_io: ProtoUnixIO, ): - self.__perform_move = perform_move - self.__perform_kick = perform_kick - self.__perform_chip = perform_chip self.controller = InputDevice(input_path) + # TODO check valid path + # TODO add auto detect controller print("Start input device " + input_path) - # self.proto_unix_io = proto_unix_io + self.proto_unix_io = proto_unix_io self.stop_event_thread = Event() self._event_thread = Thread(target=self._event_loop) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index e1e88e5e5e..064b547c5e 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -2,11 +2,14 @@ import time import os +from evdev import util + from software.thunderscope.constants import ( ROBOT_COMMUNICATIONS_TIMEOUT_S, IndividualRobotMode, EstopMode, ) +from software.thunderscope.controller_diagnostics import ControllerDiagnostics from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.robot_input_control_manager import RobotInputControlManager from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer @@ -49,8 +52,8 @@ def __init__( self.current_proto_unix_io = current_proto_unix_io self.multicast_channel = str(multicast_channel) self.interface = interface + # TODO merge estops attributes into 1 data type self.estop_mode = estop_mode - self.estop_path = estop_path self.estop_buadrate = estop_baudrate @@ -89,7 +92,9 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - # TODO move this to estop helpers + self.__robot_id_input_source_map: set[(int, IndividualRobotMode)] = set() + self.__robots_to_be_disconnected: set[int] = {} + # initialising the estop # tries to access a plugged in estop. if not found, throws an exception # if using keyboard estop, skips this step @@ -108,6 +113,18 @@ def __init__( except Exception: raise Exception(f"Invalid Estop found at location {self.estop_path}") + if input_device_path: + if os.path.exists(input_device_path) and util.is_device( + input_device_path + ): + self.__input_mode = ControlMode.XBOX + self.__controller_diagnostics = ControllerDiagnostics( + input_device_path, + ) + else: + self.__input_mode = ControlMode.DIAGNOSTICS + + def setup_for_fullsystem(self) -> None: """ Sets up a world sender, a listener for SSL vision data, and connects all robots to fullsystem as default @@ -163,7 +180,16 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param robot_id: the id of the robot whose mode we're changing """ - self.control_manager.toggle_control_mode_for_robot(robot_id, mode) + self.__robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP + self.__robot_id_input_source_map = set( + map( + lambda robot_control: (robot_id, mode) + if robot_id == robot_control[0] + else robot_control, + self.__robot_id_input_source_map, + ) + ) + # TODO remove this self.robots_connected_to_fullsystem.discard(robot_id) self.robots_connected_to_manual.discard(robot_id) From c8c12fba27a42cf0e88b3db9bc1cd5efa97627db Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 19 Jan 2024 23:31:03 -0800 Subject: [PATCH 016/138] wip --- .../thunderscope/controller_diagnostics.py | 206 ++++++++++++------ .../thunderscope/robot_communication.py | 128 +++++------ .../robot_diagnostics/chicker_widget.py | 15 +- .../diagnostics_input_widget.py | 10 +- .../robot_diagnostics/diagnostics_widget.py | 27 +++ .../thunderscope/thunderscope_config.py | 36 +-- .../thunderscope/thunderscope_main.py | 2 +- .../thunderscope/widget_setup_functions.py | 15 ++ 8 files changed, 270 insertions(+), 169 deletions(-) create mode 100644 src/software/thunderscope/robot_diagnostics/diagnostics_widget.py diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py index bc48a83110..4181976111 100644 --- a/src/software/thunderscope/controller_diagnostics.py +++ b/src/software/thunderscope/controller_diagnostics.py @@ -1,9 +1,13 @@ +from glob import glob from typing import Callable +import evdev from evdev import InputDevice, categorize, ecodes from threading import Event, Thread from software.thunderscope.proto_unix_io import ProtoUnixIO +import software.python_bindings as tbots_cpp + XBOX_MAX_RANGE = 32768 XBOX_BUTTON_MAX_RANGE = 1024 @@ -24,94 +28,149 @@ class ControllerDiagnostics(object): # TODO add function descriptions def __init__( - self, - input_path: str, - proto_unix_io: ProtoUnixIO, + self, + input_path: str, + proto_unix_io: ProtoUnixIO, ): - self.controller = InputDevice(input_path) + for device in evdev.list_devices(): + try: + self.controller = InputDevice(device) + except Exception as e: + print("Failed to initialize device with path: " + input_path + " with exception " + e) + # TODO check valid path # TODO add auto detect controller - print("Start input device " + input_path) + print("Initializing controller with device path: " + self.controller.path) self.proto_unix_io = proto_unix_io self.stop_event_thread = Event() self._event_thread = Thread(target=self._event_loop) self._event_thread.start() - self.move_x = 0 - self.move_y = 0 - self.ang_vel = 0 - self.power = 1000 + self.constants = tbots_cpp.create2021RobotConstants() + + # self.move_x = 0 + # self.move_y = 0 + # self.ang_vel = 0 + # self.power = 1000 - self.enable_dribbler = 0 - self.dribbler_speed = 0 + # self.enable_dribbler = 0 + # self.dribbler_speed = 0 def _event_loop(self): - print("start event loop") + print("Starting controller event loop") for event in self.controller.read_loop(): if self.stop_event_thread.isSet(): return self.process_event(event) def process_event(self, event): + print("Processing event: " + event) if event.type == ecodes.EV_ABS: absevent = categorize(event) event_t = ecodes.bytype[absevent.event.type][absevent.event.code] - print("EVENT: " + event_t) # TODO remove as fields and pass as args to handlers + + x_vel = 0 + y_vel = 0 + ang_vel = 0 + kick_power = 0 + + dribbler_enabled = 0 + if event_t == "ABS_X": + x_vel = self.coerce_move_value(absevent.event.value, MAX_LINEAR_SPEED_MPS) self.update_move( x=None, y=absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, ) + elif event_t == "ABS_Y": + y_vel = self.coerce_move_value(absevent.event.value, MAX_LINEAR_SPEED_MPS) self.update_move( x=-absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, y=None, ) + elif event_t == "ABS_RX": + ang_vel = self.coerce_move_value(absevent.event.value, MAX_ANGULAR_SPEED_RAD_PER_S) self.update_angular_velocity( absevent.event.value / XBOX_MAX_RANGE * MAX_ANGULAR_SPEED_RAD_PER_S ) - elif event_t == "ABS_HAT0Y": - self.update_dribbler( - self.dribbler_speed - absevent.event.value * DRIBBLER_STEPPER - ) - elif event_t == "ABS_HAT0X": - self.update_power(self.power + absevent.event.value * POWER_STEPPER) - elif event_t == "ABS_RZ": - if absevent.event.value > (XBOX_BUTTON_MAX_RANGE / 2): - self.enable_dribbler = 1 - print("dribbler enabled") - elif event_t == "ABS_Z": - if absevent.event.value < (XBOX_BUTTON_MAX_RANGE / 2): - self.enable_dribbler = 0 - print("dribbler enabled") + + # elif event_t == "ABS_HAT0Y": + # self.update_dribbler( + # self.dribbler_speed - absevent.event.value * DRIBBLER_STEPPER + # ) + # elif event_t == "ABS_HAT0X": + # self.update_power(self.power + absevent.event.value * POWER_STEPPER) + # elif event_t == "ABS_RZ": + # if absevent.event.value > (XBOX_BUTTON_MAX_RANGE / 2): + # self.enable_dribbler = 1 + # print("dribbler enabled") + # elif event_t == "ABS_Z": + # if absevent.event.value < (XBOX_BUTTON_MAX_RANGE / 2): + # self.enable_dribbler = 0 + # print("dribbler enabled") if ( - event_t == "ABS_X" - or event_t == "ABS_Y" - or event_t == "ABS_RX" - or event_t == "ABS_HAT0Y" - or event_t == "ABS_RZ" + event_t == "ABS_X" + or event_t == "ABS_Y" + or event_t == "ABS_RX" + or event_t == "ABS_HAT0Y" + or event_t == "ABS_RZ" ): - self.__perform_move( - self.dribbler_speed, self.move_x, self.move_y, self.ang_vel + self.send_move_command( + dribbler_enabled, + x_vel, + y_vel, + ang_vel ) elif event.type == ecodes.EV_KEY: - print("event code: " + str(event.code)) if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: print("kick") - self.kick() + self.send_kick_command() elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: print("chip") - self.chip() + self.send_chip_command(1) + + def send_move_command( + self, dribbler_enabled: int, x_vel: int, y_vel: int, ang_vel: int + ): + motor_control = MotorControl() - def kick(self): - self.__perform_kick(self.power) + motor_control.direct_velocity_control.velocity.x_component_meters = x_vel + motor_control.direct_velocity_control.velocity.y_component_meters = y_vel + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ang_vel - def chip(self): - self.__perform_chip(self.power) + motor_control.dribbler_speed_rpm = 0 + + # if self.enable_dribbler: + # motor_control.dribbler_speed_rpm = dribbler_speed + if dribbler_enabled: + motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm + + print("Sending motor control: " + motor_control) + + self.proto_unix_io.send_proto(MotorControl, motor_control) + + def send_kick_command(self, power: int): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.kick_speed_m_per_s = power + + print("Sending kick power control: " + power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) + + def send_chip_command(self, distance: int): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.chip_distance_meters = distance + + print("Sending chip power control: " + power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) def update_power(self, new_power): if MIN_POWER < new_power < MAX_POWER: @@ -123,29 +182,50 @@ def update_dribbler(self, new_dribbler_speed): self.dribbler_speed = new_dribbler_speed print("dribbler speed: " + str(self.dribbler_speed)) - def update_move(self, x, y): - if x is not None: - self.move_x = x - if y is not None: - self.move_y = y - - if abs(self.move_x) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS): - self.move_x = 0 - if abs(self.move_y) < (DEADZONE_PERCENTAGE * MAX_LINEAR_SPEED_MPS): - self.move_y = 0 - - self.ang_vel = 0 - - def update_angular_velocity(self, ang_vel): - self.ang_vel = ( - ang_vel - if ang_vel > (MAX_ANGULAR_SPEED_RAD_PER_S * DEADZONE_PERCENTAGE) - else 0 - ) - self.move_x = 0 - self.move_y = 0 - print("new and vel: " + str(self.ang_vel)) + + def coerce_move_value(self, value, factor): + if abs(value) < (DEADZONE_PERCENTAGE * factor): + return 0 + else: + return value / (XBOX_MAX_RANGE * factor) def close(self): + print("Closing controller thread") self.stop_event_thread.set() self._event_thread.join() + +# { +# ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], +# ('EV_KEY', 1): [ +# ('KEY_RECORD', 167), +# (['BTN_A', 'BTN_GAMEPAD', 'BTN_SOUTH'], 304), +# (['BTN_B', 'BTN_EAST'], 305), +# (['BTN_NORTH', 'BTN_X'], 307), +# (['BTN_WEST', 'BTN_Y'], 308), +# ('BTN_TL', 310), +# ('BTN_TR', 311), +# ('BTN_SELECT', 314), +# ('BTN_START', 315), +# ('BTN_MODE', 316), +# ('BTN_THUMBL', 317), +# ('BTN_THUMBR', 318) +# ], +# ('EV_ABS', 3): [ +# (('ABS_X', 0), AbsInfo(value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_Y', 1), AbsInfo(value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_Z', 2), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), +# (('ABS_RX', 3), AbsInfo(value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_RY', 4), AbsInfo(value=-485, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_RZ', 5), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), +# (('ABS_HAT0X', 16), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)), +# (('ABS_HAT0Y', 17), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)) +# ], +# ('EV_FF', 21): [ +# (['FF_EFFECT_MIN', 'FF_RUMBLE'], 80), +# ('FF_PERIODIC', 81), +# (['FF_SQUARE', 'FF_WAVEFORM_MIN'], 88), +# ('FF_TRIANGLE', 89), +# ('FF_SINE', 90), +# (['FF_GAIN', 'FF_MAX_EFFECTS'], 96) +# ] +# } diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 064b547c5e..f0cc11cd8c 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -11,7 +11,6 @@ ) from software.thunderscope.controller_diagnostics import ControllerDiagnostics from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -from software.thunderscope.robot_input_control_manager import RobotInputControlManager from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO import software.python_bindings as tbots_cpp @@ -19,9 +18,11 @@ from google.protobuf.message import Message from proto.import_all_protos import * from pyqtgraph.Qt import QtCore -from typing import Type +from typing import Type, Optional from queue import Empty +from software.thunderscope.thunderscope_main import NUM_ROBOTS + class RobotCommunication(object): @@ -67,14 +68,6 @@ def __init__( self.current_proto_unix_io.register_observer(World, self.world_buffer) - self.control_manager = RobotInputControlManager( - input_device_path, - self.send_move_command, - self.send_kick_command, - self.end_chip_command, - ) - # TODO initialize callbacks for manager - self.current_proto_unix_io.register_observer( PrimitiveSet, self.primitive_buffer ) @@ -92,8 +85,18 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - self.__robot_id_input_source_map: set[(int, IndividualRobotMode)] = set() - self.__robots_to_be_disconnected: set[int] = {} + self.robot_id_control_mode_dict: dict[int, ControlMode] = { + i:ControlMode.DIAGNOSTICS for i in range(NUM_ROBOTS) + } + self.robot_id_individual_mode_dict: dict[int, IndividualRobotMode] = { + i:IndividualRobotMode.NONE for i in range(NUM_ROBOTS) + } + + self.robot_id_estop_send_count_dict: dict[int, int] = { + i:0 for i in range(NUM_ROBOTS) + } + + # initialising the estop # tries to access a plugged in estop. if not found, throws an exception @@ -147,9 +150,14 @@ def setup_for_fullsystem(self) -> None: self.multicast_channel + "%" + self.interface, VISION_PORT, True ) - self.robots_connected_to_fullsystem = { - robot_id for robot_id in range(MAX_ROBOT_IDS_PER_SIDE) - } + for key in self.robot_id_individual_mode_dict: + # does this work in python. how do lambda not unpack tuples wtf thats an actual sin + self.robot_id_individual_mode_dict.items[key] = IndividualRobotMode.AI + + + # self.robots_connected_to_fullsystem = { + # robot_id for robot_id in range(MAX_ROBOT_IDS_PER_SIDE) + # } self.run_world_thread.start() @@ -170,6 +178,7 @@ def toggle_keyboard_estop(self) -> None: ) ) + def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ Changes the input mode for a robot between None, Manual, or AI @@ -180,27 +189,23 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param robot_id: the id of the robot whose mode we're changing """ - self.__robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP - self.__robot_id_input_source_map = set( + self.robot_id_individual_mode_dict = set( map( lambda robot_control: (robot_id, mode) if robot_id == robot_control[0] else robot_control, - self.__robot_id_input_source_map, + self.robot_id_individual_mode_dict, ) ) - # TODO remove this - self.robots_connected_to_fullsystem.discard(robot_id) - self.robots_connected_to_manual.discard(robot_id) - self.robots_to_be_disconnected.pop(robot_id, None) + self.robot_id_estop_send_count_dict.pop(robot_id, None) if mode == IndividualRobotMode.NONE: - self.robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP - elif mode == IndividualRobotMode.MANUAL: - self.robots_connected_to_manual.add(robot_id) - elif mode == IndividualRobotMode.AI: - self.robots_connected_to_fullsystem.add(robot_id) + self.robot_id_estop_send_count_dict[robot_id] = NUM_TIMES_SEND_STOP + # elif mode == IndividualRobotMode.MANUAL: + # self.robots_connected_to_manual.add(robot_id) + # elif mode == IndividualRobotMode.AI: + # self.robots_connected_to_fullsystem.add(robot_id) def toggle_input_mode(self, mode: ControlMode): """ @@ -208,7 +213,10 @@ def toggle_input_mode(self, mode: ControlMode): :param mode: Control mode to use when sending diagnostics primitives """ - self.control_manager.toggle_input_mode(mode) + if mode == ControlMode.XBOX and self.__controller_diagnostics is not None: + self.__input_mode = mode + else: + self.__input_mode = ControlMode.DIAGNOSTICS def __send_estop_state(self) -> None: """ @@ -244,7 +252,8 @@ def __should_send_packet(self) -> bool: return ( self.estop_mode != EstopMode.DISABLE_ESTOP and self.estop_is_playing - and (self.robots_connected_to_fullsystem or self.robots_connected_to_manual) + # does python do what i think it does here + and (IndividualRobotMode.AI or IndividualRobotMode.MANUAL in self.robot_id_individual_mode_dict.values()) ) def __run_world(self): @@ -268,43 +277,6 @@ def __run_world(self): # send the world proto self.send_world.send_proto(world) - def send_move_command( - self, dribbler_speed: int, move_x: int, move_y: int, ang_vel: int - ): - motor_control = MotorControl() - - motor_control.dribbler_speed_rpm = 0 - if self.enable_dribbler: - motor_control.dribbler_speed_rpm = dribbler_speed - - motor_control.direct_velocity_control.velocity.x_component_meters = move_x - motor_control.direct_velocity_control.velocity.y_component_meters = move_y - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - ang_vel - ) - - print(motor_control) - - self.proto_unix_io.send_proto(MotorControl, motor_control) - - def send_kick_command(self, power: int): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.kick_speed_m_per_s = power - - print(power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) - - def send_chip_command(self, distance: int): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = distance - - print(power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) - def __run_primitive_set(self) -> None: """Forward PrimitiveSet protos from fullsystem to the robots. @@ -328,7 +300,12 @@ def __run_primitive_set(self) -> None: robot_primitives = {} # fullsystem is running, so world data is being received - if self.robots_connected_to_fullsystem: + ai_robots : dict[int, IndividualRobotMode] = ( + filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, + self.robot_id_individual_mode_dict) + ) + + if ai_robots: # Get the primitives primitive_set = self.primitive_buffer.get( block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S @@ -336,7 +313,7 @@ def __run_primitive_set(self) -> None: fullsystem_primitives = dict(primitive_set.robot_primitives) for robot_id in fullsystem_primitives.keys(): - if robot_id in self.robots_connected_to_fullsystem: + if robot_id in ai_robots.keys(): robot_primitives[robot_id] = fullsystem_primitives[robot_id] # get the manual control primitive @@ -346,19 +323,24 @@ def __run_primitive_set(self) -> None: ) # diagnostics is running - if self.robots_connected_to_manual: + manual_robots : dict[int, IndividualRobotMode] = ( + filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, + self.robot_id_individual_mode_dict) + ) + + if manual_robots: # for all robots connected to diagnostics, set their primitive - for robot_id in self.robots_connected_to_manual: + for robot_id in manual_robots: robot_primitives[robot_id] = Primitive( direct_control=diagnostics_primitive ) # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives - for robot_id, num_times_to_stop in self.robots_to_be_disconnected.items(): + for robot_id, num_times_to_stop in self.robot_id_estop_send_count_dict.items(): if num_times_to_stop > 0: robot_primitives[robot_id] = Primitive(stop=StopPrimitive()) - self.robots_to_be_disconnected[robot_id] = num_times_to_stop - 1 + self.robot_id_estop_send_count_dict[robot_id] = num_times_to_stop - 1 # initialize total primitive set and send it primitive_set = PrimitiveSet( @@ -380,7 +362,7 @@ def __run_primitive_set(self) -> None: self.should_send_stop = False # sleep if not running fullsystem - if not self.robots_connected_to_fullsystem: + if not ai_robots: time.sleep(ROBOT_COMMUNICATIONS_TIMEOUT_S) def __forward_to_proto_unix_io(self, type: Type[Message], data: Message) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 4fe680559a..51d5e75634 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -87,12 +87,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout.addWidget(self.radio_button_box) # sliders - ( - self.geneva_slider_layout, - self.geneva_slider, - self.geneva_label, - ) = common_widgets.create_slider("Geneva Position", 0, NUM_GENEVA_ANGLES - 1, 1) - vbox_layout.addLayout(self.geneva_slider_layout) ( self.power_slider_layout, @@ -168,12 +162,10 @@ def send_command(self, command: ChickerCommandMode) -> None: """ # gets slider values - geneva_value = self.geneva_slider.value() - power_value = self.power_slider.value() power_control = PowerControl() - power_control.geneva_slot = geneva_value + power_control.geneva_slot = self.geneva_value # sends kick, chip, autokick, or autchip primitive if command == ChickerCommandMode.KICK: @@ -223,10 +215,7 @@ def change_button_state(self, button: QPushButton, enable: bool) -> None: def refresh(self) -> None: - # gets slider values and sets label to that value - geneva_value = self.geneva_slider.value() - self.geneva_label.setText(Slot.Name(geneva_value)) - + # get power value slider value and set the label to that value power_value = self.power_slider.value() self.power_label.setText(str(power_value)) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 2997af11aa..bf5bb52c47 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -13,7 +13,7 @@ class ControlMode(IntEnum): DIAGNOSTICS = 0 XBOX = 1 - +# this class name doesnt make sense class FullSystemConnectWidget(QWidget): """ Class to allow the user to switch between Manual, XBox, and Fullsystem control through Thunderscope UI @@ -23,15 +23,15 @@ class FullSystemConnectWidget(QWidget): """ # Signal to indicate if manual controls should be disabled based on boolean parameter - toggle_controls_signal = pyqtSignal(bool) + # toggle_controls_signal = pyqtSignal(bool) - def __init__(self) -> None: + def __init__(self, toggle_controls_signal) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control + :param toggle_controls_signal The signal to use for handling changes in input mode """ - super(FullSystemConnectWidget, self).__init__() - + self.toggle_controls_signal = toggle_controls_signal vbox_layout = QVBoxLayout() self.connect_options_group = QButtonGroup() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py new file mode 100644 index 0000000000..7e49c2af8f --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -0,0 +1,27 @@ +from pyqtgraph.Qt.QtCore import * +from pyqtgraph.Qt.QtWidgets import * +from software.thunderscope.proto_unix_io import ProtoUnixIO +from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget +from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import DriveAndDribblerWidget + + +class DiagnosticsWidget(QWidget): + + # Signal to indicate if manual controls should be disabled based on boolean parameter + diagnostics_input_mode_signal = pyqtSignal(bool) + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: + super(DiagnosticsWidget, self).__init__() + + vbox_layout = QVBoxLayout() + + self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) + self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) + self.chicker_widget = ChickerWidget(proto_unix_io) + + vbox_layout.addWidget(self.diagnostics_control_input_widget) + vbox_layout.addWidget(self.drive_dribbler_widget) + vbox_layout.addWidget(self.chicker_widget) + + self.setLayout(vbox_layout) + diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index a9fe956291..cfc5853490 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -219,25 +219,33 @@ def configure_base_diagnostics( widget=setup_log_widget(**{"proto_unix_io": diagnostics_proto_unix_io}), ), TScopeWidget( - name="Drive and Dribbler", - widget=setup_drive_and_dribbler_widget( + name="Diagnostics", + widget=setup_diagnostics_widget( **{"proto_unix_io": diagnostics_proto_unix_io} ), anchor="Logs", position="right", ), - TScopeWidget( - name="Chicker", - widget=setup_chicker_widget(**{"proto_unix_io": diagnostics_proto_unix_io}), - anchor="Drive and Dribbler", - position="below", - ), - TScopeWidget( - name="Manual Control Input", - widget=setup_diagnostics_input_widget(), - anchor="Chicker", - position="top", - ), + # TScopeWidget( + # name="Drive and Dribbler", + # widget=setup_drive_and_dribbler_widget( + # **{"proto_unix_io": diagnostics_proto_unix_io} + # ), + # anchor="Logs", + # position="right", + # ), + # TScopeWidget( + # name="Chicker", + # widget=setup_chicker_widget(**{"proto_unix_io": diagnostics_proto_unix_io}), + # anchor="Drive and Dribbler", + # position="below", + # ), + # TScopeWidget( + # name="Manual Control Input", + # widget=setup_diagnostics_input_widget(), + # anchor="Chicker", + # position="top", + # ), ] + extra_widgets diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index b24ce5d015..1cbe88e665 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -285,7 +285,7 @@ for tab in tscope_config.tabs: # handle the signal emitted from switching if tab.key == TabKeys.DIAGNOSTICS: - control_input_widget = tab.find_widget("Manual Control Input") + control_input_widget = tab.find_widget("Diagnostics") if control_input_widget: control_input_widget.toggle_controls_signal( lambda control_mode: robot_communication.toggle_input_mode( diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 42d45f5423..384d79dc57 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -35,6 +35,7 @@ from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( FullSystemConnectWidget, ) +from software.thunderscope.robot_diagnostics.diagnostics_widget import DiagnosticsWidget from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, ) @@ -290,3 +291,17 @@ def setup_drive_and_dribbler_widget( drive_and_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) return drive_and_dribbler_widget + + +def setup_diagnostics_widget( + proto_unix_io: ProtoUnixIO, +) -> DriveAndDribblerWidget: + """Setup the drive and dribbler widget + + :param proto_unix_io: The proto unix io object + :returns: The drive and dribbler widget + + """ + drive_and_dribbler_widget = DiagnosticsWidget(proto_unix_io) + + return drive_and_dribbler_widget From c546f9cf3591d3d978658779ee254ad0e2d13a9f Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 20 Jan 2024 12:43:55 -0800 Subject: [PATCH 017/138] wip saturday notes --- .../thunderscope/controller_diagnostics.py | 1 + .../thunderscope/robot_communication.py | 37 ++++++++++--------- .../robot_diagnostics/diagnostics_widget.py | 5 ++- .../thunderscope/thunderscope_main.py | 27 +++++++------- .../thunderscope/thunderscope_types.py | 5 --- 5 files changed, 37 insertions(+), 38 deletions(-) diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py index 4181976111..e9fdb32560 100644 --- a/src/software/thunderscope/controller_diagnostics.py +++ b/src/software/thunderscope/controller_diagnostics.py @@ -32,6 +32,7 @@ def __init__( input_path: str, proto_unix_io: ProtoUnixIO, ): + # check if estop is in the way for device in evdev.list_devices(): try: self.controller = InputDevice(device) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index f0cc11cd8c..f0e29b90d2 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -299,23 +299,6 @@ def __run_primitive_set(self) -> None: # total primitives for all robots robot_primitives = {} - # fullsystem is running, so world data is being received - ai_robots : dict[int, IndividualRobotMode] = ( - filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, - self.robot_id_individual_mode_dict) - ) - - if ai_robots: - # Get the primitives - primitive_set = self.primitive_buffer.get( - block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S - ) - - fullsystem_primitives = dict(primitive_set.robot_primitives) - for robot_id in fullsystem_primitives.keys(): - if robot_id in ai_robots.keys(): - robot_primitives[robot_id] = fullsystem_primitives[robot_id] - # get the manual control primitive diagnostics_primitive = DirectControlPrimitive( motor_control=self.motor_control_diagnostics_buffer.get(block=False), @@ -324,7 +307,7 @@ def __run_primitive_set(self) -> None: # diagnostics is running manual_robots : dict[int, IndividualRobotMode] = ( - filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, + filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.MANUAL, self.robot_id_individual_mode_dict) ) @@ -335,6 +318,24 @@ def __run_primitive_set(self) -> None: direct_control=diagnostics_primitive ) + # fullsystem is running, so world data is being received + ai_robots : dict[int, IndividualRobotMode] = ( + filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, + self.robot_id_individual_mode_dict) + ) + + if ai_robots: + # Get the primitives + primitive_set = self.primitive_buffer.get( + block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S + ) + + fullsystem_primitives = dict(primitive_set.robot_primitives) + for robot_id in fullsystem_primitives.keys(): + if robot_id in ai_robots.keys(): + robot_primitives[robot_id] = fullsystem_primitives[robot_id] + + # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives for robot_id, num_times_to_stop in self.robot_id_estop_send_count_dict.items(): diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 7e49c2af8f..50782aad99 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -9,14 +9,14 @@ class DiagnosticsWidget(QWidget): # Signal to indicate if manual controls should be disabled based on boolean parameter - diagnostics_input_mode_signal = pyqtSignal(bool) + # diagnostics_input_mode_signal = pyqtSignal(bool) def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() vbox_layout = QVBoxLayout() self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) - self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) + self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io, ) self.chicker_widget = ChickerWidget(proto_unix_io) vbox_layout.addWidget(self.diagnostics_control_input_widget) @@ -25,3 +25,4 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) + diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 1cbe88e665..dd96c8c18f 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -284,22 +284,23 @@ if args.run_diagnostics: for tab in tscope_config.tabs: # handle the signal emitted from switching - if tab.key == TabKeys.DIAGNOSTICS: - control_input_widget = tab.find_widget("Diagnostics") - if control_input_widget: - control_input_widget.toggle_controls_signal( - lambda control_mode: robot_communication.toggle_input_mode( - control_mode + if hasattr(tab, "widgets"): + robot_view_widget = tab.find_widget("Robot View") + if robot_view_widget: + robot_view_widget.control_mode_signal.connect( + lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( + robot_id, robot_mode ) ) + if tab.key == TabKeys.DIAGNOSTICS: + control_input_widget = tab.find_widget("Full System Connect") + if control_input_widget: + control_input_widget.toggle_controls_signal( + lambda control_mode: robot_communication.toggle_input_mode( + control_mode + ) + ) - robot_view_widget = tab.find_widget("Robot View") - if robot_view_widget: - robot_view_widget.control_mode_signal.connect( - lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( - robot_id, robot_mode - ) - ) if args.run_blue or args.run_yellow: robot_communication.setup_for_fullsystem() diff --git a/src/software/thunderscope/thunderscope_types.py b/src/software/thunderscope/thunderscope_types.py index 088cbe18c9..13858494a1 100644 --- a/src/software/thunderscope/thunderscope_types.py +++ b/src/software/thunderscope/thunderscope_types.py @@ -69,11 +69,6 @@ def __init__(self, name: str, key: TabKeys) -> None: def refresh(self) -> None: pass - def find_widget(self, param): - print( - "i hate python - is this syntax for super or child getting called? why pass tho" - ) - class TScopeQTTab(TScopeTab): """ From 8a81a68812c236f163afe887aea3fc9eb6ae137f Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 3 Feb 2024 01:11:36 -0800 Subject: [PATCH 018/138] wip --- src/software/thunderscope/BUILD | 13 +- .../thunderscope/controller_diagnostics.py | 232 ----------------- .../thunderscope/robot_communication.py | 76 +++--- .../thunderscope/robot_diagnostics/BUILD | 25 ++ .../controller_diagnostics.py | 237 ++++++++++++++++++ .../robot_diagnostics/diagnostics_widget.py | 15 +- .../robot_input_control_manager.py | 94 ------- .../thunderscope/thunderscope_config.py | 4 +- .../thunderscope/thunderscope_main.py | 18 +- 9 files changed, 316 insertions(+), 398 deletions(-) delete mode 100644 src/software/thunderscope/controller_diagnostics.py create mode 100644 src/software/thunderscope/robot_diagnostics/controller_diagnostics.py delete mode 100644 src/software/thunderscope/robot_input_control_manager.py diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index 46c03b7bea..64934a514f 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -35,7 +35,6 @@ py_library( "//software/thunderscope:binary_context_managers", "//software/thunderscope:constants", "//software/thunderscope:dock_style", - "//software/thunderscope:input_control_manager", "//software/thunderscope:proto_unix_io", "//software/thunderscope:robot_communication", requirement("pyqtgraph"), @@ -72,6 +71,7 @@ py_library( "//software/thunderscope/replay:proto_player", "//software/thunderscope/robot_diagnostics:chicker", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", + "//software/thunderscope/robot_diagnostics:diagnostics_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", "//software/thunderscope/robot_diagnostics:estop_view", "//software/thunderscope/robot_diagnostics:robot_info", @@ -79,17 +79,6 @@ py_library( ], ) -py_library( - name = "input_control_manager", - srcs = [ - "controller_diagnostics.py", - "robot_input_control_manager.py", - ], - deps = [ - "//software/thunderscope:constants", - ], -) - py_library( name = "thunderscope_types", srcs = ["thunderscope_types.py"], diff --git a/src/software/thunderscope/controller_diagnostics.py b/src/software/thunderscope/controller_diagnostics.py deleted file mode 100644 index e9fdb32560..0000000000 --- a/src/software/thunderscope/controller_diagnostics.py +++ /dev/null @@ -1,232 +0,0 @@ -from glob import glob -from typing import Callable - -import evdev -from evdev import InputDevice, categorize, ecodes -from threading import Event, Thread - -from software.thunderscope.proto_unix_io import ProtoUnixIO -import software.python_bindings as tbots_cpp - - -XBOX_MAX_RANGE = 32768 -XBOX_BUTTON_MAX_RANGE = 1024 - -DEADZONE_PERCENTAGE = 0.30 - -DRIBBLER_STEPPER = 100 -POWER_STEPPER = 100 - -MAX_LINEAR_SPEED_MPS = 2 - - -class ControllerDiagnostics(object): - """ - This class is responsible for reading from an Xbox controller device and - interpreting the inputs into usable inputs for robot. - """ - - # TODO add function descriptions - def __init__( - self, - input_path: str, - proto_unix_io: ProtoUnixIO, - ): - # check if estop is in the way - for device in evdev.list_devices(): - try: - self.controller = InputDevice(device) - except Exception as e: - print("Failed to initialize device with path: " + input_path + " with exception " + e) - - # TODO check valid path - # TODO add auto detect controller - print("Initializing controller with device path: " + self.controller.path) - self.proto_unix_io = proto_unix_io - - self.stop_event_thread = Event() - self._event_thread = Thread(target=self._event_loop) - self._event_thread.start() - - self.constants = tbots_cpp.create2021RobotConstants() - - # self.move_x = 0 - # self.move_y = 0 - # self.ang_vel = 0 - # self.power = 1000 - - # self.enable_dribbler = 0 - # self.dribbler_speed = 0 - - def _event_loop(self): - print("Starting controller event loop") - for event in self.controller.read_loop(): - if self.stop_event_thread.isSet(): - return - self.process_event(event) - - def process_event(self, event): - print("Processing event: " + event) - if event.type == ecodes.EV_ABS: - absevent = categorize(event) - event_t = ecodes.bytype[absevent.event.type][absevent.event.code] - # TODO remove as fields and pass as args to handlers - - x_vel = 0 - y_vel = 0 - ang_vel = 0 - kick_power = 0 - - dribbler_enabled = 0 - - if event_t == "ABS_X": - x_vel = self.coerce_move_value(absevent.event.value, MAX_LINEAR_SPEED_MPS) - self.update_move( - x=None, - y=absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, - ) - - elif event_t == "ABS_Y": - y_vel = self.coerce_move_value(absevent.event.value, MAX_LINEAR_SPEED_MPS) - self.update_move( - x=-absevent.event.value / XBOX_MAX_RANGE * MAX_LINEAR_SPEED_MPS, - y=None, - ) - - elif event_t == "ABS_RX": - ang_vel = self.coerce_move_value(absevent.event.value, MAX_ANGULAR_SPEED_RAD_PER_S) - self.update_angular_velocity( - absevent.event.value / XBOX_MAX_RANGE * MAX_ANGULAR_SPEED_RAD_PER_S - ) - - # elif event_t == "ABS_HAT0Y": - # self.update_dribbler( - # self.dribbler_speed - absevent.event.value * DRIBBLER_STEPPER - # ) - # elif event_t == "ABS_HAT0X": - # self.update_power(self.power + absevent.event.value * POWER_STEPPER) - # elif event_t == "ABS_RZ": - # if absevent.event.value > (XBOX_BUTTON_MAX_RANGE / 2): - # self.enable_dribbler = 1 - # print("dribbler enabled") - # elif event_t == "ABS_Z": - # if absevent.event.value < (XBOX_BUTTON_MAX_RANGE / 2): - # self.enable_dribbler = 0 - # print("dribbler enabled") - - if ( - event_t == "ABS_X" - or event_t == "ABS_Y" - or event_t == "ABS_RX" - or event_t == "ABS_HAT0Y" - or event_t == "ABS_RZ" - ): - self.send_move_command( - dribbler_enabled, - x_vel, - y_vel, - ang_vel - ) - elif event.type == ecodes.EV_KEY: - if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: - print("kick") - self.send_kick_command() - elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: - print("chip") - self.send_chip_command(1) - - def send_move_command( - self, dribbler_enabled: int, x_vel: int, y_vel: int, ang_vel: int - ): - motor_control = MotorControl() - - motor_control.direct_velocity_control.velocity.x_component_meters = x_vel - motor_control.direct_velocity_control.velocity.y_component_meters = y_vel - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ang_vel - - motor_control.dribbler_speed_rpm = 0 - - # if self.enable_dribbler: - # motor_control.dribbler_speed_rpm = dribbler_speed - if dribbler_enabled: - motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm - - print("Sending motor control: " + motor_control) - - self.proto_unix_io.send_proto(MotorControl, motor_control) - - def send_kick_command(self, power: int): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.kick_speed_m_per_s = power - - print("Sending kick power control: " + power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) - - def send_chip_command(self, distance: int): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = distance - - print("Sending chip power control: " + power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) - - def update_power(self, new_power): - if MIN_POWER < new_power < MAX_POWER: - self.power = new_power - print("power: " + str(self.power)) - - def update_dribbler(self, new_dribbler_speed): - if MAX_DRIBBLER_RPM > new_dribbler_speed > MIN_DRIBBLER_RPM: - self.dribbler_speed = new_dribbler_speed - print("dribbler speed: " + str(self.dribbler_speed)) - - - def coerce_move_value(self, value, factor): - if abs(value) < (DEADZONE_PERCENTAGE * factor): - return 0 - else: - return value / (XBOX_MAX_RANGE * factor) - - def close(self): - print("Closing controller thread") - self.stop_event_thread.set() - self._event_thread.join() - -# { -# ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], -# ('EV_KEY', 1): [ -# ('KEY_RECORD', 167), -# (['BTN_A', 'BTN_GAMEPAD', 'BTN_SOUTH'], 304), -# (['BTN_B', 'BTN_EAST'], 305), -# (['BTN_NORTH', 'BTN_X'], 307), -# (['BTN_WEST', 'BTN_Y'], 308), -# ('BTN_TL', 310), -# ('BTN_TR', 311), -# ('BTN_SELECT', 314), -# ('BTN_START', 315), -# ('BTN_MODE', 316), -# ('BTN_THUMBL', 317), -# ('BTN_THUMBR', 318) -# ], -# ('EV_ABS', 3): [ -# (('ABS_X', 0), AbsInfo(value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_Y', 1), AbsInfo(value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_Z', 2), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), -# (('ABS_RX', 3), AbsInfo(value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_RY', 4), AbsInfo(value=-485, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_RZ', 5), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), -# (('ABS_HAT0X', 16), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)), -# (('ABS_HAT0Y', 17), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)) -# ], -# ('EV_FF', 21): [ -# (['FF_EFFECT_MIN', 'FF_RUMBLE'], 80), -# ('FF_PERIODIC', 81), -# (['FF_SQUARE', 'FF_WAVEFORM_MIN'], 88), -# ('FF_TRIANGLE', 89), -# ('FF_SINE', 90), -# (['FF_GAIN', 'FF_MAX_EFFECTS'], 96) -# ] -# } diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index f0e29b90d2..ccb428b0f5 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -2,14 +2,11 @@ import time import os -from evdev import util - from software.thunderscope.constants import ( ROBOT_COMMUNICATIONS_TIMEOUT_S, IndividualRobotMode, EstopMode, ) -from software.thunderscope.controller_diagnostics import ControllerDiagnostics from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -48,6 +45,9 @@ def __init__( :param estop_baudrate: The baudrate of the estop """ + self.receive_ssl_wrapper = None + self.receive_ssl_referee_proto = None + self.send_world = None self.sequence_number = 0 self.last_time = time.time() self.current_proto_unix_io = current_proto_unix_io @@ -85,18 +85,16 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - self.robot_id_control_mode_dict: dict[int, ControlMode] = { - i:ControlMode.DIAGNOSTICS for i in range(NUM_ROBOTS) - } self.robot_id_individual_mode_dict: dict[int, IndividualRobotMode] = { - i:IndividualRobotMode.NONE for i in range(NUM_ROBOTS) + i: IndividualRobotMode.NONE for i in range(NUM_ROBOTS) } self.robot_id_estop_send_count_dict: dict[int, int] = { - i:0 for i in range(NUM_ROBOTS) + i: 0 for i in range(NUM_ROBOTS) } - + # default to diagnostics control. + self.__input_mode = ControlMode.DIAGNOSTICS # initialising the estop # tries to access a plugged in estop. if not found, throws an exception @@ -116,17 +114,6 @@ def __init__( except Exception: raise Exception(f"Invalid Estop found at location {self.estop_path}") - if input_device_path: - if os.path.exists(input_device_path) and util.is_device( - input_device_path - ): - self.__input_mode = ControlMode.XBOX - self.__controller_diagnostics = ControllerDiagnostics( - input_device_path, - ) - else: - self.__input_mode = ControlMode.DIAGNOSTICS - def setup_for_fullsystem(self) -> None: """ @@ -151,8 +138,7 @@ def setup_for_fullsystem(self) -> None: ) for key in self.robot_id_individual_mode_dict: - # does this work in python. how do lambda not unpack tuples wtf thats an actual sin - self.robot_id_individual_mode_dict.items[key] = IndividualRobotMode.AI + self.robot_id_individual_mode_dict[key] = IndividualRobotMode.AI # self.robots_connected_to_fullsystem = { @@ -213,10 +199,8 @@ def toggle_input_mode(self, mode: ControlMode): :param mode: Control mode to use when sending diagnostics primitives """ - if mode == ControlMode.XBOX and self.__controller_diagnostics is not None: - self.__input_mode = mode - else: - self.__input_mode = ControlMode.DIAGNOSTICS + self.__input_mode = mode + def __send_estop_state(self) -> None: """ @@ -299,32 +283,32 @@ def __run_primitive_set(self) -> None: # total primitives for all robots robot_primitives = {} - # get the manual control primitive - diagnostics_primitive = DirectControlPrimitive( - motor_control=self.motor_control_diagnostics_buffer.get(block=False), - power_control=self.power_control_diagnostics_buffer.get(block=False), - ) - - # diagnostics is running - manual_robots : dict[int, IndividualRobotMode] = ( - filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.MANUAL, - self.robot_id_individual_mode_dict) - ) + # diagnostics is running. Only send diagnostics packets if using diagnostics control mode + if self.__input_mode == ControlMode.DIAGNOSTICS: + # get the manual control primitive + diagnostics_primitive = DirectControlPrimitive( + motor_control=self.motor_control_diagnostics_buffer.get(block=False), + power_control=self.power_control_diagnostics_buffer.get(block=False), + ) - if manual_robots: + manually_controlled_robots : dict[int, IndividualRobotMode] = ( + filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.MANUAL, + self.robot_id_individual_mode_dict) + ) + if len(manually_controlled_robots) > 0: # for all robots connected to diagnostics, set their primitive - for robot_id in manual_robots: - robot_primitives[robot_id] = Primitive( - direct_control=diagnostics_primitive - ) + for robot_id in manually_controlled_robots: + robot_primitives[robot_id] = Primitive( + direct_control=diagnostics_primitive + ) # fullsystem is running, so world data is being received - ai_robots : dict[int, IndividualRobotMode] = ( + ai_controlled_robots : dict[int, IndividualRobotMode] = ( filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, self.robot_id_individual_mode_dict) ) - if ai_robots: + if ai_controlled_robots: # Get the primitives primitive_set = self.primitive_buffer.get( block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S @@ -332,7 +316,7 @@ def __run_primitive_set(self) -> None: fullsystem_primitives = dict(primitive_set.robot_primitives) for robot_id in fullsystem_primitives.keys(): - if robot_id in ai_robots.keys(): + if robot_id in ai_controlled_robots: robot_primitives[robot_id] = fullsystem_primitives[robot_id] @@ -363,7 +347,7 @@ def __run_primitive_set(self) -> None: self.should_send_stop = False # sleep if not running fullsystem - if not ai_robots: + if not ai_controlled_robots: time.sleep(ROBOT_COMMUNICATIONS_TIMEOUT_S) def __forward_to_proto_unix_io(self, type: Type[Message], data: Message) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index b02b02e3ac..540d1f15e4 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -23,6 +23,31 @@ py_library( ], ) +py_library( + name = "controller", + srcs = ["controller_diagnostics.py"], + deps = [ + "//proto:software_py_proto", + "//software/thunderscope:constants", + "//software/thunderscope:thread_safe_buffer", + requirement("pyqtgraph"), + ], +) + +py_library( + name = "diagnostics_widget", + srcs = ["diagnostics_widget.py"], + deps = [ + "//software/thunderscope/robot_diagnostics:controller", + "//software/thunderscope/robot_diagnostics:chicker", + "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", + "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", + "//software/thunderscope:constants", + "//software/thunderscope:thread_safe_buffer", + requirement("pyqtgraph"), + ], +) + py_library( name = "diagnostics_input_widget", srcs = ["diagnostics_input_widget.py"], diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py new file mode 100644 index 0000000000..3bfd19b372 --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -0,0 +1,237 @@ +import logging + +from evdev import InputDevice, categorize, ecodes, list_devices +from threading import Event, Thread + +from software.thunderscope.proto_unix_io import ProtoUnixIO +import software.python_bindings as tbots_cpp + +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode + +XBOX_MAX_RANGE = 32768 +XBOX_BUTTON_MAX_RANGE = 1024 + +DEADZONE_PERCENTAGE = 0.30 + +DRIBBLER_STEPPER = 100 +POWER_STEPPER = 100 + +MAX_LINEAR_SPEED_MPS = 2 + + +class ControllerDiagnostics(object): + """ + This class is responsible for reading from an Xbox controller device and + interpreting the inputs into usable inputs for robot. + """ + + # TODO add function descriptions + def __init__( + self, + proto_unix_io: ProtoUnixIO, + ): + # TODO: check if estop gets in the way + self.__input_mode = ControlMode.DIAGNOSTICS + for device in list_devices(): + try: + self.controller = InputDevice(device) + except Exception as e: + print("Failed to initialize device with path: " + device + " with exception " + e) + + # TODO add a way to switch between different controllers plugged in + logging.info("Initializing controller with device path: " + self.controller.path) + self.proto_unix_io = proto_unix_io + + self.__stop_event_thread = Event() + self.__event_thread = Thread(target=self.__event_loop) + self.__event_thread.start() + + self.constants = tbots_cpp.create2021RobotConstants() + + self.x_vel = 0 + self.y_vel = 0 + self.ang_vel = 0 + self.kick_power = 0 + self.dribbler_enabled = 0 + self.dribbler_speed = 0 + + + # self.move_x = 0 + # self.move_y = 0 + # self.ang_vel = 0 + # self.power = 1000 + + # self.enable_dribbler = 0 + # self.dribbler_speed = 0 + + def toggle_input_mode(self, mode: ControlMode): + """ + Changes the diagnostics input mode for all robots between Xbox and Diagnostics. + + :param mode: Control mode to use when sending diagnostics primitives + """ + self.__input_mode = mode + + + def __parse_move_event_value(self, value, factor): + if abs(value) < (DEADZONE_PERCENTAGE * factor): + return 0 + else: + return value / (XBOX_MAX_RANGE * factor) + + def __parse_dribbler_event_value(self, value): + # can probably combine these 2 parsers but abastracting the +/- is annoying so 2 funcs it is + new_speed = self.dribbler_speed - value * DRIBBLER_STEPPER + if MIN_DRIBBLER_RPM < new_speed < MAX_DRIBBLER_RPM: + return new_speed + + def __parse_kick_event_value(self, value): + new_power = self.power + value * POWER_STEPPER + if MIN_POWER < new_power < MAX_POWER: + return new_power + + def __parse_dribbler_enabled_event_value(self, value): + # there's a way to simplify this logic but im too tired rn probably lift return up and smth + if value > (XBOX_BUTTON_MAX_RANGE / 2): + return 1 + if value < (XBOX_BUTTON_MAX_RANGE / 2): + return 0 + + def __send_move_command(self): + motor_control = MotorControl() + + motor_control.direct_velocity_control.velocity.x_component_meters = self.x_vel + motor_control.direct_velocity_control.velocity.y_component_meters = self.y_vel + motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.ang_vel + + motor_control.dribbler_speed_rpm = 0 + + # if self.enable_dribbler: + # motor_control.dribbler_speed_rpm = dribbler_speed + # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' + if self.dribbler_enabled: + motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm + + logging.info("Sending motor control: " + motor_control) + + self.proto_unix_io.send_proto(MotorControl, motor_control) + + def __send_kick_command(self): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.kick_speed_m_per_s = self.kick_power + + logging.info("Sending kick power control: " + power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) + + def __send_chip_command(self): + power_control = PowerControl() + power_control.geneva_slot = 1 + power_control.chicker.chip_distance_meters = self.kick_power # not sure if we should use this value + + logging.info("Sending chip power control: " + power_control) + + self.proto_unix_io.send_proto(PowerControl, power_control) + + def __process_event(self, event): + logging.info("Processing event: " + event) + + abs_event = categorize(event) + + match event.type: + case ecodes.EV_ABS: + # grab the event type + event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] + + match event_t: + case "ABS_X": + self.x_vel = ( + self.__parse_joystick_event_value(abs_event.event.value, MAX_LINEAR_SPEED_MPS)) + + case "ABS_Y": + self.y_vel = ( + self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_MPS)) + + case "ABS_RX": + self.ang_vel = ( + self.__parse_move_event_value(abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S)) + + case "ABS_HAT0Y": + self.dribbler_speed = ( + self.__parse_dribbler_event_value(abs_event.event.value)) + + case "ABS_HAT0X": + self.kick_power = ( + self.__parse_kick_event_value(abs_event.event.value)) + + case "ABS_RZ": + self.dribbler_enabled = ( + self.__parse_dribbler_enabled_event_value(abs_event.event.value)) + + case "ABS_Z": + self.dribbler_enabled = ( + self.__parse_dribbler_enabled_event_value(abs_event.event.value)) + + + + case ecodes.EV_KEY: + if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: + logging.info("kick") + self.__send_kick_command() + elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: + logging.info("chip") + self.__send_chip_command() + + if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: + self.__send_move_command() + + + + def __event_loop(self): + logging.info("Starting controller event loop") + for event in self.controller.read_loop(): + if self.__stop_event_thread.isSet(): + return + self.__process_event(event) + + def close(self): + logging.info("Closing controller thread") + self.__stop_event_thread.set() + self.__event_thread.join() + +# { +# ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], +# ('EV_KEY', 1): [ +# ('KEY_RECORD', 167), +# (['BTN_A', 'BTN_GAMEPAD', 'BTN_SOUTH'], 304), +# (['BTN_B', 'BTN_EAST'], 305), +# (['BTN_NORTH', 'BTN_X'], 307), +# (['BTN_WEST', 'BTN_Y'], 308), +# ('BTN_TL', 310), +# ('BTN_TR', 311), +# ('BTN_SELECT', 314), +# ('BTN_START', 315), +# ('BTN_MODE', 316), +# ('BTN_THUMBL', 317), +# ('BTN_THUMBR', 318) +# ], +# ('EV_ABS', 3): [ +# (('ABS_X', 0), AbsInfo(value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_Y', 1), AbsInfo(value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_Z', 2), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), +# (('ABS_RX', 3), AbsInfo(value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_RY', 4), AbsInfo(value=-485, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), +# (('ABS_RZ', 5), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), +# (('ABS_HAT0X', 16), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)), +# (('ABS_HAT0Y', 17), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)) +# ], +# ('EV_FF', 21): [ +# (['FF_EFFECT_MIN', 'FF_RUMBLE'], 80), +# ('FF_PERIODIC', 81), +# (['FF_SQUARE', 'FF_WAVEFORM_MIN'], 88), +# ('FF_TRIANGLE', 89), +# ('FF_SINE', 90), +# (['FF_GAIN', 'FF_MAX_EFFECTS'], 96) +# ] +# } diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 50782aad99..5a1c2f9486 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,12 +1,11 @@ -from pyqtgraph.Qt.QtCore import * -from pyqtgraph.Qt.QtWidgets import * +from software.thunderscope.robot_diagnostics.controller_diagnostics import ControllerDiagnostics from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import DriveAndDribblerWidget -class DiagnosticsWidget(QWidget): +class DiagnosticsWidget(TScopeWidget): # Signal to indicate if manual controls should be disabled based on boolean parameter # diagnostics_input_mode_signal = pyqtSignal(bool) @@ -15,9 +14,19 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout = QVBoxLayout() + self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io, ) self.chicker_widget = ChickerWidget(proto_unix_io) + self.controller = ControllerDiagnostics(proto_unix_io) + + self.diagnostics_control_input_widget.toggle_control_signal( + lambda control_mode: self.controller.toggle_input_mode( + control_mode + ) + ) + + vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) diff --git a/src/software/thunderscope/robot_input_control_manager.py b/src/software/thunderscope/robot_input_control_manager.py deleted file mode 100644 index 2daaf24c28..0000000000 --- a/src/software/thunderscope/robot_input_control_manager.py +++ /dev/null @@ -1,94 +0,0 @@ -import os -from typing import Callable - -from evdev import util - -from software.thunderscope.constants import IndividualRobotMode, HANDHELD_PATH -from software.thunderscope.controller_diagnostics import ControllerDiagnostics -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode - - -class RobotInputControlManager: - """ - Sets up a manager that handles the control input source for each robot, - and forwards that to communication - - @:param xbox_enabled: If xbox control is enabled - """ - - def __init__( - self, - controller_device_path: str, - perform_move: Callable[[int, int, int, int], None], - perform_kick: Callable[[int], None], - perform_chip: Callable[[int], None], - ): - self.__perform_move = perform_move - self.__perform_kick = perform_kick - self.__perform_chip = perform_chip - self.__input_mode = ControlMode.DIAGNOSTICS - self.__controller_diagnostics = None - - self.setup_controller(controller_device_path) - - self.__robot_id_input_source_map: set[(int, IndividualRobotMode)] = set() - - self.__robots_to_be_disconnected: set[int] = {} - - def setup_controller(self, controller_device_path: str): - if controller_device_path: - if os.path.exists(controller_device_path) and util.is_device( - controller_device_path - ): - self.__input_mode = ControlMode.XBOX - self.__controller_diagnostics = ControllerDiagnostics( - controller_device_path, - self.__perform_move, - self.__perform_kick, - self.__perform_chip, - ) - else: - self.__input_mode = ControlMode.DIAGNOSTICS - - def set_full_system_control(self): - self.__robot_id_input_source_map = set( - map( - lambda robot_control: (robot_control[0], IndividualRobotMode.AI), - self.__robot_id_input_source_map, - ) - ) - - def get_diagnostics_input_mode(self) -> ControlMode: - """ - Get the diagnostics input mode - :return: - """ - return self.__input_mode - - def toggle_input_mode(self, mode: ControlMode): - """ - Process and Qt signal indicating the diagnostics control mode has changed - :param mode: - :return: - """ - if mode == ControlMode.XBOX and self.__controller_diagnostics is not None: - self.__input_mode = mode - else: - self.__input_mode = ControlMode.DIAGNOSTICS - - def toggle_control_mode_for_robot(self, robot_id: int, mode: IndividualRobotMode): - """ - Updates the control mode for the given robot id to the given mode - :param robot_id: The robot id to update - :param mode: The input mode this robot should use - one of AI, MANUAL, or NONE - """ - self.__robots_to_be_disconnected[robot_id] = NUM_TIMES_SEND_STOP - self.__robot_id_input_source_map = set( - map( - # TODO figure way to destruct params or make callback maybe - lambda robot_control: (robot_id, mode) - if robot_id == robot_control[0] - else robot_control, - self.__robot_id_input_source_map, - ) - ) diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index cfc5853490..00b1156d7f 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -28,12 +28,12 @@ class TScopeConfig: proto_unix_io_map: Dict[ ProtoUnixIOTypes, ProtoUnixIO ] # mapping of protos needed for this view - tabs: Sequence[TScopeTab] # list of tabs for this view + tabs: Sequence[TScopeQTTab] # list of tabs for this view def __init__( self, proto_unix_io_map: Dict[ProtoUnixIOTypes, ProtoUnixIO], - tabs: Sequence[TScopeTab], + tabs: Sequence[TScopeQTTab], ) -> None: self.proto_unix_io_map = proto_unix_io_map self.tabs = tabs diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index dd96c8c18f..0405fc7e7d 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -286,20 +286,20 @@ # handle the signal emitted from switching if hasattr(tab, "widgets"): robot_view_widget = tab.find_widget("Robot View") - if robot_view_widget: - robot_view_widget.control_mode_signal.connect( + if robot_view_widget is not None: + robot_view_widget.toggle_control_mode_signal.connect( lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( robot_id, robot_mode ) ) - if tab.key == TabKeys.DIAGNOSTICS: - control_input_widget = tab.find_widget("Full System Connect") - if control_input_widget: - control_input_widget.toggle_controls_signal( - lambda control_mode: robot_communication.toggle_input_mode( - control_mode - ) + # TODO: remove this below - don't need to check for input mode (xbox/diag) + diagnostics_widget = tab.find_widget("Diagnostics Widget") + if diagnostics_widget is not None: + diagnostics_widget.toggle_control_signal( + lambda control_mode: robot_communication.toggle_input_mode( + control_mode ) + ) if args.run_blue or args.run_yellow: From 91b5d01a8a162d97d93ccd03cd63fd7a56a5fc43 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 3 Feb 2024 12:46:03 -0800 Subject: [PATCH 019/138] moving lgoic out of rob comm --- .../robot_diagnostics/diagnostics_widget.py | 8 ++++++-- .../robot_diagnostics/drive_and_dribbler_widget.py | 10 ++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 5a1c2f9486..5475a6de2d 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,7 +1,7 @@ from software.thunderscope.robot_diagnostics.controller_diagnostics import ControllerDiagnostics from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget, ControlMode from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import DriveAndDribblerWidget @@ -14,9 +14,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout = QVBoxLayout() + self.__control_mode = ControlMode.DIAGNOSTICS + self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) - self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io, ) + self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.controller = ControllerDiagnostics(proto_unix_io) @@ -34,4 +36,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) + def __ + diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 73b8af8356..c55c01838e 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -22,6 +22,16 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io + self.motor_control_diagnostics_buffer = ThreadSafeBuffer(1, MotorControl) + self.power_control_diagnostics_buffer = ThreadSafeBuffer(1, PowerControl) + + self.proto_unix_io.register_observer( + MotorControl, self.motor_control_diagnostics_buffer + ) + self.proto_unix_io.register_observer( + PowerControl, self.power_control_diagnostics_buffer + ) + # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) From 5939e75b41f8c8cd66283ca8bc499c7c9d24d912 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 3 Feb 2024 13:06:48 -0800 Subject: [PATCH 020/138] wip --- .../controller_diagnostics.py | 50 +++++++------------ 1 file changed, 19 insertions(+), 31 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 3bfd19b372..20201561f2 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -30,8 +30,10 @@ def __init__( self, proto_unix_io: ProtoUnixIO, ): + self.enabled = False + # TODO: check if estop gets in the way - self.__input_mode = ControlMode.DIAGNOSTICS + for device in list_devices(): try: self.controller = InputDevice(device) @@ -48,30 +50,20 @@ def __init__( self.constants = tbots_cpp.create2021RobotConstants() - self.x_vel = 0 - self.y_vel = 0 - self.ang_vel = 0 - self.kick_power = 0 - self.dribbler_enabled = 0 - self.dribbler_speed = 0 - - - # self.move_x = 0 - # self.move_y = 0 - # self.ang_vel = 0 - # self.power = 1000 + self.x_vel = 0.0 + self.y_vel = 0.0 + self.ang_vel = 0.0 + self.kick_power = 0.0 + self.dribbler_speed = 0.0 + self.dribbler_enabled = False - # self.enable_dribbler = 0 - # self.dribbler_speed = 0 - - def toggle_input_mode(self, mode: ControlMode): + def set_enabled(self, enabled: bool): """ Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - :param mode: Control mode to use when sending diagnostics primitives + :param enabled: to which state to set controller enabled. """ - self.__input_mode = mode - + self.enabled = enabled def __parse_move_event_value(self, value, factor): if abs(value) < (DEADZONE_PERCENTAGE * factor): @@ -85,17 +77,13 @@ def __parse_dribbler_event_value(self, value): if MIN_DRIBBLER_RPM < new_speed < MAX_DRIBBLER_RPM: return new_speed - def __parse_kick_event_value(self, value): + def __parse_kick_event_value(self, value) -> int: new_power = self.power + value * POWER_STEPPER if MIN_POWER < new_power < MAX_POWER: return new_power - def __parse_dribbler_enabled_event_value(self, value): - # there's a way to simplify this logic but im too tired rn probably lift return up and smth - if value > (XBOX_BUTTON_MAX_RANGE / 2): - return 1 - if value < (XBOX_BUTTON_MAX_RANGE / 2): - return 0 + def __parse_dribbler_enabled_event_value(self, value) -> bool: + return value > (XBOX_BUTTON_MAX_RANGE / 2) def __send_move_command(self): motor_control = MotorControl() @@ -110,7 +98,9 @@ def __send_move_command(self): # motor_control.dribbler_speed_rpm = dribbler_speed # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' if self.dribbler_enabled: - motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm + motor_control.dribbler_speed_rpm = self.dribbler_enabled ? + self.constants.indefinite_dribbler_speed_rpm + logging.info("Sending motor control: " + motor_control) @@ -177,13 +167,11 @@ def __process_event(self, event): case ecodes.EV_KEY: if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: - logging.info("kick") self.__send_kick_command() elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: - logging.info("chip") self.__send_chip_command() - if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: + if self.enabled and event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: self.__send_move_command() From 10619408655233cf71fe2a618164335420285ed0 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 10 Feb 2024 15:06:44 -0800 Subject: [PATCH 021/138] wip --- .../controller_diagnostics.py | 33 ++++++++++++------- .../robot_diagnostics/diagnostics_widget.py | 15 +++------ .../drive_and_dribbler_widget.py | 2 +- 3 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 20201561f2..caf2effd2d 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -57,14 +57,6 @@ def __init__( self.dribbler_speed = 0.0 self.dribbler_enabled = False - def set_enabled(self, enabled: bool): - """ - Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - - :param enabled: to which state to set controller enabled. - """ - self.enabled = enabled - def __parse_move_event_value(self, value, factor): if abs(value) < (DEADZONE_PERCENTAGE * factor): return 0 @@ -98,8 +90,7 @@ def __send_move_command(self): # motor_control.dribbler_speed_rpm = dribbler_speed # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' if self.dribbler_enabled: - motor_control.dribbler_speed_rpm = self.dribbler_enabled ? - self.constants.indefinite_dribbler_speed_rpm + motor_control.dribbler_speed_rpm = self.dribbler_enabled ? self.constants.indefinite_dribbler_speed_rpm logging.info("Sending motor control: " + motor_control) @@ -171,7 +162,7 @@ def __process_event(self, event): elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: self.__send_chip_command() - if self.enabled and event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: + if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: self.__send_move_command() @@ -181,13 +172,31 @@ def __event_loop(self): for event in self.controller.read_loop(): if self.__stop_event_thread.isSet(): return - self.__process_event(event) + if self.enabled: + self.__process_event(event) def close(self): logging.info("Closing controller thread") self.__stop_event_thread.set() self.__event_thread.join() + def set_enabled(self, enabled: bool): + """ + Changes the diagnostics input mode for all robots between Xbox and Diagnostics. + + :param enabled: to which state to set controller enabled. + """ + self.enabled = enabled + + def set_enabled(self, enabled: bool): + """ + Changes the diagnostics input mode for all robots between Xbox and Diagnostics. + + :param enabled: to which state to set controller enabled. + """ + self.enabled = enabled + + # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 5475a6de2d..d3bc59da4f 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -14,28 +14,23 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout = QVBoxLayout() - self.__control_mode = ControlMode.DIAGNOSTICS - - self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.controller = ControllerDiagnostics(proto_unix_io) - self.diagnostics_control_input_widget.toggle_control_signal( + self.diagnostics_control_input_widget.toggle_control_signal.connect( lambda control_mode: self.controller.toggle_input_mode( - control_mode + control_mode == ControlMode.DIAGNOSTICS ) ) - - vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) self.setLayout(vbox_layout) - def __ - - + def update_mode(self, mode: ControlMode): + self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) + self.controller.set_enabled(mode == ControlMode.XBOX) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index c55c01838e..20e1147ba8 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -180,7 +180,7 @@ def setup_dribbler(self, title: str) -> QGroupBox: return group_box - def toggle_all(self, enable: bool) -> None: + def set_enabled(self, enable: bool) -> None: """ Disables or enables all sliders and buttons depending on boolean parameter From e1f40e936b255e9e2306cd736ec6445dfa29e326 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 10 Feb 2024 18:39:30 -0800 Subject: [PATCH 022/138] finally working the way i want --- .../thunderscope/robot_communication.py | 41 +++--- .../thunderscope/robot_diagnostics/BUILD | 1 + .../controller_diagnostics.py | 117 +++++++++--------- .../diagnostics_input_widget.py | 2 +- .../robot_diagnostics/diagnostics_widget.py | 21 ++-- .../robot_diagnostics/robot_view.py | 4 +- .../thunderscope/thunderscope_main.py | 16 +-- 7 files changed, 103 insertions(+), 99 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index ccb428b0f5..1093813da3 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -15,25 +15,24 @@ from google.protobuf.message import Message from proto.import_all_protos import * from pyqtgraph.Qt import QtCore -from typing import Type, Optional +from typing import Type from queue import Empty -from software.thunderscope.thunderscope_main import NUM_ROBOTS +NUM_ROBOTS = 6 class RobotCommunication(object): - """ Communicate with the robots """ def __init__( - self, - current_proto_unix_io: ProtoUnixIO, - multicast_channel: str, - interface: str, - estop_mode: EstopMode, - input_device_path: str = None, - estop_path: os.PathLike = None, - estop_baudrate: int = 115200, + self, + current_proto_unix_io: ProtoUnixIO, + multicast_channel: str, + interface: str, + estop_mode: EstopMode, + input_device_path: str = None, + estop_path: os.PathLike = None, + estop_baudrate: int = 115200, ): """Initialize the communication with the robots @@ -114,7 +113,6 @@ def __init__( except Exception: raise Exception(f"Invalid Estop found at location {self.estop_path}") - def setup_for_fullsystem(self) -> None: """ Sets up a world sender, a listener for SSL vision data, and connects all robots to fullsystem as default @@ -140,7 +138,6 @@ def setup_for_fullsystem(self) -> None: for key in self.robot_id_individual_mode_dict: self.robot_id_individual_mode_dict[key] = IndividualRobotMode.AI - # self.robots_connected_to_fullsystem = { # robot_id for robot_id in range(MAX_ROBOT_IDS_PER_SIDE) # } @@ -164,7 +161,6 @@ def toggle_keyboard_estop(self) -> None: ) ) - def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ Changes the input mode for a robot between None, Manual, or AI @@ -201,7 +197,6 @@ def toggle_input_mode(self, mode: ControlMode): """ self.__input_mode = mode - def __send_estop_state(self) -> None: """ Constant loop which sends the current estop status proto if estop is not disabled @@ -234,10 +229,11 @@ def __should_send_packet(self) -> bool: :return: boolean """ return ( - self.estop_mode != EstopMode.DISABLE_ESTOP - and self.estop_is_playing - # does python do what i think it does here - and (IndividualRobotMode.AI or IndividualRobotMode.MANUAL in self.robot_id_individual_mode_dict.values()) + self.estop_mode != EstopMode.DISABLE_ESTOP + and self.estop_is_playing + # does python do what i think it does here + and ( + IndividualRobotMode.AI or IndividualRobotMode.MANUAL in self.robot_id_individual_mode_dict.values()) ) def __run_world(self): @@ -291,19 +287,19 @@ def __run_primitive_set(self) -> None: power_control=self.power_control_diagnostics_buffer.get(block=False), ) - manually_controlled_robots : dict[int, IndividualRobotMode] = ( + manually_controlled_robots: dict[int, IndividualRobotMode] = ( filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.MANUAL, self.robot_id_individual_mode_dict) ) if len(manually_controlled_robots) > 0: - # for all robots connected to diagnostics, set their primitive + # for all robots connected to diagnostics, set their primitive for robot_id in manually_controlled_robots: robot_primitives[robot_id] = Primitive( direct_control=diagnostics_primitive ) # fullsystem is running, so world data is being received - ai_controlled_robots : dict[int, IndividualRobotMode] = ( + ai_controlled_robots: dict[int, IndividualRobotMode] = ( filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, self.robot_id_individual_mode_dict) ) @@ -319,7 +315,6 @@ def __run_primitive_set(self) -> None: if robot_id in ai_controlled_robots: robot_primitives[robot_id] = fullsystem_primitives[robot_id] - # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives for robot_id, num_times_to_stop in self.robot_id_estop_send_count_dict.items(): diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 540d1f15e4..65c4ad282e 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -42,6 +42,7 @@ py_library( "//software/thunderscope/robot_diagnostics:chicker", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", + "//software/thunderscope:thunderscope_types", "//software/thunderscope:constants", "//software/thunderscope:thread_safe_buffer", requirement("pyqtgraph"), diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index caf2effd2d..555ee66d80 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -3,11 +3,11 @@ from evdev import InputDevice, categorize, ecodes, list_devices from threading import Event, Thread +from proto.import_all_protos import * +from software.thunderscope.constants import * from software.thunderscope.proto_unix_io import ProtoUnixIO import software.python_bindings as tbots_cpp -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode - XBOX_MAX_RANGE = 32768 XBOX_BUTTON_MAX_RANGE = 1024 @@ -16,7 +16,8 @@ DRIBBLER_STEPPER = 100 POWER_STEPPER = 100 -MAX_LINEAR_SPEED_MPS = 2 +MAX_LINEAR_SPEED_METER_PER_S = 2 +MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 class ControllerDiagnostics(object): @@ -31,6 +32,7 @@ def __init__( proto_unix_io: ProtoUnixIO, ): self.enabled = False + self.controller = None # TODO: check if estop gets in the way @@ -40,12 +42,17 @@ def __init__( except Exception as e: print("Failed to initialize device with path: " + device + " with exception " + e) + if self.controller is None: + raise RuntimeError("Could not initialize a controller, there are no valid " + "controllers connected") + # TODO add a way to switch between different controllers plugged in - logging.info("Initializing controller with device path: " + self.controller.path) + logging.info( + "Initializing controller " + self.controller.info.__str__() + " and device path location: " + self.controller.path) self.proto_unix_io = proto_unix_io self.__stop_event_thread = Event() - self.__event_thread = Thread(target=self.__event_loop) + self.__event_thread = Thread(target=self.__event_loop, daemon=True) self.__event_thread.start() self.constants = tbots_cpp.create2021RobotConstants() @@ -90,8 +97,7 @@ def __send_move_command(self): # motor_control.dribbler_speed_rpm = dribbler_speed # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' if self.dribbler_enabled: - motor_control.dribbler_speed_rpm = self.dribbler_enabled ? self.constants.indefinite_dribbler_speed_rpm - + motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm logging.info("Sending motor control: " + motor_control) @@ -109,71 +115,69 @@ def __send_kick_command(self): def __send_chip_command(self): power_control = PowerControl() power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = self.kick_power # not sure if we should use this value + power_control.chicker.chip_distance_meters = self.kick_power # not sure if we should use this value logging.info("Sending chip power control: " + power_control) self.proto_unix_io.send_proto(PowerControl, power_control) def __process_event(self, event): - logging.info("Processing event: " + event) + logging.info("Processing event: " + str(event)) abs_event = categorize(event) - - match event.type: - case ecodes.EV_ABS: - # grab the event type - event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] - - match event_t: - case "ABS_X": - self.x_vel = ( - self.__parse_joystick_event_value(abs_event.event.value, MAX_LINEAR_SPEED_MPS)) - - case "ABS_Y": - self.y_vel = ( - self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_MPS)) - - case "ABS_RX": - self.ang_vel = ( - self.__parse_move_event_value(abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S)) - - case "ABS_HAT0Y": - self.dribbler_speed = ( - self.__parse_dribbler_event_value(abs_event.event.value)) - - case "ABS_HAT0X": - self.kick_power = ( - self.__parse_kick_event_value(abs_event.event.value)) - - case "ABS_RZ": - self.dribbler_enabled = ( - self.__parse_dribbler_enabled_event_value(abs_event.event.value)) - - case "ABS_Z": - self.dribbler_enabled = ( - self.__parse_dribbler_enabled_event_value(abs_event.event.value)) - - - - case ecodes.EV_KEY: - if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: - self.__send_kick_command() - elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: - self.__send_chip_command() + event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] + # TODO: nump python version so we can use pattern matching for this + # TODO: make codebase use python logging instead of stdout + logging.info("Processing event: " + str(event_t)) + if event.type == ecodes.EV_ABS: + # grab the event type + event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] + if event_t == "ABS_X": + self.x_vel = ( + self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S)) + + elif event_t == "ABS_Y": + self.y_vel = ( + self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S)) + + elif event_t == "ABS_RX": + self.ang_vel = ( + self.__parse_move_event_value(abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S)) + + elif event_t == "ABS_HAT0Y": + self.dribbler_speed = ( + self.__parse_dribbler_event_value(abs_event.event.value)) + + elif event_t == "ABS_HAT0X": + self.kick_power = ( + self.__parse_kick_event_value(abs_event.event.value)) + + elif event_t == "ABS_RZ": + self.dribbler_enabled = ( + self.__parse_dribbler_enabled_event_value(abs_event.event.value)) + + elif event_t == "ABS_Z": + self.dribbler_enabled = ( + self.__parse_dribbler_enabled_event_value(abs_event.event.value) + ) + + elif event.type == ecodes.EV_KEY: + print("event code: " + str(event.code)) + if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: + self.__send_kick_command() + + elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: + self.__send_chip_command() if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: self.__send_move_command() - - def __event_loop(self): logging.info("Starting controller event loop") for event in self.controller.read_loop(): if self.__stop_event_thread.isSet(): return - if self.enabled: - self.__process_event(event) + self.__process_event(event) def close(self): logging.info("Closing controller thread") @@ -194,8 +198,7 @@ def set_enabled(self, enabled: bool): :param enabled: to which state to set controller enabled. """ - self.enabled = enabled - + self.enabled = enabled # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index bf5bb52c47..87431935fb 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -23,7 +23,7 @@ class FullSystemConnectWidget(QWidget): """ # Signal to indicate if manual controls should be disabled based on boolean parameter - # toggle_controls_signal = pyqtSignal(bool) + toggle_controls_signal = pyqtSignal(bool) def __init__(self, toggle_controls_signal) -> None: """ diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index d3bc59da4f..2d648e619c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -3,12 +3,14 @@ from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget, ControlMode from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import DriveAndDribblerWidget +from pyqtgraph.Qt.QtCore import pyqtSignal +from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout -class DiagnosticsWidget(TScopeWidget): - +class DiagnosticsWidget(QWidget): # Signal to indicate if manual controls should be disabled based on boolean parameter - # diagnostics_input_mode_signal = pyqtSignal(bool) + diagnostics_input_mode_signal = pyqtSignal(bool) + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() @@ -19,10 +21,8 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.chicker_widget = ChickerWidget(proto_unix_io) self.controller = ControllerDiagnostics(proto_unix_io) - self.diagnostics_control_input_widget.toggle_control_signal.connect( - lambda control_mode: self.controller.toggle_input_mode( - control_mode == ControlMode.DIAGNOSTICS - ) + self.diagnostics_control_input_widget.toggle_controls_signal.connect( + lambda control_mode: self.toggle_control(control_mode) ) vbox_layout.addWidget(self.diagnostics_control_input_widget) @@ -31,6 +31,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) - def update_mode(self, mode: ControlMode): + def refresh(self): + self.diagnostics_control_input_widget.refresh() + self.drive_dribbler_widget.refresh() + self.chicker_widget.refresh() + + def toggle_control(self, mode: ControlMode): self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) self.controller.set_enabled(mode == ControlMode.XBOX) diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index dc593d9fc6..3561cb8774 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -123,7 +123,7 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ - control_mode_signal = QtCore.pyqtSignal(int, int) + toggle_control_mode_signal = QtCore.pyqtSignal(int, int) def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: @@ -146,7 +146,7 @@ def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: for id in range(MAX_ROBOT_IDS_PER_SIDE): robot_view_widget = RobotViewComponent( - id, available_control_modes, self.control_mode_signal + id, available_control_modes, self.toggle_control_mode_signal ) self.robot_view_widgets.append(robot_view_widget) self.layout.addWidget(robot_view_widget) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 0405fc7e7d..87f886774e 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -2,7 +2,7 @@ import numpy from proto.message_translation import tbots_protobuf -from software.thunderscope.robot_input_control_manager import RobotInputControlManager +from software.thunderscope.robot_diagnostics.diagnostics_widget import DiagnosticsWidget from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.thunderscope import Thunderscope from software.thunderscope.binary_context_managers import * @@ -293,13 +293,13 @@ ) ) # TODO: remove this below - don't need to check for input mode (xbox/diag) - diagnostics_widget = tab.find_widget("Diagnostics Widget") - if diagnostics_widget is not None: - diagnostics_widget.toggle_control_signal( - lambda control_mode: robot_communication.toggle_input_mode( - control_mode - ) - ) + # diagnostics_widget = tab.find_widget("Diagnostics Widget") + # if diagnostics_widget is not None: + # diagnostics_widget.toggle_controls_signal.connect( + # lambda control_mode: robot_communication.toggle_input_mode( + # control_mode + # ) + # ) if args.run_blue or args.run_yellow: From 565ab5e064c7af395aab86256d80585aedcc5fab Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 2 Mar 2024 12:37:34 -0800 Subject: [PATCH 023/138] wip --- .../thunderscope/robot_communication.py | 103 +++++++++--------- .../controller_diagnostics.py | 25 +---- .../robot_diagnostics/diagnostics_widget.py | 32 +++++- .../drive_and_dribbler_widget.py | 26 +++-- 4 files changed, 101 insertions(+), 85 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 0cb4267b15..a8714903c4 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -22,17 +22,16 @@ class RobotCommunication(object): - """ Communicate with the robots """ def __init__( - self, - current_proto_unix_io: ProtoUnixIO, - multicast_channel: str, - interface: str, - estop_mode: EstopMode, - estop_path: os.PathLike = None, - estop_baudrate: int = 115200, + self, + current_proto_unix_io: ProtoUnixIO, + multicast_channel: str, + interface: str, + estop_mode: EstopMode, + estop_path: os.PathLike = None, + estop_baudrate: int = 115200, ): """Initialize the communication with the robots @@ -63,6 +62,8 @@ def __init__( self.motor_control_diagnostics_buffer = ThreadSafeBuffer(1, MotorControl) self.power_control_diagnostics_buffer = ThreadSafeBuffer(1, PowerControl) + self.diagnostics_control_buffer = ThreadSafeBuffer(1, DirectControlPrimitive) + self.current_proto_unix_io.register_observer( PrimitiveSet, self.primitive_buffer ) @@ -81,16 +82,17 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - self.robot_id_individual_mode_dict = { - rid: IndividualRobotMode.NONE for rid in range(NUM_ROBOTS) - } + # robots controlled by full-system + self.ai_controlled_robot_ids: list[int] = [] - self.robot_id_estop_send_count_dict = { - rid: 0 for rid in range(NUM_ROBOTS) # TODO - } + # robots controlled by diagnostics + self.diagnostic_controlled_robot_ids: list[int] = [] - # default to diagnostics control. - self.__input_mode = ControlMode.DIAGNOSTICS + # map of robot id to the number of times to send a estop. + # each robot has it's own countdown + self.robot_estop_send_countdown_latches: dict[int, int] = { + rid: 0 for rid in range(NUM_ROBOTS) + } # initialising the estop # tries to access a plugged in estop. if not found, throws an exception @@ -114,6 +116,9 @@ def setup_for_fullsystem(self) -> None: """ Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ + self.ai_controlled_robot_ids = range(0, NUM_ROBOTS) + self.diagnostic_controlled_robot_ids = range(0, NUM_ROBOTS) + self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( SSL_VISION_ADDRESS, SSL_VISION_PORT, @@ -128,10 +133,6 @@ def setup_for_fullsystem(self) -> None: True, ) - for key in self.robot_id_individual_mode_dict: - self.robot_id_individual_mode_dict[key] = IndividualRobotMode.AI - - def close_for_fullsystem(self) -> None: if self.receive_ssl_wrapper: self.receive_ssl_wrapper.close() @@ -165,12 +166,13 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param mode: the mode of input for this robot's primitives :param robot_id: the id of the robot whose mode we're changing """ - self.robot_id_individual_mode_dict[robot_id] = mode - - self.robot_id_estop_send_count_dict.pop(robot_id, None) - + if mode == IndividualRobotMode.AI and robot_id not in self.ai_controlled_robots: + self.ai_controlled_robots.append(robot_id) + if mode == IndividualRobotMode.MANUAL: + self.ai_controlled_robots.remove(robot_id) + self.ai_controlled_robots.remove(robot_id) if mode == IndividualRobotMode.NONE: - self.robot_id_estop_send_count_dict[robot_id] = NUM_TIMES_SEND_STOP + self.robot_estop_send_countdown_latches[robot_id] = NUM_TIMES_SEND_STOP def toggle_input_mode(self, mode: ControlMode): """ @@ -242,44 +244,39 @@ def __run_primitive_set(self) -> None: robot_primitives = {} # diagnostics is running. Only send diagnostics packets if using diagnostics control mode - if self.__input_mode == ControlMode.DIAGNOSTICS: - # get the manual control primitive - diagnostics_primitive = DirectControlPrimitive( - motor_control=self.motor_control_diagnostics_buffer.get(block=False), - power_control=self.power_control_diagnostics_buffer.get(block=False), - ) + # get the manual control primitive + # diagnostics_primitive = DirectControlPrimitive( + # motor_control=self.motor_control_diagnostics_buffer.get(block=False), + # power_control=self.power_control_diagnostics_buffer.get(block=False), + # ) + + diagnostices_primitive = self.diagnostics_control_buffer.get(block=False) - manually_controlled_robots = filter( - lambda robot_mode: robot_mode[1] == IndividualRobotMode.MANUAL, - self.robot_id_individual_mode_dict) + for robot_id in self.diagnostics_controlled_robot_ids: + robot_primitives[robot_id] = Primitive(direct_control=diagnostics_primitive) - for robot_id in manually_controlled_robots: - robot_primitives[robot_id] = Primitive( - direct_control=diagnostics_primitive) + # filter for ai controlled robots + ai_controlled_robots = filter( + lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, + self.robot_id_individual_mode_dict) - # fullsystem is running, so world data is being received - ai_controlled_robots: dict[int, IndividualRobotMode] = ( - filter(lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, - self.robot_id_individual_mode_dict) + # Get the primitives + primitive_set = self.primitive_buffer.get( + block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S ) - if ai_controlled_robots: - # Get the primitives - primitive_set = self.primitive_buffer.get( - block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S - ) + fullsystem_primitives = dict(primitive_set.robot_primitives) - fullsystem_primitives = dict(primitive_set.robot_primitives) - for robot_id in fullsystem_primitives.keys(): - if robot_id in ai_controlled_robots: - robot_primitives[robot_id] = fullsystem_primitives[robot_id] + for robot_id in fullsystem_primitives.keys(): + if robot_id in self.ai_controlled_robots: + robot_primitives[robot_id] = fullsystem_primitives[robot_id] # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives - for robot_id, num_times_to_stop in self.robot_id_estop_send_count_dict.items(): + for robot_id, num_times_to_stop in self.robot_estop_send_countdown_latches.items(): if num_times_to_stop > 0: robot_primitives[robot_id] = Primitive(stop=StopPrimitive()) - self.robot_id_estop_send_count_dict[robot_id] = num_times_to_stop - 1 + self.robot_estop_send_countdown_latches[robot_id] = num_times_to_stop - 1 # initialize total primitive set and send it primitive_set = PrimitiveSet( @@ -301,7 +298,7 @@ def __run_primitive_set(self) -> None: self.should_send_stop = False # sleep if not running fullsystem - if not ai_controlled_robots: + if not len(ai_controlled_robots): time.sleep(ROBOT_COMMUNICATIONS_TIMEOUT_S) def __forward_to_proto_unix_io(self, type: Type[Message], data: Message) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 555ee66d80..9f97de1ad8 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -26,7 +26,6 @@ class ControllerDiagnostics(object): interpreting the inputs into usable inputs for robot. """ - # TODO add function descriptions def __init__( self, proto_unix_io: ProtoUnixIO, @@ -34,19 +33,14 @@ def __init__( self.enabled = False self.controller = None - # TODO: check if estop gets in the way - for device in list_devices(): - try: - self.controller = InputDevice(device) - except Exception as e: - print("Failed to initialize device with path: " + device + " with exception " + e) + controller = InputDevice(device) + if controller is not None: + self.controller = controller if self.controller is None: - raise RuntimeError("Could not initialize a controller, there are no valid " - "controllers connected") + raise RuntimeError("Could not initialize a controller - connect using USB") - # TODO add a way to switch between different controllers plugged in logging.info( "Initializing controller " + self.controller.info.__str__() + " and device path location: " + self.controller.path) self.proto_unix_io = proto_unix_io @@ -177,7 +171,8 @@ def __event_loop(self): for event in self.controller.read_loop(): if self.__stop_event_thread.isSet(): return - self.__process_event(event) + if self.enabled: + self.__process_event(event) def close(self): logging.info("Closing controller thread") @@ -192,14 +187,6 @@ def set_enabled(self, enabled: bool): """ self.enabled = enabled - def set_enabled(self, enabled: bool): - """ - Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - - :param enabled: to which state to set controller enabled. - """ - self.enabled = enabled - # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 2d648e619c..474e3caae8 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,3 +1,6 @@ +import threading + +from software.thunderscope.constants import IndividualRobotMode from software.thunderscope.robot_diagnostics.controller_diagnostics import ControllerDiagnostics from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget @@ -21,10 +24,17 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.chicker_widget = ChickerWidget(proto_unix_io) self.controller = ControllerDiagnostics(proto_unix_io) + + self.__control_mode = ControlMode.DIAGNOSTICS + self.diagnostics_control_input_widget.toggle_controls_signal.connect( lambda control_mode: self.toggle_control(control_mode) ) + self.run_diagnostics_thread = threading.Thread( + target=self.__run_diagnostics_primivitve_set, daemon=True + ) + vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) @@ -32,10 +42,26 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) def refresh(self): - self.diagnostics_control_input_widget.refresh() - self.drive_dribbler_widget.refresh() - self.chicker_widget.refresh() + if self.__control_mode == ControlMode.DIAGNOSTICS: + self.diagnostics_control_input_widget.refresh() + self.drive_dribbler_widget.refresh() + self.chicker_widget.refresh() + + self.drive_dribbler_widget.motor_control_diagnostics_buffer.get() + + diagnostics_primitive = DirectControlPrimitive( + motor_control=self.drive_dribbler_widget.motor_control, + power_control=self.chicker_widget.power_control, + ) + + # TODO send the diagnostics primitive + Primitive(direct_control=diagnostics_primitive) + + elif self.__control_mode == ControlMode.XBOX: + + def toggle_control(self, mode: ControlMode): + self.__control_mode = mode self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) self.controller.set_enabled(mode == ControlMode.XBOX) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 20e1147ba8..a5bc8b09f3 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -22,24 +22,28 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.motor_control_diagnostics_buffer = ThreadSafeBuffer(1, MotorControl) - self.power_control_diagnostics_buffer = ThreadSafeBuffer(1, PowerControl) - - self.proto_unix_io.register_observer( - MotorControl, self.motor_control_diagnostics_buffer - ) - self.proto_unix_io.register_observer( - PowerControl, self.power_control_diagnostics_buffer - ) + self.motor_control = self.__create_empty_mc_primitive() # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) + # TODO move dribber control to chicker..., makes more sense there self.enabled = True self.setLayout(layout) + def __create_empty_control_primitive(self): + motor_control = MotorControl() + + motor_control.dribbler_speed_rpm = 0 + motor_control.direct_velocity_control.velocity.x_component_meters = 0 + motor_control.direct_velocity_control.velocity.y_component_meters = 0 + motor_control.direct_velocity_control.angular_velocity.radians_per_second = 0 + + return motor_control + + def refresh(self) -> None: """Refresh the widget and send the a MotorControl message with the current values """ @@ -56,6 +60,8 @@ def refresh(self) -> None: self.angular_velocity_slider.value() ) + # todo update motor control + self.proto_unix_io.send_proto(MotorControl, motor_control) def value_change(self, value: float) -> str: @@ -180,7 +186,7 @@ def setup_dribbler(self, title: str) -> QGroupBox: return group_box - def set_enabled(self, enable: bool) -> None: + def set_enabled(self, enable: bool) -> None: """ Disables or enables all sliders and buttons depending on boolean parameter From 7cf3d3ca82b43f9a049c3ca5e5fc6ac32d2220c4 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 16 Mar 2024 11:49:18 -0700 Subject: [PATCH 024/138] wip --- .../thunderscope/robot_communication.py | 30 ++++++++++--------- .../robot_diagnostics/diagnostics_widget.py | 14 +++++---- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index a8714903c4..1737b9d92c 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -83,13 +83,18 @@ def __init__( ) # robots controlled by full-system - self.ai_controlled_robot_ids: list[int] = [] + # TODO: remove... + self.ai_controlled_robots: list[int] = [] # robots controlled by diagnostics - self.diagnostic_controlled_robot_ids: list[int] = [] + # TODO: remove... + self.diagnostic_controlled_robots: list[int] = [] + + # map of robot id to the individual control mode + self.robot_individual_control_mode_dict: dict[int, IndividualRobotMode] # map of robot id to the number of times to send a estop. - # each robot has it's own countdown + # each robot has it's own separate countdown self.robot_estop_send_countdown_latches: dict[int, int] = { rid: 0 for rid in range(NUM_ROBOTS) } @@ -116,8 +121,8 @@ def setup_for_fullsystem(self) -> None: """ Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ - self.ai_controlled_robot_ids = range(0, NUM_ROBOTS) - self.diagnostic_controlled_robot_ids = range(0, NUM_ROBOTS) + self.ai_controlled_robots = range(0, NUM_ROBOTS) + self.diagnostic_controlled_robots = range(0, NUM_ROBOTS) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( SSL_VISION_ADDRESS, @@ -147,7 +152,6 @@ def toggle_keyboard_estop(self) -> None: """ if self.estop_mode == EstopMode.KEYBOARD_ESTOP: self.estop_is_playing = not self.estop_is_playing - print( "Keyboard Estop changed to " + ( @@ -243,6 +247,7 @@ def __run_primitive_set(self) -> None: # total primitives for all robots robot_primitives = {} + # TODO: remove this commented out code # diagnostics is running. Only send diagnostics packets if using diagnostics control mode # get the manual control primitive # diagnostics_primitive = DirectControlPrimitive( @@ -250,15 +255,12 @@ def __run_primitive_set(self) -> None: # power_control=self.power_control_diagnostics_buffer.get(block=False), # ) - diagnostices_primitive = self.diagnostics_control_buffer.get(block=False) - - for robot_id in self.diagnostics_controlled_robot_ids: - robot_primitives[robot_id] = Primitive(direct_control=diagnostics_primitive) + # get the most recent diagnostics primitive + diagnostics_primitive = self.diagnostics_control_buffer.get(block=False) - # filter for ai controlled robots - ai_controlled_robots = filter( - lambda robot_mode: robot_mode[1] == IndividualRobotMode.AI, - self.robot_id_individual_mode_dict) + for robot_id in self.diagnostic_controlled_robots: + if robot_id in self.diagnostic_controlled_robots: + robot_primitives[robot_id] = Primitive(direct_control=diagnostics_primitive) # Get the primitives primitive_set = self.primitive_buffer.get( diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 474e3caae8..c65190892c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,6 +1,6 @@ +import logging import threading -from software.thunderscope.constants import IndividualRobotMode from software.thunderscope.robot_diagnostics.controller_diagnostics import ControllerDiagnostics from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget @@ -19,6 +19,8 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout = QVBoxLayout() + self.proto_unix_io = proto_unix_io + self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) @@ -43,21 +45,23 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def refresh(self): if self.__control_mode == ControlMode.DIAGNOSTICS: + # refresh the widgets so that they hold the most recent user control values self.diagnostics_control_input_widget.refresh() self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() - self.drive_dribbler_widget.motor_control_diagnostics_buffer.get() - + # diagnostics_primitive = DirectControlPrimitive( motor_control=self.drive_dribbler_widget.motor_control, power_control=self.chicker_widget.power_control, ) - # TODO send the diagnostics primitive - Primitive(direct_control=diagnostics_primitive) + # TODO: send the diagnostics primitive + self.proto_unix_io.send_proto(DirectControlPrimitive, diagnostics_primitive) elif self.__control_mode == ControlMode.XBOX: + # TODO: get values from controller widget + logging.debug(self.controller.ang_vel) From 798f2e4ab719ca855f02306707e302d0a4001b52 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 16 Mar 2024 23:24:41 -0700 Subject: [PATCH 025/138] wip --- .../thunderscope/robot_communication.py | 55 ++++++++----------- .../robot_diagnostics/diagnostics_widget.py | 13 +++-- 2 files changed, 30 insertions(+), 38 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 1737b9d92c..1788ae10e6 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -18,7 +18,7 @@ from typing import Type from queue import Empty -NUM_ROBOTS = 6 +NUM_ROBOTS = DIV_B_NUM_ROBOTS class RobotCommunication(object): @@ -59,20 +59,18 @@ def __init__( self.primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) + # TODO: remove motor and power buffers self.motor_control_diagnostics_buffer = ThreadSafeBuffer(1, MotorControl) self.power_control_diagnostics_buffer = ThreadSafeBuffer(1, PowerControl) - self.diagnostics_control_buffer = ThreadSafeBuffer(1, DirectControlPrimitive) + self.diagnostics_primitive_buffer = ThreadSafeBuffer(1, Primitive) self.current_proto_unix_io.register_observer( PrimitiveSet, self.primitive_buffer ) self.current_proto_unix_io.register_observer( - MotorControl, self.motor_control_diagnostics_buffer - ) - self.current_proto_unix_io.register_observer( - PowerControl, self.power_control_diagnostics_buffer + Primitive, self.diagnostics_primitive_buffer ) self.send_estop_state_thread = threading.Thread( @@ -82,19 +80,18 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - # robots controlled by full-system - # TODO: remove... - self.ai_controlled_robots: list[int] = [] - # robots controlled by diagnostics # TODO: remove... self.diagnostic_controlled_robots: list[int] = [] # map of robot id to the individual control mode - self.robot_individual_control_mode_dict: dict[int, IndividualRobotMode] + self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { + key: IndividualRobotMode.NONE for key in list(range(0, NUM_ROBOTS)) + } # map of robot id to the number of times to send a estop. # each robot has it's own separate countdown + # TODO come up with better name self.robot_estop_send_countdown_latches: dict[int, int] = { rid: 0 for rid in range(NUM_ROBOTS) } @@ -121,8 +118,9 @@ def setup_for_fullsystem(self) -> None: """ Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ - self.ai_controlled_robots = range(0, NUM_ROBOTS) - self.diagnostic_controlled_robots = range(0, NUM_ROBOTS) + self.robot_control_mode_dict.update( + (k, IndividualRobotMode.AI) for k in self.robot_control_mode_dict.keys() + ) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( SSL_VISION_ADDRESS, @@ -170,11 +168,8 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param mode: the mode of input for this robot's primitives :param robot_id: the id of the robot whose mode we're changing """ - if mode == IndividualRobotMode.AI and robot_id not in self.ai_controlled_robots: - self.ai_controlled_robots.append(robot_id) - if mode == IndividualRobotMode.MANUAL: - self.ai_controlled_robots.remove(robot_id) - self.ai_controlled_robots.remove(robot_id) + self.robot_control_mode_dict[robot_id] = mode + if mode == IndividualRobotMode.NONE: self.robot_estop_send_countdown_latches[robot_id] = NUM_TIMES_SEND_STOP @@ -247,20 +242,16 @@ def __run_primitive_set(self) -> None: # total primitives for all robots robot_primitives = {} - # TODO: remove this commented out code - # diagnostics is running. Only send diagnostics packets if using diagnostics control mode - # get the manual control primitive - # diagnostics_primitive = DirectControlPrimitive( - # motor_control=self.motor_control_diagnostics_buffer.get(block=False), - # power_control=self.power_control_diagnostics_buffer.get(block=False), - # ) - # get the most recent diagnostics primitive - diagnostics_primitive = self.diagnostics_control_buffer.get(block=False) + diagnostics_primitive = self.diagnostics_primitive_buffer.get(block=False) + + # filter for diagnostics controlled robots + diagnostics_robots = list(robot_id for robot_id, mode in self.robot_control_mode_dict.items() if + mode == IndividualRobotMode.MANUAL) - for robot_id in self.diagnostic_controlled_robots: - if robot_id in self.diagnostic_controlled_robots: - robot_primitives[robot_id] = Primitive(direct_control=diagnostics_primitive) + # set robot primitives for diagnostics robots + for robot_id in diagnostics_robots: + robot_primitives[robot_id] = diagnostics_primitive # Get the primitives primitive_set = self.primitive_buffer.get( @@ -270,7 +261,7 @@ def __run_primitive_set(self) -> None: fullsystem_primitives = dict(primitive_set.robot_primitives) for robot_id in fullsystem_primitives.keys(): - if robot_id in self.ai_controlled_robots: + if self.robot_control_mode_dict[int(robot_id)] == IndividualRobotMode.AI: robot_primitives[robot_id] = fullsystem_primitives[robot_id] # sends a final stop primitive to all disconnected robots and removes them from list @@ -300,7 +291,7 @@ def __run_primitive_set(self) -> None: self.should_send_stop = False # sleep if not running fullsystem - if not len(ai_controlled_robots): + if IndividualRobotMode.AI not in self.robot_control_mode_dict.values(): time.sleep(ROBOT_COMMUNICATIONS_TIMEOUT_S) def __forward_to_proto_unix_io(self, type: Type[Message], data: Message) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index c65190892c..a7364c15ad 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -51,16 +51,17 @@ def refresh(self): self.chicker_widget.refresh() # - diagnostics_primitive = DirectControlPrimitive( - motor_control=self.drive_dribbler_widget.motor_control, - power_control=self.chicker_widget.power_control, + diagnostics_primitive = Primitive( + direct_control=DirectControlPrimitive( + motor_control=self.drive_dribbler_widget.motor_control, + power_control=self.chicker_widget.power_control, + ) ) - # TODO: send the diagnostics primitive - self.proto_unix_io.send_proto(DirectControlPrimitive, diagnostics_primitive) + self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) elif self.__control_mode == ControlMode.XBOX: - # TODO: get values from controller widget + # TODO: get values from controller widget, placeholder log for now logging.debug(self.controller.ang_vel) From 2a1fad628b80f88442d4a4b768e86b26fa44d1aa Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 16 Mar 2024 23:25:32 -0700 Subject: [PATCH 026/138] wip --- src/software/thunderscope/robot_communication.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 1788ae10e6..26e2b11a5d 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -59,10 +59,6 @@ def __init__( self.primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) - # TODO: remove motor and power buffers - self.motor_control_diagnostics_buffer = ThreadSafeBuffer(1, MotorControl) - self.power_control_diagnostics_buffer = ThreadSafeBuffer(1, PowerControl) - self.diagnostics_primitive_buffer = ThreadSafeBuffer(1, Primitive) self.current_proto_unix_io.register_observer( @@ -80,10 +76,6 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - # robots controlled by diagnostics - # TODO: remove... - self.diagnostic_controlled_robots: list[int] = [] - # map of robot id to the individual control mode self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { key: IndividualRobotMode.NONE for key in list(range(0, NUM_ROBOTS)) From f6138e4937e033318c57d1923bc76b4dfe4f17ab Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 16 Mar 2024 23:30:14 -0700 Subject: [PATCH 027/138] formatting --- .../thunderscope/robot_communication.py | 48 ++++++++++------- .../thunderscope/robot_diagnostics/BUILD | 10 ++-- .../controller_diagnostics.py | 54 ++++++++++++------- .../diagnostics_input_widget.py | 1 + .../robot_diagnostics/diagnostics_widget.py | 20 ++++--- .../drive_and_dribbler_widget.py | 1 - .../thunderscope/widget_setup_functions.py | 4 +- 7 files changed, 84 insertions(+), 54 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 26e2b11a5d..83941ebed9 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -16,7 +16,6 @@ from proto.import_all_protos import * from pyqtgraph.Qt import QtCore from typing import Type -from queue import Empty NUM_ROBOTS = DIV_B_NUM_ROBOTS @@ -25,13 +24,13 @@ class RobotCommunication(object): """ Communicate with the robots """ def __init__( - self, - current_proto_unix_io: ProtoUnixIO, - multicast_channel: str, - interface: str, - estop_mode: EstopMode, - estop_path: os.PathLike = None, - estop_baudrate: int = 115200, + self, + current_proto_unix_io: ProtoUnixIO, + multicast_channel: str, + interface: str, + estop_mode: EstopMode, + estop_path: os.PathLike = None, + estop_baudrate: int = 115200, ): """Initialize the communication with the robots @@ -205,11 +204,13 @@ def __should_send_packet(self) -> bool: :return: boolean """ return ( - self.estop_mode != EstopMode.DISABLE_ESTOP - and self.estop_is_playing - and (IndividualRobotMode.AI or - IndividualRobotMode.MANUAL - in self.robot_id_individual_mode_dict.values()) + self.estop_mode != EstopMode.DISABLE_ESTOP + and self.estop_is_playing + and ( + IndividualRobotMode.AI + or IndividualRobotMode.MANUAL + in self.robot_id_individual_mode_dict.values() + ) ) def __run_primitive_set(self) -> None: @@ -238,8 +239,11 @@ def __run_primitive_set(self) -> None: diagnostics_primitive = self.diagnostics_primitive_buffer.get(block=False) # filter for diagnostics controlled robots - diagnostics_robots = list(robot_id for robot_id, mode in self.robot_control_mode_dict.items() if - mode == IndividualRobotMode.MANUAL) + diagnostics_robots = list( + robot_id + for robot_id, mode in self.robot_control_mode_dict.items() + if mode == IndividualRobotMode.MANUAL + ) # set robot primitives for diagnostics robots for robot_id in diagnostics_robots: @@ -253,15 +257,23 @@ def __run_primitive_set(self) -> None: fullsystem_primitives = dict(primitive_set.robot_primitives) for robot_id in fullsystem_primitives.keys(): - if self.robot_control_mode_dict[int(robot_id)] == IndividualRobotMode.AI: + if ( + self.robot_control_mode_dict[int(robot_id)] + == IndividualRobotMode.AI + ): robot_primitives[robot_id] = fullsystem_primitives[robot_id] # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives - for robot_id, num_times_to_stop in self.robot_estop_send_countdown_latches.items(): + for ( + robot_id, + num_times_to_stop, + ) in self.robot_estop_send_countdown_latches.items(): if num_times_to_stop > 0: robot_primitives[robot_id] = Primitive(stop=StopPrimitive()) - self.robot_estop_send_countdown_latches[robot_id] = num_times_to_stop - 1 + self.robot_estop_send_countdown_latches[robot_id] = ( + num_times_to_stop - 1 + ) # initialize total primitive set and send it primitive_set = PrimitiveSet( diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index c304bc38f0..a747c9515c 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -38,13 +38,13 @@ py_library( name = "diagnostics_widget", srcs = ["diagnostics_widget.py"], deps = [ - "//software/thunderscope/robot_diagnostics:controller", - "//software/thunderscope/robot_diagnostics:chicker", - "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", - "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", - "//software/thunderscope:thunderscope_types", "//software/thunderscope:constants", "//software/thunderscope:thread_safe_buffer", + "//software/thunderscope:thunderscope_types", + "//software/thunderscope/robot_diagnostics:chicker", + "//software/thunderscope/robot_diagnostics:controller", + "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", + "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 9f97de1ad8..9e11b56148 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -27,8 +27,7 @@ class ControllerDiagnostics(object): """ def __init__( - self, - proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.enabled = False self.controller = None @@ -42,7 +41,11 @@ def __init__( raise RuntimeError("Could not initialize a controller - connect using USB") logging.info( - "Initializing controller " + self.controller.info.__str__() + " and device path location: " + self.controller.path) + "Initializing controller " + + self.controller.info.__str__() + + " and device path location: " + + self.controller.path + ) self.proto_unix_io = proto_unix_io self.__stop_event_thread = Event() @@ -83,7 +86,9 @@ def __send_move_command(self): motor_control.direct_velocity_control.velocity.x_component_meters = self.x_vel motor_control.direct_velocity_control.velocity.y_component_meters = self.y_vel - motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.ang_vel + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.ang_vel + ) motor_control.dribbler_speed_rpm = 0 @@ -91,7 +96,9 @@ def __send_move_command(self): # motor_control.dribbler_speed_rpm = dribbler_speed # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' if self.dribbler_enabled: - motor_control.dribbler_speed_rpm = self.constants.indefinite_dribbler_speed_rpm + motor_control.dribbler_speed_rpm = ( + self.constants.indefinite_dribbler_speed_rpm + ) logging.info("Sending motor control: " + motor_control) @@ -109,7 +116,9 @@ def __send_kick_command(self): def __send_chip_command(self): power_control = PowerControl() power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = self.kick_power # not sure if we should use this value + power_control.chicker.chip_distance_meters = ( + self.kick_power + ) # not sure if we should use this value logging.info("Sending chip power control: " + power_control) @@ -127,32 +136,36 @@ def __process_event(self, event): # grab the event type event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] if event_t == "ABS_X": - self.x_vel = ( - self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S)) + self.x_vel = self.__parse_move_event_value( + abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S + ) elif event_t == "ABS_Y": - self.y_vel = ( - self.__parse_move_event_value(abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S)) + self.y_vel = self.__parse_move_event_value( + abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S + ) elif event_t == "ABS_RX": - self.ang_vel = ( - self.__parse_move_event_value(abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S)) + self.ang_vel = self.__parse_move_event_value( + abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S + ) elif event_t == "ABS_HAT0Y": - self.dribbler_speed = ( - self.__parse_dribbler_event_value(abs_event.event.value)) + self.dribbler_speed = self.__parse_dribbler_event_value( + abs_event.event.value + ) elif event_t == "ABS_HAT0X": - self.kick_power = ( - self.__parse_kick_event_value(abs_event.event.value)) + self.kick_power = self.__parse_kick_event_value(abs_event.event.value) elif event_t == "ABS_RZ": - self.dribbler_enabled = ( - self.__parse_dribbler_enabled_event_value(abs_event.event.value)) + self.dribbler_enabled = self.__parse_dribbler_enabled_event_value( + abs_event.event.value + ) elif event_t == "ABS_Z": - self.dribbler_enabled = ( - self.__parse_dribbler_enabled_event_value(abs_event.event.value) + self.dribbler_enabled = self.__parse_dribbler_enabled_event_value( + abs_event.event.value ) elif event.type == ecodes.EV_KEY: @@ -187,6 +200,7 @@ def set_enabled(self, enabled: bool): """ self.enabled = enabled + # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 87431935fb..9442a770d9 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -13,6 +13,7 @@ class ControlMode(IntEnum): DIAGNOSTICS = 0 XBOX = 1 + # this class name doesnt make sense class FullSystemConnectWidget(QWidget): """ diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index a7364c15ad..193f4b748c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,11 +1,18 @@ import logging import threading -from software.thunderscope.robot_diagnostics.controller_diagnostics import ControllerDiagnostics +from software.thunderscope.robot_diagnostics.controller_diagnostics import ( + ControllerDiagnostics, +) from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import FullSystemConnectWidget, ControlMode -from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import DriveAndDribblerWidget +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( + FullSystemConnectWidget, + ControlMode, +) +from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( + DriveAndDribblerWidget, +) from pyqtgraph.Qt.QtCore import pyqtSignal from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout @@ -21,12 +28,13 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.diagnostics_control_input_widget = FullSystemConnectWidget(self.diagnostics_input_mode_signal) + self.diagnostics_control_input_widget = FullSystemConnectWidget( + self.diagnostics_input_mode_signal + ) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.controller = ControllerDiagnostics(proto_unix_io) - self.__control_mode = ControlMode.DIAGNOSTICS self.diagnostics_control_input_widget.toggle_controls_signal.connect( @@ -64,8 +72,6 @@ def refresh(self): # TODO: get values from controller widget, placeholder log for now logging.debug(self.controller.ang_vel) - - def toggle_control(self, mode: ControlMode): self.__control_mode = mode self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index a5bc8b09f3..ff82b14029 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -43,7 +43,6 @@ def __create_empty_control_primitive(self): return motor_control - def refresh(self) -> None: """Refresh the widget and send the a MotorControl message with the current values """ diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 9a47c394ac..3684366b12 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -354,9 +354,7 @@ def setup_drive_and_dribbler_widget( return drive_and_dribbler_widget -def setup_diagnostics_widget( - proto_unix_io: ProtoUnixIO, -) -> DriveAndDribblerWidget: +def setup_diagnostics_widget(proto_unix_io: ProtoUnixIO,) -> DriveAndDribblerWidget: """Setup the drive and dribbler widget :param proto_unix_io: The proto unix io object From 75b62280a3921fc697fd3f2080cae63d623f972f Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 17 Mar 2024 00:05:46 -0700 Subject: [PATCH 028/138] cleanup --- src/software/thunderscope/constants.py | 5 +- .../thunderscope/robot_communication.py | 18 ++++---- .../robot_diagnostics/chicker_widget.py | 7 +-- .../controller_diagnostics.py | 2 +- .../robot_diagnostics/diagnostics_widget.py | 18 ++++---- .../drive_and_dribbler_widget.py | 18 +++----- .../robot_diagnostics/robot_view.py | 4 +- .../thunderscope/thunderscope_config.py | 46 ++++++------------- .../thunderscope/thunderscope_main.py | 4 +- .../thunderscope/thunderscope_types.py | 10 ++-- .../thunderscope/widget_setup_functions.py | 8 ++-- 11 files changed, 58 insertions(+), 82 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 2771aa3c4e..787cbe3dcf 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -20,7 +20,7 @@ class ProtoUnixIOTypes(Enum): CURRENT = 5 -class TabKeys(str, Enum): +class TabNames(str, Enum): """ Different keys for tabs used in various Thunderscope views """ @@ -141,9 +141,6 @@ class EstopMode(IntEnum): ESTOP_PATH_1 = "/dev/ttyACM0" ESTOP_PATH_2 = "/dev/ttyUSB0" -# Path to check for handheld controller when running diagnostics -HANDHELD_PATH = "/dev/input/input1" - # Mapping between RobotStatus Error Codes and their dialog messages ERROR_CODE_MESSAGES = { ErrorCode.HIGH_CAP: "High Cap", diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 83941ebed9..f1777a2d11 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -1,21 +1,21 @@ import threading import time import os +import software.python_bindings as tbots_cpp -from software.thunderscope.constants import ( - ROBOT_COMMUNICATIONS_TIMEOUT_S, - IndividualRobotMode, - EstopMode, -) +from typing import Type +from google.protobuf.message import Message +from pyqtgraph.Qt import QtCore from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO -import software.python_bindings as tbots_cpp from software.py_constants import * -from google.protobuf.message import Message from proto.import_all_protos import * -from pyqtgraph.Qt import QtCore -from typing import Type +from software.thunderscope.constants import ( + ROBOT_COMMUNICATIONS_TIMEOUT_S, + IndividualRobotMode, + EstopMode, +) NUM_ROBOTS = DIV_B_NUM_ROBOTS diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 51d5e75634..4aacf45143 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -33,6 +33,8 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(ChickerWidget, self).__init__() + self.power_control = PowerControl() + vbox_layout = QVBoxLayout() self.radio_buttons_group = QButtonGroup() self.proto_unix_io = proto_unix_io @@ -180,7 +182,7 @@ def send_command(self, command: ChickerCommandMode) -> None: ) # sends proto - self.proto_unix_io.send_proto(PowerControl, power_control) + self.power_control = power_control # clears the proto buffer for kick or chip commands # so only one kick / chip is sent @@ -195,8 +197,7 @@ def clear_proto_buffer(self) -> None: So sending an empty message overwrites the cache and prevents spamming commands If buffer is full, blocks execution until buffer has space """ - power_control = PowerControl() - self.proto_unix_io.send_proto(PowerControl, power_control, True) + self.power_control = PowerControl() def change_button_state(self, button: QPushButton, enable: bool) -> None: """Change button color and clickable state. diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 9e11b56148..8e0267ebd8 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -20,7 +20,7 @@ MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 -class ControllerDiagnostics(object): +class ControllerInputHandler(object): """ This class is responsible for reading from an Xbox controller device and interpreting the inputs into usable inputs for robot. diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 193f4b748c..d8fe2a0ad4 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,20 +1,20 @@ import logging import threading -from software.thunderscope.robot_diagnostics.controller_diagnostics import ( - ControllerDiagnostics, -) +from pyqtgraph.Qt.QtCore import pyqtSignal +from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( FullSystemConnectWidget, ControlMode, ) +from software.thunderscope.robot_diagnostics.controller_diagnostics import ( + ControllerInputHandler, +) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, ) -from pyqtgraph.Qt.QtCore import pyqtSignal -from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout class DiagnosticsWidget(QWidget): @@ -28,12 +28,14 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io + self.controller = ControllerInputHandler(proto_unix_io) + self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) + self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = FullSystemConnectWidget( self.diagnostics_input_mode_signal ) - self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) - self.chicker_widget = ChickerWidget(proto_unix_io) - self.controller = ControllerDiagnostics(proto_unix_io) + + self.__control_mode = ControlMode.DIAGNOSTICS diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index ff82b14029..177da37e5a 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -15,6 +15,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: :param proto_unix_io: the proto_unix_io object """ + + super(DriveAndDribblerWidget, self).__init__() + + self.motor_control = MotorControl() + self.input_a = time.time() self.constants = tbots_cpp.create2021RobotConstants() QWidget.__init__(self) @@ -33,15 +38,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(layout) - def __create_empty_control_primitive(self): - motor_control = MotorControl() - - motor_control.dribbler_speed_rpm = 0 - motor_control.direct_velocity_control.velocity.x_component_meters = 0 - motor_control.direct_velocity_control.velocity.y_component_meters = 0 - motor_control.direct_velocity_control.angular_velocity.radians_per_second = 0 - - return motor_control def refresh(self) -> None: """Refresh the widget and send the a MotorControl message with the current values @@ -59,9 +55,7 @@ def refresh(self) -> None: self.angular_velocity_slider.value() ) - # todo update motor control - - self.proto_unix_io.send_proto(MotorControl, motor_control) + self.motor_control = motor_control def value_change(self, value: float) -> str: """ diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index f76e292d7d..55bce1e261 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -88,7 +88,7 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ - toggle_control_mode_signal = QtCore.pyqtSignal(int, int) + control_mode_signal = QtCore.pyqtSignal(int, int) def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: @@ -109,7 +109,7 @@ def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: for id in range(MAX_ROBOT_IDS_PER_SIDE): robot_view_widget = RobotViewComponent( - id, available_control_modes, self.toggle_control_mode_signal + id, available_control_modes, self.control_mode_signal ) self.robot_view_widgets.append(robot_view_widget) self.layout.addWidget(robot_view_widget) diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index 58d7e1f5ce..17ecc6e2ef 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -1,7 +1,7 @@ from software.thunderscope.common.frametime_counter import FrameTimeCounter from software.thunderscope.widget_setup_functions import * from software.thunderscope.constants import ( - TabKeys, + TabNames, ProtoUnixIOTypes, GAME_CONTROLLER_URL, ) @@ -278,26 +278,6 @@ def configure_base_diagnostics( anchor="Logs", position="right", ), - # TScopeWidget( - # name="Drive and Dribbler", - # widget=setup_drive_and_dribbler_widget( - # **{"proto_unix_io": diagnostics_proto_unix_io} - # ), - # anchor="Logs", - # position="right", - # ), - # TScopeWidget( - # name="Chicker", - # widget=setup_chicker_widget(**{"proto_unix_io": diagnostics_proto_unix_io}), - # anchor="Drive and Dribbler", - # position="below", - # ), - # TScopeWidget( - # name="Manual Control Input", - # widget=setup_diagnostics_input_widget(), - # anchor="Chicker", - # position="top", - # ), ] + extra_widgets @@ -333,7 +313,7 @@ def configure_two_ai_gamecontroller_view( tabs=[ TScopeQTTab( name="Blue FullSystem", - key=TabKeys.BLUE, + key=TabNames.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -348,7 +328,7 @@ def configure_two_ai_gamecontroller_view( ), TScopeQTTab( name="Yellow FullSystem", - key=TabKeys.YELLOW, + key=TabNames.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -365,7 +345,7 @@ def configure_two_ai_gamecontroller_view( ), TScopeWebTab( name="Gamecontroller", - key=TabKeys.GAMECONTROLLER, + key=TabNames.GAMECONTROLLER, url=GAME_CONTROLLER_URL, ), ], @@ -404,7 +384,7 @@ def configure_simulated_test_view( tabs=[ TScopeQTTab( name="Blue FullSystem", - key=TabKeys.BLUE, + key=TabNames.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -415,7 +395,7 @@ def configure_simulated_test_view( ), TScopeQTTab( name="Yellow FullSystem", - key=TabKeys.YELLOW, + key=TabNames.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -464,7 +444,7 @@ def configure_field_test_view( tabs = [ TScopeQTTab( name="Yellow FullSystem", - key=TabKeys.YELLOW, + key=TabNames.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -480,7 +460,7 @@ def configure_field_test_view( tabs = [ TScopeQTTab( name="Blue FullSystem", - key=TabKeys.BLUE, + key=TabNames.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -521,7 +501,7 @@ def configure_replay_view( tabs.append( TScopeQTTab( name="Blue FullSystem", - key=TabKeys.BLUE, + key=TabNames.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -539,7 +519,7 @@ def configure_replay_view( tabs.append( TScopeQTTab( name="Yellow FullSystem", - key=TabKeys.YELLOW, + key=TabNames.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -601,7 +581,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Blue Fullsystem", - key=TabKeys.BLUE, + key=TabNames.BLUE, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.BLUE], sim_proto_unix_io=proto_unix_io_map[ProtoUnixIOTypes.SIM], @@ -621,7 +601,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Yellow Fullsystem", - key=TabKeys.YELLOW, + key=TabNames.YELLOW, widgets=configure_base_fullsystem( full_system_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.YELLOW @@ -660,7 +640,7 @@ def get_extra_widgets(proto_unix_io): tabs.append( TScopeQTTab( name="Robot Diagnostics", - key=TabKeys.DIAGNOSTICS, + key=TabNames.DIAGNOSTICS, widgets=configure_base_diagnostics( diagnostics_proto_unix_io=proto_unix_io_map[ ProtoUnixIOTypes.DIAGNOSTICS diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index cb98fb8e53..508e3e9c46 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -13,7 +13,7 @@ import proto.message_translation.tbots_protobuf as tbots_protobuf from software.thunderscope.robot_communication import RobotCommunication from software.thunderscope.replay.proto_logger import ProtoLogger -from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes, TabKeys +from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes, TabNames from software.thunderscope.estop_helpers import get_estop_config from software.thunderscope.proto_unix_io import ProtoUnixIO import software.thunderscope.thunderscope_config as config @@ -321,7 +321,7 @@ if hasattr(tab, "widgets"): robot_view_widget = tab.find_widget("Robot View") if robot_view_widget is not None: - robot_view_widget.toggle_control_mode_signal.connect( + robot_view_widget.control_mode_signal.connect( lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( robot_id, robot_mode ) diff --git a/src/software/thunderscope/thunderscope_types.py b/src/software/thunderscope/thunderscope_types.py index 0b8f683565..47a7aab2d7 100644 --- a/src/software/thunderscope/thunderscope_types.py +++ b/src/software/thunderscope/thunderscope_types.py @@ -1,6 +1,6 @@ from typing import Callable, Optional, Sequence, Any, Dict from software.thunderscope.common.frametime_counter import FrameTimeCounter -from software.thunderscope.constants import TabKeys +from software.thunderscope.constants import TabNames import PyQt6 from PyQt6.QtWebEngineWidgets import QWebEngineView @@ -60,10 +60,10 @@ class TScopeTab: """ name: str # name of tab - key: TabKeys # key to identify this tab + key: TabNames # key to identify this tab dock_area: QWidget # Dock Area for this tab - def __init__(self, name: str, key: TabKeys) -> None: + def __init__(self, name: str, key: TabNames) -> None: self.name = name self.key = key @@ -86,7 +86,7 @@ class TScopeQTTab(TScopeTab): def __init__( self, name: str, - key: TabKeys, + key: TabNames, widgets: Sequence[TScopeWidget], refresh_func_counter: FrameTimeCounter = None, ) -> None: @@ -186,7 +186,7 @@ class TScopeWebTab(TScopeTab): url: str # url of webpage displayed by this tab - def __init__(self, name: str, key: TabKeys, url: str) -> None: + def __init__(self, name: str, key: TabNames, url: str) -> None: super().__init__(name, key) self.url = url diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 3684366b12..092d94ba06 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -358,9 +358,11 @@ def setup_diagnostics_widget(proto_unix_io: ProtoUnixIO,) -> DriveAndDribblerWid """Setup the drive and dribbler widget :param proto_unix_io: The proto unix io object - :returns: The drive and dribbler widget + :returns: The diagnostics widget that contains + the control input switch, drive & dribbler sliders, + chicker control and controller handler """ - drive_and_dribbler_widget = DiagnosticsWidget(proto_unix_io) + diagnostics_widget = DiagnosticsWidget(proto_unix_io) - return drive_and_dribbler_widget + return diagnostics_widget From e984e3686cd7ac73ba894b3019350813bf99366a Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 17 Mar 2024 00:08:47 -0700 Subject: [PATCH 029/138] formatting --- .../thunderscope/robot_diagnostics/diagnostics_widget.py | 2 -- .../thunderscope/robot_diagnostics/drive_and_dribbler_widget.py | 1 - 2 files changed, 3 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index d8fe2a0ad4..f5a70ba088 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -35,8 +35,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.diagnostics_input_mode_signal ) - - self.__control_mode = ControlMode.DIAGNOSTICS self.diagnostics_control_input_widget.toggle_controls_signal.connect( diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 177da37e5a..0459e07a52 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -38,7 +38,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(layout) - def refresh(self) -> None: """Refresh the widget and send the a MotorControl message with the current values """ From 63b93eb71964e51b6cdb4d326cc342b025950838 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 17 Mar 2024 00:11:41 -0700 Subject: [PATCH 030/138] changed to id --- src/software/thunderscope/robot_communication.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index f1777a2d11..261ffd1ae2 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -77,7 +77,7 @@ def __init__( # map of robot id to the individual control mode self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { - key: IndividualRobotMode.NONE for key in list(range(0, NUM_ROBOTS)) + robot_id: IndividualRobotMode.NONE for robot_id in list(range(0, NUM_ROBOTS)) } # map of robot id to the number of times to send a estop. @@ -110,7 +110,7 @@ def setup_for_fullsystem(self) -> None: Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ self.robot_control_mode_dict.update( - (k, IndividualRobotMode.AI) for k in self.robot_control_mode_dict.keys() + (robot_id, IndividualRobotMode.AI) for robot_id in self.robot_control_mode_dict.keys() ) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( From b3e8a41d4f711618c9d554421d4a1db7ee98d97d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sun, 17 Mar 2024 16:05:53 +0000 Subject: [PATCH 031/138] [pre-commit.ci lite] apply automatic fixes --- src/software/thunderscope/robot_communication.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 261ffd1ae2..6dabf5b850 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -77,7 +77,8 @@ def __init__( # map of robot id to the individual control mode self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { - robot_id: IndividualRobotMode.NONE for robot_id in list(range(0, NUM_ROBOTS)) + robot_id: IndividualRobotMode.NONE + for robot_id in list(range(0, NUM_ROBOTS)) } # map of robot id to the number of times to send a estop. @@ -110,7 +111,8 @@ def setup_for_fullsystem(self) -> None: Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ self.robot_control_mode_dict.update( - (robot_id, IndividualRobotMode.AI) for robot_id in self.robot_control_mode_dict.keys() + (robot_id, IndividualRobotMode.AI) + for robot_id in self.robot_control_mode_dict.keys() ) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( From 35be0c56f8fa5e0db2044382ebb5c8755fc29d47 Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 18 Mar 2024 07:55:58 -0700 Subject: [PATCH 032/138] clean up and added todo's --- .../thunderscope/robot_communication.py | 7 ++- .../controller_diagnostics.py | 45 +++++++++++-------- .../robot_diagnostics/diagnostics_widget.py | 18 ++++---- .../drive_and_dribbler_widget.py | 2 +- 4 files changed, 42 insertions(+), 30 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 261ffd1ae2..71b09a42a1 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -77,7 +77,8 @@ def __init__( # map of robot id to the individual control mode self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { - robot_id: IndividualRobotMode.NONE for robot_id in list(range(0, NUM_ROBOTS)) + robot_id: IndividualRobotMode.NONE + for robot_id in list(range(0, NUM_ROBOTS)) } # map of robot id to the number of times to send a estop. @@ -110,7 +111,8 @@ def setup_for_fullsystem(self) -> None: Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ self.robot_control_mode_dict.update( - (robot_id, IndividualRobotMode.AI) for robot_id in self.robot_control_mode_dict.keys() + (robot_id, IndividualRobotMode.AI) + for robot_id in self.robot_control_mode_dict.keys() ) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( @@ -313,6 +315,7 @@ def __enter__(self) -> "self": """ # Create the multicast listeners + # TODO: log a better error message if the interface not specified self.receive_robot_status = tbots_cpp.RobotStatusProtoListener( self.multicast_channel + "%" + self.interface, ROBOT_STATUS_PORT, diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 8e0267ebd8..5602b7e3cb 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -26,40 +26,47 @@ class ControllerInputHandler(object): interpreting the inputs into usable inputs for robot. """ + # TODO: remove proto_unix_io, and set Motor/Power control as class fields + # TODO: add class init wrapper for easier handling of controller connection def __init__( self, proto_unix_io: ProtoUnixIO, ): + self.proto_unix_io = proto_unix_io self.enabled = False self.controller = None + # find an for device in list_devices(): controller = InputDevice(device) + # TODO: read the device name and check that it's xbox controller if controller is not None: self.controller = controller - if self.controller is None: - raise RuntimeError("Could not initialize a controller - connect using USB") + if self.controller is not None: + logging.info( + "Initializing controller " + + self.controller.info.__str__() + + " and device path location: " + + self.controller.path + ) - logging.info( - "Initializing controller " - + self.controller.info.__str__() - + " and device path location: " - + self.controller.path - ) - self.proto_unix_io = proto_unix_io + self.__stop_event_thread = Event() + self.__event_thread = Thread(target=self.__event_loop, daemon=True) + self.__event_thread.start() - self.__stop_event_thread = Event() - self.__event_thread = Thread(target=self.__event_loop, daemon=True) - self.__event_thread.start() + self.constants = tbots_cpp.create2021RobotConstants() - self.constants = tbots_cpp.create2021RobotConstants() + self.x_vel = 0.0 + self.y_vel = 0.0 + self.ang_vel = 0.0 + self.kick_power = 0.0 + self.dribbler_speed = 0.0 + self.dribbler_enabled = False - self.x_vel = 0.0 - self.y_vel = 0.0 - self.ang_vel = 0.0 - self.kick_power = 0.0 - self.dribbler_speed = 0.0 - self.dribbler_enabled = False + else: + logging.info( + "Could not initialize a handheld controller device - check USB connections" + ) def __parse_move_event_value(self, value, factor): if abs(value) < (DEADZONE_PERCENTAGE * factor): diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index f5a70ba088..0d40af0979 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,8 +1,9 @@ import logging -import threading from pyqtgraph.Qt.QtCore import pyqtSignal from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout +from proto.import_all_protos import * + from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( @@ -28,7 +29,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.controller = ControllerInputHandler(proto_unix_io) + self.controller_handler = ControllerInputHandler(proto_unix_io) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = FullSystemConnectWidget( @@ -41,9 +42,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: lambda control_mode: self.toggle_control(control_mode) ) - self.run_diagnostics_thread = threading.Thread( - target=self.__run_diagnostics_primivitve_set, daemon=True - ) + # self.run_diagnostics_thread = threading.Thread( + # target=self.__run_diagnostics_primivitve_set, daemon=True + # ) vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) @@ -58,7 +59,7 @@ def refresh(self): self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() - # + # TODO: throws error on exit diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( motor_control=self.drive_dribbler_widget.motor_control, @@ -70,9 +71,10 @@ def refresh(self): elif self.__control_mode == ControlMode.XBOX: # TODO: get values from controller widget, placeholder log for now - logging.debug(self.controller.ang_vel) + if self.controller_handler.controller is not None: + logging.debug(self.controller_handler.ang_vel) def toggle_control(self, mode: ControlMode): self.__control_mode = mode self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) - self.controller.set_enabled(mode == ControlMode.XBOX) + self.controller_handler.set_enabled(mode == ControlMode.XBOX) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 0459e07a52..77b1e068fe 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -27,7 +27,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.motor_control = self.__create_empty_mc_primitive() + self.motor_control = MotorControl() # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) From 9427ff182de1d5fa9fc07c0050ddbccd1fc476f2 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 23 Mar 2024 08:39:29 -0700 Subject: [PATCH 033/138] clean up and added todo's addressing pr comments --- src/software/thunderscope/robot_communication.py | 12 +++--------- .../robot_diagnostics/controller_diagnostics.py | 2 ++ 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 71b09a42a1..703ee9c1e7 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -17,6 +17,7 @@ EstopMode, ) +# TODO: should use MAX_ROBOT_IDS_PER_SIDE instead or DIV_A?! NUM_ROBOTS = DIV_B_NUM_ROBOTS @@ -162,18 +163,10 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param robot_id: the id of the robot whose mode we're changing """ self.robot_control_mode_dict[robot_id] = mode - + # TODO: add else case that sets to 0 if mode == IndividualRobotMode.NONE: self.robot_estop_send_countdown_latches[robot_id] = NUM_TIMES_SEND_STOP - def toggle_input_mode(self, mode: ControlMode): - """ - Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - - :param mode: Control mode to use when sending diagnostics primitives - """ - self.__input_mode = mode - def __send_estop_state(self) -> None: """ Constant loop which sends the current estop status proto if estop is not disabled @@ -260,6 +253,7 @@ def __run_primitive_set(self) -> None: for robot_id in fullsystem_primitives.keys(): if ( + # TODO: cast shouldn't be needed... self.robot_control_mode_dict[int(robot_id)] == IndividualRobotMode.AI ): diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 5602b7e3cb..556d80a8bd 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -19,6 +19,7 @@ MAX_LINEAR_SPEED_METER_PER_S = 2 MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 +# TODO: change all logging to DEBUG level or remove entirely... class ControllerInputHandler(object): """ @@ -189,6 +190,7 @@ def __process_event(self, event): def __event_loop(self): logging.info("Starting controller event loop") for event in self.controller.read_loop(): + # TODO: only run loop if self.enabled is set if self.__stop_event_thread.isSet(): return if self.enabled: From d97e92bb0b1bf4905a7e964e7ec921bad485f619 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Sat, 23 Mar 2024 15:55:27 +0000 Subject: [PATCH 034/138] [pre-commit.ci lite] apply automatic fixes --- .../thunderscope/robot_diagnostics/controller_diagnostics.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 556d80a8bd..fb9a059c57 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -21,6 +21,7 @@ # TODO: change all logging to DEBUG level or remove entirely... + class ControllerInputHandler(object): """ This class is responsible for reading from an Xbox controller device and From c966fa1594bf4a94ae8bc70af7c434e3459e8c17 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 23 Mar 2024 08:55:43 -0700 Subject: [PATCH 035/138] formatting --- .../thunderscope/robot_diagnostics/controller_diagnostics.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 556d80a8bd..fb9a059c57 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -21,6 +21,7 @@ # TODO: change all logging to DEBUG level or remove entirely... + class ControllerInputHandler(object): """ This class is responsible for reading from an Xbox controller device and From 1494757c28a631f3f95c28e2061d87cd4579bb65 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 23 Mar 2024 11:27:15 -0700 Subject: [PATCH 036/138] formatting --- .../field_tests/movement_robot_field_test.py | 277 +++++++++--------- 1 file changed, 138 insertions(+), 139 deletions(-) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 1abec4878a..75815af427 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -10,7 +10,6 @@ from software.logger.logger import createLogger from software.simulated_tests.robot_enters_region import RobotEventuallyEntersRegion from proto.message_translation.tbots_protobuf import create_world_state -import math logger = createLogger(__name__) @@ -76,144 +75,144 @@ # this test can only be run on the field -def test_basic_rotation(field_test_runner): - test_angles = [0, 45, 90, 180, 270, 0] - - world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - if len(world.friendly_team.team_robots) == 0: - raise Exception("The first world received had no robots in it!") - - print("Here are the robots:") - print( - [ - robot.current_state.global_position - for robot in world.friendly_team.team_robots - ] - ) - - id = world.friendly_team.team_robots[0].id - print(f"Running test on robot {id}") - - robot = world.friendly_team.team_robots[0] - rob_pos_p = robot.current_state.global_position - logger.info("staying in pos {rob_pos_p}") - - for angle in test_angles: - move_tactic = MoveTactic() - move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.final_speed = 0.0 - move_tactic.dribbler_mode = DribblerMode.OFF - move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) - move_tactic.ball_collision_type = BallCollisionType.AVOID - move_tactic.auto_chip_or_kick.CopyFrom( - AutoChipOrKick(autokick_speed_m_per_s=0.0) - ) - move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT - move_tactic.target_spin_rev_per_s = 0.0 - - # Setup Tactic - params = AssignedTacticPlayControlParams() - - params.assigned_tactics[id].move.CopyFrom(move_tactic) - - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, - ) - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(stop_tactic) - # send the stop tactic - field_test_runner.set_tactics(params, True) - - # validate by eye - logger.info(f"robot set to {angle} orientation") - - time.sleep(2) - - -def test_one_robots_square(field_test_runner): - world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - if len(world.friendly_team.team_robots) == 0: - raise Exception("The first world received had no robots in it!") - - print("Here are the robots:") - print( - [ - robot.current_state.global_position - for robot in world.friendly_team.team_robots - ] - ) - - id = world.friendly_team.team_robots[0].id - print(f"Running test on robot {id}") - - point1 = Point(x_meters=-0.3, y_meters=0.6) - point2 = Point(x_meters=-0.3, y_meters=-0.6) - point3 = Point(x_meters=-1.5, y_meters=-0.6) - point4 = Point(x_meters=-1.5, y_meters=0.6) - - tactic_0 = MoveTactic( - destination=point1, - final_speed=0.0, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - target_spin_rev_per_s=0.0, - ) - tactic_1 = MoveTactic( - destination=point2, - final_speed=0.0, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - target_spin_rev_per_s=0.0, - ) - tactic_2 = MoveTactic( - destination=point3, - final_speed=0.0, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - target_spin_rev_per_s=0.0, - ) - tactic_3 = MoveTactic( - destination=point4, - final_speed=0.0, - dribbler_mode=DribblerMode.OFF, - final_orientation=Angle(radians=-math.pi / 2), - ball_collision_type=BallCollisionType.AVOID, - auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), - max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - target_spin_rev_per_s=0.0, - ) - tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - - for tactic in tactics: - print(f"Going to {tactic.destination}") - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].move.CopyFrom(tactic) - - field_test_runner.set_tactics(params, True) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, - ) - - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() - params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(stop_tactic) +# def test_basic_rotation(field_test_runner): +# test_angles = [0, 45, 90, 180, 270, 0] +# +# world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) +# if len(world.friendly_team.team_robots) == 0: +# raise Exception("The first world received had no robots in it!") +# +# print("Here are the robots:") +# print( +# [ +# robot.current_state.global_position +# for robot in world.friendly_team.team_robots +# ] +# ) +# +# id = world.friendly_team.team_robots[0].id +# print(f"Running test on robot {id}") +# +# robot = world.friendly_team.team_robots[0] +# rob_pos_p = robot.current_state.global_position +# logger.info("staying in pos {rob_pos_p}") +# +# for angle in test_angles: +# move_tactic = MoveTactic() +# move_tactic.destination.CopyFrom(rob_pos_p) +# move_tactic.final_speed = 0.0 +# move_tactic.dribbler_mode = DribblerMode.OFF +# move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) +# move_tactic.ball_collision_type = BallCollisionType.AVOID +# move_tactic.auto_chip_or_kick.CopyFrom( +# AutoChipOrKick(autokick_speed_m_per_s=0.0) +# ) +# move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT +# move_tactic.target_spin_rev_per_s = 0.0 +# +# # Setup Tactic +# params = AssignedTacticPlayControlParams() +# +# params.assigned_tactics[id].move.CopyFrom(move_tactic) +# +# field_test_runner.set_tactics(params, True) +# field_test_runner.run_test( +# always_validation_sequence_set=[[]], +# eventually_validation_sequence_set=[[]], +# test_timeout_s=5, +# ) +# # Send a stop tactic after the test finishes +# stop_tactic = StopTactic() +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].stop.CopyFrom(stop_tactic) +# # send the stop tactic +# field_test_runner.set_tactics(params, True) +# +# # validate by eye +# logger.info(f"robot set to {angle} orientation") +# +# time.sleep(2) +# +# +# def test_one_robots_square(field_test_runner): +# world = field_test_runner.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) +# if len(world.friendly_team.team_robots) == 0: +# raise Exception("The first world received had no robots in it!") +# +# print("Here are the robots:") +# print( +# [ +# robot.current_state.global_position +# for robot in world.friendly_team.team_robots +# ] +# ) +# +# id = world.friendly_team.team_robots[0].id +# print(f"Running test on robot {id}") +# +# point1 = Point(x_meters=-0.3, y_meters=0.6) +# point2 = Point(x_meters=-0.3, y_meters=-0.6) +# point3 = Point(x_meters=-1.5, y_meters=-0.6) +# point4 = Point(x_meters=-1.5, y_meters=0.6) +# +# tactic_0 = MoveTactic( +# destination=point1, +# final_speed=0.0, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# target_spin_rev_per_s=0.0, +# ) +# tactic_1 = MoveTactic( +# destination=point2, +# final_speed=0.0, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# target_spin_rev_per_s=0.0, +# ) +# tactic_2 = MoveTactic( +# destination=point3, +# final_speed=0.0, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# target_spin_rev_per_s=0.0, +# ) +# tactic_3 = MoveTactic( +# destination=point4, +# final_speed=0.0, +# dribbler_mode=DribblerMode.OFF, +# final_orientation=Angle(radians=-math.pi / 2), +# ball_collision_type=BallCollisionType.AVOID, +# auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), +# max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, +# target_spin_rev_per_s=0.0, +# ) +# tactics = [tactic_0, tactic_1, tactic_2, tactic_3] +# +# for tactic in tactics: +# print(f"Going to {tactic.destination}") +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].move.CopyFrom(tactic) +# +# field_test_runner.set_tactics(params, True) +# field_test_runner.run_test( +# always_validation_sequence_set=[[]], +# eventually_validation_sequence_set=[[]], +# test_timeout_s=4, +# ) +# +# # Send a stop tactic after the test finishes +# stop_tactic = StopTactic() +# params = AssignedTacticPlayControlParams() +# params.assigned_tactics[id].stop.CopyFrom(stop_tactic) if __name__ == "__main__": From f8a24db9ae7a8fd4157bb3cccf386e2ca66c305f Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 23 Mar 2024 12:14:58 -0700 Subject: [PATCH 037/138] addressing comments --- src/shared/constants.h | 7 ++--- .../thunderscope/robot_communication.py | 26 ++++++++++--------- .../robot_diagnostics/chicker_widget.py | 2 +- .../controller_diagnostics.py | 4 ++- .../diagnostics_input_widget.py | 6 +++-- .../robot_diagnostics/diagnostics_widget.py | 4 +-- .../robot_diagnostics/robot_view.py | 3 --- .../thunderscope/widget_setup_functions.py | 14 +--------- 8 files changed, 29 insertions(+), 37 deletions(-) diff --git a/src/shared/constants.h b/src/shared/constants.h index 4e4f414850..d9fe33b0e5 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -111,6 +111,10 @@ static const unsigned int MAX_ROBOT_IDS_PER_SIDE = 8; // The total number of possible robot ids between two teams static const unsigned int MAX_ROBOT_IDS = MAX_ROBOT_IDS_PER_SIDE * 2; +// How many robots are allowed in each division +static const unsigned DIV_A_NUM_ROBOTS = 11; +static const unsigned DIV_B_NUM_ROBOTS = 6; + // Battery Constants static const unsigned NUM_CELLS_IN_BATTERY = 3; static const unsigned NUM_BATTERIES_IN_SERIES = 2; @@ -129,9 +133,6 @@ static const double MAX_CAPACITOR_VOLTAGE = 250.0 + 50.0; // +50v headroom static const unsigned int ROBOT_CHIP_ANGLE_DEGREES = 45; static const double CHICKER_TIMEOUT = 3 * MILLISECONDS_PER_SECOND; -// How many robots are allowed in each division -static const unsigned DIV_A_NUM_ROBOTS = 11; -static const unsigned DIV_B_NUM_ROBOTS = 6; // Kick Spd to Pulse Width Safety Constraint Constants diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 703ee9c1e7..ac59836127 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -18,7 +18,7 @@ ) # TODO: should use MAX_ROBOT_IDS_PER_SIDE instead or DIV_A?! -NUM_ROBOTS = DIV_B_NUM_ROBOTS +NUM_ROBOTS = MAX_ROBOT_IDS_PER_SIDE class RobotCommunication(object): @@ -77,7 +77,7 @@ def __init__( ) # map of robot id to the individual control mode - self.robot_control_mode_dict: dict[int, IndividualRobotMode] = { + self.robot_control_mode_map: dict[int, IndividualRobotMode] = { robot_id: IndividualRobotMode.NONE for robot_id in list(range(0, NUM_ROBOTS)) } @@ -85,7 +85,7 @@ def __init__( # map of robot id to the number of times to send a estop. # each robot has it's own separate countdown # TODO come up with better name - self.robot_estop_send_countdown_latches: dict[int, int] = { + self.robot_stop_primitive_count_map: dict[int, int] = { rid: 0 for rid in range(NUM_ROBOTS) } @@ -111,9 +111,9 @@ def setup_for_fullsystem(self) -> None: """ Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ - self.robot_control_mode_dict.update( + self.robot_control_mode_map.update( (robot_id, IndividualRobotMode.AI) - for robot_id in self.robot_control_mode_dict.keys() + for robot_id in self.robot_control_mode_map.keys() ) self.receive_ssl_wrapper = tbots_cpp.SSLWrapperPacketProtoListener( @@ -162,10 +162,12 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param mode: the mode of input for this robot's primitives :param robot_id: the id of the robot whose mode we're changing """ - self.robot_control_mode_dict[robot_id] = mode + self.robot_control_mode_map[robot_id] = mode # TODO: add else case that sets to 0 if mode == IndividualRobotMode.NONE: - self.robot_estop_send_countdown_latches[robot_id] = NUM_TIMES_SEND_STOP + self.robot_stop_primitive_count_map[robot_id] = NUM_TIMES_SEND_STOP + else: + self.robot_stop_primitive_count_map[robot_id] = 0 def __send_estop_state(self) -> None: """ @@ -236,7 +238,7 @@ def __run_primitive_set(self) -> None: # filter for diagnostics controlled robots diagnostics_robots = list( robot_id - for robot_id, mode in self.robot_control_mode_dict.items() + for robot_id, mode in self.robot_control_mode_map.items() if mode == IndividualRobotMode.MANUAL ) @@ -254,7 +256,7 @@ def __run_primitive_set(self) -> None: for robot_id in fullsystem_primitives.keys(): if ( # TODO: cast shouldn't be needed... - self.robot_control_mode_dict[int(robot_id)] + self.robot_control_mode_map[int(robot_id)] == IndividualRobotMode.AI ): robot_primitives[robot_id] = fullsystem_primitives[robot_id] @@ -264,10 +266,10 @@ def __run_primitive_set(self) -> None: for ( robot_id, num_times_to_stop, - ) in self.robot_estop_send_countdown_latches.items(): + ) in self.robot_stop_primitive_count_map.items(): if num_times_to_stop > 0: robot_primitives[robot_id] = Primitive(stop=StopPrimitive()) - self.robot_estop_send_countdown_latches[robot_id] = ( + self.robot_stop_primitive_count_map[robot_id] = ( num_times_to_stop - 1 ) @@ -291,7 +293,7 @@ def __run_primitive_set(self) -> None: self.should_send_stop = False # sleep if not running fullsystem - if IndividualRobotMode.AI not in self.robot_control_mode_dict.values(): + if IndividualRobotMode.AI not in self.robot_control_mode_map.values(): time.sleep(ROBOT_COMMUNICATIONS_TIMEOUT_S) def __forward_to_proto_unix_io(self, type: Type[Message], data: Message) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 4aacf45143..11c00c652b 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -181,7 +181,7 @@ def send_command(self, command: ChickerCommandMode) -> None: power_value ) - # sends proto + # update the proto to be sent self.power_control = power_control # clears the proto buffer for kick or chip commands diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index fb9a059c57..1aaf104d95 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -43,7 +43,9 @@ def __init__( # TODO: read the device name and check that it's xbox controller if controller is not None: self.controller = controller + break + # TODO: move to setup function and call it when xbox controller plugged dynamically if self.controller is not None: logging.info( "Initializing controller " @@ -108,7 +110,7 @@ def __send_move_command(self): motor_control.dribbler_speed_rpm = ( self.constants.indefinite_dribbler_speed_rpm ) - + # TODO: verbose logging is the solution for logging instead of trace level logging.info("Sending motor control: " + motor_control) self.proto_unix_io.send_proto(MotorControl, motor_control) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 9442a770d9..7ffd42c10d 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -15,7 +15,8 @@ class ControlMode(IntEnum): # this class name doesnt make sense -class FullSystemConnectWidget(QWidget): +# +class DiagnosticsInputModeWidget(QWidget): """ Class to allow the user to switch between Manual, XBox, and Fullsystem control through Thunderscope UI @@ -24,6 +25,7 @@ class FullSystemConnectWidget(QWidget): """ # Signal to indicate if manual controls should be disabled based on boolean parameter + # TODO: signal logic is flipped... toggle_controls_signal = pyqtSignal(bool) def __init__(self, toggle_controls_signal) -> None: @@ -31,7 +33,7 @@ def __init__(self, toggle_controls_signal) -> None: Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control :param toggle_controls_signal The signal to use for handling changes in input mode """ - super(FullSystemConnectWidget, self).__init__() + super(DiagnosticsInputModeWidget, self).__init__() self.toggle_controls_signal = toggle_controls_signal vbox_layout = QVBoxLayout() self.connect_options_group = QButtonGroup() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 0d40af0979..a17ca1a533 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -7,7 +7,7 @@ from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( - FullSystemConnectWidget, + DiagnosticsInputModeWidget, ControlMode, ) from software.thunderscope.robot_diagnostics.controller_diagnostics import ( @@ -32,7 +32,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.controller_handler = ControllerInputHandler(proto_unix_io) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) - self.diagnostics_control_input_widget = FullSystemConnectWidget( + self.diagnostics_control_input_widget = DiagnosticsInputModeWidget( self.diagnostics_input_mode_signal ) diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index 55bce1e261..27d727a550 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -87,9 +87,6 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ - - control_mode_signal = QtCore.pyqtSignal(int, int) - def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: """ diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 092d94ba06..dba8150faf 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -39,7 +39,7 @@ from software.thunderscope.play.refereeinfo_widget import RefereeInfoWidget from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( - FullSystemConnectWidget, + DiagnosticsInputModeWidget, ) from software.thunderscope.robot_diagnostics.diagnostics_widget import DiagnosticsWidget from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( @@ -328,18 +328,6 @@ def setup_chicker_widget(proto_unix_io: ProtoUnixIO) -> ChickerWidget: return chicker_widget -def setup_diagnostics_input_widget() -> FullSystemConnectWidget: - """ - Sets up the diagnostics input widget - - :returns: the diagnostics input widget - """ - - diagnostics_input_widget = FullSystemConnectWidget() - - return diagnostics_input_widget - - def setup_drive_and_dribbler_widget( proto_unix_io: ProtoUnixIO, ) -> DriveAndDribblerWidget: From d446ec8af07eba952914aceb54cfc185d48e7610 Mon Sep 17 00:00:00 2001 From: boris Date: Sat, 23 Mar 2024 12:15:21 -0700 Subject: [PATCH 038/138] formatting --- src/software/thunderscope/robot_diagnostics/robot_view.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index 27d727a550..43c72f493f 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -87,6 +87,7 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ + def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: """ From 4d56abcda1c87e8492d97a301d46a370bfc4f652 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 18:22:49 -0700 Subject: [PATCH 039/138] updated import order --- src/software/thunderscope/robot_communication.py | 2 +- .../thunderscope/robot_diagnostics/controller_diagnostics.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index ac59836127..6c31e52831 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -6,11 +6,11 @@ from typing import Type from google.protobuf.message import Message from pyqtgraph.Qt import QtCore +from proto.import_all_protos import * from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO from software.py_constants import * -from proto.import_all_protos import * from software.thunderscope.constants import ( ROBOT_COMMUNICATIONS_TIMEOUT_S, IndividualRobotMode, diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 1aaf104d95..e051b51512 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -1,12 +1,11 @@ import logging - from evdev import InputDevice, categorize, ecodes, list_devices from threading import Event, Thread +import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.proto_unix_io import ProtoUnixIO -import software.python_bindings as tbots_cpp XBOX_MAX_RANGE = 32768 XBOX_BUTTON_MAX_RANGE = 1024 From 1b99fc45fde4c0992e8d6487686fac0a1ac648dd Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 18:41:55 -0700 Subject: [PATCH 040/138] updated requirements' pyqtgraph dep --- environment_setup/ubuntu20_requirements.txt | 2 +- environment_setup/ubuntu22_requirements.txt | 2 +- src/software/thunderscope/requirements.txt | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 53e6dd2562..a9d5a1ba37 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,5 +1,5 @@ protobuf==3.20.2 -pyqtgraph==0.13.3 +pyqtgraph==0.13.4 autoflake==1.4 pyqt6==6.5.0 PyQt6-Qt6==6.5.0 diff --git a/environment_setup/ubuntu22_requirements.txt b/environment_setup/ubuntu22_requirements.txt index 8c8e57d25e..b50f3dd7e8 100644 --- a/environment_setup/ubuntu22_requirements.txt +++ b/environment_setup/ubuntu22_requirements.txt @@ -1,5 +1,5 @@ protobuf==3.20.2 -pyqtgraph==0.12.4 +pyqtgraph==0.13.3 autoflake==1.4 pyqt6==6.5.0 PyQt6-Qt6==6.5.0 diff --git a/src/software/thunderscope/requirements.txt b/src/software/thunderscope/requirements.txt index fef73d9322..aba373e2c8 100644 --- a/src/software/thunderscope/requirements.txt +++ b/src/software/thunderscope/requirements.txt @@ -1,2 +1,2 @@ numpy==1.24.4 -pyqtgraph==0.13.3 +pyqtgraph==0.13.4 From 867c6bc22188f22297fa3bd6a58091327c187032 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 18:42:14 -0700 Subject: [PATCH 041/138] added constants from other branch, handheld_demo_4, for ref --- src/software/thunderscope/constants.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 787cbe3dcf..ff9095b798 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -318,3 +318,18 @@ class TrailValues: DEFAULT_TRAIL_LENGTH = 20 DEFAULT_TRAIL_SAMPLING_RATE = 0 + +class ControllerConstants: + # TODO: added here for reference and to not haver to switch branches, remove later + MAX_LINEAR_SPEED_MPS = 2.0 + MIN_LINEAR_SPEED_MPS = -2.0 + + MAX_ANGULAR_SPEED_RAD_PER_S = 20 + MIN_ANGULAR_SPEED_RAD_PER_S = -20 + + MAX_DRIBBLER_RPM = 10000 + MIN_DRIBBLER_RPM = -10000 + + MIN_POWER = 1000 + MAX_POWER = 20000 + From c4627dc0b2b2f3af22c72e07e6132014c8e4b1c8 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 19:08:59 -0700 Subject: [PATCH 042/138] undo requirements, too much bumped --- environment_setup/ubuntu20_requirements.txt | 2 +- src/software/thunderscope/requirements.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index a9d5a1ba37..53e6dd2562 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,5 +1,5 @@ protobuf==3.20.2 -pyqtgraph==0.13.4 +pyqtgraph==0.13.3 autoflake==1.4 pyqt6==6.5.0 PyQt6-Qt6==6.5.0 diff --git a/src/software/thunderscope/requirements.txt b/src/software/thunderscope/requirements.txt index aba373e2c8..fef73d9322 100644 --- a/src/software/thunderscope/requirements.txt +++ b/src/software/thunderscope/requirements.txt @@ -1,2 +1,2 @@ numpy==1.24.4 -pyqtgraph==0.13.4 +pyqtgraph==0.13.3 From 82222ed34fd1acfd8baf931805ab79e018e23597 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 19:09:30 -0700 Subject: [PATCH 043/138] pr comments: removed todos --- src/software/thunderscope/robot_communication.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 6c31e52831..faa30c3372 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -163,7 +163,6 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): :param robot_id: the id of the robot whose mode we're changing """ self.robot_control_mode_map[robot_id] = mode - # TODO: add else case that sets to 0 if mode == IndividualRobotMode.NONE: self.robot_stop_primitive_count_map[robot_id] = NUM_TIMES_SEND_STOP else: @@ -311,7 +310,6 @@ def __enter__(self) -> "self": """ # Create the multicast listeners - # TODO: log a better error message if the interface not specified self.receive_robot_status = tbots_cpp.RobotStatusProtoListener( self.multicast_channel + "%" + self.interface, ROBOT_STATUS_PORT, From 64b8250745a9631fd2a208044d38d8481d6e2172 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 23:46:14 -0700 Subject: [PATCH 044/138] added constants --- src/software/thunderscope/constants.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index ff9095b798..e582c1da94 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -320,12 +320,16 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 class ControllerConstants: + # TODO: add some more here by testing available ones + VALID_CONTROLLER_NAMES = ['Microsoft Xbox One X pad'] + # TODO: added here for reference and to not haver to switch branches, remove later MAX_LINEAR_SPEED_MPS = 2.0 MIN_LINEAR_SPEED_MPS = -2.0 MAX_ANGULAR_SPEED_RAD_PER_S = 20 - MIN_ANGULAR_SPEED_RAD_PER_S = -20 + # TODO: field test values + # MIN_ANGULAR_SPEED_RAD_PER_S = -20 MAX_DRIBBLER_RPM = 10000 MIN_DRIBBLER_RPM = -10000 @@ -333,3 +337,14 @@ class ControllerConstants: MIN_POWER = 1000 MAX_POWER = 20000 + XBOX_MAX_RANGE = 32768 + XBOX_BUTTON_MAX_RANGE = 1024 + + DEADZONE_PERCENTAGE = 0.30 + + DRIBBLER_STEPPER = 100 + POWER_STEPPER = 100 + + MAX_LINEAR_SPEED_METER_PER_S = 2 + MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 + From 15df7612f29cd49f635f6f8da688b501e8d072c2 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 23:46:26 -0700 Subject: [PATCH 045/138] returned back the lost signal --- src/software/thunderscope/robot_diagnostics/robot_view.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index 43c72f493f..55bce1e261 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -88,6 +88,8 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ + control_mode_signal = QtCore.pyqtSignal(int, int) + def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: """ From ac95b3b77591b2824828de36a0292024058a6c33 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 26 Mar 2024 23:46:52 -0700 Subject: [PATCH 046/138] added todo --- src/software/thunderscope/robot_communication.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index faa30c3372..adfdf04278 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -50,8 +50,10 @@ def __init__( self.current_proto_unix_io = current_proto_unix_io self.multicast_channel = str(multicast_channel) self.interface = interface - self.estop_mode = estop_mode + # TODO: move estop state management out of robot_communication, + # separate PR, reuse controller code possibly + self.estop_mode = estop_mode self.estop_path = estop_path self.estop_buadrate = estop_baudrate From 635f45d7e2b786fb00b99b82e48eb87e4f45b29f Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 00:20:39 -0700 Subject: [PATCH 047/138] added refresh button and status --- src/software/logger/logger.py | 2 +- .../thunderscope/robot_diagnostics/BUILD | 10 ++ .../controller_communication.py | 9 + .../controller_diagnostics.py | 166 +++++++++--------- .../robot_diagnostics/controller_view.py | 48 +++++ .../diagnostics_input_widget.py | 7 +- .../robot_diagnostics/diagnostics_widget.py | 55 ++++-- .../drive_and_dribbler_widget.py | 2 - .../thunderscope/thunderscope_main.py | 2 +- 9 files changed, 195 insertions(+), 106 deletions(-) create mode 100644 src/software/thunderscope/robot_diagnostics/controller_communication.py create mode 100644 src/software/thunderscope/robot_diagnostics/controller_view.py diff --git a/src/software/logger/logger.py b/src/software/logger/logger.py index e555223226..248336478e 100644 --- a/src/software/logger/logger.py +++ b/src/software/logger/logger.py @@ -1,7 +1,7 @@ import logging logging.basicConfig( - level=logging.INFO, + level=logging.DEBUG, format="%(asctime)s - [%(levelname)s] - [%(threadName)s] - %(name)s - (%(filename)s).%(funcName)s(%(lineno)d) - %(message)s", ) diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index a747c9515c..463daf51ca 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -34,6 +34,15 @@ py_library( ], ) +py_library( + name = "controller_view", + srcs = ["controller_view.py"], + deps = [ + "//software/thunderscope:constants", + requirement("pyqtgraph"), + ], +) + py_library( name = "diagnostics_widget", srcs = ["diagnostics_widget.py"], @@ -45,6 +54,7 @@ py_library( "//software/thunderscope/robot_diagnostics:controller", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", + "//software/thunderscope/robot_diagnostics:controller_view", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/controller_communication.py b/src/software/thunderscope/robot_diagnostics/controller_communication.py new file mode 100644 index 0000000000..143e41bd46 --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/controller_communication.py @@ -0,0 +1,9 @@ +from software.thunderscope.robot_diagnostics.controller_view import ControllerStatusView + + +class ControllerConnectionHandler(QWidget): + + def __enter__(self) -> None: + super(ControllerConnectionHandler, self).__init__() + + self.controller_view = ControllerStatusView() \ No newline at end of file diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index e051b51512..5752dff9a6 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -7,16 +7,6 @@ from software.thunderscope.constants import * from software.thunderscope.proto_unix_io import ProtoUnixIO -XBOX_MAX_RANGE = 32768 -XBOX_BUTTON_MAX_RANGE = 1024 - -DEADZONE_PERCENTAGE = 0.30 - -DRIBBLER_STEPPER = 100 -POWER_STEPPER = 100 - -MAX_LINEAR_SPEED_METER_PER_S = 2 -MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 # TODO: change all logging to DEBUG level or remove entirely... @@ -30,23 +20,16 @@ class ControllerInputHandler(object): # TODO: remove proto_unix_io, and set Motor/Power control as class fields # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.proto_unix_io = proto_unix_io self.enabled = False self.controller = None - # find an - for device in list_devices(): - controller = InputDevice(device) - # TODO: read the device name and check that it's xbox controller - if controller is not None: - self.controller = controller - break + self.__setup_controller() - # TODO: move to setup function and call it when xbox controller plugged dynamically if self.controller is not None: - logging.info( + logging.debug( "Initializing controller " + self.controller.info.__str__() + " and device path location: " @@ -66,12 +49,42 @@ def __init__( self.dribbler_speed = 0.0 self.dribbler_enabled = False + self.motor_control = MotorControl() + self.power_control = PowerControl() + else: - logging.info( + logging.debug("Tried to initialize a handheld controller from list available devices:") + logging.debug(list_devices()) + logging.debug( "Could not initialize a handheld controller device - check USB connections" ) - def __parse_move_event_value(self, value, factor): + def get_latest_primitive_command(self): + if self.controller_initialized(): + return DirectControlPrimitive( + motor_control=self.motor_control, + power_control=self.power_control, + ) + else: + return None + + def controller_initialized(self): + return self.controller is not None + + def __setup_controller(self): + for device in list_devices(): + controller = InputDevice(device) + if (controller is not None and + controller.name in ControllerConstants.VALID_CONTROLLER_NAMES): + self.controller = controller + break + + @staticmethod + def __parse_dribbler_enabled_event_value(value) -> bool: + return value > (XBOX_BUTTON_MAX_RANGE / 2) + + @staticmethod + def __parse_move_event_value(value, factor): if abs(value) < (DEADZONE_PERCENTAGE * factor): return 0 else: @@ -82,69 +95,27 @@ def __parse_dribbler_event_value(self, value): new_speed = self.dribbler_speed - value * DRIBBLER_STEPPER if MIN_DRIBBLER_RPM < new_speed < MAX_DRIBBLER_RPM: return new_speed + else: + return 0 def __parse_kick_event_value(self, value) -> int: - new_power = self.power + value * POWER_STEPPER + new_power = self.kick_power + value * POWER_STEPPER if MIN_POWER < new_power < MAX_POWER: return new_power - - def __parse_dribbler_enabled_event_value(self, value) -> bool: - return value > (XBOX_BUTTON_MAX_RANGE / 2) - - def __send_move_command(self): - motor_control = MotorControl() - - motor_control.direct_velocity_control.velocity.x_component_meters = self.x_vel - motor_control.direct_velocity_control.velocity.y_component_meters = self.y_vel - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.ang_vel - ) - - motor_control.dribbler_speed_rpm = 0 - - # if self.enable_dribbler: - # motor_control.dribbler_speed_rpm = dribbler_speed - # maybe just use indefinite instead? or have setting to turn on 'smooth scrolling' - if self.dribbler_enabled: - motor_control.dribbler_speed_rpm = ( - self.constants.indefinite_dribbler_speed_rpm - ) - # TODO: verbose logging is the solution for logging instead of trace level - logging.info("Sending motor control: " + motor_control) - - self.proto_unix_io.send_proto(MotorControl, motor_control) - - def __send_kick_command(self): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.kick_speed_m_per_s = self.kick_power - - logging.info("Sending kick power control: " + power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) - - def __send_chip_command(self): - power_control = PowerControl() - power_control.geneva_slot = 1 - power_control.chicker.chip_distance_meters = ( - self.kick_power - ) # not sure if we should use this value - - logging.info("Sending chip power control: " + power_control) - - self.proto_unix_io.send_proto(PowerControl, power_control) + else: + return 0 def __process_event(self, event): - logging.info("Processing event: " + str(event)) + logging.debug("Processing event: " + str(event)) abs_event = categorize(event) - event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] - # TODO: nump python version so we can use pattern matching for this - # TODO: make codebase use python logging instead of stdout - logging.info("Processing event: " + str(event_t)) + + # TODO: bump python version so we can use pattern matching for this if event.type == ecodes.EV_ABS: # grab the event type event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] + logging.debug("Processing joystick event type: " + str(event_t)) + if event_t == "ABS_X": self.x_vel = self.__parse_move_event_value( abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S @@ -179,27 +150,49 @@ def __process_event(self, event): ) elif event.type == ecodes.EV_KEY: - print("event code: " + str(event.code)) + logging.debug("Processing button event: " + str(event.code)) if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: - self.__send_kick_command() + self.power_control.geneva_slot = 1 + self.power_control.chicker.kick_speed_m_per_s = self.kick_power elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: - self.__send_chip_command() + self.power_control.geneva_slot = 1 + # TODO: not sure if we should use this value + self.power_control.chicker.chip_distance_meters = ( + self.kick_power + ) if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: - self.__send_move_command() + (self.motor_control.direct_velocity_control. + velocity).x_component_meters = self.x_vel + (self.motor_control.direct_velocity_control. + velocity).y_component_meters = self.y_vel + (self.motor_control.direct_velocity_control. + angular_velocity).radians_per_second = self.ang_vel + + self.motor_control.dribbler_speed_rpm = 0 + + # if self.enable_dribbler: + # motor_control.dribbler_speed_rpm = dribbler_speed + # TODO: maybe just use indefinite instead? or + # have setting to turn on 'smooth scrolling' + if self.dribbler_enabled: + self.motor_control.dribbler_speed_rpm = ( + self.constants.indefinite_dribbler_speed_rpm + ) def __event_loop(self): - logging.info("Starting controller event loop") - for event in self.controller.read_loop(): - # TODO: only run loop if self.enabled is set - if self.__stop_event_thread.isSet(): - return - if self.enabled: - self.__process_event(event) + logging.debug("Starting handheld controller event loop") + if self.enabled: + for event in self.controller.read_loop(): + # TODO: only run loop if self.enabled is set + if self.__stop_event_thread.isSet(): + return + else: + self.__process_event(event) def close(self): - logging.info("Closing controller thread") + logging.debug("Closing controller thread") self.__stop_event_thread.set() self.__event_thread.join() @@ -211,7 +204,6 @@ def set_enabled(self, enabled: bool): """ self.enabled = enabled - # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ diff --git a/src/software/thunderscope/robot_diagnostics/controller_view.py b/src/software/thunderscope/robot_diagnostics/controller_view.py new file mode 100644 index 0000000000..530f77795b --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/controller_view.py @@ -0,0 +1,48 @@ +import pyqtgraph as pg +from proto.import_all_protos import * +from pyqtgraph.Qt import QtCore, QtGui +from pyqtgraph.Qt.QtWidgets import * + +import software.thunderscope.common.common_widgets as common_widgets +from software.py_constants import * + + +class ControllerStatusView(QLabel): + """Class to show whether a handheld controller is connected to thunderscope and initialized, + or no controller is connected at all. + """ + + def __init__(self) -> None: + super().__init__() + + # TODO align text center for status + self.state: dict[str, (str, str)] = { + "On": ("Handheld Controller is Connected & Initialized", "background-color: green"), + "Off": ("No Handheld Controller is Connected...", "background-color: red") + } + + self.connected = False + self.set_view_state('Off') + + def set_view_state(self, state_discriminator='Off'): + # bruh python doesn't even have value-types or unions + # how do you even do anything in this language and still maintain a sanity ffs i legit cant + self.setText(self.state[state_discriminator][0]) + self.setStyleSheet(self.state[state_discriminator][1]) + + def set_connected(self): + self.connected = True + self.set_view_state('On') + + def set_disconnected(self): + self.connected = False + self.set_view_state('Off') + + def refresh(self) -> None: + """Refresh the label + """ + + if self.connected: + self.set_view_state('Ok') + else: + self.set_view_state('Off') diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 7ffd42c10d..0ef28f20f1 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -47,6 +47,8 @@ def __init__(self, toggle_controls_signal) -> None: self.diagnostics_control_button = self.connect_options[ControlMode.DIAGNOSTICS] self.xbox_control_button = self.connect_options[ControlMode.XBOX] + self.xbox_control_button.setEnabled(False) + self.diagnostics_control_button.clicked.connect( lambda: self.switch_control_mode(ControlMode.DIAGNOSTICS) ) @@ -74,5 +76,6 @@ def switch_control_mode(self, mode: ControlMode) -> None: self.toggle_controls_signal.emit(self.control_mode == ControlMode.DIAGNOSTICS) - def refresh(self) -> None: - pass + def refresh(self, enable_xbox=False) -> None: + self.xbox_control_button.setEnabled(enable_xbox) + diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index a17ca1a533..f9910fc953 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,11 +1,12 @@ import logging - -from pyqtgraph.Qt.QtCore import pyqtSignal -from pyqtgraph.Qt.QtWidgets import QWidget, QVBoxLayout +from pyqtgraph.Qt import QtCore, QtGui +from pyqtgraph.Qt.QtWidgets import * +from pyqtgraph.Qt.QtCore import * from proto.import_all_protos import * from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget +from software.thunderscope.robot_diagnostics.controller_view import ControllerStatusView from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputModeWidget, ControlMode, @@ -25,15 +26,13 @@ class DiagnosticsWidget(QWidget): def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() - vbox_layout = QVBoxLayout() - self.proto_unix_io = proto_unix_io self.controller_handler = ControllerInputHandler(proto_unix_io) self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputModeWidget( - self.diagnostics_input_mode_signal + self.diagnostics_input_mode_signal, ) self.__control_mode = ControlMode.DIAGNOSTICS @@ -42,19 +41,43 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: lambda control_mode: self.toggle_control(control_mode) ) - # self.run_diagnostics_thread = threading.Thread( - # target=self.__run_diagnostics_primivitve_set, daemon=True - # ) + # Inlined Controller Status and Refresh Button Widget + # Comment for PR: + # Easier to have it at this widget level for + # communication and logic handling to other widgets + + self.controller_refresh_button = QPushButton() + self.controller_refresh_button.setText("Re-initialize Handheld Controller") + self.controller_refresh_button.clicked.connect(self.refresh_controller) + + self.controller_status = ControllerStatusView() + vbox_layout = QVBoxLayout() + + vbox_layout.addWidget(self.controller_refresh_button) + vbox_layout.addWidget(self.controller_status) vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) self.setLayout(vbox_layout) + def refresh_controller(self) -> None: + logging.debug("Attempting to reinitialize handheld controller...") + if self.controller_handler.controller is None: + self.controller_status.set_connected() + self.controller_handler = ControllerInputHandler(self.proto_unix_io) + else: self.controller_status.set_disconnected() + def refresh(self): + # check if controller is initialized and set status + if self.controller_handler.controller_initialized(): + self.controller_status.set_connected() + else: + self.controller_status.set_disconnected() + if self.__control_mode == ControlMode.DIAGNOSTICS: - # refresh the widgets so that they hold the most recent user control values + # get the values from GUI sliders and send values to proto_unix_io self.diagnostics_control_input_widget.refresh() self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() @@ -70,9 +93,15 @@ def refresh(self): self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) elif self.__control_mode == ControlMode.XBOX: - # TODO: get values from controller widget, placeholder log for now - if self.controller_handler.controller is not None: - logging.debug(self.controller_handler.ang_vel) + # get the values from handheld controller and send values to proto_unix_io + diagnostics_primitive = self.controller_handler.get_latest_primitive_command() + + if diagnostics_primitive is not None: + logging.debug("Sending diagnostics primitive: " + diagnostics_primitive) + self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) + else : + logging.debug("Sending stop primitive") + self.proto_unix_io.send_proto(Primitive, StopPrimitive()) def toggle_control(self, mode: ControlMode): self.__control_mode = mode diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 77b1e068fe..c9673aa1d5 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -27,8 +27,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.motor_control = MotorControl() - # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 508e3e9c46..e4dc59a63a 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -38,7 +38,7 @@ if __name__ == "__main__": - logging.getLogger().setLevel(logging.INFO) + logging.getLogger().setLevel(logging.DEBUG) # Setup parser parser = argparse.ArgumentParser( From 87d40d6af2ecc245fb2cb030fc511b26a7c32a83 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 00:21:44 -0700 Subject: [PATCH 048/138] formatting --- src/software/thunderscope/constants.py | 4 +-- .../thunderscope/robot_diagnostics/BUILD | 2 +- .../controller_communication.py | 3 +- .../controller_diagnostics.py | 35 +++++++++++-------- .../robot_diagnostics/controller_view.py | 21 ++++++----- .../diagnostics_input_widget.py | 1 - .../robot_diagnostics/diagnostics_widget.py | 9 +++-- 7 files changed, 42 insertions(+), 33 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index e582c1da94..a4859c877b 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -319,9 +319,10 @@ class TrailValues: DEFAULT_TRAIL_LENGTH = 20 DEFAULT_TRAIL_SAMPLING_RATE = 0 + class ControllerConstants: # TODO: add some more here by testing available ones - VALID_CONTROLLER_NAMES = ['Microsoft Xbox One X pad'] + VALID_CONTROLLER_NAMES = ["Microsoft Xbox One X pad"] # TODO: added here for reference and to not haver to switch branches, remove later MAX_LINEAR_SPEED_MPS = 2.0 @@ -347,4 +348,3 @@ class ControllerConstants: MAX_LINEAR_SPEED_METER_PER_S = 2 MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 - diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 463daf51ca..4b1a7bf35d 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -52,9 +52,9 @@ py_library( "//software/thunderscope:thunderscope_types", "//software/thunderscope/robot_diagnostics:chicker", "//software/thunderscope/robot_diagnostics:controller", + "//software/thunderscope/robot_diagnostics:controller_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", - "//software/thunderscope/robot_diagnostics:controller_view", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/controller_communication.py b/src/software/thunderscope/robot_diagnostics/controller_communication.py index 143e41bd46..73de4904a4 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_communication.py +++ b/src/software/thunderscope/robot_diagnostics/controller_communication.py @@ -2,8 +2,7 @@ class ControllerConnectionHandler(QWidget): - def __enter__(self) -> None: super(ControllerConnectionHandler, self).__init__() - self.controller_view = ControllerStatusView() \ No newline at end of file + self.controller_view = ControllerStatusView() diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 5752dff9a6..1e2cdd0704 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -20,7 +20,7 @@ class ControllerInputHandler(object): # TODO: remove proto_unix_io, and set Motor/Power control as class fields # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.proto_unix_io = proto_unix_io self.enabled = False @@ -53,7 +53,9 @@ def __init__( self.power_control = PowerControl() else: - logging.debug("Tried to initialize a handheld controller from list available devices:") + logging.debug( + "Tried to initialize a handheld controller from list available devices:" + ) logging.debug(list_devices()) logging.debug( "Could not initialize a handheld controller device - check USB connections" @@ -62,8 +64,7 @@ def __init__( def get_latest_primitive_command(self): if self.controller_initialized(): return DirectControlPrimitive( - motor_control=self.motor_control, - power_control=self.power_control, + motor_control=self.motor_control, power_control=self.power_control, ) else: return None @@ -74,8 +75,10 @@ def controller_initialized(self): def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) - if (controller is not None and - controller.name in ControllerConstants.VALID_CONTROLLER_NAMES): + if ( + controller is not None + and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES + ): self.controller = controller break @@ -158,17 +161,18 @@ def __process_event(self, event): elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: self.power_control.geneva_slot = 1 # TODO: not sure if we should use this value - self.power_control.chicker.chip_distance_meters = ( - self.kick_power - ) + self.power_control.chicker.chip_distance_meters = self.kick_power if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: - (self.motor_control.direct_velocity_control. - velocity).x_component_meters = self.x_vel - (self.motor_control.direct_velocity_control. - velocity).y_component_meters = self.y_vel - (self.motor_control.direct_velocity_control. - angular_velocity).radians_per_second = self.ang_vel + ( + self.motor_control.direct_velocity_control.velocity + ).x_component_meters = self.x_vel + ( + self.motor_control.direct_velocity_control.velocity + ).y_component_meters = self.y_vel + ( + self.motor_control.direct_velocity_control.angular_velocity + ).radians_per_second = self.ang_vel self.motor_control.dribbler_speed_rpm = 0 @@ -204,6 +208,7 @@ def set_enabled(self, enabled: bool): """ self.enabled = enabled + # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ diff --git a/src/software/thunderscope/robot_diagnostics/controller_view.py b/src/software/thunderscope/robot_diagnostics/controller_view.py index 530f77795b..06259a248f 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_view.py @@ -17,32 +17,35 @@ def __init__(self) -> None: # TODO align text center for status self.state: dict[str, (str, str)] = { - "On": ("Handheld Controller is Connected & Initialized", "background-color: green"), - "Off": ("No Handheld Controller is Connected...", "background-color: red") + "On": ( + "Handheld Controller is Connected & Initialized", + "background-color: green", + ), + "Off": ("No Handheld Controller is Connected...", "background-color: red"), } self.connected = False - self.set_view_state('Off') + self.set_view_state("Off") - def set_view_state(self, state_discriminator='Off'): + def set_view_state(self, state_discriminator="Off"): # bruh python doesn't even have value-types or unions - # how do you even do anything in this language and still maintain a sanity ffs i legit cant + # how do you even do anything in this language and still maintain a sanity ffs i legit can't self.setText(self.state[state_discriminator][0]) self.setStyleSheet(self.state[state_discriminator][1]) def set_connected(self): self.connected = True - self.set_view_state('On') + self.set_view_state("On") def set_disconnected(self): self.connected = False - self.set_view_state('Off') + self.set_view_state("Off") def refresh(self) -> None: """Refresh the label """ if self.connected: - self.set_view_state('Ok') + self.set_view_state("Ok") else: - self.set_view_state('Off') + self.set_view_state("Off") diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 0ef28f20f1..1a982a0841 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -78,4 +78,3 @@ def switch_control_mode(self, mode: ControlMode) -> None: def refresh(self, enable_xbox=False) -> None: self.xbox_control_button.setEnabled(enable_xbox) - diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index f9910fc953..328e42e7be 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -67,7 +67,8 @@ def refresh_controller(self) -> None: if self.controller_handler.controller is None: self.controller_status.set_connected() self.controller_handler = ControllerInputHandler(self.proto_unix_io) - else: self.controller_status.set_disconnected() + else: + self.controller_status.set_disconnected() def refresh(self): # check if controller is initialized and set status @@ -94,12 +95,14 @@ def refresh(self): elif self.__control_mode == ControlMode.XBOX: # get the values from handheld controller and send values to proto_unix_io - diagnostics_primitive = self.controller_handler.get_latest_primitive_command() + diagnostics_primitive = ( + self.controller_handler.get_latest_primitive_command() + ) if diagnostics_primitive is not None: logging.debug("Sending diagnostics primitive: " + diagnostics_primitive) self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - else : + else: logging.debug("Sending stop primitive") self.proto_unix_io.send_proto(Primitive, StopPrimitive()) From e9f7ca0e5dd280d3ab1f19e49c47cf50b706b828 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 00:22:48 -0700 Subject: [PATCH 049/138] added todo --- .../thunderscope/robot_diagnostics/controller_diagnostics.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 1e2cdd0704..b918814cbf 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -209,6 +209,7 @@ def set_enabled(self, enabled: bool): self.enabled = enabled +# TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], # ('EV_KEY', 1): [ From 857256ad929b90b8d63876cf154087a6716be8ca Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 08:04:47 -0700 Subject: [PATCH 050/138] bob's the rebuilder --- .../controller_diagnostics.py | 154 ++++++++---------- .../robot_diagnostics/diagnostics_widget.py | 2 +- 2 files changed, 65 insertions(+), 91 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index b918814cbf..1a30393c0f 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -10,6 +10,10 @@ # TODO: change all logging to DEBUG level or remove entirely... +class MoveEventType(Enum): + LINEAR = 1 + ROTATIONAL = 2 + class ControllerInputHandler(object): """ @@ -20,7 +24,7 @@ class ControllerInputHandler(object): # TODO: remove proto_unix_io, and set Motor/Power control as class fields # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.proto_unix_io = proto_unix_io self.enabled = False @@ -42,13 +46,6 @@ def __init__( self.constants = tbots_cpp.create2021RobotConstants() - self.x_vel = 0.0 - self.y_vel = 0.0 - self.ang_vel = 0.0 - self.kick_power = 0.0 - self.dribbler_speed = 0.0 - self.dribbler_enabled = False - self.motor_control = MotorControl() self.power_control = PowerControl() @@ -76,117 +73,95 @@ def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES + controller is not None + and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES ): self.controller = controller break - @staticmethod - def __parse_dribbler_enabled_event_value(value) -> bool: - return value > (XBOX_BUTTON_MAX_RANGE / 2) + def process_move_event_value(self, event_type, event_value) -> None: + if event_type == "ABS_X": + self.motor_control.direct_velocity_control.velocity.x_component_meters = \ + self.__parse_move_event_value(MoveEventType.LINEAR, event_value) + + elif event_type == "ABS_Y": + self.motor_control.direct_velocity_control.velocity.y_component_meters = \ + self.__parse_move_event_value(MoveEventType.LINEAR, event_value) + + elif event_type == "ABS_RX": + self.motor_control.direct_velocity_control.velocity.radians_per_second = \ + self.__parse_move_event_value(MoveEventType.ROTATIONAL, event_value) @staticmethod - def __parse_move_event_value(value, factor): - if abs(value) < (DEADZONE_PERCENTAGE * factor): + def __parse_move_event_value(event_type: MoveEventType, event_value: int) -> int: + factor = MAX_ANGULAR_SPEED_RAD_PER_S \ + if event_type == MoveEventType.ROTATIONAL \ + else MAX_LINEAR_SPEED_METER_PER_S + + if abs(event_value) < (DEADZONE_PERCENTAGE * factor): return 0 else: - return value / (XBOX_MAX_RANGE * factor) + return event_value / (XBOX_MAX_RANGE * factor) - def __parse_dribbler_event_value(self, value): - # can probably combine these 2 parsers but abastracting the +/- is annoying so 2 funcs it is - new_speed = self.dribbler_speed - value * DRIBBLER_STEPPER - if MIN_DRIBBLER_RPM < new_speed < MAX_DRIBBLER_RPM: - return new_speed - else: - return 0 + @staticmethod + def __parse_dribbler_enabled_event_value(value: int) -> bool: + return value > (XBOX_BUTTON_MAX_RANGE / 2) - def __parse_kick_event_value(self, value) -> int: - new_power = self.kick_power + value * POWER_STEPPER + @staticmethod + def __parse_kick_event_value(value) -> int: + new_power = value * POWER_STEPPER if MIN_POWER < new_power < MAX_POWER: return new_power else: return 0 def __process_event(self, event): - logging.debug("Processing event: " + str(event)) + kick_power = 0 + dribbler_enabled = False abs_event = categorize(event) + event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] + + logging.debug("Processing controller event with type " + str(event_type) + + " and with value " + abs_event.event.value) # TODO: bump python version so we can use pattern matching for this if event.type == ecodes.EV_ABS: - # grab the event type - event_t = ecodes.bytype[abs_event.event.type][abs_event.event.code] - logging.debug("Processing joystick event type: " + str(event_t)) - - if event_t == "ABS_X": - self.x_vel = self.__parse_move_event_value( - abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S - ) - - elif event_t == "ABS_Y": - self.y_vel = self.__parse_move_event_value( - abs_event.event.value, MAX_LINEAR_SPEED_METER_PER_S + if event_type in ["ABS_X", "ABS_Y", "ABS_RX"]: + self.process_move_event_value( + event_type, abs_event.event.value ) - elif event_t == "ABS_RX": - self.ang_vel = self.__parse_move_event_value( - abs_event.event.value, MAX_ANGULAR_SPEED_RAD_PER_S - ) - - elif event_t == "ABS_HAT0Y": - self.dribbler_speed = self.__parse_dribbler_event_value( - abs_event.event.value - ) - - elif event_t == "ABS_HAT0X": - self.kick_power = self.__parse_kick_event_value(abs_event.event.value) - - elif event_t == "ABS_RZ": - self.dribbler_enabled = self.__parse_dribbler_enabled_event_value( - abs_event.event.value - ) - - elif event_t == "ABS_Z": - self.dribbler_enabled = self.__parse_dribbler_enabled_event_value( - abs_event.event.value - ) + if event_type == "ABS_HAT0X": + kick_power = self.__parse_kick_event_value( + abs_event.event.value + ) + if event_type == "ABS_RZ" or "ABS_Z": + dribbler_enabled = self.__parse_dribbler_enabled_event_value( + abs_event.event.value + ) - elif event.type == ecodes.EV_KEY: - logging.debug("Processing button event: " + str(event.code)) + # TODO: possible to use `event_type` instead of `event.type` + if event.type == ecodes.EV_KEY: if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: self.power_control.geneva_slot = 1 - self.power_control.chicker.kick_speed_m_per_s = self.kick_power + self.power_control.chicker.kick_speed_m_per_s = kick_power elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: self.power_control.geneva_slot = 1 # TODO: not sure if we should use this value - self.power_control.chicker.chip_distance_meters = self.kick_power - - if event.type in ["ABS_X", "ABS_Y", "ABS_RX"]: - ( - self.motor_control.direct_velocity_control.velocity - ).x_component_meters = self.x_vel - ( - self.motor_control.direct_velocity_control.velocity - ).y_component_meters = self.y_vel - ( - self.motor_control.direct_velocity_control.angular_velocity - ).radians_per_second = self.ang_vel - - self.motor_control.dribbler_speed_rpm = 0 - - # if self.enable_dribbler: - # motor_control.dribbler_speed_rpm = dribbler_speed - # TODO: maybe just use indefinite instead? or - # have setting to turn on 'smooth scrolling' - if self.dribbler_enabled: - self.motor_control.dribbler_speed_rpm = ( - self.constants.indefinite_dribbler_speed_rpm - ) + self.power_control.chicker.chip_distance_meters = kick_power + + # TODO: move to get primitive call + self.motor_control.dribbler_speed_rpm = 0 + + if dribbler_enabled: + self.motor_control.dribbler_speed_rpm = ( + self.constants.indefinite_dribbler_speed_rpm + ) def __event_loop(self): - logging.debug("Starting handheld controller event loop") + logging.debug("Starting handheld controller event handling thread") if self.enabled: for event in self.controller.read_loop(): # TODO: only run loop if self.enabled is set @@ -196,11 +171,11 @@ def __event_loop(self): self.__process_event(event) def close(self): - logging.debug("Closing controller thread") + logging.debug("Closing handheld controller event handling thread") self.__stop_event_thread.set() self.__event_thread.join() - def set_enabled(self, enabled: bool): + def set_controller_enabled(self, enabled: bool): """ Changes the diagnostics input mode for all robots between Xbox and Diagnostics. @@ -208,7 +183,6 @@ def set_enabled(self, enabled: bool): """ self.enabled = enabled - # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 328e42e7be..d267065466 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -109,4 +109,4 @@ def refresh(self): def toggle_control(self, mode: ControlMode): self.__control_mode = mode self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) - self.controller_handler.set_enabled(mode == ControlMode.XBOX) + self.controller_handler.set_controller_enabled(mode == ControlMode.XBOX) From ebb53c13d366dab8fa05cb9115fd0795962f13fd Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 08:05:26 -0700 Subject: [PATCH 051/138] formatting, ofcourse and alas --- .../controller_diagnostics.py | 45 +++++++++++-------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 1a30393c0f..76c08e045d 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -10,6 +10,7 @@ # TODO: change all logging to DEBUG level or remove entirely... + class MoveEventType(Enum): LINEAR = 1 ROTATIONAL = 2 @@ -24,7 +25,7 @@ class ControllerInputHandler(object): # TODO: remove proto_unix_io, and set Motor/Power control as class fields # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.proto_unix_io = proto_unix_io self.enabled = False @@ -73,30 +74,35 @@ def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES + controller is not None + and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES ): self.controller = controller break def process_move_event_value(self, event_type, event_value) -> None: if event_type == "ABS_X": - self.motor_control.direct_velocity_control.velocity.x_component_meters = \ - self.__parse_move_event_value(MoveEventType.LINEAR, event_value) + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( + MoveEventType.LINEAR, event_value + ) elif event_type == "ABS_Y": - self.motor_control.direct_velocity_control.velocity.y_component_meters = \ - self.__parse_move_event_value(MoveEventType.LINEAR, event_value) + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + MoveEventType.LINEAR, event_value + ) elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.velocity.radians_per_second = \ - self.__parse_move_event_value(MoveEventType.ROTATIONAL, event_value) + self.motor_control.direct_velocity_control.velocity.radians_per_second = self.__parse_move_event_value( + MoveEventType.ROTATIONAL, event_value + ) @staticmethod def __parse_move_event_value(event_type: MoveEventType, event_value: int) -> int: - factor = MAX_ANGULAR_SPEED_RAD_PER_S \ - if event_type == MoveEventType.ROTATIONAL \ + factor = ( + MAX_ANGULAR_SPEED_RAD_PER_S + if event_type == MoveEventType.ROTATIONAL else MAX_LINEAR_SPEED_METER_PER_S + ) if abs(event_value) < (DEADZONE_PERCENTAGE * factor): return 0 @@ -122,20 +128,20 @@ def __process_event(self, event): abs_event = categorize(event) event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] - logging.debug("Processing controller event with type " + str(event_type) + - " and with value " + abs_event.event.value) + logging.debug( + "Processing controller event with type " + + str(event_type) + + " and with value " + + abs_event.event.value + ) # TODO: bump python version so we can use pattern matching for this if event.type == ecodes.EV_ABS: if event_type in ["ABS_X", "ABS_Y", "ABS_RX"]: - self.process_move_event_value( - event_type, abs_event.event.value - ) + self.process_move_event_value(event_type, abs_event.event.value) if event_type == "ABS_HAT0X": - kick_power = self.__parse_kick_event_value( - abs_event.event.value - ) + kick_power = self.__parse_kick_event_value(abs_event.event.value) if event_type == "ABS_RZ" or "ABS_Z": dribbler_enabled = self.__parse_dribbler_enabled_event_value( abs_event.event.value @@ -183,6 +189,7 @@ def set_controller_enabled(self, enabled: bool): """ self.enabled = enabled + # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], From 2d883d26a217cd5d49a4da775d62abd58a4b0df3 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 11:18:10 -0700 Subject: [PATCH 052/138] added reminder --- src/software/field_tests/movement_robot_field_test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 75815af427..5381281eda 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -15,6 +15,7 @@ # TODO 2908: Support running this test in both simulator or field mode +# TODO: remove commented out tests # this test can be run either in simulation or on the field # @pytest.mark.parametrize( # "robot_x_destination, robot_y_destination", From e772a0d7fe1e5f70396457234db4123dc170c995 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 11:18:20 -0700 Subject: [PATCH 053/138] back to info level! --- src/software/logger/logger.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/logger/logger.py b/src/software/logger/logger.py index 248336478e..e555223226 100644 --- a/src/software/logger/logger.py +++ b/src/software/logger/logger.py @@ -1,7 +1,7 @@ import logging logging.basicConfig( - level=logging.DEBUG, + level=logging.INFO, format="%(asctime)s - [%(levelname)s] - [%(threadName)s] - %(name)s - (%(filename)s).%(funcName)s(%(lineno)d) - %(message)s", ) From a57fb71e5f290a4a68c416f0f48e2a6b5b3e7684 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 11:53:30 -0700 Subject: [PATCH 054/138] we do a lil cleanup --- src/software/thunderscope/constants.py | 34 ++++---- .../controller_diagnostics.py | 86 ++++++++++--------- 2 files changed, 62 insertions(+), 58 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index a4859c877b..ba5197c614 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -320,31 +320,29 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 +# TODO: field test values class ControllerConstants: # TODO: add some more here by testing available ones - VALID_CONTROLLER_NAMES = ["Microsoft Xbox One X pad"] + # need to add playstation and older versions of xbox + VALID_CONTROLLER_NAMES = [ + "Microsoft Xbox One X pad" + ] + + XBOX_MAX_RANGE = 32768.0 + XBOX_BUTTON_MAX_RANGE = 1024.0 + DEADZONE_PERCENTAGE = 0.30 # TODO: added here for reference and to not haver to switch branches, remove later MAX_LINEAR_SPEED_MPS = 2.0 MIN_LINEAR_SPEED_MPS = -2.0 - MAX_ANGULAR_SPEED_RAD_PER_S = 20 - # TODO: field test values - # MIN_ANGULAR_SPEED_RAD_PER_S = -20 - - MAX_DRIBBLER_RPM = 10000 - MIN_DRIBBLER_RPM = -10000 + MAX_LINEAR_SPEED_METER_PER_S = 2.0 + MAX_ANGULAR_SPEED_RAD_PER_S = 20.0 - MIN_POWER = 1000 - MAX_POWER = 20000 - - XBOX_MAX_RANGE = 32768 - XBOX_BUTTON_MAX_RANGE = 1024 - - DEADZONE_PERCENTAGE = 0.30 + DRIBBLER_STEPPER = 100.0 + DRIBBLER_INDEFINITE_SPEED = -10000.0 - DRIBBLER_STEPPER = 100 - POWER_STEPPER = 100 + POWER_STEPPER = 100.0 + MIN_POWER = 1000.0 + MAX_POWER = 20000.0 - MAX_LINEAR_SPEED_METER_PER_S = 2 - MAX_ANGULAR_SPEED_RAD_PER_S = 10.0 diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 76c08e045d..1b078ac60d 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -1,10 +1,12 @@ import logging +import numpy from evdev import InputDevice, categorize, ecodes, list_devices from threading import Event, Thread import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * +from software.thunderscope.constants import ControllerConstants from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -18,8 +20,8 @@ class MoveEventType(Enum): class ControllerInputHandler(object): """ - This class is responsible for reading from an Xbox controller device and - interpreting the inputs into usable inputs for robot. + This class is responsible for reading from a handheld controller device and + interpreting the inputs into usable inputs for the robot. """ # TODO: remove proto_unix_io, and set Motor/Power control as class fields @@ -47,6 +49,7 @@ def __init__( self.constants = tbots_cpp.create2021RobotConstants() + # Fields for holding the diagnostics types that get sent self.motor_control = MotorControl() self.power_control = PowerControl() @@ -96,33 +99,9 @@ def process_move_event_value(self, event_type, event_value) -> None: MoveEventType.ROTATIONAL, event_value ) - @staticmethod - def __parse_move_event_value(event_type: MoveEventType, event_value: int) -> int: - factor = ( - MAX_ANGULAR_SPEED_RAD_PER_S - if event_type == MoveEventType.ROTATIONAL - else MAX_LINEAR_SPEED_METER_PER_S - ) - - if abs(event_value) < (DEADZONE_PERCENTAGE * factor): - return 0 - else: - return event_value / (XBOX_MAX_RANGE * factor) - - @staticmethod - def __parse_dribbler_enabled_event_value(value: int) -> bool: - return value > (XBOX_BUTTON_MAX_RANGE / 2) - - @staticmethod - def __parse_kick_event_value(value) -> int: - new_power = value * POWER_STEPPER - if MIN_POWER < new_power < MAX_POWER: - return new_power - else: - return 0 - def __process_event(self, event): - kick_power = 0 + kick_power = 0.0 + dribbler_speed = 0.0 dribbler_enabled = False abs_event = categorize(event) @@ -140,12 +119,18 @@ def __process_event(self, event): if event_type in ["ABS_X", "ABS_Y", "ABS_RX"]: self.process_move_event_value(event_type, abs_event.event.value) + # TODO: can enable smooth scrolling for dribbler by chekcing for "ABS_HAT0Y" event if event_type == "ABS_HAT0X": - kick_power = self.__parse_kick_event_value(abs_event.event.value) + dribbler_speed = self.__parse_kick_event_value(abs_event.event.value) + + if event_type == "ABS_HAT0Y": + kick_power = self.__parse_dribble_event_value(abs_event.event.value) + if event_type == "ABS_RZ" or "ABS_Z": - dribbler_enabled = self.__parse_dribbler_enabled_event_value( + if self.__parse_dribbler_enabled_event_value( abs_event.event.value - ) + ): + self.motor_control.dribbler_speed_rpm = dribbler_speed # TODO: possible to use `event_type` instead of `event.type` if event.type == ecodes.EV_KEY: @@ -155,22 +140,43 @@ def __process_event(self, event): elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: self.power_control.geneva_slot = 1 - # TODO: not sure if we should use this value self.power_control.chicker.chip_distance_meters = kick_power - # TODO: move to get primitive call - self.motor_control.dribbler_speed_rpm = 0 + @staticmethod + def __parse_move_event_value(event_type: MoveEventType, event_value: float) -> float: + factor = ( + ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S + if event_type == MoveEventType.ROTATIONAL + else ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S + if event_type == MoveEventType.LINEAR + else 1.0 + ) - if dribbler_enabled: - self.motor_control.dribbler_speed_rpm = ( - self.constants.indefinite_dribbler_speed_rpm - ) + if abs(event_value) < (ControllerConstants.DEADZONE_PERCENTAGE * factor): + return 0 + else: + return numpy.clip(event_value, 0, ControllerConstants.XBOX_MAX_RANGE * factor) + + @staticmethod + def __parse_dribbler_enabled_event_value(value: float) -> bool: + return value > (ControllerConstants.XBOX_BUTTON_MAX_RANGE / 2.0) + + @staticmethod + def __parse_dribble_event_value(value: float) -> float: + return numpy.clip(value * ControllerConstants.DRIBBLER_STEPPER, + 0, + ControllerConstants.DRIBBLER_INDEFINITE_SPEED) + + @staticmethod + def __parse_kick_event_value(value: float) -> float: + return numpy.clip(value * ControllerConstants.POWER_STEPPER, + ControllerConstants.MIN_POWER, + ControllerConstants.MAX_POWER) def __event_loop(self): - logging.debug("Starting handheld controller event handling thread") + logging.debug("Starting handheld controller event handling loop") if self.enabled: for event in self.controller.read_loop(): - # TODO: only run loop if self.enabled is set if self.__stop_event_thread.isSet(): return else: From 0b8f5db73514383e7bc8754a0dc22d7b12530506 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 11:54:05 -0700 Subject: [PATCH 055/138] formatting --- src/software/thunderscope/constants.py | 5 +--- .../controller_diagnostics.py | 29 +++++++++++-------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index ba5197c614..21189f46db 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -324,9 +324,7 @@ class TrailValues: class ControllerConstants: # TODO: add some more here by testing available ones # need to add playstation and older versions of xbox - VALID_CONTROLLER_NAMES = [ - "Microsoft Xbox One X pad" - ] + VALID_CONTROLLER_NAMES = ["Microsoft Xbox One X pad"] XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 @@ -345,4 +343,3 @@ class ControllerConstants: POWER_STEPPER = 100.0 MIN_POWER = 1000.0 MAX_POWER = 20000.0 - diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 1b078ac60d..d2bc20df31 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -102,7 +102,6 @@ def process_move_event_value(self, event_type, event_value) -> None: def __process_event(self, event): kick_power = 0.0 dribbler_speed = 0.0 - dribbler_enabled = False abs_event = categorize(event) event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] @@ -127,9 +126,7 @@ def __process_event(self, event): kick_power = self.__parse_dribble_event_value(abs_event.event.value) if event_type == "ABS_RZ" or "ABS_Z": - if self.__parse_dribbler_enabled_event_value( - abs_event.event.value - ): + if self.__parse_dribbler_enabled_event_value(abs_event.event.value): self.motor_control.dribbler_speed_rpm = dribbler_speed # TODO: possible to use `event_type` instead of `event.type` @@ -143,7 +140,9 @@ def __process_event(self, event): self.power_control.chicker.chip_distance_meters = kick_power @staticmethod - def __parse_move_event_value(event_type: MoveEventType, event_value: float) -> float: + def __parse_move_event_value( + event_type: MoveEventType, event_value: float + ) -> float: factor = ( ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S if event_type == MoveEventType.ROTATIONAL @@ -155,7 +154,9 @@ def __parse_move_event_value(event_type: MoveEventType, event_value: float) -> f if abs(event_value) < (ControllerConstants.DEADZONE_PERCENTAGE * factor): return 0 else: - return numpy.clip(event_value, 0, ControllerConstants.XBOX_MAX_RANGE * factor) + return numpy.clip( + event_value, 0, ControllerConstants.XBOX_MAX_RANGE * factor + ) @staticmethod def __parse_dribbler_enabled_event_value(value: float) -> bool: @@ -163,15 +164,19 @@ def __parse_dribbler_enabled_event_value(value: float) -> bool: @staticmethod def __parse_dribble_event_value(value: float) -> float: - return numpy.clip(value * ControllerConstants.DRIBBLER_STEPPER, - 0, - ControllerConstants.DRIBBLER_INDEFINITE_SPEED) + return numpy.clip( + value * ControllerConstants.DRIBBLER_STEPPER, + 0, + ControllerConstants.DRIBBLER_INDEFINITE_SPEED, + ) @staticmethod def __parse_kick_event_value(value: float) -> float: - return numpy.clip(value * ControllerConstants.POWER_STEPPER, - ControllerConstants.MIN_POWER, - ControllerConstants.MAX_POWER) + return numpy.clip( + value * ControllerConstants.POWER_STEPPER, + ControllerConstants.MIN_POWER, + ControllerConstants.MAX_POWER, + ) def __event_loop(self): logging.debug("Starting handheld controller event handling loop") From 28d314ac643fa1046d29f83d5d6b1ba6227cb433 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 27 Mar 2024 16:52:25 -0700 Subject: [PATCH 056/138] debugging --- src/proto/primitive.proto | 2 +- src/software/thunderscope/constants.py | 5 +- .../thunderscope/robot_communication.py | 3 + .../robot_diagnostics/chicker_widget.py | 45 +++++++-------- .../controller_diagnostics.py | 56 ++++++++++++------- .../robot_diagnostics/diagnostics_widget.py | 11 +++- 6 files changed, 71 insertions(+), 51 deletions(-) diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index 4a24f290ff..256f1770f0 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -167,7 +167,7 @@ message MotorControl DirectVelocityControl direct_velocity_control = 2; } - int32 dribbler_speed_rpm = 4; + float dribbler_speed_rpm = 4; } message DirectControlPrimitive diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 21189f46db..ac459f016f 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -324,7 +324,10 @@ class TrailValues: class ControllerConstants: # TODO: add some more here by testing available ones # need to add playstation and older versions of xbox - VALID_CONTROLLER_NAMES = ["Microsoft Xbox One X pad"] + VALID_CONTROLLER_NAMES = [ + 'Microsoft Xbox One X pad', + 'Microsoft X-Box 360 pad' + ] XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index adfdf04278..4195e0d66a 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -355,6 +355,9 @@ def __exit__(self, type, value, traceback) -> None: self.close_for_fullsystem() + # TODO: if `--disable_communication` is set, this throws and error on exit + # if the close order is changed, then message swaps to + # ... shutdown UDP socket for TbotsProto::RobotStatus (<- used to be Log). The boost ... self.receive_robot_log.close() self.receive_robot_status.close() self.run_primitive_set_thread.join() diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 11c00c652b..970b15eff1 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -33,15 +33,28 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(ChickerWidget, self).__init__() + self.proto_unix_io = proto_unix_io self.power_control = PowerControl() + # initial values + self.power_value = 1 + vbox_layout = QVBoxLayout() - self.radio_buttons_group = QButtonGroup() - self.proto_unix_io = proto_unix_io + self.setLayout(vbox_layout) - # Initialising the buttons + # Power slider for kicking & chipping + ( + self.power_slider_layout, + self.power_slider, + self.power_label, + ) = common_widgets.create_slider( + "Power (m/s) (Chipper power is fixed)", 1, 10, 1 + ) + vbox_layout.addLayout(self.power_slider_layout) - # push button group box + # Initializing kick & chip buttons + self.button_clickable_map = {"no_auto": True, "auto_kick": True, "auto_chip": True} + self.radio_buttons_group = QButtonGroup() self.push_button_box, self.push_buttons = common_widgets.create_buttons( ["Kick", "Chip"] ) @@ -50,7 +63,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout.addWidget(self.push_button_box) - # radio button group box + # Initializing auto kick & chip buttons self.radio_button_box, self.radio_buttons = common_widgets.create_radio( ["No Auto", "Auto Kick", "Auto Chip"], self.radio_buttons_group ) @@ -88,26 +101,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout.addWidget(self.radio_button_box) - # sliders - - ( - self.power_slider_layout, - self.power_slider, - self.power_label, - ) = common_widgets.create_slider( - "Power (m/s) (Chipper power is fixed)", 1, 10, 1 - ) - vbox_layout.addLayout(self.power_slider_layout) - - self.setLayout(vbox_layout) - - # to manage the state of radio buttons - to make sure message is only sent once - self.radio_checkable = {"no_auto": True, "auto_kick": True, "auto_chip": True} - - # initial values - self.geneva_value = 3 - self.power_value = 1 - def send_command_and_timeout(self, command: ChickerCommandMode) -> None: """ If buttons are enabled, sends a Kick command and disables buttons @@ -167,7 +160,7 @@ def send_command(self, command: ChickerCommandMode) -> None: power_value = self.power_slider.value() power_control = PowerControl() - power_control.geneva_slot = self.geneva_value + power_control.geneva_slot = 3 # sends kick, chip, autokick, or autchip primitive if command == ChickerCommandMode.KICK: diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index d2bc20df31..e927e3ddc9 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -27,7 +27,7 @@ class ControllerInputHandler(object): # TODO: remove proto_unix_io, and set Motor/Power control as class fields # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, proto_unix_io: ProtoUnixIO, ): self.proto_unix_io = proto_unix_io self.enabled = False @@ -35,11 +35,12 @@ def __init__( self.__setup_controller() - if self.controller is not None: + if self.controller_initialized(): + self.enabled = True logging.debug( - "Initializing controller " - + self.controller.info.__str__() - + " and device path location: " + "Initialized handheld controller " + + "\"" + self.controller.name + "\"" + + " and located at path: " + self.controller.path ) @@ -53,11 +54,13 @@ def __init__( self.motor_control = MotorControl() self.power_control = PowerControl() - else: + + elif self.controller_initialized(): + logging.debug( "Tried to initialize a handheld controller from list available devices:" ) - logging.debug(list_devices()) + logging.debug(list(map(lambda device: InputDevice(device).name, list_devices()))) logging.debug( "Could not initialize a handheld controller device - check USB connections" ) @@ -71,14 +74,16 @@ def get_latest_primitive_command(self): return None def controller_initialized(self): + # if self.controller is not None: + # logging.debug(self.controller.read_one()) return self.controller is not None def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES + controller is not None + and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES ): self.controller = controller break @@ -95,11 +100,15 @@ def process_move_event_value(self, event_type, event_value) -> None: ) elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.velocity.radians_per_second = self.__parse_move_event_value( - MoveEventType.ROTATIONAL, event_value + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.__parse_move_event_value( + MoveEventType.ROTATIONAL, event_value + ) ) def __process_event(self, event): + logging.debug(event) + kick_power = 0.0 dribbler_speed = 0.0 @@ -110,7 +119,7 @@ def __process_event(self, event): "Processing controller event with type " + str(event_type) + " and with value " - + abs_event.event.value + + str(abs_event.event.value) ) # TODO: bump python version so we can use pattern matching for this @@ -127,7 +136,7 @@ def __process_event(self, event): if event_type == "ABS_RZ" or "ABS_Z": if self.__parse_dribbler_enabled_event_value(abs_event.event.value): - self.motor_control.dribbler_speed_rpm = dribbler_speed + self.motor_control.dribbler_speed_rpm = int(dribbler_speed) # TODO: possible to use `event_type` instead of `event.type` if event.type == ecodes.EV_KEY: @@ -141,7 +150,7 @@ def __process_event(self, event): @staticmethod def __parse_move_event_value( - event_type: MoveEventType, event_value: float + event_type: MoveEventType, event_value: float ) -> float: factor = ( ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S @@ -181,11 +190,19 @@ def __parse_kick_event_value(value: float) -> float: def __event_loop(self): logging.debug("Starting handheld controller event handling loop") if self.enabled: - for event in self.controller.read_loop(): - if self.__stop_event_thread.isSet(): - return - else: - self.__process_event(event) + try: + for event in self.controller.read_loop(): + if self.__stop_event_thread.isSet(): + return + else: + self.__process_event(event) + except OSError as ose: + logging.debug('Caught an OSError while reading handheld controller event loop!') + logging.debug('Error message: ' + str(ose)) + logging.debug('Check physical handheld controller USB connection') + except Exception as e: + logging.critical('Caught an unexpected error while reading handheld controller event loop!') + logging.critical('Error message: ' + str(e)) def close(self): logging.debug("Closing handheld controller event handling thread") @@ -200,7 +217,6 @@ def set_controller_enabled(self, enabled: bool): """ self.enabled = enabled - # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index d267065466..0e792f0c58 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -64,11 +64,16 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def refresh_controller(self) -> None: logging.debug("Attempting to reinitialize handheld controller...") - if self.controller_handler.controller is None: - self.controller_status.set_connected() + if not self.controller_handler.controller_initialized(): self.controller_handler = ControllerInputHandler(self.proto_unix_io) + if self.controller_handler.controller_initialized(): + logging.debug("Successfully reinitialized handheld controller") + self.controller_status.set_connected() + else: + logging.debug("Failed to reinitialize handheld controller") + self.controller_status.set_disconnected() else: - self.controller_status.set_disconnected() + logging.debug("Handheld controller is initialized and connected...") def refresh(self): # check if controller is initialized and set status From 54ceb4d1281cb197ecc37bd2e0a61ab08ca052f9 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 06:26:05 -0700 Subject: [PATCH 057/138] removed unused constants --- src/software/thunderscope/constants.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index ac459f016f..215f327734 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -320,11 +320,8 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 -# TODO: field test values class ControllerConstants: - # TODO: add some more here by testing available ones - # need to add playstation and older versions of xbox - VALID_CONTROLLER_NAMES = [ + VALID_CONTROLLERS = [ 'Microsoft Xbox One X pad', 'Microsoft X-Box 360 pad' ] @@ -333,10 +330,6 @@ class ControllerConstants: XBOX_BUTTON_MAX_RANGE = 1024.0 DEADZONE_PERCENTAGE = 0.30 - # TODO: added here for reference and to not haver to switch branches, remove later - MAX_LINEAR_SPEED_MPS = 2.0 - MIN_LINEAR_SPEED_MPS = -2.0 - MAX_LINEAR_SPEED_METER_PER_S = 2.0 MAX_ANGULAR_SPEED_RAD_PER_S = 20.0 From ee6899c05bf09668990aa8b56c6c6f341f31aa31 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 06:26:14 -0700 Subject: [PATCH 058/138] not even needed --- .../robot_diagnostics/controller_communication.py | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 src/software/thunderscope/robot_diagnostics/controller_communication.py diff --git a/src/software/thunderscope/robot_diagnostics/controller_communication.py b/src/software/thunderscope/robot_diagnostics/controller_communication.py deleted file mode 100644 index 73de4904a4..0000000000 --- a/src/software/thunderscope/robot_diagnostics/controller_communication.py +++ /dev/null @@ -1,8 +0,0 @@ -from software.thunderscope.robot_diagnostics.controller_view import ControllerStatusView - - -class ControllerConnectionHandler(QWidget): - def __enter__(self) -> None: - super(ControllerConnectionHandler, self).__init__() - - self.controller_view = ControllerStatusView() From 904f2451f23acc61d682bd0cd30a31f14afcf29e Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 06:26:42 -0700 Subject: [PATCH 059/138] cleanup --- .../thunderscope/robot_communication.py | 56 ++++--- .../controller_diagnostics.py | 141 +++++++++--------- .../robot_diagnostics/controller_view.py | 1 - .../diagnostics_input_widget.py | 1 - .../robot_diagnostics/diagnostics_widget.py | 17 ++- .../drive_and_dribbler_widget.py | 1 - 6 files changed, 106 insertions(+), 111 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 4195e0d66a..dcbc241423 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -17,7 +17,6 @@ EstopMode, ) -# TODO: should use MAX_ROBOT_IDS_PER_SIDE instead or DIV_A?! NUM_ROBOTS = MAX_ROBOT_IDS_PER_SIDE @@ -50,13 +49,6 @@ def __init__( self.current_proto_unix_io = current_proto_unix_io self.multicast_channel = str(multicast_channel) self.interface = interface - - # TODO: move estop state management out of robot_communication, - # separate PR, reuse controller code possibly - self.estop_mode = estop_mode - self.estop_path = estop_path - self.estop_buadrate = estop_baudrate - self.running = False self.primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) @@ -79,17 +71,20 @@ def __init__( ) # map of robot id to the individual control mode - self.robot_control_mode_map: dict[int, IndividualRobotMode] = { - robot_id: IndividualRobotMode.NONE - for robot_id in list(range(0, NUM_ROBOTS)) - } - - # map of robot id to the number of times to send a estop. - # each robot has it's own separate countdown - # TODO come up with better name - self.robot_stop_primitive_count_map: dict[int, int] = { - rid: 0 for rid in range(NUM_ROBOTS) - } + self.robot_control_mode_map: dict[int, IndividualRobotMode] = {} + + # map of robot id to the number of times to send a stop primitive + self.robot_stop_primitive_count_map: dict[int, int] = {} + + # load control mode and stop primitive maps with default values + for robot_id in range(NUM_ROBOTS): + self.robot_control_mode_map[robot_id] = IndividualRobotMode.NONE + self.robot_stop_primitive_count_map[robot_id] = 0 + + # TODO: (#3174): move estop state management out of robot_communication + self.estop_mode = estop_mode + self.estop_path = estop_path + self.estop_buadrate = estop_baudrate # initialising the estop # tries to access a plugged in estop. if not found, throws an exception @@ -157,18 +152,20 @@ def toggle_keyboard_estop(self) -> None: def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ - Changes the input mode for a robot between None, Manual, or AI - If changing from anything to None, add robot to disconnected map - So we can send multiple stop primitives to make sure it stops + Changes the input mode for a robot between NONE, MANUAL, or AI + If changing from MANUAL OR AI to NONE, add robot id to stop primitive + map so that multiple stop primitives are sent - safety number one priority :param mode: the mode of input for this robot's primitives :param robot_id: the id of the robot whose mode we're changing """ + self.robot_control_mode_map[robot_id] = mode - if mode == IndividualRobotMode.NONE: - self.robot_stop_primitive_count_map[robot_id] = NUM_TIMES_SEND_STOP - else: - self.robot_stop_primitive_count_map[robot_id] = 0 + self.robot_stop_primitive_count_map[robot_id] = ( + NUM_TIMES_SEND_STOP + if mode == IndividualRobotMode.NONE + else 0 + ) def __send_estop_state(self) -> None: """ @@ -312,6 +309,9 @@ def __enter__(self) -> "self": """ # Create the multicast listeners + # TODO (#3172): `--disable_communication` still requires an interface to be configured + # error thrown on string concatenation of `self.interface`, + # since it is NoneType object if not passed as argument to tscope... self.receive_robot_status = tbots_cpp.RobotStatusProtoListener( self.multicast_channel + "%" + self.interface, ROBOT_STATUS_PORT, @@ -355,9 +355,7 @@ def __exit__(self, type, value, traceback) -> None: self.close_for_fullsystem() - # TODO: if `--disable_communication` is set, this throws and error on exit - # if the close order is changed, then message swaps to - # ... shutdown UDP socket for TbotsProto::RobotStatus (<- used to be Log). The boost ... + # TODO (#3172): if `--disable_communication` is set, this throws a runtime error on exit self.receive_robot_log.close() self.receive_robot_status.close() self.run_primitive_set_thread.join() diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index e927e3ddc9..ae467cd173 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -7,10 +7,6 @@ from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import ControllerConstants -from software.thunderscope.proto_unix_io import ProtoUnixIO - - -# TODO: change all logging to DEBUG level or remove entirely... class MoveEventType(Enum): @@ -21,22 +17,18 @@ class MoveEventType(Enum): class ControllerInputHandler(object): """ This class is responsible for reading from a handheld controller device and - interpreting the inputs into usable inputs for the robot. + interpreting the device inputs into usable inputs for the robots. """ - - # TODO: remove proto_unix_io, and set Motor/Power control as class fields - # TODO: add class init wrapper for easier handling of controller connection def __init__( - self, proto_unix_io: ProtoUnixIO, + self, ): - self.proto_unix_io = proto_unix_io self.enabled = False self.controller = None self.__setup_controller() if self.controller_initialized(): - self.enabled = True + logging.debug( "Initialized handheld controller " + "\"" + self.controller.name + "\"" @@ -44,19 +36,23 @@ def __init__( + self.controller.path ) + self.constants = tbots_cpp.create2021RobotConstants() + + # event that is used to stop the controller event loop self.__stop_event_thread = Event() - self.__event_thread = Thread(target=self.__event_loop, daemon=True) - self.__event_thread.start() - self.constants = tbots_cpp.create2021RobotConstants() + # thread that is responisble for reading controller events + self.__event_thread = Thread(target=self.__event_loop, daemon=True) - # Fields for holding the diagnostics types that get sent + # fields for holding the diagnostics types that get sent self.motor_control = MotorControl() self.power_control = PowerControl() + # start reading controller events + self.enabled = True + self.__event_thread.start() elif self.controller_initialized(): - logging.debug( "Tried to initialize a handheld controller from list available devices:" ) @@ -65,30 +61,59 @@ def __init__( "Could not initialize a handheld controller device - check USB connections" ) + def controller_initialized(self) -> bool: + return self.controller is not None + + def set_controller_enabled(self, enabled: bool): + """ + Changes the diagnostics input mode for all robots between Xbox and Diagnostics. + + :param enabled: to which state to set controller enabled. + """ + self.enabled = enabled + def get_latest_primitive_command(self): if self.controller_initialized(): return DirectControlPrimitive( - motor_control=self.motor_control, power_control=self.power_control, + motor_control=self.motor_control, + power_control=self.power_control, ) else: return None - def controller_initialized(self): - # if self.controller is not None: - # logging.debug(self.controller.read_one()) - return self.controller is not None - def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) if ( controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLER_NAMES + and controller.name in ControllerConstants.VALID_CONTROLLERS ): self.controller = controller break - def process_move_event_value(self, event_type, event_value) -> None: + def close(self): + logging.debug("Closing handheld controller event handling thread") + self.__stop_event_thread.set() + self.__event_thread.join() + + def __event_loop(self): + logging.debug("Starting handheld controller event handling loop") + if self.enabled: + try: + for event in self.controller.read_loop(): + if self.__stop_event_thread.isSet(): + return + else: + self.__process_event(event) + except OSError as ose: + logging.debug('Caught an OSError while reading handheld controller event loop!') + logging.debug('Error message: ' + str(ose)) + logging.debug('Check physical handheld controller USB connection') + except Exception as e: + logging.critical('Caught an unexpected error while reading handheld controller event loop!') + logging.critical('Error message: ' + str(e)) + + def __process_move_event_value(self, event_type, event_value) -> None: if event_type == "ABS_X": self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( MoveEventType.LINEAR, event_value @@ -107,27 +132,29 @@ def process_move_event_value(self, event_type, event_value) -> None: ) def __process_event(self, event): - logging.debug(event) - kick_power = 0.0 dribbler_speed = 0.0 abs_event = categorize(event) event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] - logging.debug( - "Processing controller event with type " - + str(event_type) - + " and with value " - + str(abs_event.event.value) - ) - - # TODO: bump python version so we can use pattern matching for this + # # TODO (#3165): Use trace level logging here + # logging.debug( + # "Processing controller event with type " + # + str(event_type) + # + " and with value " + # + str(abs_event.event.value) + # ) + + # TODO (#3175): new python versions have a pattern matching, + # that could be useful for handling lots of if statements. + # Some documentation for future: + # https://peps.python.org/pep-0636/ + # https://docs.python.org/3/reference/compound_stmts.html#match if event.type == ecodes.EV_ABS: if event_type in ["ABS_X", "ABS_Y", "ABS_RX"]: - self.process_move_event_value(event_type, abs_event.event.value) + self.__process_move_event_value(event_type, abs_event.event.value) - # TODO: can enable smooth scrolling for dribbler by chekcing for "ABS_HAT0Y" event if event_type == "ABS_HAT0X": dribbler_speed = self.__parse_kick_event_value(abs_event.event.value) @@ -136,16 +163,16 @@ def __process_event(self, event): if event_type == "ABS_RZ" or "ABS_Z": if self.__parse_dribbler_enabled_event_value(abs_event.event.value): - self.motor_control.dribbler_speed_rpm = int(dribbler_speed) + self.motor_control.dribbler_speed_rpm = float(dribbler_speed) - # TODO: possible to use `event_type` instead of `event.type` if event.type == ecodes.EV_KEY: - if event.code == ecodes.ecodes["BTN_A"] and event.value == 1: - self.power_control.geneva_slot = 1 + # TODO: try testing event_type instead of checking in `ecodes.ecodes` map + if event.code == ecodes.ecodes["BTN_A"] and event.value == 1.0: + self.power_control.geneva_slot = 3.0 self.power_control.chicker.kick_speed_m_per_s = kick_power - elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1: - self.power_control.geneva_slot = 1 + elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1.0: + self.power_control.geneva_slot = 3.0 self.power_control.chicker.chip_distance_meters = kick_power @staticmethod @@ -187,36 +214,6 @@ def __parse_kick_event_value(value: float) -> float: ControllerConstants.MAX_POWER, ) - def __event_loop(self): - logging.debug("Starting handheld controller event handling loop") - if self.enabled: - try: - for event in self.controller.read_loop(): - if self.__stop_event_thread.isSet(): - return - else: - self.__process_event(event) - except OSError as ose: - logging.debug('Caught an OSError while reading handheld controller event loop!') - logging.debug('Error message: ' + str(ose)) - logging.debug('Check physical handheld controller USB connection') - except Exception as e: - logging.critical('Caught an unexpected error while reading handheld controller event loop!') - logging.critical('Error message: ' + str(e)) - - def close(self): - logging.debug("Closing handheld controller event handling thread") - self.__stop_event_thread.set() - self.__event_thread.join() - - def set_controller_enabled(self, enabled: bool): - """ - Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - - :param enabled: to which state to set controller enabled. - """ - self.enabled = enabled - # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/controller_view.py b/src/software/thunderscope/robot_diagnostics/controller_view.py index 06259a248f..6b5fdade2f 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_view.py @@ -15,7 +15,6 @@ class ControllerStatusView(QLabel): def __init__(self) -> None: super().__init__() - # TODO align text center for status self.state: dict[str, (str, str)] = { "On": ( "Handheld Controller is Connected & Initialized", diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 1a982a0841..37b5bcf3b3 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -25,7 +25,6 @@ class DiagnosticsInputModeWidget(QWidget): """ # Signal to indicate if manual controls should be disabled based on boolean parameter - # TODO: signal logic is flipped... toggle_controls_signal = pyqtSignal(bool) def __init__(self, toggle_controls_signal) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 0e792f0c58..ef794ccc7c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -48,7 +48,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.controller_refresh_button = QPushButton() self.controller_refresh_button.setText("Re-initialize Handheld Controller") - self.controller_refresh_button.clicked.connect(self.refresh_controller) + self.controller_refresh_button.clicked.connect(self.__refresh_controller) self.controller_status = ControllerStatusView() @@ -62,7 +62,12 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) - def refresh_controller(self) -> None: + def toggle_control(self, mode: ControlMode): + self.__control_mode = mode + self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) + self.controller_handler.set_controller_enabled(mode == ControlMode.XBOX) + + def __refresh_controller(self) -> None: logging.debug("Attempting to reinitialize handheld controller...") if not self.controller_handler.controller_initialized(): self.controller_handler = ControllerInputHandler(self.proto_unix_io) @@ -88,7 +93,6 @@ def refresh(self): self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() - # TODO: throws error on exit diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( motor_control=self.drive_dribbler_widget.motor_control, @@ -111,7 +115,6 @@ def refresh(self): logging.debug("Sending stop primitive") self.proto_unix_io.send_proto(Primitive, StopPrimitive()) - def toggle_control(self, mode: ControlMode): - self.__control_mode = mode - self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) - self.controller_handler.set_controller_enabled(mode == ControlMode.XBOX) + # TODO: ensure this is actually called and closed properly + def close(self): + self.controller_handler.close() diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index c9673aa1d5..f533f33b5e 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -30,7 +30,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) - # TODO move dribber control to chicker..., makes more sense there self.enabled = True From 1f66789d33132bbcc831becb0e200a2a25f4f03c Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 06:26:53 -0700 Subject: [PATCH 060/138] bump evdev to latest --- environment_setup/ubuntu20_requirements.txt | 2 +- environment_setup/ubuntu22_requirements.txt | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 53e6dd2562..e2ead28c8e 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -15,4 +15,4 @@ PyOpenGL==3.1.6 PyOpenGL-accelerate==3.1.5 psutil==5.9.0 numpy==1.24.4 -evdev==1.6.1 +evdev==1.7.0 diff --git a/environment_setup/ubuntu22_requirements.txt b/environment_setup/ubuntu22_requirements.txt index b50f3dd7e8..13cd5283a0 100644 --- a/environment_setup/ubuntu22_requirements.txt +++ b/environment_setup/ubuntu22_requirements.txt @@ -14,3 +14,4 @@ qt-material==2.12 PyOpenGL==3.1.6 psutil==5.9.0 numpy==1.24.4 +evdev==1.7.0 From 6874cae4991e6565b64d92e7748c0526b6af7fe2 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 06:28:17 -0700 Subject: [PATCH 061/138] formatting --- src/software/thunderscope/constants.py | 5 +-- .../thunderscope/robot_communication.py | 4 +- .../robot_diagnostics/chicker_widget.py | 6 ++- .../controller_diagnostics.py | 45 ++++++++++--------- 4 files changed, 32 insertions(+), 28 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 215f327734..f27675605f 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -321,10 +321,7 @@ class TrailValues: class ControllerConstants: - VALID_CONTROLLERS = [ - 'Microsoft Xbox One X pad', - 'Microsoft X-Box 360 pad' - ] + VALID_CONTROLLERS = ["Microsoft Xbox One X pad", "Microsoft X-Box 360 pad"] XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index dcbc241423..f0ccc341fc 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -162,9 +162,7 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): self.robot_control_mode_map[robot_id] = mode self.robot_stop_primitive_count_map[robot_id] = ( - NUM_TIMES_SEND_STOP - if mode == IndividualRobotMode.NONE - else 0 + NUM_TIMES_SEND_STOP if mode == IndividualRobotMode.NONE else 0 ) def __send_estop_state(self) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 970b15eff1..2a0565d198 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -53,7 +53,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout.addLayout(self.power_slider_layout) # Initializing kick & chip buttons - self.button_clickable_map = {"no_auto": True, "auto_kick": True, "auto_chip": True} + self.button_clickable_map = { + "no_auto": True, + "auto_kick": True, + "auto_chip": True, + } self.radio_buttons_group = QButtonGroup() self.push_button_box, self.push_buttons = common_widgets.create_buttons( ["Kick", "Chip"] diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index ae467cd173..b8e3e9132a 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -19,9 +19,8 @@ class ControllerInputHandler(object): This class is responsible for reading from a handheld controller device and interpreting the device inputs into usable inputs for the robots. """ - def __init__( - self, - ): + + def __init__(self,): self.enabled = False self.controller = None @@ -31,7 +30,9 @@ def __init__( logging.debug( "Initialized handheld controller " - + "\"" + self.controller.name + "\"" + + '"' + + self.controller.name + + '"' + " and located at path: " + self.controller.path ) @@ -41,7 +42,7 @@ def __init__( # event that is used to stop the controller event loop self.__stop_event_thread = Event() - # thread that is responisble for reading controller events + # thread that is responsible for reading controller events self.__event_thread = Thread(target=self.__event_loop, daemon=True) # fields for holding the diagnostics types that get sent @@ -56,7 +57,9 @@ def __init__( logging.debug( "Tried to initialize a handheld controller from list available devices:" ) - logging.debug(list(map(lambda device: InputDevice(device).name, list_devices()))) + logging.debug( + list(map(lambda device: InputDevice(device).name, list_devices())) + ) logging.debug( "Could not initialize a handheld controller device - check USB connections" ) @@ -75,8 +78,7 @@ def set_controller_enabled(self, enabled: bool): def get_latest_primitive_command(self): if self.controller_initialized(): return DirectControlPrimitive( - motor_control=self.motor_control, - power_control=self.power_control, + motor_control=self.motor_control, power_control=self.power_control, ) else: return None @@ -85,8 +87,8 @@ def __setup_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLERS + controller is not None + and controller.name in ControllerConstants.VALID_CONTROLLERS ): self.controller = controller break @@ -106,12 +108,16 @@ def __event_loop(self): else: self.__process_event(event) except OSError as ose: - logging.debug('Caught an OSError while reading handheld controller event loop!') - logging.debug('Error message: ' + str(ose)) - logging.debug('Check physical handheld controller USB connection') + logging.debug( + "Caught an OSError while reading handheld controller event loop!" + ) + logging.debug("Error message: " + str(ose)) + logging.debug("Check physical handheld controller USB connection") except Exception as e: - logging.critical('Caught an unexpected error while reading handheld controller event loop!') - logging.critical('Error message: ' + str(e)) + logging.critical( + "Caught an unexpected error while reading handheld controller event loop!" + ) + logging.critical("Error message: " + str(e)) def __process_move_event_value(self, event_type, event_value) -> None: if event_type == "ABS_X": @@ -125,10 +131,8 @@ def __process_move_event_value(self, event_type, event_value) -> None: ) elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.__parse_move_event_value( - MoveEventType.ROTATIONAL, event_value - ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( + MoveEventType.ROTATIONAL, event_value ) def __process_event(self, event): @@ -177,7 +181,7 @@ def __process_event(self, event): @staticmethod def __parse_move_event_value( - event_type: MoveEventType, event_value: float + event_type: MoveEventType, event_value: float ) -> float: factor = ( ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S @@ -214,6 +218,7 @@ def __parse_kick_event_value(value: float) -> float: ControllerConstants.MAX_POWER, ) + # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], From b5fc9db738c7627dee572eea61b5d970d37c0c67 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 12:34:24 -0700 Subject: [PATCH 062/138] refactoring and working locally --- .../thunderscope/robot_diagnostics/BUILD | 6 +- .../controller_diagnostics.py | 119 +++++++++++------- ...ller_view.py => controller_status_view.py} | 36 +++--- .../diagnostics_input_widget.py | 48 +++---- .../robot_diagnostics/diagnostics_widget.py | 97 +++++++------- .../drive_and_dribbler_widget.py | 97 +++++++------- 6 files changed, 216 insertions(+), 187 deletions(-) rename src/software/thunderscope/robot_diagnostics/{controller_view.py => controller_status_view.py} (61%) diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 4b1a7bf35d..55b28e3e1e 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -35,8 +35,8 @@ py_library( ) py_library( - name = "controller_view", - srcs = ["controller_view.py"], + name = "controller_status_view", + srcs = ["controller_status_view.py"], deps = [ "//software/thunderscope:constants", requirement("pyqtgraph"), @@ -52,7 +52,7 @@ py_library( "//software/thunderscope:thunderscope_types", "//software/thunderscope/robot_diagnostics:chicker", "//software/thunderscope/robot_diagnostics:controller", - "//software/thunderscope/robot_diagnostics:controller_view", + "//software/thunderscope/robot_diagnostics:controller_status_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", requirement("pyqtgraph"), diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index b8e3e9132a..dd332db8f0 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -1,12 +1,20 @@ import logging +from multiprocessing import Process + import numpy from evdev import InputDevice, categorize, ecodes, list_devices -from threading import Event, Thread +from threading import Event import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import ControllerConstants +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode + + +# TODO: +# following is logged on controller connection during runtime: +# `qt.qpa.input.events: scroll event from unregistered device 17` class MoveEventType(Enum): @@ -21,8 +29,11 @@ class ControllerInputHandler(object): """ def __init__(self,): - self.enabled = False self.controller = None + self.__controller_event_loop_handler_process = None + + self.motor_control = MotorControl() + self.power_control = PowerControl() self.__setup_controller() @@ -40,18 +51,9 @@ def __init__(self,): self.constants = tbots_cpp.create2021RobotConstants() # event that is used to stop the controller event loop - self.__stop_event_thread = Event() - - # thread that is responsible for reading controller events - self.__event_thread = Thread(target=self.__event_loop, daemon=True) - - # fields for holding the diagnostics types that get sent - self.motor_control = MotorControl() - self.power_control = PowerControl() + self.__stop_thread_signal_event = Event() - # start reading controller events - self.enabled = True - self.__event_thread.start() + self.__setup_new_event_listener_process() elif self.controller_initialized(): logging.debug( @@ -67,13 +69,22 @@ def __init__(self,): def controller_initialized(self) -> bool: return self.controller is not None - def set_controller_enabled(self, enabled: bool): + def toggle_controller_listener(self, mode: ControlMode): """ Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - :param enabled: to which state to set controller enabled. + :param mode: current mode of control. """ - self.enabled = enabled + if mode == ControlMode.DIAGNOSTICS: + logging.debug("Terminating controller event handling process") + self.__controller_event_loop_handler_process.terminate() + self.__controller_event_loop_handler_process = None + + elif mode == ControlMode.HANDHELD: + logging.debug("Setting up new controller event handling process") + # self.__setup_event_listener_process() + self.__setup_new_event_listener_process() + self.__start_event_listener_process() def get_latest_primitive_command(self): if self.controller_initialized(): @@ -93,31 +104,51 @@ def __setup_controller(self): self.controller = controller break + def __start_event_listener_process(self): + # start the process for reading controller events + if not self.__controller_event_loop_handler_process.is_alive(): + self.__controller_event_loop_handler_process.start() + + def __setup_new_event_listener_process(self): + """ + initializes & starts a new process that runs the event processing loop + """ + + # # TODO (#3165): Use trace level logging here + # logging.debug("Starting controller event loop process") + if self.__controller_event_loop_handler_process is None: + self.__controller_event_loop_handler_process = ( + Process(target=self.__event_loop, daemon=True) + ) + def close(self): - logging.debug("Closing handheld controller event handling thread") - self.__stop_event_thread.set() - self.__event_thread.join() + # # TODO (#3165): Use trace level logging here + self.__stop_thread_signal_event.set() + self.__controller_event_loop_handler_process.join() def __event_loop(self): - logging.debug("Starting handheld controller event handling loop") - if self.enabled: - try: - for event in self.controller.read_loop(): - if self.__stop_event_thread.isSet(): - return - else: - self.__process_event(event) - except OSError as ose: - logging.debug( - "Caught an OSError while reading handheld controller event loop!" - ) - logging.debug("Error message: " + str(ose)) - logging.debug("Check physical handheld controller USB connection") - except Exception as e: - logging.critical( - "Caught an unexpected error while reading handheld controller event loop!" - ) - logging.critical("Error message: " + str(e)) + # # TODO (#3165): Use trace level logging here + # logging.debug("Starting handheld controller event handling loop") + try: + for event in self.controller.read_loop(): + if self.__stop_thread_signal_event.isSet(): + return + else: + self.__process_event(event) + + except OSError as ose: + logging.debug( + "Caught an OSError while reading handheld controller event loop!" + ) + logging.debug("Error message: " + str(ose)) + logging.debug("Check physical handheld controller USB connection") + return + except Exception as e: + logging.critical( + "Caught an unexpected error while reading handheld controller event loop!" + ) + logging.critical("Error message: " + str(e)) + return def __process_move_event_value(self, event_type, event_value) -> None: if event_type == "ABS_X": @@ -126,13 +157,17 @@ def __process_move_event_value(self, event_type, event_value) -> None: ) elif event_type == "ABS_Y": - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( - MoveEventType.LINEAR, event_value + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + self.__parse_move_event_value( + MoveEventType.LINEAR, event_value + ) ) elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - MoveEventType.ROTATIONAL, event_value + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.__parse_move_event_value( + MoveEventType.ROTATIONAL, event_value + ) ) def __process_event(self, event): diff --git a/src/software/thunderscope/robot_diagnostics/controller_view.py b/src/software/thunderscope/robot_diagnostics/controller_status_view.py similarity index 61% rename from src/software/thunderscope/robot_diagnostics/controller_view.py rename to src/software/thunderscope/robot_diagnostics/controller_status_view.py index 6b5fdade2f..74ca4393a7 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_status_view.py @@ -1,3 +1,6 @@ +import logging +from enum import Enum + import pyqtgraph as pg from proto.import_all_protos import * from pyqtgraph.Qt import QtCore, QtGui @@ -7,6 +10,11 @@ from software.py_constants import * +class ControllerConnected(Enum): + CONNECTED = 1 + DISCONNECTED = 2 + + class ControllerStatusView(QLabel): """Class to show whether a handheld controller is connected to thunderscope and initialized, or no controller is connected at all. @@ -15,36 +23,26 @@ class ControllerStatusView(QLabel): def __init__(self) -> None: super().__init__() - self.state: dict[str, (str, str)] = { - "On": ( + self.state: dict[ControllerConnected, (str, str)] = { + ControllerConnected.CONNECTED: ( "Handheld Controller is Connected & Initialized", "background-color: green", ), - "Off": ("No Handheld Controller is Connected...", "background-color: red"), + ControllerConnected.DISCONNECTED: ( + "No Handheld Controller is Connected...", "background-color: red" + ), } self.connected = False - self.set_view_state("Off") + self.set_view_state(ControllerConnected.DISCONNECTED) - def set_view_state(self, state_discriminator="Off"): + def set_view_state(self, state_discriminator=ControllerConnected.DISCONNECTED): # bruh python doesn't even have value-types or unions # how do you even do anything in this language and still maintain a sanity ffs i legit can't self.setText(self.state[state_discriminator][0]) self.setStyleSheet(self.state[state_discriminator][1]) - def set_connected(self): - self.connected = True - self.set_view_state("On") - - def set_disconnected(self): - self.connected = False - self.set_view_state("Off") - - def refresh(self) -> None: + def refresh(self, connected=ControllerConnected.DISCONNECTED) -> None: """Refresh the label """ - - if self.connected: - self.set_view_state("Ok") - else: - self.set_view_state("Off") + self.set_view_state(connected) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 37b5bcf3b3..6ebb9b2a6f 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -1,3 +1,5 @@ +from typing import Callable + from pyqtgraph.Qt.QtCore import * from pyqtgraph.Qt.QtWidgets import * from software.py_constants import * @@ -11,7 +13,7 @@ class ControlMode(IntEnum): """ DIAGNOSTICS = 0 - XBOX = 1 + HANDHELD = 1 # this class name doesnt make sense @@ -25,55 +27,45 @@ class DiagnosticsInputModeWidget(QWidget): """ # Signal to indicate if manual controls should be disabled based on boolean parameter - toggle_controls_signal = pyqtSignal(bool) - def __init__(self, toggle_controls_signal) -> None: + def __init__(self, on_control_mode_switch_callback : Callable[[ControlMode], None]) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control - :param toggle_controls_signal The signal to use for handling changes in input mode + :param on_control_mode_switch_callback The signal to use for handling changes in input mode """ super(DiagnosticsInputModeWidget, self).__init__() - self.toggle_controls_signal = toggle_controls_signal + self.on_control_mode_switch_callback = on_control_mode_switch_callback + vbox_layout = QVBoxLayout() - self.connect_options_group = QButtonGroup() - radio_button_names = ["Diagnostics Control", "XBox Control"] + self.connect_options_group = QButtonGroup() self.connect_options_box, self.connect_options = common_widgets.create_radio( - radio_button_names, self.connect_options_group + ["Diagnostics Control", "Handheld Control"], self.connect_options_group ) self.diagnostics_control_button = self.connect_options[ControlMode.DIAGNOSTICS] - self.xbox_control_button = self.connect_options[ControlMode.XBOX] - - self.xbox_control_button.setEnabled(False) + self.handheld_control_button = self.connect_options[ControlMode.HANDHELD] self.diagnostics_control_button.clicked.connect( - lambda: self.switch_control_mode(ControlMode.DIAGNOSTICS) + lambda: on_control_mode_switch_callback(ControlMode.DIAGNOSTICS) ) - self.xbox_control_button.clicked.connect( - lambda: self.switch_control_mode(ControlMode.XBOX) + self.handheld_control_button.clicked.connect( + lambda: on_control_mode_switch_callback(ControlMode.HANDHELD) ) + self.handheld_control_button.setEnabled(False) self.diagnostics_control_button.setChecked(True) - self.control_mode = ControlMode.DIAGNOSTICS vbox_layout.addWidget(self.connect_options_box) self.setLayout(vbox_layout) - def switch_control_mode(self, mode: ControlMode) -> None: - """ - Switches the control mode to the given mode - - Emits a signal to indicate whether diagnostics controls should be disabled or not - - :param mode: mode to switch to (one of ControlMode values) - - """ - self.control_mode = mode + def enable_handheld(self): + self.handheld_control_button.setEnabled(True) - self.toggle_controls_signal.emit(self.control_mode == ControlMode.DIAGNOSTICS) + def disable_handheld(self): + self.handheld_control_button.setEnabled(False) - def refresh(self, enable_xbox=False) -> None: - self.xbox_control_button.setEnabled(enable_xbox) + def refresh(self, mode=ControlMode.DIAGNOSTICS) -> None: + self.handheld_control_button.setEnabled(mode == ControlMode.HANDHELD) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index ef794ccc7c..0af36074ec 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -6,7 +6,7 @@ from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.controller_view import ControllerStatusView +from software.thunderscope.robot_diagnostics.controller_status_view import ControllerStatusView, ControllerConnected from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputModeWidget, ControlMode, @@ -20,37 +20,31 @@ class DiagnosticsWidget(QWidget): - # Signal to indicate if manual controls should be disabled based on boolean parameter - diagnostics_input_mode_signal = pyqtSignal(bool) + # Signal to indicate if manual controls should be disabled based on enum mode parameter + diagnostics_input_mode_signal = pyqtSignal(ControlMode) def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() self.proto_unix_io = proto_unix_io + self.__control_mode = ControlMode.DIAGNOSTICS - self.controller_handler = ControllerInputHandler(proto_unix_io) + # initialize widgets + self.controller_status = ControllerStatusView() + self.controller_handler = ControllerInputHandler() self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputModeWidget( - self.diagnostics_input_mode_signal, - ) - - self.__control_mode = ControlMode.DIAGNOSTICS - - self.diagnostics_control_input_widget.toggle_controls_signal.connect( - lambda control_mode: self.toggle_control(control_mode) + lambda control_mode: self.toggle_control(control_mode), ) - # Inlined Controller Status and Refresh Button Widget - # Comment for PR: - # Easier to have it at this widget level for - # communication and logic handling to other widgets + # self.diagnostics_control_input_widget.toggle_controls_signal.connect( + # lambda control_mode: self.toggle_control(control_mode) + # ) self.controller_refresh_button = QPushButton() self.controller_refresh_button.setText("Re-initialize Handheld Controller") - self.controller_refresh_button.clicked.connect(self.__refresh_controller) - - self.controller_status = ControllerStatusView() + self.controller_refresh_button.clicked.connect(self.__refresh_controller_onclick_handler) vbox_layout = QVBoxLayout() @@ -62,34 +56,15 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) + self.check_controller_connection() + def toggle_control(self, mode: ControlMode): self.__control_mode = mode - self.drive_dribbler_widget.set_enabled(mode == ControlMode.DIAGNOSTICS) - self.controller_handler.set_controller_enabled(mode == ControlMode.XBOX) - - def __refresh_controller(self) -> None: - logging.debug("Attempting to reinitialize handheld controller...") - if not self.controller_handler.controller_initialized(): - self.controller_handler = ControllerInputHandler(self.proto_unix_io) - if self.controller_handler.controller_initialized(): - logging.debug("Successfully reinitialized handheld controller") - self.controller_status.set_connected() - else: - logging.debug("Failed to reinitialize handheld controller") - self.controller_status.set_disconnected() - else: - logging.debug("Handheld controller is initialized and connected...") + self.drive_dribbler_widget.set_enabled(mode) + self.controller_handler.toggle_controller_listener(mode) def refresh(self): - # check if controller is initialized and set status - if self.controller_handler.controller_initialized(): - self.controller_status.set_connected() - else: - self.controller_status.set_disconnected() - if self.__control_mode == ControlMode.DIAGNOSTICS: - # get the values from GUI sliders and send values to proto_unix_io - self.diagnostics_control_input_widget.refresh() self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() @@ -102,19 +77,47 @@ def refresh(self): self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - elif self.__control_mode == ControlMode.XBOX: - # get the values from handheld controller and send values to proto_unix_io + elif self.__control_mode == ControlMode.HANDHELD: diagnostics_primitive = ( self.controller_handler.get_latest_primitive_command() ) if diagnostics_primitive is not None: - logging.debug("Sending diagnostics primitive: " + diagnostics_primitive) self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) else: - logging.debug("Sending stop primitive") self.proto_unix_io.send_proto(Primitive, StopPrimitive()) - # TODO: ensure this is actually called and closed properly - def close(self): + def check_controller_connection(self): + if self.controller_handler.controller_initialized(): + # controller is connected + logging.debug("Handheld controller is already initialized and connected...") + self.controller_status.refresh(ControllerConnected.CONNECTED) + self.diagnostics_control_input_widget.refresh(ControlMode.HANDHELD) + + else: + # controller is NOT connected, try connecting... + self.controller_handler = ControllerInputHandler() + if self.controller_handler.controller_initialized(): + logging.debug("Successfully reinitialized handheld controller") + self.controller_status.refresh(ControllerConnected.CONNECTED) + self.diagnostics_control_input_widget.refresh(ControlMode.HANDHELD) + + else: + logging.debug("Failed to reinitialize handheld controller") + self.controller_status.refresh(ControllerConnected.DISCONNECTED) + self.diagnostics_control_input_widget.refresh(ControlMode.DIAGNOSTICS) + + def __refresh_controller_onclick_handler(self) -> None: + logging.debug("Attempting to reinitialize handheld controller...") + self.check_controller_connection() + + # TODO: investigate why these methods aren't called on user hitting close button + def closeEvent(self, event): + logging.debug("bruh") + self.controller_handler.close() + event.accept() + + def __exit__(self, event): + logging.debug("bruh") self.controller_handler.close() + event.accept() diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index f533f33b5e..7bf7347180 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -2,6 +2,8 @@ from pyqtgraph.Qt.QtWidgets import * import time import software.python_bindings as tbots_cpp + +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.common import common_widgets from proto.import_all_protos import * @@ -175,60 +177,59 @@ def setup_dribbler(self, title: str) -> QGroupBox: return group_box - def set_enabled(self, enable: bool) -> None: + def set_enabled(self, mode: ControlMode) -> None: """ Disables or enables all sliders and buttons depending on boolean parameter Updates listener functions and stylesheets accordingly :param enable: boolean parameter, True is enable and False is disable """ - if enable: - if not self.enabled: - # disconnect all sliders - self.disconnect_sliders() - - # enable all sliders by adding listener to update label with slider value - common_widgets.enable_slider( - self.x_velocity_slider, self.x_velocity_label, self.value_change - ) - common_widgets.enable_slider( - self.y_velocity_slider, self.y_velocity_label, self.value_change - ) - common_widgets.enable_slider( - self.angular_velocity_slider, - self.angular_velocity_label, - self.value_change, - ) - common_widgets.enable_slider( - self.dribbler_speed_rpm_slider, - self.dribbler_speed_rpm_label, - self.value_change, - ) - - # enable buttons - common_widgets.change_button_state(self.stop_and_reset_dribbler, True) - common_widgets.change_button_state(self.stop_and_reset_direct, True) - - # change enabled field - self.enabled = True - else: - if self.enabled: - # reset slider values and disconnect - self.reset_all_sliders() - self.disconnect_sliders() - - # disable all sliders by adding listener to keep slider value the same - common_widgets.disable_slider(self.x_velocity_slider) - common_widgets.disable_slider(self.y_velocity_slider) - common_widgets.disable_slider(self.angular_velocity_slider) - common_widgets.disable_slider(self.dribbler_speed_rpm_slider) - - # disable buttons - common_widgets.change_button_state(self.stop_and_reset_dribbler, False) - common_widgets.change_button_state(self.stop_and_reset_direct, False) - - # change enabled field - self.enabled = False + if mode == ControlMode.DIAGNOSTICS: + # if not self.enabled: + # disconnect all sliders + self.disconnect_sliders() + + # enable all sliders by adding listener to update label with slider value + common_widgets.enable_slider( + self.x_velocity_slider, self.x_velocity_label, self.value_change + ) + common_widgets.enable_slider( + self.y_velocity_slider, self.y_velocity_label, self.value_change + ) + common_widgets.enable_slider( + self.angular_velocity_slider, + self.angular_velocity_label, + self.value_change, + ) + common_widgets.enable_slider( + self.dribbler_speed_rpm_slider, + self.dribbler_speed_rpm_label, + self.value_change, + ) + + # enable buttons + common_widgets.change_button_state(self.stop_and_reset_dribbler, True) + common_widgets.change_button_state(self.stop_and_reset_direct, True) + + # change enabled field + self.enabled = True + elif mode == ControlMode.HANDHELD: + # reset slider values and disconnect + self.reset_all_sliders() + self.disconnect_sliders() + + # disable all sliders by adding listener to keep slider value the same + common_widgets.disable_slider(self.x_velocity_slider) + common_widgets.disable_slider(self.y_velocity_slider) + common_widgets.disable_slider(self.angular_velocity_slider) + common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + + # disable buttons + common_widgets.change_button_state(self.stop_and_reset_dribbler, False) + common_widgets.change_button_state(self.stop_and_reset_direct, False) + + # change enabled field + self.enabled = False def disconnect_sliders(self) -> None: """ From d91cdb761d8c3947b69713a22245cf1bf5338417 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 12:34:57 -0700 Subject: [PATCH 063/138] formatting --- .../robot_diagnostics/controller_diagnostics.py | 16 ++++++---------- .../robot_diagnostics/controller_status_view.py | 4 ++-- .../diagnostics_input_widget.py | 4 +++- .../robot_diagnostics/diagnostics_widget.py | 9 +++++++-- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index dd332db8f0..da7b07a3d9 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -117,8 +117,8 @@ def __setup_new_event_listener_process(self): # # TODO (#3165): Use trace level logging here # logging.debug("Starting controller event loop process") if self.__controller_event_loop_handler_process is None: - self.__controller_event_loop_handler_process = ( - Process(target=self.__event_loop, daemon=True) + self.__controller_event_loop_handler_process = Process( + target=self.__event_loop, daemon=True ) def close(self): @@ -157,17 +157,13 @@ def __process_move_event_value(self, event_type, event_value) -> None: ) elif event_type == "ABS_Y": - self.motor_control.direct_velocity_control.velocity.y_component_meters = ( - self.__parse_move_event_value( - MoveEventType.LINEAR, event_value - ) + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + MoveEventType.LINEAR, event_value ) elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.__parse_move_event_value( - MoveEventType.ROTATIONAL, event_value - ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( + MoveEventType.ROTATIONAL, event_value ) def __process_event(self, event): diff --git a/src/software/thunderscope/robot_diagnostics/controller_status_view.py b/src/software/thunderscope/robot_diagnostics/controller_status_view.py index 74ca4393a7..8a215d2600 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_status_view.py @@ -1,4 +1,3 @@ -import logging from enum import Enum import pyqtgraph as pg @@ -29,7 +28,8 @@ def __init__(self) -> None: "background-color: green", ), ControllerConnected.DISCONNECTED: ( - "No Handheld Controller is Connected...", "background-color: red" + "No Handheld Controller is Connected...", + "background-color: red", ), } diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 6ebb9b2a6f..f77b69c20c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -28,7 +28,9 @@ class DiagnosticsInputModeWidget(QWidget): # Signal to indicate if manual controls should be disabled based on boolean parameter - def __init__(self, on_control_mode_switch_callback : Callable[[ControlMode], None]) -> None: + def __init__( + self, on_control_mode_switch_callback: Callable[[ControlMode], None] + ) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control :param on_control_mode_switch_callback The signal to use for handling changes in input mode diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 0af36074ec..8dec806053 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -6,7 +6,10 @@ from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.controller_status_view import ControllerStatusView, ControllerConnected +from software.thunderscope.robot_diagnostics.controller_status_view import ( + ControllerStatusView, + ControllerConnected, +) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputModeWidget, ControlMode, @@ -44,7 +47,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.controller_refresh_button = QPushButton() self.controller_refresh_button.setText("Re-initialize Handheld Controller") - self.controller_refresh_button.clicked.connect(self.__refresh_controller_onclick_handler) + self.controller_refresh_button.clicked.connect( + self.__refresh_controller_onclick_handler + ) vbox_layout = QVBoxLayout() From a0f6947999792ac70de8ff90d40fc7c52dbc9100 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 12:39:08 -0700 Subject: [PATCH 064/138] formatting --- src/software/field_tests/field_test_fixture.py | 8 ++++++-- .../robot_diagnostics/controller_diagnostics.py | 11 +++++------ 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 12cdcf0eeb..eeec34f08e 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -439,10 +439,14 @@ def field_test_runner(): ) yield runner print( - f"\n\nTo replay this test for the blue team, go to the `src` folder and run \n./tbots.py run thunderscope --blue_log {blue_logger.log_folder}", + f"\n\n" + f"To replay this test for the blue team, go to the `src` folder and run \n." + f"/tbots.py run thunderscope --blue_log {blue_logger.log_folder}", flush=True, ) print( - f"\n\nTo replay this test for the yellow team, go to the `src` folder and run \n./tbots.py run thunderscope --yellow_log {yellow_logger.log_folder}", + f"\n\n " + f"To replay this test for the yellow team, go to the `src` folder and run \n" + f"./tbots.py run thunderscope --yellow_log {yellow_logger.log_folder}", flush=True, ) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index da7b07a3d9..95998ac26b 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -12,8 +12,7 @@ from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -# TODO: -# following is logged on controller connection during runtime: +# TODO: the following is logged on controller connection during runtime: # `qt.qpa.input.events: scroll event from unregistered device 17` @@ -114,7 +113,7 @@ def __setup_new_event_listener_process(self): initializes & starts a new process that runs the event processing loop """ - # # TODO (#3165): Use trace level logging here + # TODO (#3165): Use trace level logging here # logging.debug("Starting controller event loop process") if self.__controller_event_loop_handler_process is None: self.__controller_event_loop_handler_process = Process( @@ -122,12 +121,12 @@ def __setup_new_event_listener_process(self): ) def close(self): - # # TODO (#3165): Use trace level logging here + # TODO (#3165): Use trace level logging here self.__stop_thread_signal_event.set() self.__controller_event_loop_handler_process.join() def __event_loop(self): - # # TODO (#3165): Use trace level logging here + # TODO (#3165): Use trace level logging here # logging.debug("Starting handheld controller event handling loop") try: for event in self.controller.read_loop(): @@ -173,7 +172,7 @@ def __process_event(self, event): abs_event = categorize(event) event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] - # # TODO (#3165): Use trace level logging here + # TODO (#3165): Use trace level logging here # logging.debug( # "Processing controller event with type " # + str(event_type) From b59f64efd2de13be47ac5108f502558cf53a3056 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 28 Mar 2024 16:52:28 -0700 Subject: [PATCH 065/138] fix ci --- src/proto/primitive.proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index 256f1770f0..4a24f290ff 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -167,7 +167,7 @@ message MotorControl DirectVelocityControl direct_velocity_control = 2; } - float dribbler_speed_rpm = 4; + int32 dribbler_speed_rpm = 4; } message DirectControlPrimitive From 9485892a2ed44062d02b4f372e66a24710641762 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 18:01:17 -0700 Subject: [PATCH 066/138] import correct logger ("wait it's always been there?" "always has been") --- src/software/thunderscope/robot_diagnostics/BUILD | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 55b28e3e1e..4dd8e59296 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -30,6 +30,7 @@ py_library( "//proto:software_py_proto", "//software/thunderscope:constants", "//software/thunderscope:thread_safe_buffer", + "//software/logger:py_logger", requirement("pyqtgraph"), ], ) @@ -55,6 +56,7 @@ py_library( "//software/thunderscope/robot_diagnostics:controller_status_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", + "//software/logger:py_logger", requirement("pyqtgraph"), ], ) From a3d931494db5993bbd94f0c4e8cb1ed910fc0d75 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 18:50:00 -0700 Subject: [PATCH 067/138] remove unused import --- src/software/thunderscope/thunderscope_main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 4a24c46bb6..f93209c1c3 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -13,7 +13,7 @@ import proto.message_translation.tbots_protobuf as tbots_protobuf from software.thunderscope.robot_communication import RobotCommunication from software.thunderscope.replay.proto_logger import ProtoLogger -from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes, TabNames +from software.thunderscope.constants import EstopMode, ProtoUnixIOTypes from software.thunderscope.estop_helpers import get_estop_config from software.thunderscope.proto_unix_io import ProtoUnixIO import software.thunderscope.thunderscope_config as config From 6e9f7cf5440f1938bfcbe2be8f49c3f09a52bb6f Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:16:42 -0700 Subject: [PATCH 068/138] reorganize init and clean up comments --- .../robot_diagnostics/chicker_widget.py | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 2a0565d198..13d6d61a56 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -5,6 +5,7 @@ from proto.import_all_protos import * from enum import Enum import software.thunderscope.common.common_widgets as common_widgets +from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -42,7 +43,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout = QVBoxLayout() self.setLayout(vbox_layout) - # Power slider for kicking & chipping + # Initializing power slider for kicking & chipping ( self.power_slider_layout, self.power_slider, @@ -53,21 +54,25 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: vbox_layout.addLayout(self.power_slider_layout) # Initializing kick & chip buttons - self.button_clickable_map = { - "no_auto": True, - "auto_kick": True, - "auto_chip": True, - } - self.radio_buttons_group = QButtonGroup() self.push_button_box, self.push_buttons = common_widgets.create_buttons( ["Kick", "Chip"] ) self.kick_button = self.push_buttons[0] self.chip_button = self.push_buttons[1] + # set buttons to be initially enabled + # TODO: don't need boolean flags, refactor them out + self.kick_chip_buttons_enable = True + vbox_layout.addWidget(self.push_button_box) # Initializing auto kick & chip buttons + self.button_clickable_map = { + "no_auto": True, + "auto_kick": True, + "auto_chip": True, + } + self.radio_buttons_group = QButtonGroup() self.radio_button_box, self.radio_buttons = common_widgets.create_radio( ["No Auto", "Auto Kick", "Auto Chip"], self.radio_buttons_group ) @@ -75,16 +80,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.auto_kick_button = self.radio_buttons[1] self.auto_chip_button = self.radio_buttons[2] - # set buttons to be initially enabled - self.kick_chip_buttons_enable = True - - # indicating that no auto button is selected by default + # Set no auto button to be selected by default on launch self.no_auto_button.setChecked(True) self.no_auto_selected = True - # adding onclick functions for buttons - - # kick button and chip button connected to send_command_and_timeout with their respective commands + # Initialize on-click handlers for kick & chip buttons. self.kick_button.clicked.connect( lambda: self.send_command_and_timeout(ChickerCommandMode.KICK) ) @@ -92,7 +92,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: lambda: self.send_command_and_timeout(ChickerCommandMode.CHIP) ) - # no auto button enables both buttons, while auto kick and auto chip disable both buttons + # Initialize on-click handlers for no auto, auto kick and auto chip buttons. self.no_auto_button.toggled.connect( lambda: self.set_should_enable_buttons(True) ) From e04ac17e1ab1878fbf8e589dce9cab217d39d8d7 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:17:19 -0700 Subject: [PATCH 069/138] rename enum and cleanup --- .../robot_diagnostics/controller_status_view.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_status_view.py b/src/software/thunderscope/robot_diagnostics/controller_status_view.py index 8a215d2600..03e57f6994 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_status_view.py @@ -9,7 +9,7 @@ from software.py_constants import * -class ControllerConnected(Enum): +class ControllerConnectionState(Enum): CONNECTED = 1 DISCONNECTED = 2 @@ -22,27 +22,25 @@ class ControllerStatusView(QLabel): def __init__(self) -> None: super().__init__() - self.state: dict[ControllerConnected, (str, str)] = { - ControllerConnected.CONNECTED: ( + self.state: dict[ControllerConnectionState, (str, str)] = { + ControllerConnectionState.CONNECTED: ( "Handheld Controller is Connected & Initialized", "background-color: green", ), - ControllerConnected.DISCONNECTED: ( + ControllerConnectionState.DISCONNECTED: ( "No Handheld Controller is Connected...", "background-color: red", ), } self.connected = False - self.set_view_state(ControllerConnected.DISCONNECTED) + self.set_view_state(ControllerConnectionState.DISCONNECTED) - def set_view_state(self, state_discriminator=ControllerConnected.DISCONNECTED): - # bruh python doesn't even have value-types or unions - # how do you even do anything in this language and still maintain a sanity ffs i legit can't + def set_view_state(self, state_discriminator=ControllerConnectionState.DISCONNECTED): self.setText(self.state[state_discriminator][0]) self.setStyleSheet(self.state[state_discriminator][1]) - def refresh(self, connected=ControllerConnected.DISCONNECTED) -> None: + def refresh(self, connected=ControllerConnectionState.DISCONNECTED) -> None: """Refresh the label """ self.set_view_state(connected) From 6b81fcf74dd9504b5e068a6f5d80e04c3057c1e5 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:17:53 -0700 Subject: [PATCH 070/138] simplify --- .../robot_diagnostics/diagnostics_input_widget.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index f77b69c20c..12736f12e1 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -6,6 +6,8 @@ import software.thunderscope.common.common_widgets as common_widgets from enum import IntEnum +from software.thunderscope.robot_diagnostics.controller_status_view import ControllerConnectionState + class ControlMode(IntEnum): """ @@ -63,11 +65,5 @@ def __init__( self.setLayout(vbox_layout) - def enable_handheld(self): - self.handheld_control_button.setEnabled(True) - - def disable_handheld(self): - self.handheld_control_button.setEnabled(False) - - def refresh(self, mode=ControlMode.DIAGNOSTICS) -> None: - self.handheld_control_button.setEnabled(mode == ControlMode.HANDHELD) + def refresh(self, status: ControllerConnectionState) -> None: + self.handheld_control_button.setEnabled(status == ControllerConnectionState.CONNECTED) From 7d30a5598e956756226833d879106e3084adf11c Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:19:41 -0700 Subject: [PATCH 071/138] rename method and remove boolean field, some cleanup --- .../drive_and_dribbler_widget.py | 37 +++++++++---------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 7bf7347180..be57571381 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -33,28 +33,30 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) - self.enabled = True - self.setLayout(layout) - def refresh(self) -> None: - """Refresh the widget and send the a MotorControl message with the current values + def refresh(self, mode: ControlMode) -> None: + """ + Refresh the widget's sliders + Enable or disable this widgets sliders and buttons based on mode value + Collect motor control values and persist in the primitive field """ - motor_control = MotorControl() - motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) - motor_control.direct_velocity_control.velocity.x_component_meters = ( + # Update this widgets accessibility to the user based on the ControlMode parameter + self.update_widget_accessibility(mode) + + self.motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) + + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( self.x_velocity_slider.value() ) - motor_control.direct_velocity_control.velocity.y_component_meters = ( + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( self.y_velocity_slider.value() ) - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( self.angular_velocity_slider.value() ) - self.motor_control = motor_control - def value_change(self, value: float) -> str: """ Converts the given float value to a string label @@ -177,15 +179,15 @@ def setup_dribbler(self, title: str) -> QGroupBox: return group_box - def set_enabled(self, mode: ControlMode) -> None: + def update_widget_accessibility(self, mode: ControlMode) -> None: """ - Disables or enables all sliders and buttons depending on boolean parameter + Disables or enables all sliders and buttons depending on ControlMode parameter. + Sliders are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode Updates listener functions and stylesheets accordingly - :param enable: boolean parameter, True is enable and False is disable + :param mode: ControlMode enum parameter """ if mode == ControlMode.DIAGNOSTICS: - # if not self.enabled: # disconnect all sliders self.disconnect_sliders() @@ -211,8 +213,6 @@ def set_enabled(self, mode: ControlMode) -> None: common_widgets.change_button_state(self.stop_and_reset_dribbler, True) common_widgets.change_button_state(self.stop_and_reset_direct, True) - # change enabled field - self.enabled = True elif mode == ControlMode.HANDHELD: # reset slider values and disconnect self.reset_all_sliders() @@ -228,9 +228,6 @@ def set_enabled(self, mode: ControlMode) -> None: common_widgets.change_button_state(self.stop_and_reset_dribbler, False) common_widgets.change_button_state(self.stop_and_reset_direct, False) - # change enabled field - self.enabled = False - def disconnect_sliders(self) -> None: """ Disconnect listener for changing values for all sliders From 5fff8db0bc8981212bce406e024be2e456c76782 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:20:55 -0700 Subject: [PATCH 072/138] simplify and cleanup --- .../robot_diagnostics/diagnostics_widget.py | 78 +++++++++---------- 1 file changed, 38 insertions(+), 40 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 8dec806053..f746dd46c6 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -3,19 +3,20 @@ from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * from proto.import_all_protos import * +from software.logger.logger import createLogger from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.controller_status_view import ( ControllerStatusView, - ControllerConnected, + ControllerConnectionState, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputModeWidget, ControlMode, ) from software.thunderscope.robot_diagnostics.controller_diagnostics import ( - ControllerInputHandler, + ControllerHandler, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, @@ -29,50 +30,70 @@ class DiagnosticsWidget(QWidget): def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() + self.logger = createLogger("RobotDiagnostics") + self.proto_unix_io = proto_unix_io self.__control_mode = ControlMode.DIAGNOSTICS # initialize widgets self.controller_status = ControllerStatusView() - self.controller_handler = ControllerInputHandler() self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputModeWidget( lambda control_mode: self.toggle_control(control_mode), ) - # self.diagnostics_control_input_widget.toggle_controls_signal.connect( - # lambda control_mode: self.toggle_control(control_mode) - # ) + # initialize controller + self.controller_handler = ControllerHandler(self.logger) + # initialize controller refresh button self.controller_refresh_button = QPushButton() self.controller_refresh_button.setText("Re-initialize Handheld Controller") self.controller_refresh_button.clicked.connect( self.__refresh_controller_onclick_handler ) + # TODO: move to another class. + # Layout for the entire diagnostics tab vbox_layout = QVBoxLayout() - vbox_layout.addWidget(self.controller_refresh_button) - vbox_layout.addWidget(self.controller_status) + # Layout for controller button & status + hbox_layout = QHBoxLayout() + + hbox_layout.addWidget(self.controller_status) + hbox_layout.addWidget(self.controller_refresh_button) + hbox_layout.setStretch(0, 4) + hbox_layout.setStretch(1, 1) + + vbox_layout.addLayout(hbox_layout) + vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) self.setLayout(vbox_layout) - self.check_controller_connection() - def toggle_control(self, mode: ControlMode): self.__control_mode = mode - self.drive_dribbler_widget.set_enabled(mode) - self.controller_handler.toggle_controller_listener(mode) + + self.drive_dribbler_widget.refresh(mode) + self.chicker_widget.refresh(mode) + self.controller_handler.refresh(mode) def refresh(self): - if self.__control_mode == ControlMode.DIAGNOSTICS: - self.drive_dribbler_widget.refresh() - self.chicker_widget.refresh() + """ + Refreshes sub-widgets so that they display the most recent values, as well as + the most recent values are available for use. + """ + controller_status = self.controller_handler.get_controller_connection_status() + self.controller_status.refresh(controller_status) + self.diagnostics_control_input_widget.refresh(controller_status) + + mode = self.__control_mode + self.drive_dribbler_widget.refresh(mode) + self.chicker_widget.refresh(mode) + if self.__control_mode == ControlMode.DIAGNOSTICS: diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( motor_control=self.drive_dribbler_widget.motor_control, @@ -84,7 +105,7 @@ def refresh(self): elif self.__control_mode == ControlMode.HANDHELD: diagnostics_primitive = ( - self.controller_handler.get_latest_primitive_command() + self.controller_handler.get_latest_primitive_controls() ) if diagnostics_primitive is not None: @@ -92,37 +113,14 @@ def refresh(self): else: self.proto_unix_io.send_proto(Primitive, StopPrimitive()) - def check_controller_connection(self): - if self.controller_handler.controller_initialized(): - # controller is connected - logging.debug("Handheld controller is already initialized and connected...") - self.controller_status.refresh(ControllerConnected.CONNECTED) - self.diagnostics_control_input_widget.refresh(ControlMode.HANDHELD) - - else: - # controller is NOT connected, try connecting... - self.controller_handler = ControllerInputHandler() - if self.controller_handler.controller_initialized(): - logging.debug("Successfully reinitialized handheld controller") - self.controller_status.refresh(ControllerConnected.CONNECTED) - self.diagnostics_control_input_widget.refresh(ControlMode.HANDHELD) - - else: - logging.debug("Failed to reinitialize handheld controller") - self.controller_status.refresh(ControllerConnected.DISCONNECTED) - self.diagnostics_control_input_widget.refresh(ControlMode.DIAGNOSTICS) - def __refresh_controller_onclick_handler(self) -> None: - logging.debug("Attempting to reinitialize handheld controller...") - self.check_controller_connection() + self.controller_handler.initialize_controller() # TODO: investigate why these methods aren't called on user hitting close button def closeEvent(self, event): - logging.debug("bruh") self.controller_handler.close() event.accept() def __exit__(self, event): - logging.debug("bruh") self.controller_handler.close() event.accept() From 4f2bd9bd35eb5cbb590df531665ad268a5d5b99e Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:22:26 -0700 Subject: [PATCH 073/138] disable widget controls based on control mode --- .../robot_diagnostics/chicker_widget.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 13d6d61a56..4a22970dd8 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -206,12 +206,20 @@ def change_button_state(self, button: QPushButton, enable: bool) -> None: """ if enable: button.setStyleSheet("background-color: White") - button.setCheckable(True) + button.setEnabled(True) else: button.setStyleSheet("background-color: Grey") - button.setCheckable(False) + button.setEnabled(False) - def refresh(self) -> None: + def update_widget_accessibility(self, mode: ControlMode): + self.auto_kick_button.setEnabled(mode == ControlMode.DIAGNOSTICS) + self.auto_chip_button.setEnabled(mode == ControlMode.DIAGNOSTICS) + self.set_should_enable_buttons(mode == ControlMode.DIAGNOSTICS) + + def refresh(self, mode: ControlMode) -> None: + + # Update this widgets accessibility to the user based on the ControlMode parameter + self.update_widget_accessibility(mode) # get power value slider value and set the label to that value power_value = self.power_slider.value() From 380ed30aaaedf476afad09d271bca63637510c94 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:23:57 -0700 Subject: [PATCH 074/138] Added input event type enum and a dictionary for supported controllers, with mappings from enum type to event code --- src/software/thunderscope/constants.py | 65 +++++++++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 08295c257b..bcdce63b28 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -318,8 +318,71 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 +# TODO: come up with less ambiguous names +class InputEventType(Enum): + MOVE_X = 1 + MOVE_Y = 2 + ROTATE = 3 + KICK = 4 + CHIP = 5 + KICK_POWER = 6 + DRIBBLER_SPEED = 7 + DRIBBLER_ENABLE_1 = 8 + DRIBBLER_ENABLE_2 = 9 + + class ControllerConstants: - VALID_CONTROLLERS = ["Microsoft Xbox One X pad", "Microsoft X-Box 360 pad"] + XboxEventCodes = { + # Name: "ABS_X", Type: EV_ABS + # AbsInfo: value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 + # Canonical: Left joystick X-axis + InputEventType.MOVE_X: 0, + + # Name: "ABS_Y", Type: EV_ABS + # AbsInfo: value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 + # Canonical: Left joystick Y-axis + InputEventType.MOVE_Y: 1, + + # Name: "ABS_RX", Type: EV_ABS + # AbsInfo: value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 + # Canonical: Right joystick X-axis + InputEventType.ROTATE: 3, + + # Name: "BTN_A", Type: EV_KEY + # Canonical: "A" Button + InputEventType.KICK: 304, + + # Name: "BTN_Y", Type: EV_KEY + # Canonical: "Y" Button + InputEventType.CHIP: 308, + + # Name: "ABS_HAT0X", Type: EV_ABS + # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 + # Canonical: D-pad X-axis + InputEventType.KICK_POWER: 16, + + # Name: "ABS_HAT0Y", Type: EV_ABS + # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 + # Canonical: D-pad Y-axis + InputEventType.DRIBBLER_SPEED: 17, + + # Name: "ABS_Z", Type: EV_ABS + # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 + # Canonical: Left trigger + InputEventType.DRIBBLER_ENABLE_1: 2, + + # Name: "ABS_RZ", Type: EV_ABS + # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 + # Canonical: Right trigger + InputEventType.DRIBBLER_ENABLE_2: 5 + } + + CONTROLLER_NAME_CODES_MAP = { + "Microsoft Xbox One X pad": XboxEventCodes, + "Microsoft X-Box 360 pad": XboxEventCodes, + } + + INPUT_DELAY_THRESHOLD = 0.005 XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 From 78651f21d1eed351fec531519263771cf8b644ba Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:25:03 -0700 Subject: [PATCH 075/138] standardized event processing to use event code, simplified for readability, fixed bug with proccessing old events --- .../controller_diagnostics.py | 285 ++++++++++-------- 1 file changed, 158 insertions(+), 127 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index 95998ac26b..fcf89c0e2f 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -1,14 +1,16 @@ import logging -from multiprocessing import Process +import time import numpy -from evdev import InputDevice, categorize, ecodes, list_devices +from multiprocessing import Process +from evdev import InputDevice, categorize, ecodes, list_devices, InputEvent from threading import Event import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import ControllerConstants +from software.thunderscope.robot_diagnostics.controller_status_view import ControllerConnectionState from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode @@ -21,24 +23,41 @@ class MoveEventType(Enum): ROTATIONAL = 2 -class ControllerInputHandler(object): +class ControllerHandler(object): """ This class is responsible for reading from a handheld controller device and interpreting the device inputs into usable inputs for the robots. """ - def __init__(self,): + def __init__(self, logger): + self.logger = logger self.controller = None - self.__controller_event_loop_handler_process = None + self.controller_codes = None + # Control primitives that are directly updated with + # parsed controller inputs on every iteration of the event loop self.motor_control = MotorControl() self.power_control = PowerControl() - self.__setup_controller() + # These fields are here to temporarily persist the controller's input. + # They are read once once certain buttons are pressed on the controller, + # and the values are inserted into the control primitives above. + self.kick_power_accumulator = 0 + self.dribbler_speed_accumulator = 0 + + self.constants = tbots_cpp.create2021RobotConstants() + + # event that is used to stop the controller event loop + # self.__stop_thread_signal_event = Event() + + # Set-up process that will run the event loop + self.__setup_new_event_listener_process() - if self.controller_initialized(): + self.initialize_controller() - logging.debug( + if self.get_controller_connection_status() == ControllerConnectionState.CONNECTED: + + self.logger.debug( "Initialized handheld controller " + '"' + self.controller.name @@ -47,60 +66,63 @@ def __init__(self,): + self.controller.path ) - self.constants = tbots_cpp.create2021RobotConstants() - - # event that is used to stop the controller event loop - self.__stop_thread_signal_event = Event() - - self.__setup_new_event_listener_process() - - elif self.controller_initialized(): - logging.debug( - "Tried to initialize a handheld controller from list available devices:" + elif self.get_controller_connection_status() == ControllerConnectionState.DISCONNECTED: + self.logger.debug( + "Failed to initialize a handheld controller, check USB connection" ) - logging.debug( + self.logger.debug("Tried the following available devices:") + self.logger.debug( list(map(lambda device: InputDevice(device).name, list_devices())) ) - logging.debug( - "Could not initialize a handheld controller device - check USB connections" - ) - def controller_initialized(self) -> bool: - return self.controller is not None + def get_controller_connection_status(self) -> ControllerConnectionState: + return ControllerConnectionState.CONNECTED \ + if self.controller is not None \ + else ControllerConnectionState.DISCONNECTED - def toggle_controller_listener(self, mode: ControlMode): + def refresh(self, mode: ControlMode): """ - Changes the diagnostics input mode for all robots between Xbox and Diagnostics. - - :param mode: current mode of control. + Refresh this class. + Spawns a new process that listens and processes + handheld controller device events if Control.Mode is Diagnostics, + Otherwise, terminates the current listener process if it is running, + to avoid busy waiting + :param mode: The current user requested mode for controlling the robot """ if mode == ControlMode.DIAGNOSTICS: - logging.debug("Terminating controller event handling process") - self.__controller_event_loop_handler_process.terminate() - self.__controller_event_loop_handler_process = None + self.logger.debug("Terminating controller event handling process") + if self.__controller_event_loop_handler_process.is_alive(): + # self.__stop_thread_signal_event.set() + self.__controller_event_loop_handler_process.terminate() elif mode == ControlMode.HANDHELD: - logging.debug("Setting up new controller event handling process") - # self.__setup_event_listener_process() + self.logger.debug("Setting up new controller event handling process") self.__setup_new_event_listener_process() self.__start_event_listener_process() - def get_latest_primitive_command(self): - if self.controller_initialized(): - return DirectControlPrimitive( - motor_control=self.motor_control, power_control=self.power_control, - ) - else: - return None + def get_latest_primitive_controls(self): + primitive = DirectControlPrimitive( + motor_control=self.motor_control, power_control=self.power_control + ) + # TODO: pre-emptive bugfix: need to reset controls, epecially power so that + # the control message isn't set to what is essentially auto-kick/chip + # self.motor_control = MotorControl() + self.power_control = PowerControl() + return primitive - def __setup_controller(self): + def initialize_controller(self): + """ + Attempt to initialize a controller. + The first controller that is recognized a valid controller will be used + """ for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.VALID_CONTROLLERS + controller is not None + and controller.name in ControllerConstants.CONTROLLER_NAME_CODES_MAP ): self.controller = controller + self.controller_codes = ControllerConstants.CONTROLLER_NAME_CODES_MAP[controller.name] break def __start_event_listener_process(self): @@ -110,122 +132,131 @@ def __start_event_listener_process(self): def __setup_new_event_listener_process(self): """ - initializes & starts a new process that runs the event processing loop + initializes a new process that will run the event processing loop """ - # TODO (#3165): Use trace level logging here - # logging.debug("Starting controller event loop process") - if self.__controller_event_loop_handler_process is None: - self.__controller_event_loop_handler_process = Process( - target=self.__event_loop, daemon=True - ) + # TODO (#3165): Use trace level self.logger here + # self.logger.debug("Starting controller event loop process") + self.__controller_event_loop_handler_process = Process( + target=self.__event_loop, daemon=True + ) def close(self): + """ + Shut down the controller event loop + :return: + """ # TODO (#3165): Use trace level logging here - self.__stop_thread_signal_event.set() - self.__controller_event_loop_handler_process.join() + # self.logger.debug("Shutdown down controller event loop process") + # self.__stop_thread_signal_event.set() + self.__controller_event_loop_handler_process.terminate() def __event_loop(self): - # TODO (#3165): Use trace level logging here - # logging.debug("Starting handheld controller event handling loop") + # TODO (#3165): Use trace level self.logger here + self.logger.debug("Starting handheld controller event handling loop") try: for event in self.controller.read_loop(): - if self.__stop_thread_signal_event.isSet(): - return - else: + # TODO: is this a useful comment? + # All events accumulate into a file and will be read back eventually, + # even if handheld mode is disabled. This time check ensures that + # only the events that have occurred very recently are processed, and + # any events that occur before switching to handheld mode are ignored + if time.time() - event.timestamp() < ControllerConstants.INPUT_DELAY_THRESHOLD: self.__process_event(event) - except OSError as ose: - logging.debug( + self.logger.debug( "Caught an OSError while reading handheld controller event loop!" ) - logging.debug("Error message: " + str(ose)) - logging.debug("Check physical handheld controller USB connection") + self.logger.debug("Error message: " + str(ose)) + self.logger.debug("Check physical handheld controller USB connection!") return except Exception as e: - logging.critical( + self.logger.critical( "Caught an unexpected error while reading handheld controller event loop!" ) - logging.critical("Error message: " + str(e)) + self.logger.critical("Error message: " + str(e)) return - def __process_move_event_value(self, event_type, event_value) -> None: - if event_type == "ABS_X": - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( - MoveEventType.LINEAR, event_value - ) + def __process_event(self, event: InputEvent): - elif event_type == "ABS_Y": - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( - MoveEventType.LINEAR, event_value - ) + # abs_event = categorize(event) + event_type = ecodes.bytype[event.type][event.code] + + # if event.type == ecodes.EV_ABS or event.type == ecodes.EV_KEY: + # TODO (#3165): Use trace level self.logger here + self.logger.debug( + "Processing controller event with type: " + + str(event_type) + + ", with code: " + + str(event.code) + + ", and with value: " + + str(event.value) + ) - elif event_type == "ABS_RX": - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - MoveEventType.ROTATIONAL, event_value + if event.code == self.controller_codes[InputEventType.MOVE_X]: + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( + self.__parse_move_event_value( + ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, + event.value + ) ) - def __process_event(self, event): - kick_power = 0.0 - dribbler_speed = 0.0 + if event.code == self.controller_codes[InputEventType.MOVE_Y]: + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + self.__parse_move_event_value( + ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, + event.value + ) + ) - abs_event = categorize(event) - event_type = ecodes.bytype[abs_event.event.type][abs_event.event.code] + if event.code == self.controller_codes[InputEventType.ROTATE]: + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.__parse_move_event_value( + ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S, + event.value + ) + ) - # TODO (#3165): Use trace level logging here - # logging.debug( - # "Processing controller event with type " - # + str(event_type) - # + " and with value " - # + str(abs_event.event.value) - # ) - - # TODO (#3175): new python versions have a pattern matching, - # that could be useful for handling lots of if statements. - # Some documentation for future: - # https://peps.python.org/pep-0636/ - # https://docs.python.org/3/reference/compound_stmts.html#match - if event.type == ecodes.EV_ABS: - if event_type in ["ABS_X", "ABS_Y", "ABS_RX"]: - self.__process_move_event_value(event_type, abs_event.event.value) - - if event_type == "ABS_HAT0X": - dribbler_speed = self.__parse_kick_event_value(abs_event.event.value) - - if event_type == "ABS_HAT0Y": - kick_power = self.__parse_dribble_event_value(abs_event.event.value) - - if event_type == "ABS_RZ" or "ABS_Z": - if self.__parse_dribbler_enabled_event_value(abs_event.event.value): - self.motor_control.dribbler_speed_rpm = float(dribbler_speed) - - if event.type == ecodes.EV_KEY: - # TODO: try testing event_type instead of checking in `ecodes.ecodes` map - if event.code == ecodes.ecodes["BTN_A"] and event.value == 1.0: - self.power_control.geneva_slot = 3.0 - self.power_control.chicker.kick_speed_m_per_s = kick_power - - elif event.code == ecodes.ecodes["BTN_Y"] and event.value == 1.0: - self.power_control.geneva_slot = 3.0 - self.power_control.chicker.chip_distance_meters = kick_power + if event.code == self.controller_codes[InputEventType.KICK_POWER]: + self.kick_power_accumulator = self.__parse_kick_event_value(event.value) + + if event.code == self.controller_codes[InputEventType.DRIBBLER_SPEED]: + self.dribbler_speed_accumulator = self.__parse_dribble_event_value(event.value) + + # Left and right triggers + if ( + event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_1] or + event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_2] + ): + dribbler_enabled = self.__parse_dribbler_enabled_event_value(event.value) + if dribbler_enabled: + self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator + + # "A" button + if ( + event.code == self.controller_codes[InputEventType.KICK] and + event.value == 1 + ): + self.power_control.geneva_slot = 3 + self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator + + # "A" button + if ( + event.code == self.controller_codes[InputEventType.CHIP] and + event.value == 1 + ): + self.power_control.geneva_slot = 3 + self.power_control.chicker.chip_distance_meters = self.kick_power_accumulator @staticmethod def __parse_move_event_value( - event_type: MoveEventType, event_value: float + event_value: float, scaling_factor: float ) -> float: - factor = ( - ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S - if event_type == MoveEventType.ROTATIONAL - else ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S - if event_type == MoveEventType.LINEAR - else 1.0 - ) - - if abs(event_value) < (ControllerConstants.DEADZONE_PERCENTAGE * factor): + if abs(event_value) < (ControllerConstants.DEADZONE_PERCENTAGE * scaling_factor): return 0 else: return numpy.clip( - event_value, 0, ControllerConstants.XBOX_MAX_RANGE * factor + event_value, 0, ControllerConstants.XBOX_MAX_RANGE * scaling_factor ) @staticmethod From 65d497726b771bdf2064303258c60a9dcef4bc7d Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:27:15 -0700 Subject: [PATCH 076/138] formatting --- src/software/thunderscope/constants.py | 10 +-- .../thunderscope/robot_diagnostics/BUILD | 4 +- .../controller_diagnostics.py | 84 ++++++++++--------- .../controller_status_view.py | 4 +- .../diagnostics_input_widget.py | 8 +- .../robot_diagnostics/diagnostics_widget.py | 1 - .../drive_and_dribbler_widget.py | 4 +- 7 files changed, 61 insertions(+), 54 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index bcdce63b28..530ad5b1d5 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -337,44 +337,36 @@ class ControllerConstants: # AbsInfo: value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick X-axis InputEventType.MOVE_X: 0, - # Name: "ABS_Y", Type: EV_ABS # AbsInfo: value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick Y-axis InputEventType.MOVE_Y: 1, - # Name: "ABS_RX", Type: EV_ABS # AbsInfo: value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Right joystick X-axis InputEventType.ROTATE: 3, - # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button InputEventType.KICK: 304, - # Name: "BTN_Y", Type: EV_KEY # Canonical: "Y" Button InputEventType.CHIP: 308, - # Name: "ABS_HAT0X", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad X-axis InputEventType.KICK_POWER: 16, - # Name: "ABS_HAT0Y", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad Y-axis InputEventType.DRIBBLER_SPEED: 17, - # Name: "ABS_Z", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Left trigger InputEventType.DRIBBLER_ENABLE_1: 2, - # Name: "ABS_RZ", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Right trigger - InputEventType.DRIBBLER_ENABLE_2: 5 + InputEventType.DRIBBLER_ENABLE_2: 5, } CONTROLLER_NAME_CODES_MAP = { diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 4dd8e59296..f402b4a120 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -28,9 +28,9 @@ py_library( srcs = ["controller_diagnostics.py"], deps = [ "//proto:software_py_proto", + "//software/logger:py_logger", "//software/thunderscope:constants", "//software/thunderscope:thread_safe_buffer", - "//software/logger:py_logger", requirement("pyqtgraph"), ], ) @@ -48,6 +48,7 @@ py_library( name = "diagnostics_widget", srcs = ["diagnostics_widget.py"], deps = [ + "//software/logger:py_logger", "//software/thunderscope:constants", "//software/thunderscope:thread_safe_buffer", "//software/thunderscope:thunderscope_types", @@ -56,7 +57,6 @@ py_library( "//software/thunderscope/robot_diagnostics:controller_status_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", - "//software/logger:py_logger", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py index fcf89c0e2f..9d71a92bf7 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py @@ -1,16 +1,16 @@ -import logging import time import numpy from multiprocessing import Process from evdev import InputDevice, categorize, ecodes, list_devices, InputEvent -from threading import Event import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import ControllerConstants -from software.thunderscope.robot_diagnostics.controller_status_view import ControllerConnectionState +from software.thunderscope.robot_diagnostics.controller_status_view import ( + ControllerConnectionState, +) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode @@ -55,7 +55,10 @@ def __init__(self, logger): self.initialize_controller() - if self.get_controller_connection_status() == ControllerConnectionState.CONNECTED: + if ( + self.get_controller_connection_status() + == ControllerConnectionState.CONNECTED + ): self.logger.debug( "Initialized handheld controller " @@ -66,7 +69,10 @@ def __init__(self, logger): + self.controller.path ) - elif self.get_controller_connection_status() == ControllerConnectionState.DISCONNECTED: + elif ( + self.get_controller_connection_status() + == ControllerConnectionState.DISCONNECTED + ): self.logger.debug( "Failed to initialize a handheld controller, check USB connection" ) @@ -76,9 +82,11 @@ def __init__(self, logger): ) def get_controller_connection_status(self) -> ControllerConnectionState: - return ControllerConnectionState.CONNECTED \ - if self.controller is not None \ + return ( + ControllerConnectionState.CONNECTED + if self.controller is not None else ControllerConnectionState.DISCONNECTED + ) def refresh(self, mode: ControlMode): """ @@ -118,11 +126,13 @@ def initialize_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name in ControllerConstants.CONTROLLER_NAME_CODES_MAP + controller is not None + and controller.name in ControllerConstants.CONTROLLER_NAME_CODES_MAP ): self.controller = controller - self.controller_codes = ControllerConstants.CONTROLLER_NAME_CODES_MAP[controller.name] + self.controller_codes = ControllerConstants.CONTROLLER_NAME_CODES_MAP[ + controller.name + ] break def __start_event_listener_process(self): @@ -161,7 +171,10 @@ def __event_loop(self): # even if handheld mode is disabled. This time check ensures that # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are ignored - if time.time() - event.timestamp() < ControllerConstants.INPUT_DELAY_THRESHOLD: + if ( + time.time() - event.timestamp() + < ControllerConstants.INPUT_DELAY_THRESHOLD + ): self.__process_event(event) except OSError as ose: self.logger.debug( @@ -194,39 +207,32 @@ def __process_event(self, event: InputEvent): ) if event.code == self.controller_codes[InputEventType.MOVE_X]: - self.motor_control.direct_velocity_control.velocity.x_component_meters = ( - self.__parse_move_event_value( - ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, - event.value - ) + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( + ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, event.value ) if event.code == self.controller_codes[InputEventType.MOVE_Y]: - self.motor_control.direct_velocity_control.velocity.y_component_meters = ( - self.__parse_move_event_value( - ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, - event.value - ) + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, event.value ) if event.code == self.controller_codes[InputEventType.ROTATE]: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.__parse_move_event_value( - ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S, - event.value - ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( + ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S, event.value ) if event.code == self.controller_codes[InputEventType.KICK_POWER]: self.kick_power_accumulator = self.__parse_kick_event_value(event.value) if event.code == self.controller_codes[InputEventType.DRIBBLER_SPEED]: - self.dribbler_speed_accumulator = self.__parse_dribble_event_value(event.value) + self.dribbler_speed_accumulator = self.__parse_dribble_event_value( + event.value + ) # Left and right triggers if ( - event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_1] or - event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_2] + event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_1] + or event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_2] ): dribbler_enabled = self.__parse_dribbler_enabled_event_value(event.value) if dribbler_enabled: @@ -234,25 +240,27 @@ def __process_event(self, event: InputEvent): # "A" button if ( - event.code == self.controller_codes[InputEventType.KICK] and - event.value == 1 + event.code == self.controller_codes[InputEventType.KICK] + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator # "A" button if ( - event.code == self.controller_codes[InputEventType.CHIP] and - event.value == 1 + event.code == self.controller_codes[InputEventType.CHIP] + and event.value == 1 ): self.power_control.geneva_slot = 3 - self.power_control.chicker.chip_distance_meters = self.kick_power_accumulator + self.power_control.chicker.chip_distance_meters = ( + self.kick_power_accumulator + ) @staticmethod - def __parse_move_event_value( - event_value: float, scaling_factor: float - ) -> float: - if abs(event_value) < (ControllerConstants.DEADZONE_PERCENTAGE * scaling_factor): + def __parse_move_event_value(event_value: float, scaling_factor: float) -> float: + if abs(event_value) < ( + ControllerConstants.DEADZONE_PERCENTAGE * scaling_factor + ): return 0 else: return numpy.clip( diff --git a/src/software/thunderscope/robot_diagnostics/controller_status_view.py b/src/software/thunderscope/robot_diagnostics/controller_status_view.py index 03e57f6994..c127ae8149 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/controller_status_view.py @@ -36,7 +36,9 @@ def __init__(self) -> None: self.connected = False self.set_view_state(ControllerConnectionState.DISCONNECTED) - def set_view_state(self, state_discriminator=ControllerConnectionState.DISCONNECTED): + def set_view_state( + self, state_discriminator=ControllerConnectionState.DISCONNECTED + ): self.setText(self.state[state_discriminator][0]) self.setStyleSheet(self.state[state_discriminator][1]) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 12736f12e1..dffeef8dd3 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -6,7 +6,9 @@ import software.thunderscope.common.common_widgets as common_widgets from enum import IntEnum -from software.thunderscope.robot_diagnostics.controller_status_view import ControllerConnectionState +from software.thunderscope.robot_diagnostics.controller_status_view import ( + ControllerConnectionState, +) class ControlMode(IntEnum): @@ -66,4 +68,6 @@ def __init__( self.setLayout(vbox_layout) def refresh(self, status: ControllerConnectionState) -> None: - self.handheld_control_button.setEnabled(status == ControllerConnectionState.CONNECTED) + self.handheld_control_button.setEnabled( + status == ControllerConnectionState.CONNECTED + ) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index f746dd46c6..7c6382c8d2 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,4 +1,3 @@ -import logging from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index be57571381..3a2b73d533 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -45,7 +45,9 @@ def refresh(self, mode: ControlMode) -> None: # Update this widgets accessibility to the user based on the ControlMode parameter self.update_widget_accessibility(mode) - self.motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) + self.motor_control.dribbler_speed_rpm = int( + self.dribbler_speed_rpm_slider.value() + ) self.motor_control.direct_velocity_control.velocity.x_component_meters = ( self.x_velocity_slider.value() From ea818d0980f957ee079373eda1ef625bafd18cdb Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 21:41:01 -0700 Subject: [PATCH 077/138] bazel sync error temp fix --- src/software/networking/radio/BUILD | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/software/networking/radio/BUILD b/src/software/networking/radio/BUILD index fe1e5bffa5..e0acffdb97 100644 --- a/src/software/networking/radio/BUILD +++ b/src/software/networking/radio/BUILD @@ -1,15 +1,16 @@ package(default_visibility = ["//visibility:public"]) -cc_library( - name = "proto_radio_client", - hdrs = [ - "proto_radio_client.hpp", - ], - deps = [ - "//software/logger", - "//software/util/typename", - ], -) +# TODO: This is giving bazel sync error, comment out for now + #cc_library( + # name = "proto_radio_client", + # hdrs = [ + # "proto_radio_client.hpp", + # ], + # deps = [ + # "//software/logger", + # "//software/util/typename", + # ], + #) cc_library( name = "proto_radio_listener", From 4702158daacf0b89d748eb6844ef44b19dba5d12 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 22:54:36 -0700 Subject: [PATCH 078/138] renaming some stuff and more cleanup --- .../thunderscope/robot_diagnostics/BUILD | 4 ++-- .../diagnostics_input_widget.py | 6 ++--- .../robot_diagnostics/diagnostics_widget.py | 16 +++++++------- ...gnostics.py => handheld_device_manager.py} | 10 ++------- ...view.py => handheld_device_status_view.py} | 2 +- .../thunderscope/widget_setup_functions.py | 22 +++++-------------- 6 files changed, 22 insertions(+), 38 deletions(-) rename src/software/thunderscope/robot_diagnostics/{controller_diagnostics.py => handheld_device_manager.py} (98%) rename src/software/thunderscope/robot_diagnostics/{controller_status_view.py => handheld_device_status_view.py} (97%) diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index f402b4a120..91a62fe9ac 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -25,7 +25,7 @@ py_library( py_library( name = "controller", - srcs = ["controller_diagnostics.py"], + srcs = ["handheld_device_manager.py"], deps = [ "//proto:software_py_proto", "//software/logger:py_logger", @@ -37,7 +37,7 @@ py_library( py_library( name = "controller_status_view", - srcs = ["controller_status_view.py"], + srcs = ["handheld_device_status_view.py"], deps = [ "//software/thunderscope:constants", requirement("pyqtgraph"), diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index dffeef8dd3..b9fa58737a 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -6,7 +6,7 @@ import software.thunderscope.common.common_widgets as common_widgets from enum import IntEnum -from software.thunderscope.robot_diagnostics.controller_status_view import ( +from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( ControllerConnectionState, ) @@ -22,7 +22,7 @@ class ControlMode(IntEnum): # this class name doesnt make sense # -class DiagnosticsInputModeWidget(QWidget): +class DiagnosticsInputToggleWidget(QWidget): """ Class to allow the user to switch between Manual, XBox, and Fullsystem control through Thunderscope UI @@ -39,7 +39,7 @@ def __init__( Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control :param on_control_mode_switch_callback The signal to use for handling changes in input mode """ - super(DiagnosticsInputModeWidget, self).__init__() + super(DiagnosticsInputToggleWidget, self).__init__() self.on_control_mode_switch_callback = on_control_mode_switch_callback vbox_layout = QVBoxLayout() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 7c6382c8d2..b5446d641c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -6,16 +6,16 @@ from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.controller_status_view import ( - ControllerStatusView, +from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( + HandheldDeviceStatusView, ControllerConnectionState, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( - DiagnosticsInputModeWidget, + DiagnosticsInputToggleWidget, ControlMode, ) -from software.thunderscope.robot_diagnostics.controller_diagnostics import ( - ControllerHandler, +from software.thunderscope.robot_diagnostics.handheld_device_manager import ( + HandheldDeviceManager, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, @@ -35,15 +35,15 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.__control_mode = ControlMode.DIAGNOSTICS # initialize widgets - self.controller_status = ControllerStatusView() + self.controller_status = HandheldDeviceStatusView() self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) - self.diagnostics_control_input_widget = DiagnosticsInputModeWidget( + self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( lambda control_mode: self.toggle_control(control_mode), ) # initialize controller - self.controller_handler = ControllerHandler(self.logger) + self.controller_handler = HandheldDeviceManager(self.logger) # initialize controller refresh button self.controller_refresh_button = QPushButton() diff --git a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py similarity index 98% rename from src/software/thunderscope/robot_diagnostics/controller_diagnostics.py rename to src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 9d71a92bf7..30e51c8fca 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_diagnostics.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -8,7 +8,7 @@ from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import ControllerConstants -from software.thunderscope.robot_diagnostics.controller_status_view import ( +from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( ControllerConnectionState, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode @@ -17,13 +17,7 @@ # TODO: the following is logged on controller connection during runtime: # `qt.qpa.input.events: scroll event from unregistered device 17` - -class MoveEventType(Enum): - LINEAR = 1 - ROTATIONAL = 2 - - -class ControllerHandler(object): +class HandheldDeviceManager(object): """ This class is responsible for reading from a handheld controller device and interpreting the device inputs into usable inputs for the robots. diff --git a/src/software/thunderscope/robot_diagnostics/controller_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py similarity index 97% rename from src/software/thunderscope/robot_diagnostics/controller_status_view.py rename to src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index c127ae8149..b24a99af7c 100644 --- a/src/software/thunderscope/robot_diagnostics/controller_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -14,7 +14,7 @@ class ControllerConnectionState(Enum): DISCONNECTED = 2 -class ControllerStatusView(QLabel): +class HandheldDeviceStatusView(QLabel): """Class to show whether a handheld controller is connected to thunderscope and initialized, or no controller is connected at all. """ diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 343f81fbe5..a2ca5c6b70 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -38,7 +38,7 @@ from software.thunderscope.play.refereeinfo_widget import RefereeInfoWidget from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( - DiagnosticsInputModeWidget, + DiagnosticsInputToggleWidget, ) from software.thunderscope.robot_diagnostics.diagnostics_widget import DiagnosticsWidget from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( @@ -336,22 +336,12 @@ def setup_chicker_widget(proto_unix_io: ProtoUnixIO) -> ChickerWidget: return chicker_widget -def setup_drive_and_dribbler_widget( - proto_unix_io: ProtoUnixIO, -) -> DriveAndDribblerWidget: - """Setup the drive and dribbler widget - - :param proto_unix_io: The proto unix io object - :returns: The drive and dribbler widget - - """ - drive_and_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) - - return drive_and_dribbler_widget - - def setup_diagnostics_widget(proto_unix_io: ProtoUnixIO,) -> DriveAndDribblerWidget: - """Setup the drive and dribbler widget + """Setup the diagnostics widget, which contains the following sub-widgets: + - ChickerWidget + - DriveAndDribblerWidget + - DiagnosticsInputWidget + - ControllerStatusViewWidget :param proto_unix_io: The proto unix io object :returns: The diagnostics widget that contains From 2b786d40ef99b80e9138f68c95a08b121822d985 Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 22:56:33 -0700 Subject: [PATCH 079/138] formatting --- .../thunderscope/robot_diagnostics/handheld_device_manager.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 30e51c8fca..d62d5d33d9 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -17,6 +17,7 @@ # TODO: the following is logged on controller connection during runtime: # `qt.qpa.input.events: scroll event from unregistered device 17` + class HandheldDeviceManager(object): """ This class is responsible for reading from a handheld controller device and From f0d21b008d5ed64c54a62725a247445a8f2249bf Mon Sep 17 00:00:00 2001 From: boris Date: Sun, 31 Mar 2024 23:06:40 -0700 Subject: [PATCH 080/138] lil cleanup --- .../robot_diagnostics/diagnostics_input_widget.py | 1 - .../thunderscope/robot_diagnostics/diagnostics_widget.py | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index b9fa58737a..8f2ffbe0b0 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -40,7 +40,6 @@ def __init__( :param on_control_mode_switch_callback The signal to use for handling changes in input mode """ super(DiagnosticsInputToggleWidget, self).__init__() - self.on_control_mode_switch_callback = on_control_mode_switch_callback vbox_layout = QVBoxLayout() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index b5446d641c..35f09861bd 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -39,7 +39,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( - lambda control_mode: self.toggle_control(control_mode), + lambda control_mode: self.__toggle_control_and_refresh(control_mode), ) # initialize controller @@ -72,7 +72,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) - def toggle_control(self, mode: ControlMode): + def __toggle_control_and_refresh(self, mode: ControlMode): self.__control_mode = mode self.drive_dribbler_widget.refresh(mode) From 5f729df0c9ead179c95fd9e13cbd2bc5baf7c98b Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 5 Apr 2024 00:40:52 -0700 Subject: [PATCH 081/138] changed controller config structure, returned back to using thread so that class attr is updated properly, fixed bugs when controller is disconnected, updated pyqt to remove logging bug --- environment_setup/ubuntu20_requirements.txt | 4 +- src/software/thunderscope/constants.py | 66 +++++-- .../diagnostics_input_widget.py | 2 + .../robot_diagnostics/diagnostics_widget.py | 7 + .../handheld_device_manager.py | 169 ++++++++++-------- .../thunderscope/widget_setup_functions.py | 13 +- 6 files changed, 157 insertions(+), 104 deletions(-) diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 25322e71e2..6a8d6a8815 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,8 +1,8 @@ protobuf==3.20.2 pyqtgraph==0.13.3 autoflake==1.4 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 pyqtdarktheme==1.1.0 diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index e13ceb7e42..cca9bfab7e 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -319,8 +319,7 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 -# TODO: come up with less ambiguous names -class InputEventType(Enum): +class RobotControlType(Enum): MOVE_X = 1 MOVE_Y = 2 ROTATE = 3 @@ -332,53 +331,86 @@ class InputEventType(Enum): DRIBBLER_ENABLE_2 = 9 -class ControllerConstants: - XboxEventCodes = { +# TODO: come up with less ambiguous/wierd name +class HandheldDeviceConfigKeys(Enum): + CODE = 1 + MAX_VALUE = 2 + + +class HandheldDeviceConstants: + XboxConfig = { # Name: "ABS_X", Type: EV_ABS # AbsInfo: value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick X-axis - InputEventType.MOVE_X: 0, + RobotControlType.MOVE_X: { + HandheldDeviceConfigKeys.CODE: 0, + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + }, # Name: "ABS_Y", Type: EV_ABS # AbsInfo: value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick Y-axis - InputEventType.MOVE_Y: 1, + RobotControlType.MOVE_Y: { + HandheldDeviceConfigKeys.CODE: 1, + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + }, # Name: "ABS_RX", Type: EV_ABS # AbsInfo: value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Right joystick X-axis - InputEventType.ROTATE: 3, + RobotControlType.ROTATE: { + HandheldDeviceConfigKeys.CODE: 3, + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + }, # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button - InputEventType.KICK: 304, + RobotControlType.KICK: { + HandheldDeviceConfigKeys.CODE: 304, + }, # Name: "BTN_Y", Type: EV_KEY # Canonical: "Y" Button - InputEventType.CHIP: 308, + RobotControlType.CHIP: { + HandheldDeviceConfigKeys.CODE: 308, + }, # Name: "ABS_HAT0X", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad X-axis - InputEventType.KICK_POWER: 16, + RobotControlType.KICK_POWER: { + HandheldDeviceConfigKeys.CODE: 16, + HandheldDeviceConfigKeys.MAX_VALUE: 1.0 + }, # Name: "ABS_HAT0Y", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad Y-axis - InputEventType.DRIBBLER_SPEED: 17, + RobotControlType.DRIBBLER_SPEED: { + HandheldDeviceConfigKeys.CODE: 17, + HandheldDeviceConfigKeys.MAX_VALUE: 1.0 + }, # Name: "ABS_Z", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Left trigger - InputEventType.DRIBBLER_ENABLE_1: 2, + RobotControlType.DRIBBLER_ENABLE_1: { + HandheldDeviceConfigKeys.CODE: 2, + HandheldDeviceConfigKeys.MAX_VALUE: 1023.0 + }, # Name: "ABS_RZ", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Right trigger - InputEventType.DRIBBLER_ENABLE_2: 5, + RobotControlType.DRIBBLER_ENABLE_2: { + HandheldDeviceConfigKeys.CODE: 5, + HandheldDeviceConfigKeys.MAX_VALUE: 1023.0 + }, } - CONTROLLER_NAME_CODES_MAP = { - "Microsoft Xbox One X pad": XboxEventCodes, - "Microsoft X-Box 360 pad": XboxEventCodes, + CONTROLLER_NAME_CONFIG_MAP = { + "Microsoft Xbox One X pad": XboxConfig, + "Microsoft X-Box 360 pad": XboxConfig, } - INPUT_DELAY_THRESHOLD = 0.005 + # TODO remove these two XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 + + INPUT_DELAY_THRESHOLD = 0.005 DEADZONE_PERCENTAGE = 0.30 MAX_LINEAR_SPEED_METER_PER_S = 2.0 diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 8f2ffbe0b0..fc6bdf1d34 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -67,6 +67,8 @@ def __init__( self.setLayout(vbox_layout) def refresh(self, status: ControllerConnectionState) -> None: + if status == ControllerConnectionState.DISCONNECTED: + self.diagnostics_control_button.click() self.handheld_control_button.setEnabled( status == ControllerConnectionState.CONNECTED ) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 35f09861bd..a0154a6a4f 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -117,9 +117,16 @@ def __refresh_controller_onclick_handler(self) -> None: # TODO: investigate why these methods aren't called on user hitting close button def closeEvent(self, event): + self.logger.info("test") + self.controller_handler.close() + event.accept() + + def close(self, event): + self.logger.info("test") self.controller_handler.close() event.accept() def __exit__(self, event): + self.logger.info("test") self.controller_handler.close() event.accept() diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index d62d5d33d9..b41421393b 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -1,13 +1,15 @@ import time +from threading import Thread, Event +from typing import Optional +import evdev.ecodes import numpy -from multiprocessing import Process -from evdev import InputDevice, categorize, ecodes, list_devices, InputEvent +from evdev import InputDevice, ecodes, list_devices, InputEvent import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * -from software.thunderscope.constants import ControllerConstants +from software.thunderscope.constants import HandheldDeviceConstants from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( ControllerConnectionState, ) @@ -15,6 +17,7 @@ # TODO: the following is logged on controller connection during runtime: +# This is fixed in version pyqt 6.6.1 # `qt.qpa.input.events: scroll event from unregistered device 17` @@ -26,8 +29,8 @@ class HandheldDeviceManager(object): def __init__(self, logger): self.logger = logger - self.controller = None - self.controller_codes = None + self.controller: Optional[InputDevice] = None + self.controller_config = None # Control primitives that are directly updated with # parsed controller inputs on every iteration of the event loop @@ -43,39 +46,13 @@ def __init__(self, logger): self.constants = tbots_cpp.create2021RobotConstants() # event that is used to stop the controller event loop - # self.__stop_thread_signal_event = Event() + self.__stop_thread_signal_event = Event() # Set-up process that will run the event loop self.__setup_new_event_listener_process() self.initialize_controller() - if ( - self.get_controller_connection_status() - == ControllerConnectionState.CONNECTED - ): - - self.logger.debug( - "Initialized handheld controller " - + '"' - + self.controller.name - + '"' - + " and located at path: " - + self.controller.path - ) - - elif ( - self.get_controller_connection_status() - == ControllerConnectionState.DISCONNECTED - ): - self.logger.debug( - "Failed to initialize a handheld controller, check USB connection" - ) - self.logger.debug("Tried the following available devices:") - self.logger.debug( - list(map(lambda device: InputDevice(device).name, list_devices())) - ) - def get_controller_connection_status(self) -> ControllerConnectionState: return ( ControllerConnectionState.CONNECTED @@ -93,10 +70,10 @@ def refresh(self, mode: ControlMode): :param mode: The current user requested mode for controlling the robot """ if mode == ControlMode.DIAGNOSTICS: - self.logger.debug("Terminating controller event handling process") if self.__controller_event_loop_handler_process.is_alive(): - # self.__stop_thread_signal_event.set() - self.__controller_event_loop_handler_process.terminate() + self.logger.debug("Terminating controller event handling process") + self.__stop_thread_signal_event.set() + self.__controller_event_loop_handler_process.join() elif mode == ControlMode.HANDHELD: self.logger.debug("Setting up new controller event handling process") @@ -122,14 +99,34 @@ def initialize_controller(self): controller = InputDevice(device) if ( controller is not None - and controller.name in ControllerConstants.CONTROLLER_NAME_CODES_MAP + and controller.name in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): + self.__stop_thread_signal_event.clear() self.controller = controller - self.controller_codes = ControllerConstants.CONTROLLER_NAME_CODES_MAP[ + self.controller_config = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ controller.name ] break + if self.get_controller_connection_status() == ControllerConnectionState.CONNECTED: + self.logger.debug( + "Initialized handheld controller " + + '"' + + self.controller.name + + '"' + + " and located at path: " + + self.controller.path + ) + + elif self.get_controller_connection_status() == ControllerConnectionState.DISCONNECTED: + self.logger.debug( + "Failed to initialize a handheld controller, check USB connection" + ) + self.logger.debug("Tried the following available devices:") + self.logger.debug( + list(map(lambda d: InputDevice(d).name, list_devices())) + ) + def __start_event_listener_process(self): # start the process for reading controller events if not self.__controller_event_loop_handler_process.is_alive(): @@ -142,7 +139,7 @@ def __setup_new_event_listener_process(self): # TODO (#3165): Use trace level self.logger here # self.logger.debug("Starting controller event loop process") - self.__controller_event_loop_handler_process = Process( + self.__controller_event_loop_handler_process = Thread( target=self.__event_loop, daemon=True ) @@ -153,25 +150,30 @@ def close(self): """ # TODO (#3165): Use trace level logging here # self.logger.debug("Shutdown down controller event loop process") - # self.__stop_thread_signal_event.set() - self.__controller_event_loop_handler_process.terminate() + self.__stop_thread_signal_event.set() + self.__controller_event_loop_handler_process.join() def __event_loop(self): # TODO (#3165): Use trace level self.logger here self.logger.debug("Starting handheld controller event handling loop") try: - for event in self.controller.read_loop(): + while True: + if self.__stop_thread_signal_event.is_set(): + return + event = self.controller.read_one() # TODO: is this a useful comment? # All events accumulate into a file and will be read back eventually, - # even if handheld mode is disabled. This time check ensures that + # even if handheld mode is disabled. This time based recency check ensures that # only the events that have occurred very recently are processed, and - # any events that occur before switching to handheld mode are ignored + # any events that occur before switching to handheld mode are dropped & ignored if ( + event is not None and time.time() - event.timestamp() - < ControllerConstants.INPUT_DELAY_THRESHOLD + < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) except OSError as ose: + self.__clear_controller() self.logger.debug( "Caught an OSError while reading handheld controller event loop!" ) @@ -179,12 +181,16 @@ def __event_loop(self): self.logger.debug("Check physical handheld controller USB connection!") return except Exception as e: + self.__clear_controller() self.logger.critical( "Caught an unexpected error while reading handheld controller event loop!" ) self.logger.critical("Error message: " + str(e)) return + def __clear_controller(self): + self.controller = None + def __process_event(self, event: InputEvent): # abs_event = categorize(event) @@ -201,49 +207,66 @@ def __process_event(self, event: InputEvent): + str(event.value) ) - if event.code == self.controller_codes[InputEventType.MOVE_X]: - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( - ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, event.value + # TODO: need to check type + + if event.code == self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE]: + self.logger.debug("type: " + str(event.type)) + self.logger.debug("move x code: " + str(event.code)) + self.logger.debug("config code: " + str(self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE])) + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( + self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.MAX_VALUE], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + ) ) - if event.code == self.controller_codes[InputEventType.MOVE_Y]: - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( - ControllerConstants.MAX_LINEAR_SPEED_METER_PER_S, event.value + if event.code == self.controller_config[RobotControlType.MOVE_Y][HandheldDeviceConfigKeys.CODE]: + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.MAX_VALUE], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S + ) ) - if event.code == self.controller_codes[InputEventType.ROTATE]: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - ControllerConstants.MAX_ANGULAR_SPEED_RAD_PER_S, event.value + if event.code == self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.CODE]: + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.MAX_VALUE], + scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S + ) ) - if event.code == self.controller_codes[InputEventType.KICK_POWER]: + if event.code == self.controller_config[RobotControlType.KICK_POWER][HandheldDeviceConfigKeys.CODE]: self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - if event.code == self.controller_codes[InputEventType.DRIBBLER_SPEED]: + if event.code == self.controller_config[RobotControlType.DRIBBLER_SPEED][HandheldDeviceConfigKeys.CODE]: self.dribbler_speed_accumulator = self.__parse_dribble_event_value( event.value ) - # Left and right triggers if ( - event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_1] - or event.code == self.controller_codes[InputEventType.DRIBBLER_ENABLE_2] + event.code == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][HandheldDeviceConfigKeys.CODE] + or event.code == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.CODE] ): - dribbler_enabled = self.__parse_dribbler_enabled_event_value(event.value) + dribbler_enabled = self.__parse_dribbler_enabled_event_value( + value=event.value, + max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.MAX_VALUE] + ) if dribbler_enabled: self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator - # "A" button if ( - event.code == self.controller_codes[InputEventType.KICK] + event.code == self.controller_config[RobotControlType.KICK][HandheldDeviceConfigKeys.CODE] and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator - # "A" button if ( - event.code == self.controller_codes[InputEventType.CHIP] + event.code == self.controller_config[RobotControlType.CHIP][HandheldDeviceConfigKeys.CODE] and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -252,34 +275,34 @@ def __process_event(self, event: InputEvent): ) @staticmethod - def __parse_move_event_value(event_value: float, scaling_factor: float) -> float: + def __parse_move_event_value(event_value: float, max_value: float, scaling_factor: float) -> float: if abs(event_value) < ( - ControllerConstants.DEADZONE_PERCENTAGE * scaling_factor + HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor ): return 0 else: return numpy.clip( - event_value, 0, ControllerConstants.XBOX_MAX_RANGE * scaling_factor + event_value, 0, max_value * scaling_factor ) @staticmethod - def __parse_dribbler_enabled_event_value(value: float) -> bool: - return value > (ControllerConstants.XBOX_BUTTON_MAX_RANGE / 2.0) + def __parse_dribbler_enabled_event_value(value: float, max_value: float) -> bool: + return value > (max_value / 2.0) @staticmethod def __parse_dribble_event_value(value: float) -> float: return numpy.clip( - value * ControllerConstants.DRIBBLER_STEPPER, + value * HandheldDeviceConstants.DRIBBLER_STEPPER, 0, - ControllerConstants.DRIBBLER_INDEFINITE_SPEED, + HandheldDeviceConstants.DRIBBLER_INDEFINITE_SPEED, ) @staticmethod def __parse_kick_event_value(value: float) -> float: return numpy.clip( - value * ControllerConstants.POWER_STEPPER, - ControllerConstants.MIN_POWER, - ControllerConstants.MAX_POWER, + value * HandheldDeviceConstants.POWER_STEPPER, + HandheldDeviceConstants.MIN_POWER, + HandheldDeviceConstants.MAX_POWER, ) diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 41ed5406fa..9aec3f0dc6 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -29,7 +29,6 @@ gl_trail_layer, ) - from software.thunderscope.common.proto_configuration_widget import ( ProtoConfigurationWidget, ) @@ -50,6 +49,7 @@ from software.thunderscope.robot_diagnostics.estop_view import EstopView from software.thunderscope.replay.proto_player import ProtoPlayer + ################################ # FULLSYSTEM RELATED WIDGETS # ################################ @@ -331,17 +331,6 @@ def setup_estop_view(proto_unix_io) -> EstopView: return estop_view -def setup_chicker_widget(proto_unix_io: ProtoUnixIO) -> ChickerWidget: - """Setup the chicker widget for robot diagnostics - - :param proto_unix_io: The proto unix io object - :returns: The chicker widget - - """ - chicker_widget = ChickerWidget(proto_unix_io) - return chicker_widget - - def setup_diagnostics_widget(proto_unix_io: ProtoUnixIO,) -> DriveAndDribblerWidget: """Setup the diagnostics widget, which contains the following sub-widgets: - ChickerWidget From 55e5f324708ac3e612a1f38ae09787c5ecd6ff52 Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 5 Apr 2024 00:41:37 -0700 Subject: [PATCH 082/138] formatting --- src/software/thunderscope/constants.py | 25 ++-- .../handheld_device_manager.py | 131 ++++++++++++------ 2 files changed, 101 insertions(+), 55 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index cca9bfab7e..031c2b1b44 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -331,7 +331,7 @@ class RobotControlType(Enum): DRIBBLER_ENABLE_2 = 9 -# TODO: come up with less ambiguous/wierd name +# TODO: come up with less ambiguous/weird name class HandheldDeviceConfigKeys(Enum): CODE = 1 MAX_VALUE = 2 @@ -344,59 +344,55 @@ class HandheldDeviceConstants: # Canonical: Left joystick X-axis RobotControlType.MOVE_X: { HandheldDeviceConfigKeys.CODE: 0, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, }, # Name: "ABS_Y", Type: EV_ABS # AbsInfo: value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick Y-axis RobotControlType.MOVE_Y: { HandheldDeviceConfigKeys.CODE: 1, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, }, # Name: "ABS_RX", Type: EV_ABS # AbsInfo: value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Right joystick X-axis RobotControlType.ROTATE: { HandheldDeviceConfigKeys.CODE: 3, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0 + HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, }, # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button - RobotControlType.KICK: { - HandheldDeviceConfigKeys.CODE: 304, - }, + RobotControlType.KICK: {HandheldDeviceConfigKeys.CODE: 304,}, # Name: "BTN_Y", Type: EV_KEY # Canonical: "Y" Button - RobotControlType.CHIP: { - HandheldDeviceConfigKeys.CODE: 308, - }, + RobotControlType.CHIP: {HandheldDeviceConfigKeys.CODE: 308,}, # Name: "ABS_HAT0X", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad X-axis RobotControlType.KICK_POWER: { HandheldDeviceConfigKeys.CODE: 16, - HandheldDeviceConfigKeys.MAX_VALUE: 1.0 + HandheldDeviceConfigKeys.MAX_VALUE: 1.0, }, # Name: "ABS_HAT0Y", Type: EV_ABS # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad Y-axis RobotControlType.DRIBBLER_SPEED: { HandheldDeviceConfigKeys.CODE: 17, - HandheldDeviceConfigKeys.MAX_VALUE: 1.0 + HandheldDeviceConfigKeys.MAX_VALUE: 1.0, }, # Name: "ABS_Z", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Left trigger RobotControlType.DRIBBLER_ENABLE_1: { HandheldDeviceConfigKeys.CODE: 2, - HandheldDeviceConfigKeys.MAX_VALUE: 1023.0 + HandheldDeviceConfigKeys.MAX_VALUE: 1023.0, }, # Name: "ABS_RZ", Type: EV_ABS # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Right trigger RobotControlType.DRIBBLER_ENABLE_2: { HandheldDeviceConfigKeys.CODE: 5, - HandheldDeviceConfigKeys.MAX_VALUE: 1023.0 + HandheldDeviceConfigKeys.MAX_VALUE: 1023.0, }, } @@ -405,7 +401,6 @@ class HandheldDeviceConstants: "Microsoft X-Box 360 pad": XboxConfig, } - # TODO remove these two XBOX_MAX_RANGE = 32768.0 XBOX_BUTTON_MAX_RANGE = 1024.0 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index b41421393b..2fd66ffe86 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -99,7 +99,8 @@ def initialize_controller(self): controller = InputDevice(device) if ( controller is not None - and controller.name in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + and controller.name + in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() self.controller = controller @@ -108,7 +109,10 @@ def initialize_controller(self): ] break - if self.get_controller_connection_status() == ControllerConnectionState.CONNECTED: + if ( + self.get_controller_connection_status() + == ControllerConnectionState.CONNECTED + ): self.logger.debug( "Initialized handheld controller " + '"' @@ -118,14 +122,15 @@ def initialize_controller(self): + self.controller.path ) - elif self.get_controller_connection_status() == ControllerConnectionState.DISCONNECTED: + elif ( + self.get_controller_connection_status() + == ControllerConnectionState.DISCONNECTED + ): self.logger.debug( "Failed to initialize a handheld controller, check USB connection" ) self.logger.debug("Tried the following available devices:") - self.logger.debug( - list(map(lambda d: InputDevice(d).name, list_devices())) - ) + self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __start_event_listener_process(self): # start the process for reading controller events @@ -167,8 +172,8 @@ def __event_loop(self): # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are dropped & ignored if ( - event is not None and - time.time() - event.timestamp() + event is not None + and time.time() - event.timestamp() < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) @@ -209,64 +214,110 @@ def __process_event(self, event: InputEvent): # TODO: need to check type - if event.code == self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE]: + if ( + event.code + == self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.CODE + ] + ): self.logger.debug("type: " + str(event.type)) self.logger.debug("move x code: " + str(event.code)) - self.logger.debug("config code: " + str(self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE])) - self.motor_control.direct_velocity_control.velocity.x_component_meters = ( - self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.MAX_VALUE], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + self.logger.debug( + "config code: " + + str( + self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.CODE + ] ) ) + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + ) - if event.code == self.controller_config[RobotControlType.MOVE_Y][HandheldDeviceConfigKeys.CODE]: - self.motor_control.direct_velocity_control.velocity.y_component_meters = ( - self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.MAX_VALUE], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S - ) + if ( + event.code + == self.controller_config[RobotControlType.MOVE_Y][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, ) - if event.code == self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.CODE]: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.MAX_VALUE], - scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S - ) + if ( + event.code + == self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S, ) - if event.code == self.controller_config[RobotControlType.KICK_POWER][HandheldDeviceConfigKeys.CODE]: + if ( + event.code + == self.controller_config[RobotControlType.KICK_POWER][ + HandheldDeviceConfigKeys.CODE + ] + ): self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - if event.code == self.controller_config[RobotControlType.DRIBBLER_SPEED][HandheldDeviceConfigKeys.CODE]: + if ( + event.code + == self.controller_config[RobotControlType.DRIBBLER_SPEED][ + HandheldDeviceConfigKeys.CODE + ] + ): self.dribbler_speed_accumulator = self.__parse_dribble_event_value( event.value ) if ( - event.code == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][HandheldDeviceConfigKeys.CODE] - or event.code == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ + HandheldDeviceConfigKeys.CODE + ] + or event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ + HandheldDeviceConfigKeys.CODE + ] ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( value=event.value, - max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.MAX_VALUE] + max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ + HandheldDeviceConfigKeys.MAX_VALUE + ], ) if dribbler_enabled: self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator if ( - event.code == self.controller_config[RobotControlType.KICK][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.KICK][ + HandheldDeviceConfigKeys.CODE + ] and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator if ( - event.code == self.controller_config[RobotControlType.CHIP][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.CHIP][ + HandheldDeviceConfigKeys.CODE + ] and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -275,15 +326,15 @@ def __process_event(self, event: InputEvent): ) @staticmethod - def __parse_move_event_value(event_value: float, max_value: float, scaling_factor: float) -> float: + def __parse_move_event_value( + event_value: float, max_value: float, scaling_factor: float + ) -> float: if abs(event_value) < ( HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor ): return 0 else: - return numpy.clip( - event_value, 0, max_value * scaling_factor - ) + return numpy.clip(event_value, 0, max_value * scaling_factor) @staticmethod def __parse_dribbler_enabled_event_value(value: float, max_value: float) -> bool: From 136b911f5e2aab7d6def6d87128624496b349d40 Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 5 Apr 2024 15:56:21 -0700 Subject: [PATCH 083/138] removed abs info and clean up, added event type check --- src/software/thunderscope/constants.py | 11 - .../handheld_device_manager.py | 219 ++++++++---------- 2 files changed, 103 insertions(+), 127 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 031c2b1b44..f6aa4a08f4 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -340,21 +340,18 @@ class HandheldDeviceConfigKeys(Enum): class HandheldDeviceConstants: XboxConfig = { # Name: "ABS_X", Type: EV_ABS - # AbsInfo: value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick X-axis RobotControlType.MOVE_X: { HandheldDeviceConfigKeys.CODE: 0, HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, }, # Name: "ABS_Y", Type: EV_ABS - # AbsInfo: value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Left joystick Y-axis RobotControlType.MOVE_Y: { HandheldDeviceConfigKeys.CODE: 1, HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, }, # Name: "ABS_RX", Type: EV_ABS - # AbsInfo: value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0 # Canonical: Right joystick X-axis RobotControlType.ROTATE: { HandheldDeviceConfigKeys.CODE: 3, @@ -367,28 +364,24 @@ class HandheldDeviceConstants: # Canonical: "Y" Button RobotControlType.CHIP: {HandheldDeviceConfigKeys.CODE: 308,}, # Name: "ABS_HAT0X", Type: EV_ABS - # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad X-axis RobotControlType.KICK_POWER: { HandheldDeviceConfigKeys.CODE: 16, HandheldDeviceConfigKeys.MAX_VALUE: 1.0, }, # Name: "ABS_HAT0Y", Type: EV_ABS - # AbsInfo: value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0 # Canonical: D-pad Y-axis RobotControlType.DRIBBLER_SPEED: { HandheldDeviceConfigKeys.CODE: 17, HandheldDeviceConfigKeys.MAX_VALUE: 1.0, }, # Name: "ABS_Z", Type: EV_ABS - # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Left trigger RobotControlType.DRIBBLER_ENABLE_1: { HandheldDeviceConfigKeys.CODE: 2, HandheldDeviceConfigKeys.MAX_VALUE: 1023.0, }, # Name: "ABS_RZ", Type: EV_ABS - # AbsInfo: value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0 # Canonical: Right trigger RobotControlType.DRIBBLER_ENABLE_2: { HandheldDeviceConfigKeys.CODE: 5, @@ -401,10 +394,6 @@ class HandheldDeviceConstants: "Microsoft X-Box 360 pad": XboxConfig, } - # TODO remove these two - XBOX_MAX_RANGE = 32768.0 - XBOX_BUTTON_MAX_RANGE = 1024.0 - INPUT_DELAY_THRESHOLD = 0.005 DEADZONE_PERCENTAGE = 0.30 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 2fd66ffe86..6137eee9de 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -20,6 +20,8 @@ # This is fixed in version pyqt 6.6.1 # `qt.qpa.input.events: scroll event from unregistered device 17` +# TODO: function docs + class HandheldDeviceManager(object): """ @@ -63,10 +65,9 @@ def get_controller_connection_status(self) -> ControllerConnectionState: def refresh(self, mode: ControlMode): """ Refresh this class. - Spawns a new process that listens and processes - handheld controller device events if Control.Mode is Diagnostics, - Otherwise, terminates the current listener process if it is running, - to avoid busy waiting + Spawns a new thread that runs the handheld device event + processing function if Control.Mode is Diagnostics, + otherwise, stops and joins the thread, if it is running :param mode: The current user requested mode for controlling the robot """ if mode == ControlMode.DIAGNOSTICS: @@ -84,7 +85,7 @@ def get_latest_primitive_controls(self): primitive = DirectControlPrimitive( motor_control=self.motor_control, power_control=self.power_control ) - # TODO: pre-emptive bugfix: need to reset controls, epecially power so that + # TODO: pre-emptive bugfix: need to reset controls, especially power so that # the control message isn't set to what is essentially auto-kick/chip # self.motor_control = MotorControl() self.power_control = PowerControl() @@ -93,7 +94,7 @@ def get_latest_primitive_controls(self): def initialize_controller(self): """ Attempt to initialize a controller. - The first controller that is recognized a valid controller will be used + The first controller that is recognized as a valid controller will be used """ for device in list_devices(): controller = InputDevice(device) @@ -198,132 +199,118 @@ def __clear_controller(self): def __process_event(self, event: InputEvent): - # abs_event = categorize(event) - event_type = ecodes.bytype[event.type][event.code] - - # if event.type == ecodes.EV_ABS or event.type == ecodes.EV_KEY: # TODO (#3165): Use trace level self.logger here self.logger.debug( "Processing controller event with type: " - + str(event_type) + + str(ecodes.bytype[event.type][event.code]) + ", with code: " + str(event.code) + ", and with value: " + str(event.value) ) - # TODO: need to check type - - if ( - event.code - == self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.CODE - ] - ): - self.logger.debug("type: " + str(event.type)) - self.logger.debug("move x code: " + str(event.code)) - self.logger.debug( - "config code: " - + str( - self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.CODE - ] + if event.type == ecodes.EV_ABS: + if ( + event.code + == self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, ) - ) - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, - ) - if ( - event.code - == self.controller_config[RobotControlType.MOVE_Y][ - HandheldDeviceConfigKeys.CODE - ] - ): - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, - ) + if ( + event.code + == self.controller_config[RobotControlType.MOVE_Y][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + ) - if ( - event.code - == self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.CODE - ] - ): - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S, - ) + if ( + event.code + == self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S, + ) - if ( - event.code - == self.controller_config[RobotControlType.KICK_POWER][ - HandheldDeviceConfigKeys.CODE - ] - ): - self.kick_power_accumulator = self.__parse_kick_event_value(event.value) + if ( + event.code + == self.controller_config[RobotControlType.KICK_POWER][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - if ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_SPEED][ - HandheldDeviceConfigKeys.CODE - ] - ): - self.dribbler_speed_accumulator = self.__parse_dribble_event_value( - event.value - ) + if ( + event.code + == self.controller_config[RobotControlType.DRIBBLER_SPEED][ + HandheldDeviceConfigKeys.CODE + ] + ): + self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( + event.value + ) - if ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ - HandheldDeviceConfigKeys.CODE - ] - or event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ - HandheldDeviceConfigKeys.CODE - ] - ): - dribbler_enabled = self.__parse_dribbler_enabled_event_value( - value=event.value, - max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - ) - if dribbler_enabled: - self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator + if ( + event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ + HandheldDeviceConfigKeys.CODE + ] + or event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ + HandheldDeviceConfigKeys.CODE + ] + ): + dribbler_enabled = self.__parse_dribbler_enabled_event_value( + value=event.value, + max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + ) + if dribbler_enabled: + self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator - if ( - event.code - == self.controller_config[RobotControlType.KICK][ - HandheldDeviceConfigKeys.CODE - ] - and event.value == 1 - ): - self.power_control.geneva_slot = 3 - self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator + if event.type == ecodes.EV_KEY: + if ( + event.code + == self.controller_config[RobotControlType.KICK][ + HandheldDeviceConfigKeys.CODE + ] + and event.value == 1 + ): + self.power_control.geneva_slot = 3 + self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator - if ( - event.code - == self.controller_config[RobotControlType.CHIP][ - HandheldDeviceConfigKeys.CODE - ] - and event.value == 1 - ): - self.power_control.geneva_slot = 3 - self.power_control.chicker.chip_distance_meters = ( - self.kick_power_accumulator - ) + if ( + event.code + == self.controller_config[RobotControlType.CHIP][ + HandheldDeviceConfigKeys.CODE + ] + and event.value == 1 + ): + self.power_control.geneva_slot = 3 + self.power_control.chicker.chip_distance_meters = ( + self.kick_power_accumulator + ) @staticmethod def __parse_move_event_value( @@ -341,7 +328,7 @@ def __parse_dribbler_enabled_event_value(value: float, max_value: float) -> bool return value > (max_value / 2.0) @staticmethod - def __parse_dribble_event_value(value: float) -> float: + def __parse_dribbler_event_value(value: float) -> float: return numpy.clip( value * HandheldDeviceConstants.DRIBBLER_STEPPER, 0, From e07e1aded68afbe5c7f5724fd0d4e54df13ff342 Mon Sep 17 00:00:00 2001 From: boris Date: Fri, 5 Apr 2024 16:20:42 -0700 Subject: [PATCH 084/138] formatting --- .../robot_diagnostics/handheld_device_manager.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 6137eee9de..9ea0cc2baa 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -282,12 +282,14 @@ def __process_event(self, event: InputEvent): ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( value=event.value, - max_value=self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ - HandheldDeviceConfigKeys.MAX_VALUE - ], + max_value=self.controller_config[ + RobotControlType.DRIBBLER_ENABLE_2 + ][HandheldDeviceConfigKeys.MAX_VALUE], ) if dribbler_enabled: - self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator + self.motor_control.dribbler_speed_rpm = ( + self.dribbler_speed_accumulator + ) if event.type == ecodes.EV_KEY: if ( @@ -298,7 +300,9 @@ def __process_event(self, event: InputEvent): and event.value == 1 ): self.power_control.geneva_slot = 3 - self.power_control.chicker.kick_speed_m_per_s = self.kick_power_accumulator + self.power_control.chicker.kick_speed_m_per_s = ( + self.kick_power_accumulator + ) if ( event.code From 003d3d375ef0fbe8f552ea040936fb3b489897f2 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:24:39 -0700 Subject: [PATCH 085/138] update signal name --- .../thunderscope/robot_diagnostics/robot_info.py | 10 +++++----- .../thunderscope/robot_diagnostics/robot_view.py | 10 +++++----- src/software/thunderscope/thunderscope_main.py | 4 ++-- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index d4a26a55eb..afc4ab7fc7 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -77,7 +77,7 @@ def __init__( self, robot_id: int, available_control_modes: List[IndividualRobotMode], - control_mode_signal: Type[QtCore.pyqtSignal], + individual_robot_control_mode_signal: Type[QtCore.pyqtSignal], ) -> None: """ Initialize a single robot's info widget @@ -85,13 +85,13 @@ def __init__( :param robot_id: id of robot whose info is being displayed :param available_control_modes: the currently available input modes for the robots according to what mode thunderscope is run in - :param control_mode_signal: signal that should be emitted when a robot changes control mode + :param individual_robot_control_mode_signal: signal that should be emitted when a robot changes control mode """ super().__init__() self.robot_id = robot_id - self.control_mode_signal = control_mode_signal + self.individual_robot_control_mode_signal = individual_robot_control_mode_signal self.time_of_last_robot_status = time.time() @@ -208,8 +208,8 @@ def create_control_mode_menu( ) control_mode_menu.currentIndexChanged.connect( - lambda mode, robot_id=self.robot_id: self.control_mode_signal.emit( - mode, robot_id + lambda mode, robot_id=self.robot_id: self.individual_robot_control_mode_signal.emit( + robot_id, mode ) ) diff --git a/src/software/thunderscope/robot_diagnostics/robot_view.py b/src/software/thunderscope/robot_diagnostics/robot_view.py index 55bce1e261..c6c16aa219 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_view.py +++ b/src/software/thunderscope/robot_diagnostics/robot_view.py @@ -26,7 +26,7 @@ def __init__( self, robot_id: int, available_control_modes: List[IndividualRobotMode], - control_mode_signal: Type[QtCore.pyqtSignal], + individual_robot_control_mode_signal: Type[QtCore.pyqtSignal], ): """ Sets up a Robot Info Widget and a Robot Status Widget for each robot @@ -36,7 +36,7 @@ def __init__( :param robot_id: id of the current robot :param available_control_modes: the currently available input modes for the robots according to what mode thunderscope is run in - :param control_mode_signal: the signal to emit when control mode changes + :param individual_robot_control_mode_signal: the signal to emit when control mode changes in order to communicate to robot communications """ super().__init__() @@ -45,7 +45,7 @@ def __init__( self.layout = QVBoxLayout() self.robot_info = RobotInfo( - robot_id, available_control_modes, control_mode_signal + robot_id, available_control_modes, individual_robot_control_mode_signal ) self.layout.addWidget(self.robot_info) @@ -88,7 +88,7 @@ class RobotView(QScrollArea): Contains signal to communicate with robot diagnostics when control mode changes """ - control_mode_signal = QtCore.pyqtSignal(int, int) + individual_robot_control_mode_signal = QtCore.pyqtSignal(int, IndividualRobotMode) def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: @@ -109,7 +109,7 @@ def __init__(self, available_control_modes: List[IndividualRobotMode]) -> None: for id in range(MAX_ROBOT_IDS_PER_SIDE): robot_view_widget = RobotViewComponent( - id, available_control_modes, self.control_mode_signal + id, available_control_modes, self.individual_robot_control_mode_signal ) self.robot_view_widgets.append(robot_view_widget) self.layout.addWidget(robot_view_widget) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index f93209c1c3..36937c3a18 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -329,8 +329,8 @@ if hasattr(tab, "widgets"): robot_view_widget = tab.find_widget("Robot View") if robot_view_widget is not None: - robot_view_widget.control_mode_signal.connect( - lambda robot_mode, robot_id: robot_communication.toggle_robot_control_mode( + robot_view_widget.individual_robot_control_mode_signal.connect( + lambda robot_id, robot_mode: robot_communication.toggle_individual_robot_control_mode( robot_id, robot_mode ) ) From 6f8f36772215df30af068288aaa9f7a0bc5ba87d Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:24:58 -0700 Subject: [PATCH 086/138] update logic for consistency and naming --- .../thunderscope/robot_communication.py | 64 ++++++++++--------- 1 file changed, 33 insertions(+), 31 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 0ec5ce66a1..b192305099 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -54,12 +54,12 @@ def __init__( self.running = False self.enable_radio = enable_radio - self.primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) + self.fullsystem_primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) self.diagnostics_primitive_buffer = ThreadSafeBuffer(1, Primitive) self.current_proto_unix_io.register_observer( - PrimitiveSet, self.primitive_buffer + PrimitiveSet, self.fullsystem_primitive_buffer ) self.current_proto_unix_io.register_observer( @@ -73,16 +73,20 @@ def __init__( target=self.__run_primitive_set, daemon=True ) - # map of robot id to the individual control mode + # dynamic map of robot id to the individual control mode self.robot_control_mode_map: dict[int, IndividualRobotMode] = {} - # map of robot id to the number of times to send a stop primitive - self.robot_stop_primitive_count_map: dict[int, int] = {} + # static map of robot id to stop primitive + self.robot_stop_primitives_map: dict[int, StopPrimitive] = {} + + # dynamic map of robot id to the number of times to send a stop primitive + self.robot_stop_primitive_send_count_map: dict[int, int] = {} # load control mode and stop primitive maps with default values for robot_id in range(NUM_ROBOTS): self.robot_control_mode_map[robot_id] = IndividualRobotMode.NONE - self.robot_stop_primitive_count_map[robot_id] = 0 + self.robot_stop_primitives_map[robot_id] = Primitive(stop=StopPrimitive()) + self.robot_stop_primitive_send_count_map[robot_id] = 0 # TODO: (#3174): move estop state management out of robot_communication self.estop_mode = estop_mode @@ -153,7 +157,7 @@ def toggle_keyboard_estop(self) -> None: ) ) - def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): + def toggle_individual_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ Changes the input mode for a robot between NONE, MANUAL, or AI If changing from MANUAL OR AI to NONE, add robot id to stop primitive @@ -164,7 +168,7 @@ def toggle_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): """ self.robot_control_mode_map[robot_id] = mode - self.robot_stop_primitive_count_map[robot_id] = ( + self.robot_stop_primitive_send_count_map[robot_id] = ( NUM_TIMES_SEND_STOP if mode == IndividualRobotMode.NONE else 0 ) @@ -228,8 +232,8 @@ def __run_primitive_set(self) -> None: """ while self.running: - # total primitives for all robots - robot_primitives = {} + # map of robot id to diagnostics/fullsystem primitive map + robot_primitives_map = {} # get the most recent diagnostics primitive diagnostics_primitive = self.diagnostics_primitive_buffer.get(block=False) @@ -241,34 +245,35 @@ def __run_primitive_set(self) -> None: if mode == IndividualRobotMode.MANUAL ) - # set robot primitives for diagnostics robots + # set diagnostics primitives for diagnostics robots for robot_id in diagnostics_robots: - robot_primitives[robot_id] = diagnostics_primitive + robot_primitives_map[robot_id] = diagnostics_primitive - # Get the primitives - primitive_set = self.primitive_buffer.get( + # get the most recent fullsystem primitives + fullsystem_primitive_set = self.fullsystem_primitive_buffer.get( block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S ) - fullsystem_primitives = dict(primitive_set.robot_primitives) + # filter for fullsystem controlled robots + fullsystem_robots = list( + robot_id + for robot_id, mode in self.robot_control_mode_map.items() + if mode == IndividualRobotMode.AI + ) - for robot_id in fullsystem_primitives.keys(): - if ( - # TODO: cast shouldn't be needed... - self.robot_control_mode_map[int(robot_id)] - == IndividualRobotMode.AI - ): - robot_primitives[robot_id] = fullsystem_primitives[robot_id] + # set fullsystem primitives for fullsystem robots + for robot_id in fullsystem_robots: + robot_primitives_map[robot_id] = fullsystem_primitive_set.robot_primitives[robot_id] # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives for ( robot_id, num_times_to_stop, - ) in self.robot_stop_primitive_count_map.items(): + ) in self.robot_stop_primitive_send_count_map.items(): if num_times_to_stop > 0: - robot_primitives[robot_id] = Primitive(stop=StopPrimitive()) - self.robot_stop_primitive_count_map[robot_id] = ( + robot_primitives_map[robot_id] = Primitive(stop=StopPrimitive()) + self.robot_stop_primitive_send_count_map[robot_id] = ( num_times_to_stop - 1 ) @@ -276,12 +281,9 @@ def __run_primitive_set(self) -> None: primitive_set = PrimitiveSet( time_sent=Timestamp(epoch_timestamp_seconds=time.time()), stay_away_from_ball=False, - robot_primitives=robot_primitives - if not self.should_send_stop - else { - robot_id: Primitive(stop=StopPrimitive()) - for robot_id in robot_primitives.keys() - }, + robot_primitives=self.robot_stop_primitives_map + if self.should_send_stop + else robot_primitives_map, sequence_number=self.sequence_number, ) From 3bb67dd528ea0a7ece6137758efbc95d58c0930c Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:25:15 -0700 Subject: [PATCH 087/138] moved button --- .../handheld_device_status_view.py | 59 +++++++++++++++---- 1 file changed, 47 insertions(+), 12 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index b24a99af7c..76672b4995 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -1,48 +1,83 @@ from enum import Enum +from typing import Type import pyqtgraph as pg + from proto.import_all_protos import * from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * +from pyqtgraph.Qt.QtCore import * import software.thunderscope.common.common_widgets as common_widgets from software.py_constants import * -class ControllerConnectionState(Enum): +class HandheldDeviceConnectionStatus(Enum): CONNECTED = 1 DISCONNECTED = 2 -class HandheldDeviceStatusView(QLabel): +class HandheldDeviceStatusView(QWidget): """Class to show whether a handheld controller is connected to thunderscope and initialized, or no controller is connected at all. """ - def __init__(self) -> None: + def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> None: super().__init__() - self.state: dict[ControllerConnectionState, (str, str)] = { - ControllerConnectionState.CONNECTED: ( + self.reinitialize_controller_signal = reinitialize_controller_signal + + self.handheld_device_status = QLabel() + + self.status_label_view_map: dict[HandheldDeviceConnectionStatus, (str, str)] = { + HandheldDeviceConnectionStatus.CONNECTED: ( "Handheld Controller is Connected & Initialized", "background-color: green", ), - ControllerConnectionState.DISCONNECTED: ( + HandheldDeviceConnectionStatus.DISCONNECTED: ( "No Handheld Controller is Connected...", "background-color: red", ), } - self.connected = False - self.set_view_state(ControllerConnectionState.DISCONNECTED) + self.group_box = QGroupBox() + + # initialize controller refresh button + self.handheld_device_reinitialize_button = QPushButton() + self.handheld_device_reinitialize_button.setText("Re-initialize Handheld Controller") + self.handheld_device_reinitialize_button.clicked.connect( + self.reinitialize_controller_signal + ) + + # layout for the whole widget + self.vbox_layout = QVBoxLayout() + + # layout for controller button & status + self.hbox_layout = QHBoxLayout() + + # set layout and spacing + self.hbox_layout.addWidget(self.handheld_device_status) + self.hbox_layout.addWidget(self.handheld_device_reinitialize_button) + # self.hbox_layout.setStretch(0, 4) + # self.hbox_layout.setStretch(1, 1) + + # set groupbox to contain layout with status and button + self.group_box.setLayout(self.hbox_layout) + + # add box to whole widget layout + self.vbox_layout.addWidget(self.group_box) + + # set the layout for the whole widget + self.setLayout(self.hbox_layout) + self.set_view_state(HandheldDeviceConnectionStatus.DISCONNECTED) def set_view_state( - self, state_discriminator=ControllerConnectionState.DISCONNECTED + self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED ): - self.setText(self.state[state_discriminator][0]) - self.setStyleSheet(self.state[state_discriminator][1]) + self.handheld_device_status.setText(self.status_label_view_map[connection_state][0]) + self.handheld_device_status.setStyleSheet(self.status_label_view_map[connection_state][1]) - def refresh(self, connected=ControllerConnectionState.DISCONNECTED) -> None: + def refresh(self, connected=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: """Refresh the label """ self.set_view_state(connected) From 7c22169436ade8cf7ba89f9f7c43120460f18494 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:25:32 -0700 Subject: [PATCH 088/138] split into two functions --- .../thunderscope/common/common_widgets.py | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/software/thunderscope/common/common_widgets.py b/src/software/thunderscope/common/common_widgets.py index e42d466852..15da2cea15 100644 --- a/src/software/thunderscope/common/common_widgets.py +++ b/src/software/thunderscope/common/common_widgets.py @@ -406,20 +406,24 @@ def set_table_data( table.setHorizontalHeaderLabels(horizontal_headers) -def change_button_state(button, enable): - """Change button color and clickable state. - +def enable_button(button): + """ + Disables the given button and sets ui to reflect that visually to the user. :param button: button to change the state of - :param enable: bool: if True: enable this button, if False: disable :returns: None + """ + button.setStyleSheet("background-color: White") + button.setEnabled(True) + +def disable_button(button): + """Disables the given button and sets ui to reflect that visually to the user. + + :param button: button to change the state of + :returns: None """ - if enable: - button.setStyleSheet("background-color: White") - button.setCheckable(True) - else: - button.setStyleSheet("background-color: Grey") - button.setCheckable(False) + button.setStyleSheet("background-color: Grey") + button.setEnabled(False) def disable_slider(slider): From e78be52fa32e9409c0754254383c7f2980c81386 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:25:44 -0700 Subject: [PATCH 089/138] field testing constants --- src/software/thunderscope/constants.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index f6aa4a08f4..5976c2d47d 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -391,12 +391,14 @@ class HandheldDeviceConstants: CONTROLLER_NAME_CONFIG_MAP = { "Microsoft Xbox One X pad": XboxConfig, + "Microsoft X-Box One S pad": XboxConfig, "Microsoft X-Box 360 pad": XboxConfig, } - INPUT_DELAY_THRESHOLD = 0.005 - DEADZONE_PERCENTAGE = 0.30 + INPUT_DELAY_THRESHOLD = 0.01 + DEADZONE_PERCENTAGE = 0.03 + # TODO: remove, not needed MAX_LINEAR_SPEED_METER_PER_S = 2.0 MAX_ANGULAR_SPEED_RAD_PER_S = 20.0 From d8a997daf9423560c6017cc22937ff1eac76d07c Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:26:04 -0700 Subject: [PATCH 090/138] update logic based on pr comments --- .../diagnostics_input_widget.py | 26 ++++++++----------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index fc6bdf1d34..70009eb421 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -1,13 +1,13 @@ -from typing import Callable +from typing import Callable, Type -from pyqtgraph.Qt.QtCore import * +from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * from software.py_constants import * import software.thunderscope.common.common_widgets as common_widgets from enum import IntEnum from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( - ControllerConnectionState, + HandheldDeviceConnectionStatus, ) @@ -20,8 +20,6 @@ class ControlMode(IntEnum): HANDHELD = 1 -# this class name doesnt make sense -# class DiagnosticsInputToggleWidget(QWidget): """ Class to allow the user to switch between Manual, XBox, and Fullsystem control through Thunderscope UI @@ -30,17 +28,17 @@ class DiagnosticsInputToggleWidget(QWidget): """ - # Signal to indicate if manual controls should be disabled based on boolean parameter - def __init__( - self, on_control_mode_switch_callback: Callable[[ControlMode], None] + self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal] ) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control - :param on_control_mode_switch_callback The signal to use for handling changes in input mode + :param on_control_mode_switch_callback The callback to use for handling changes in input mode """ super(DiagnosticsInputToggleWidget, self).__init__() + self.diagnostics_input_mode_signal = diagnostics_input_mode_signal + vbox_layout = QVBoxLayout() self.connect_options_group = QButtonGroup() @@ -53,10 +51,10 @@ def __init__( self.handheld_control_button = self.connect_options[ControlMode.HANDHELD] self.diagnostics_control_button.clicked.connect( - lambda: on_control_mode_switch_callback(ControlMode.DIAGNOSTICS) + lambda: self.diagnostics_input_mode_signal.emit(ControlMode.DIAGNOSTICS) ) self.handheld_control_button.clicked.connect( - lambda: on_control_mode_switch_callback(ControlMode.HANDHELD) + lambda: self.diagnostics_input_mode_signal.emit(ControlMode.HANDHELD) ) self.handheld_control_button.setEnabled(False) @@ -66,9 +64,7 @@ def __init__( self.setLayout(vbox_layout) - def refresh(self, status: ControllerConnectionState) -> None: - if status == ControllerConnectionState.DISCONNECTED: - self.diagnostics_control_button.click() + def refresh(self, status: HandheldDeviceConnectionStatus) -> None: self.handheld_control_button.setEnabled( - status == ControllerConnectionState.CONNECTED + status == HandheldDeviceConnectionStatus.CONNECTED ) From 874a5a99d6bda7ad38f65865d7329e409785f081 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:26:28 -0700 Subject: [PATCH 091/138] simplify --- .../robot_diagnostics/diagnostics_widget.py | 84 +++++++++++-------- 1 file changed, 48 insertions(+), 36 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index a0154a6a4f..fc239700cf 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -8,7 +8,7 @@ from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( HandheldDeviceStatusView, - ControllerConnectionState, + HandheldDeviceConnectionStatus, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputToggleWidget, @@ -18,54 +18,48 @@ HandheldDeviceManager, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( - DriveAndDribblerWidget, + DriveAndDribblerWidget, SliderType, ) class DiagnosticsWidget(QWidget): - # Signal to indicate if manual controls should be disabled based on enum mode parameter + # signal to indicate if manual controls should be disabled based on enum mode parameter diagnostics_input_mode_signal = pyqtSignal(ControlMode) + # signal to indicate that controller should be reinitialized + reinitialize_controller_signal = pyqtSignal() + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() self.logger = createLogger("RobotDiagnostics") self.proto_unix_io = proto_unix_io + + # default start with diagnostics input self.__control_mode = ControlMode.DIAGNOSTICS # initialize widgets - self.controller_status = HandheldDeviceStatusView() - self.drive_dribbler_widget = DriveAndDribblerWidget(proto_unix_io) + self.controller_status = HandheldDeviceStatusView(self.reinitialize_controller_signal) + self.drive_dribbler_widget = DriveAndDribblerWidget() self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( - lambda control_mode: self.__toggle_control_and_refresh(control_mode), - ) - - # initialize controller - self.controller_handler = HandheldDeviceManager(self.logger) - - # initialize controller refresh button - self.controller_refresh_button = QPushButton() - self.controller_refresh_button.setText("Re-initialize Handheld Controller") - self.controller_refresh_button.clicked.connect( - self.__refresh_controller_onclick_handler + self.diagnostics_input_mode_signal ) - # TODO: move to another class. - # Layout for the entire diagnostics tab - vbox_layout = QVBoxLayout() + # connect input mode signal with handler + self.diagnostics_input_mode_signal.connect(self.__toggle_control_and_refresh) - # Layout for controller button & status - hbox_layout = QHBoxLayout() + # connect controller reinitialization signal with handler + self.reinitialize_controller_signal.connect(self.__reinitialize_controller) - hbox_layout.addWidget(self.controller_status) - hbox_layout.addWidget(self.controller_refresh_button) - hbox_layout.setStretch(0, 4) - hbox_layout.setStretch(1, 1) + # initialize controller + self.controller_handler = HandheldDeviceManager(proto_unix_io, self.logger) - vbox_layout.addLayout(hbox_layout) + # layout for the entire diagnostics tab + vbox_layout = QVBoxLayout() + vbox_layout.addWidget(self.controller_status) vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) @@ -84,13 +78,17 @@ def refresh(self): Refreshes sub-widgets so that they display the most recent values, as well as the most recent values are available for use. """ + # get the most recent state on whether controller is connected from controller handler controller_status = self.controller_handler.get_controller_connection_status() + + # update controller status view with most recent controller status self.controller_status.refresh(controller_status) + + # update diagnostic ui with most recent recent controller status self.diagnostics_control_input_widget.refresh(controller_status) - mode = self.__control_mode - self.drive_dribbler_widget.refresh(mode) - self.chicker_widget.refresh(mode) + if controller_status == HandheldDeviceConnectionStatus.DISCONNECTED: + self.__toggle_control_and_refresh(ControlMode.DIAGNOSTICS) if self.__control_mode == ControlMode.DIAGNOSTICS: diagnostics_primitive = Primitive( @@ -103,16 +101,30 @@ def refresh(self): self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) elif self.__control_mode == ControlMode.HANDHELD: - diagnostics_primitive = ( - self.controller_handler.get_latest_primitive_controls() + # diagnostics_primitive = ( + # self.controller_handler.get_latest_primitive_controls() + # ) + + self.drive_dribbler_widget.set_slider( + SliderType.XVelocitySlider, + self.controller_handler.move_x + ) + self.drive_dribbler_widget.set_slider( + SliderType.YVelocitySlider, + self.controller_handler.move_y + ) + self.drive_dribbler_widget.set_slider( + SliderType.DribblerVelocitySlider, + self.controller_handler.dribbler_speed_accumulator ) - if diagnostics_primitive is not None: - self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - else: - self.proto_unix_io.send_proto(Primitive, StopPrimitive()) + # if diagnostics_primitive is not None: + # self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) + # else: + # self.proto_unix_io.send_proto(Primitive, StopPrimitive()) - def __refresh_controller_onclick_handler(self) -> None: + def __reinitialize_controller(self) -> None: + self.controller_handler.clear_controller() self.controller_handler.initialize_controller() # TODO: investigate why these methods aren't called on user hitting close button From 13d3e3db5306e7e7e96f9a9bf7574142096ec2e9 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:26:51 -0700 Subject: [PATCH 092/138] added public method to update sliders values --- .../drive_and_dribbler_widget.py | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 3a2b73d533..49fc5ee245 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -1,3 +1,5 @@ +from enum import Enum + from pyqtgraph.Qt.QtCore import Qt from pyqtgraph.Qt.QtWidgets import * import time @@ -7,15 +9,18 @@ from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.common import common_widgets from proto.import_all_protos import * -from software.thunderscope.proto_unix_io import ProtoUnixIO -class DriveAndDribblerWidget(QWidget): - def __init__(self, proto_unix_io: ProtoUnixIO) -> None: - """Initialize the widget to control the robot's motors +class SliderType(Enum): + XVelocitySlider = 0 + YVelocitySlider = 1 + DribblerVelocitySlider = 2 - :param proto_unix_io: the proto_unix_io object +class DriveAndDribblerWidget(QWidget): + def __init__(self) -> None: + """ + Initialize the widget to control the robot's motors """ super(DriveAndDribblerWidget, self).__init__() @@ -27,14 +32,27 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: QWidget.__init__(self) layout = QVBoxLayout() - self.proto_unix_io = proto_unix_io - # Add widgets to layout layout.addWidget(self.setup_direct_velocity("Drive")) layout.addWidget(self.setup_dribbler("Dribbler")) self.setLayout(layout) + def set_slider(self, slider: SliderType, value: float): + """ + Sets the x movement slider value + :param slider: the type of slider to set + :param value: the new value to set for the given slider type + :return: None + """ + print(value) + if slider == SliderType.XVelocitySlider: + self.x_velocity_slider.setValue(value) + elif slider == SliderType.YVelocitySlider: + self.x_velocity_slider.setValue(value) + elif slider == SliderType.DribblerVelocitySlider: + self.x_velocity_slider.setValue(value) + def refresh(self, mode: ControlMode) -> None: """ Refresh the widget's sliders @@ -62,19 +80,16 @@ def refresh(self, mode: ControlMode) -> None: def value_change(self, value: float) -> str: """ Converts the given float value to a string label - :param value: float value to be converted - """ value = float(value) value_str = "%.2f" % value return value_str def setup_direct_velocity(self, title: str) -> QGroupBox: - """Create a widget to control the direct velocity of the robot's motors - + """ + Create a widget to control the direct velocity of the robot's motors :param title: the name of the slider - """ group_box = QGroupBox(title) @@ -212,13 +227,13 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: ) # enable buttons - common_widgets.change_button_state(self.stop_and_reset_dribbler, True) - common_widgets.change_button_state(self.stop_and_reset_direct, True) + common_widgets.enable_button(self.stop_and_reset_dribbler) + common_widgets.enable_button(self.stop_and_reset_direct) elif mode == ControlMode.HANDHELD: # reset slider values and disconnect self.reset_all_sliders() - self.disconnect_sliders() + # self.disconnect_sliders() # disable all sliders by adding listener to keep slider value the same common_widgets.disable_slider(self.x_velocity_slider) @@ -227,8 +242,8 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: common_widgets.disable_slider(self.dribbler_speed_rpm_slider) # disable buttons - common_widgets.change_button_state(self.stop_and_reset_dribbler, False) - common_widgets.change_button_state(self.stop_and_reset_direct, False) + common_widgets.disable_button(self.stop_and_reset_dribbler) + common_widgets.disable_button(self.stop_and_reset_direct) def disconnect_sliders(self) -> None: """ From 54982924cbdd0bce02f74113a0c0f675f38ef7b3 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:27:14 -0700 Subject: [PATCH 093/138] field testing wip --- .../handheld_device_manager.py | 232 +++++++++++------- 1 file changed, 141 insertions(+), 91 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 9ea0cc2baa..ac0c6d4d8a 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -10,8 +10,9 @@ from proto.import_all_protos import * from software.thunderscope.constants import * from software.thunderscope.constants import HandheldDeviceConstants +from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( - ControllerConnectionState, + HandheldDeviceConnectionStatus, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode @@ -29,18 +30,24 @@ class HandheldDeviceManager(object): interpreting the device inputs into usable inputs for the robots. """ - def __init__(self, logger): + def __init__(self, proto_unix_io: ProtoUnixIO, logger): + self.proto_unix_io = proto_unix_io self.logger = logger self.controller: Optional[InputDevice] = None self.controller_config = None # Control primitives that are directly updated with # parsed controller inputs on every iteration of the event loop + self.move_x = 0 + self.move_y = 0 + self.ang_vel = 0 self.motor_control = MotorControl() self.power_control = PowerControl() + self.__initialize_empty_controls() + # These fields are here to temporarily persist the controller's input. - # They are read once once certain buttons are pressed on the controller, + # They are read once certain buttons are pressed on the controller, # and the values are inserted into the control primitives above. self.kick_power_accumulator = 0 self.dribbler_speed_accumulator = 0 @@ -55,11 +62,11 @@ def __init__(self, logger): self.initialize_controller() - def get_controller_connection_status(self) -> ControllerConnectionState: + def get_controller_connection_status(self) -> HandheldDeviceConnectionStatus: return ( - ControllerConnectionState.CONNECTED + HandheldDeviceConnectionStatus.CONNECTED if self.controller is not None - else ControllerConnectionState.DISCONNECTED + else HandheldDeviceConnectionStatus.DISCONNECTED ) def refresh(self, mode: ControlMode): @@ -81,15 +88,19 @@ def refresh(self, mode: ControlMode): self.__setup_new_event_listener_process() self.__start_event_listener_process() + # TODO not needed anymore def get_latest_primitive_controls(self): - primitive = DirectControlPrimitive( - motor_control=self.motor_control, power_control=self.power_control + diagnostics_primitive = Primitive( + direct_control=DirectControlPrimitive( + motor_control=self.motor_control, + power_control=self.power_control + ) ) # TODO: pre-emptive bugfix: need to reset controls, especially power so that # the control message isn't set to what is essentially auto-kick/chip - # self.motor_control = MotorControl() self.power_control = PowerControl() - return primitive + # self.motor_control = MotorControl() + return diagnostics_primitive def initialize_controller(self): """ @@ -99,9 +110,9 @@ def initialize_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name - in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + controller is not None + and controller.name + in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() self.controller = controller @@ -111,8 +122,8 @@ def initialize_controller(self): break if ( - self.get_controller_connection_status() - == ControllerConnectionState.CONNECTED + self.get_controller_connection_status() + == HandheldDeviceConnectionStatus.CONNECTED ): self.logger.debug( "Initialized handheld controller " @@ -124,8 +135,8 @@ def initialize_controller(self): ) elif ( - self.get_controller_connection_status() - == ControllerConnectionState.DISCONNECTED + self.get_controller_connection_status() + == HandheldDeviceConnectionStatus.DISCONNECTED ): self.logger.debug( "Failed to initialize a handheld controller, check USB connection" @@ -173,13 +184,27 @@ def __event_loop(self): # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are dropped & ignored if ( - event is not None - and time.time() - event.timestamp() - < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD + event is not None + and time.time() - event.timestamp() + < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) + + # else: + # self.__initialize_empty_controls() + # diagnostics_primitive = Primitive( + # direct_control=DirectControlPrimitive( + # motor_control=self.motor_control, + # power_control=self.power_control + # ) + # ) + # self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) + self.send_primitive() + + time.sleep(0.005) + except OSError as ose: - self.__clear_controller() + self.clear_controller() self.logger.debug( "Caught an OSError while reading handheld controller event loop!" ) @@ -187,98 +212,128 @@ def __event_loop(self): self.logger.debug("Check physical handheld controller USB connection!") return except Exception as e: - self.__clear_controller() + self.clear_controller() self.logger.critical( "Caught an unexpected error while reading handheld controller event loop!" ) self.logger.critical("Error message: " + str(e)) return - def __clear_controller(self): + def send_primitive(self): + motor_control = MotorControl() + + motor_control.dribbler_speed_rpm = 0 + motor_control.direct_velocity_control.velocity.x_component_meters = self.move_x + motor_control.direct_velocity_control.velocity.y_component_meters = self.move_y + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.ang_vel + ) + # self.logger.debug(motor_control) + + diagnostics_primitive = Primitive( + direct_control=DirectControlPrimitive( + motor_control=motor_control, + power_control=PowerControl() + ) + ) + self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) + + def clear_controller(self): self.controller = None + def __initialize_empty_controls(self): + self.move_x = 0.0 + self.move_y = 0.0 + self.ang_vel = 0.0 + def __process_event(self, event: InputEvent): # TODO (#3165): Use trace level self.logger here - self.logger.debug( - "Processing controller event with type: " - + str(ecodes.bytype[event.type][event.code]) - + ", with code: " - + str(event.code) - + ", and with value: " - + str(event.value) - ) + # self.logger.debug( + # "Processing controller event with type: " + # + str(ecodes.bytype[event.type][event.code]) + # + ", with code: " + # + str(event.code) + # + ", and with value: " + # + str(event.value) + # ) if event.type == ecodes.EV_ABS: + self.logger.debug( + "Processing controller event with type: " + + str(ecodes.bytype[event.type][event.code]) + + ", with code: " + + str(event.code) + + ", and with value: " + + str(event.value) + ) if ( - event.code - == self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE] ): - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( + # self.motor_control.direct_velocity_control.velocity.x_component_meters + self.move_x = self.__parse_move_event_value( event_value=event.value, max_value=self.controller_config[RobotControlType.MOVE_X][ HandheldDeviceConfigKeys.MAX_VALUE ], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + scaling_factor=self.constants.robot_max_speed_m_per_s, ) + self.ang_vel = 0.0 + self.logger.debug(self.move_x) if ( - event.code - == self.controller_config[RobotControlType.MOVE_Y][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.MOVE_Y][HandheldDeviceConfigKeys.CODE] ): - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( + # self.motor_control.direct_velocity_control.velocity.y_component_meters + self.move_y = self.__parse_move_event_value( event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][ + max_value=self.controller_config[RobotControlType.MOVE_Y][ HandheldDeviceConfigKeys.MAX_VALUE ], - scaling_factor=HandheldDeviceConstants.MAX_LINEAR_SPEED_METER_PER_S, + scaling_factor=self.constants.robot_max_speed_m_per_s, ) + self.ang_vel = 0.0 + self.logger.debug(self.move_y) if ( - event.code - == self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.CODE] ): - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=HandheldDeviceConstants.MAX_ANGULAR_SPEED_RAD_PER_S, + # self.motor_control.direct_velocity_control.angular_velocity.radians_per_second + self.ang_vel = ( + self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=self.constants.robot_max_ang_speed_rad_per_s, + ) ) + self.move_x = 0.0 + self.move_y = 0.0 + self.logger.debug(self.ang_vel) if ( - event.code - == self.controller_config[RobotControlType.KICK_POWER][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.KICK_POWER][HandheldDeviceConfigKeys.CODE] ): self.kick_power_accumulator = self.__parse_kick_event_value(event.value) if ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_SPEED][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.DRIBBLER_SPEED][HandheldDeviceConfigKeys.CODE] ): self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( event.value ) if ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ - HandheldDeviceConfigKeys.CODE - ] - or event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ - HandheldDeviceConfigKeys.CODE - ] + event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][HandheldDeviceConfigKeys.CODE] + or event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.CODE] ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( value=event.value, @@ -293,11 +348,9 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( - event.code - == self.controller_config[RobotControlType.KICK][ - HandheldDeviceConfigKeys.CODE - ] - and event.value == 1 + event.code + == self.controller_config[RobotControlType.KICK][HandheldDeviceConfigKeys.CODE] + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = ( @@ -305,11 +358,9 @@ def __process_event(self, event: InputEvent): ) if ( - event.code - == self.controller_config[RobotControlType.CHIP][ - HandheldDeviceConfigKeys.CODE - ] - and event.value == 1 + event.code + == self.controller_config[RobotControlType.CHIP][HandheldDeviceConfigKeys.CODE] + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.chip_distance_meters = ( @@ -318,36 +369,35 @@ def __process_event(self, event: InputEvent): @staticmethod def __parse_move_event_value( - event_value: float, max_value: float, scaling_factor: float + event_value: float, max_value: float, scaling_factor: float ) -> float: - if abs(event_value) < ( - HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor + # TODO: rename parameters to better names eg max_raw_value and scaling_multiplier + # TODO can redo logic for better flow probably + normalized = event_value / max_value * scaling_factor + if abs(normalized) < ( + HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor ): return 0 else: - return numpy.clip(event_value, 0, max_value * scaling_factor) + return normalized - @staticmethod - def __parse_dribbler_enabled_event_value(value: float, max_value: float) -> bool: + def __parse_dribbler_enabled_event_value(self, value: float, max_value: float) -> bool: return value > (max_value / 2.0) - @staticmethod - def __parse_dribbler_event_value(value: float) -> float: + def __parse_dribbler_event_value(self, value: float) -> float: return numpy.clip( value * HandheldDeviceConstants.DRIBBLER_STEPPER, 0, HandheldDeviceConstants.DRIBBLER_INDEFINITE_SPEED, ) - @staticmethod - def __parse_kick_event_value(value: float) -> float: + def __parse_kick_event_value(self, value: float) -> float: return numpy.clip( value * HandheldDeviceConstants.POWER_STEPPER, HandheldDeviceConstants.MIN_POWER, HandheldDeviceConstants.MAX_POWER, ) - # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], From ae461c696b8e18be744a1d8314ba22528ad2dc8e Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 12:55:44 -0700 Subject: [PATCH 094/138] update ubuntu 22 reqs --- environment_setup/ubuntu22_requirements.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/environment_setup/ubuntu22_requirements.txt b/environment_setup/ubuntu22_requirements.txt index 569ee2e8ea..56d7be38be 100644 --- a/environment_setup/ubuntu22_requirements.txt +++ b/environment_setup/ubuntu22_requirements.txt @@ -1,8 +1,8 @@ protobuf==3.20.2 pyqtgraph==0.13.3 autoflake==1.4 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 pyqtdarktheme==1.1.0 From e505684d21699f00dc95a4cff896d4c092e7bf91 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 13:19:06 -0700 Subject: [PATCH 095/138] sliders now update to match controller value on handheld mode --- .../robot_diagnostics/diagnostics_widget.py | 2 +- .../drive_and_dribbler_widget.py | 16 ++++++------ .../handheld_device_manager.py | 26 +++---------------- 3 files changed, 13 insertions(+), 31 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index fc239700cf..22bcd15030 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -115,7 +115,7 @@ def refresh(self): ) self.drive_dribbler_widget.set_slider( SliderType.DribblerVelocitySlider, - self.controller_handler.dribbler_speed_accumulator + self.controller_handler.ang_vel ) # if diagnostics_primitive is not None: diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 49fc5ee245..17b11b62a9 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -14,6 +14,7 @@ class SliderType(Enum): XVelocitySlider = 0 YVelocitySlider = 1 + AngularVelocitySlider = 2 DribblerVelocitySlider = 2 @@ -45,13 +46,12 @@ def set_slider(self, slider: SliderType, value: float): :param value: the new value to set for the given slider type :return: None """ - print(value) if slider == SliderType.XVelocitySlider: self.x_velocity_slider.setValue(value) elif slider == SliderType.YVelocitySlider: - self.x_velocity_slider.setValue(value) - elif slider == SliderType.DribblerVelocitySlider: - self.x_velocity_slider.setValue(value) + self.y_velocity_slider.setValue(value) + elif slider == SliderType.AngularVelocitySlider: + self.angular_velocity_slider.setValue(value) def refresh(self, mode: ControlMode) -> None: """ @@ -236,10 +236,10 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: # self.disconnect_sliders() # disable all sliders by adding listener to keep slider value the same - common_widgets.disable_slider(self.x_velocity_slider) - common_widgets.disable_slider(self.y_velocity_slider) - common_widgets.disable_slider(self.angular_velocity_slider) - common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + # common_widgets.disable_slider(self.x_velocity_slider) + # common_widgets.disable_slider(self.y_velocity_slider) + # common_widgets.disable_slider(self.angular_velocity_slider) + # common_widgets.disable_slider(self.dribbler_speed_rpm_slider) # disable buttons common_widgets.disable_button(self.stop_and_reset_dribbler) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index ac0c6d4d8a..224bc76c7d 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -178,7 +178,6 @@ def __event_loop(self): if self.__stop_thread_signal_event.is_set(): return event = self.controller.read_one() - # TODO: is this a useful comment? # All events accumulate into a file and will be read back eventually, # even if handheld mode is disabled. This time based recency check ensures that # only the events that have occurred very recently are processed, and @@ -190,18 +189,8 @@ def __event_loop(self): ): self.__process_event(event) - # else: - # self.__initialize_empty_controls() - # diagnostics_primitive = Primitive( - # direct_control=DirectControlPrimitive( - # motor_control=self.motor_control, - # power_control=self.power_control - # ) - # ) - # self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) self.send_primitive() - - time.sleep(0.005) + time.sleep(0.0005) except OSError as ose: self.clear_controller() @@ -228,7 +217,6 @@ def send_primitive(self): motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( self.ang_vel ) - # self.logger.debug(motor_control) diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( @@ -279,7 +267,6 @@ def __process_event(self, event: InputEvent): ], scaling_factor=self.constants.robot_max_speed_m_per_s, ) - self.ang_vel = 0.0 self.logger.debug(self.move_x) if ( @@ -294,8 +281,6 @@ def __process_event(self, event: InputEvent): ], scaling_factor=self.constants.robot_max_speed_m_per_s, ) - self.ang_vel = 0.0 - self.logger.debug(self.move_y) if ( event.code @@ -311,17 +296,14 @@ def __process_event(self, event: InputEvent): scaling_factor=self.constants.robot_max_ang_speed_rad_per_s, ) ) - self.move_x = 0.0 - self.move_y = 0.0 - self.logger.debug(self.ang_vel) - if ( + elif ( event.code == self.controller_config[RobotControlType.KICK_POWER][HandheldDeviceConfigKeys.CODE] ): self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - if ( + elif ( event.code == self.controller_config[RobotControlType.DRIBBLER_SPEED][HandheldDeviceConfigKeys.CODE] ): @@ -329,7 +311,7 @@ def __process_event(self, event: InputEvent): event.value ) - if ( + elif ( event.code == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][HandheldDeviceConfigKeys.CODE] or event.code From d357fc99b08c0c175501cf243cc409dca6ba9edd Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 13:19:55 -0700 Subject: [PATCH 096/138] formatting --- .../thunderscope/robot_communication.py | 8 +- .../diagnostics_input_widget.py | 6 +- .../robot_diagnostics/diagnostics_widget.py | 16 +-- .../handheld_device_manager.py | 107 ++++++++++-------- .../handheld_device_status_view.py | 12 +- 5 files changed, 87 insertions(+), 62 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index b192305099..dcdd38b5a9 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -157,7 +157,9 @@ def toggle_keyboard_estop(self) -> None: ) ) - def toggle_individual_robot_control_mode(self, robot_id: int, mode: IndividualRobotMode): + def toggle_individual_robot_control_mode( + self, robot_id: int, mode: IndividualRobotMode + ): """ Changes the input mode for a robot between NONE, MANUAL, or AI If changing from MANUAL OR AI to NONE, add robot id to stop primitive @@ -263,7 +265,9 @@ def __run_primitive_set(self) -> None: # set fullsystem primitives for fullsystem robots for robot_id in fullsystem_robots: - robot_primitives_map[robot_id] = fullsystem_primitive_set.robot_primitives[robot_id] + robot_primitives_map[ + robot_id + ] = fullsystem_primitive_set.robot_primitives[robot_id] # sends a final stop primitive to all disconnected robots and removes them from list # in order to prevent robots acting on cached old primitives diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 70009eb421..346a4ca4e8 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -1,4 +1,4 @@ -from typing import Callable, Type +from typing import Type from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * @@ -28,9 +28,7 @@ class DiagnosticsInputToggleWidget(QWidget): """ - def __init__( - self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal] - ) -> None: + def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control :param on_control_mode_switch_callback The callback to use for handling changes in input mode diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 22bcd15030..e59efc78e9 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -18,7 +18,8 @@ HandheldDeviceManager, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( - DriveAndDribblerWidget, SliderType, + DriveAndDribblerWidget, + SliderType, ) @@ -40,7 +41,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.__control_mode = ControlMode.DIAGNOSTICS # initialize widgets - self.controller_status = HandheldDeviceStatusView(self.reinitialize_controller_signal) + self.controller_status = HandheldDeviceStatusView( + self.reinitialize_controller_signal + ) self.drive_dribbler_widget = DriveAndDribblerWidget() self.chicker_widget = ChickerWidget(proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( @@ -106,16 +109,13 @@ def refresh(self): # ) self.drive_dribbler_widget.set_slider( - SliderType.XVelocitySlider, - self.controller_handler.move_x + SliderType.XVelocitySlider, self.controller_handler.move_x ) self.drive_dribbler_widget.set_slider( - SliderType.YVelocitySlider, - self.controller_handler.move_y + SliderType.YVelocitySlider, self.controller_handler.move_y ) self.drive_dribbler_widget.set_slider( - SliderType.DribblerVelocitySlider, - self.controller_handler.ang_vel + SliderType.DribblerVelocitySlider, self.controller_handler.ang_vel ) # if diagnostics_primitive is not None: diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 224bc76c7d..280972ec85 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -92,8 +92,7 @@ def refresh(self, mode: ControlMode): def get_latest_primitive_controls(self): diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( - motor_control=self.motor_control, - power_control=self.power_control + motor_control=self.motor_control, power_control=self.power_control ) ) # TODO: pre-emptive bugfix: need to reset controls, especially power so that @@ -110,9 +109,9 @@ def initialize_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name - in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + controller is not None + and controller.name + in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() self.controller = controller @@ -122,8 +121,8 @@ def initialize_controller(self): break if ( - self.get_controller_connection_status() - == HandheldDeviceConnectionStatus.CONNECTED + self.get_controller_connection_status() + == HandheldDeviceConnectionStatus.CONNECTED ): self.logger.debug( "Initialized handheld controller " @@ -135,8 +134,8 @@ def initialize_controller(self): ) elif ( - self.get_controller_connection_status() - == HandheldDeviceConnectionStatus.DISCONNECTED + self.get_controller_connection_status() + == HandheldDeviceConnectionStatus.DISCONNECTED ): self.logger.debug( "Failed to initialize a handheld controller, check USB connection" @@ -183,9 +182,9 @@ def __event_loop(self): # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are dropped & ignored if ( - event is not None - and time.time() - event.timestamp() - < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD + event is not None + and time.time() - event.timestamp() + < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) @@ -220,8 +219,7 @@ def send_primitive(self): diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( - motor_control=motor_control, - power_control=PowerControl() + motor_control=motor_control, power_control=PowerControl() ) ) self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) @@ -256,8 +254,10 @@ def __process_event(self, event: InputEvent): + str(event.value) ) if ( - event.code - == self.controller_config[RobotControlType.MOVE_X][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.MOVE_X][ + HandheldDeviceConfigKeys.CODE + ] ): # self.motor_control.direct_velocity_control.velocity.x_component_meters self.move_x = self.__parse_move_event_value( @@ -270,8 +270,10 @@ def __process_event(self, event: InputEvent): self.logger.debug(self.move_x) if ( - event.code - == self.controller_config[RobotControlType.MOVE_Y][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.MOVE_Y][ + HandheldDeviceConfigKeys.CODE + ] ): # self.motor_control.direct_velocity_control.velocity.y_component_meters self.move_y = self.__parse_move_event_value( @@ -283,39 +285,47 @@ def __process_event(self, event: InputEvent): ) if ( - event.code - == self.controller_config[RobotControlType.ROTATE][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.CODE + ] ): # self.motor_control.direct_velocity_control.angular_velocity.radians_per_second - self.ang_vel = ( - self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=self.constants.robot_max_ang_speed_rad_per_s, - ) + self.ang_vel = self.__parse_move_event_value( + event_value=event.value, + max_value=self.controller_config[RobotControlType.ROTATE][ + HandheldDeviceConfigKeys.MAX_VALUE + ], + scaling_factor=self.constants.robot_max_ang_speed_rad_per_s, ) elif ( - event.code - == self.controller_config[RobotControlType.KICK_POWER][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.KICK_POWER][ + HandheldDeviceConfigKeys.CODE + ] ): self.kick_power_accumulator = self.__parse_kick_event_value(event.value) elif ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_SPEED][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.DRIBBLER_SPEED][ + HandheldDeviceConfigKeys.CODE + ] ): self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( event.value ) elif ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][HandheldDeviceConfigKeys.CODE] - or event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][HandheldDeviceConfigKeys.CODE] + event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ + HandheldDeviceConfigKeys.CODE + ] + or event.code + == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ + HandheldDeviceConfigKeys.CODE + ] ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( value=event.value, @@ -330,9 +340,11 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( - event.code - == self.controller_config[RobotControlType.KICK][HandheldDeviceConfigKeys.CODE] - and event.value == 1 + event.code + == self.controller_config[RobotControlType.KICK][ + HandheldDeviceConfigKeys.CODE + ] + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = ( @@ -340,9 +352,11 @@ def __process_event(self, event: InputEvent): ) if ( - event.code - == self.controller_config[RobotControlType.CHIP][HandheldDeviceConfigKeys.CODE] - and event.value == 1 + event.code + == self.controller_config[RobotControlType.CHIP][ + HandheldDeviceConfigKeys.CODE + ] + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.chip_distance_meters = ( @@ -351,19 +365,21 @@ def __process_event(self, event: InputEvent): @staticmethod def __parse_move_event_value( - event_value: float, max_value: float, scaling_factor: float + event_value: float, max_value: float, scaling_factor: float ) -> float: # TODO: rename parameters to better names eg max_raw_value and scaling_multiplier # TODO can redo logic for better flow probably normalized = event_value / max_value * scaling_factor if abs(normalized) < ( - HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor + HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor ): return 0 else: return normalized - def __parse_dribbler_enabled_event_value(self, value: float, max_value: float) -> bool: + def __parse_dribbler_enabled_event_value( + self, value: float, max_value: float + ) -> bool: return value > (max_value / 2.0) def __parse_dribbler_event_value(self, value: float) -> float: @@ -380,6 +396,7 @@ def __parse_kick_event_value(self, value: float) -> float: HandheldDeviceConstants.MAX_POWER, ) + # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 76672b4995..ddb10b4483 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -44,7 +44,9 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N # initialize controller refresh button self.handheld_device_reinitialize_button = QPushButton() - self.handheld_device_reinitialize_button.setText("Re-initialize Handheld Controller") + self.handheld_device_reinitialize_button.setText( + "Re-initialize Handheld Controller" + ) self.handheld_device_reinitialize_button.clicked.connect( self.reinitialize_controller_signal ) @@ -74,8 +76,12 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N def set_view_state( self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED ): - self.handheld_device_status.setText(self.status_label_view_map[connection_state][0]) - self.handheld_device_status.setStyleSheet(self.status_label_view_map[connection_state][1]) + self.handheld_device_status.setText( + self.status_label_view_map[connection_state][0] + ) + self.handheld_device_status.setStyleSheet( + self.status_label_view_map[connection_state][1] + ) def refresh(self, connected=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: """Refresh the label From 29ad0250c957d3b291be700871056bb1c18ee5de Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 18:15:58 -0700 Subject: [PATCH 097/138] updated to use builtin dict instead of rebuilding a wheel --- src/software/thunderscope/constants.py | 94 ++++++++++++------- .../handheld_device_manager.py | 85 +++++------------ 2 files changed, 83 insertions(+), 96 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 5976c2d47d..92625dacbc 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -1,3 +1,6 @@ +from dataclasses import dataclass +from typing import Optional + from pyqtgraph.Qt import QtCore, QtGui from proto.import_all_protos import * from enum import Enum, IntEnum @@ -337,62 +340,83 @@ class HandheldDeviceConfigKeys(Enum): MAX_VALUE = 2 +# nomenclature: HDIE stands for Handheld-Device-Input-Event + +@dataclass +class HDIEField: + event_code: int + max_value: Optional[float] + + +@dataclass +class HDIEConfig: + move_x: HDIEField + move_y: HDIEField + move_rot: HDIEField + kick: HDIEField + chip: HDIEField + chicker_power: HDIEField + dribbler_speed: HDIEField + primary_dribbler_enable: HDIEField + secondary_dribbler_enable: HDIEField + + class HandheldDeviceConstants: - XboxConfig = { + NewXboxConfig = HDIEConfig( # Name: "ABS_X", Type: EV_ABS # Canonical: Left joystick X-axis - RobotControlType.MOVE_X: { - HandheldDeviceConfigKeys.CODE: 0, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, - }, + move_x=HDIEField( + event_code=0, + max_value=32767.0 + ), # Name: "ABS_Y", Type: EV_ABS # Canonical: Left joystick Y-axis - RobotControlType.MOVE_Y: { - HandheldDeviceConfigKeys.CODE: 1, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, - }, + move_y=HDIEField( + event_code=1, + max_value=32767.0 + ), # Name: "ABS_RX", Type: EV_ABS # Canonical: Right joystick X-axis - RobotControlType.ROTATE: { - HandheldDeviceConfigKeys.CODE: 3, - HandheldDeviceConfigKeys.MAX_VALUE: 32767.0, - }, + move_rot=HDIEField( + event_code=3, + max_value=32767.0 + ), # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button - RobotControlType.KICK: {HandheldDeviceConfigKeys.CODE: 304,}, + kick=HDIEField(304), # Name: "BTN_Y", Type: EV_KEY # Canonical: "Y" Button - RobotControlType.CHIP: {HandheldDeviceConfigKeys.CODE: 308,}, + chip=HDIEField(308), # Name: "ABS_HAT0X", Type: EV_ABS # Canonical: D-pad X-axis - RobotControlType.KICK_POWER: { - HandheldDeviceConfigKeys.CODE: 16, - HandheldDeviceConfigKeys.MAX_VALUE: 1.0, - }, + chicker_power=HDIEField( + event_code=16, + max_value=1.0 + ), # Name: "ABS_HAT0Y", Type: EV_ABS # Canonical: D-pad Y-axis - RobotControlType.DRIBBLER_SPEED: { - HandheldDeviceConfigKeys.CODE: 17, - HandheldDeviceConfigKeys.MAX_VALUE: 1.0, - }, + dribbler_speed=HDIEField( + event_code=17, + max_value=1.0 + ), # Name: "ABS_Z", Type: EV_ABS # Canonical: Left trigger - RobotControlType.DRIBBLER_ENABLE_1: { - HandheldDeviceConfigKeys.CODE: 2, - HandheldDeviceConfigKeys.MAX_VALUE: 1023.0, - }, + primary_dribbler_enable=HDIEField( + event_code=2, + max_value=1023.0 + ), # Name: "ABS_RZ", Type: EV_ABS # Canonical: Right trigger - RobotControlType.DRIBBLER_ENABLE_2: { - HandheldDeviceConfigKeys.CODE: 5, - HandheldDeviceConfigKeys.MAX_VALUE: 1023.0, - }, - } + secondary_dribbler_enable=HDIEField( + event_code=5, + max_value=1023.0 + ) + ) CONTROLLER_NAME_CONFIG_MAP = { - "Microsoft Xbox One X pad": XboxConfig, - "Microsoft X-Box One S pad": XboxConfig, - "Microsoft X-Box 360 pad": XboxConfig, + "Microsoft Xbox One X pad": NewXboxConfig, + "Microsoft X-Box One S pad": NewXboxConfig, + "Microsoft X-Box 360 pad": NewXboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 280972ec85..c82c6f9d6f 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -17,11 +17,7 @@ from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -# TODO: the following is logged on controller connection during runtime: -# This is fixed in version pyqt 6.6.1 -# `qt.qpa.input.events: scroll event from unregistered device 17` - -# TODO: function docs +# TODO: function docs lollll class HandheldDeviceManager(object): @@ -88,7 +84,6 @@ def refresh(self, mode: ControlMode): self.__setup_new_event_listener_process() self.__start_event_listener_process() - # TODO not needed anymore def get_latest_primitive_controls(self): diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( @@ -115,7 +110,7 @@ def initialize_controller(self): ): self.__stop_thread_signal_event.clear() self.controller = controller - self.controller_config = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ + self.controller_config: HDIEConfig = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ controller.name ] break @@ -222,6 +217,7 @@ def send_primitive(self): motor_control=motor_control, power_control=PowerControl() ) ) + self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) def clear_controller(self): @@ -254,64 +250,43 @@ def __process_event(self, event: InputEvent): + str(event.value) ) if ( - event.code - == self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.CODE - ] + event.code == self.controller_config.move_x.event_code ): # self.motor_control.direct_velocity_control.velocity.x_component_meters self.move_x = self.__parse_move_event_value( event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_X][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=self.constants.robot_max_speed_m_per_s, + max_value=self.controller_config.move_x.max_value, + normalizing_multiplier=self.constants.robot_max_speed_m_per_s, ) self.logger.debug(self.move_x) if ( - event.code - == self.controller_config[RobotControlType.MOVE_Y][ - HandheldDeviceConfigKeys.CODE - ] + event.code == self.controller_config.move_y.event_code ): # self.motor_control.direct_velocity_control.velocity.y_component_meters self.move_y = self.__parse_move_event_value( event_value=event.value, - max_value=self.controller_config[RobotControlType.MOVE_Y][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=self.constants.robot_max_speed_m_per_s, + max_value=self.controller_config.move_y.max_value, + normalizing_multiplier=self.constants.robot_max_speed_m_per_s, ) if ( - event.code - == self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.CODE - ] + event.code == self.controller_config.move_rot.event_code ): # self.motor_control.direct_velocity_control.angular_velocity.radians_per_second self.ang_vel = self.__parse_move_event_value( event_value=event.value, - max_value=self.controller_config[RobotControlType.ROTATE][ - HandheldDeviceConfigKeys.MAX_VALUE - ], - scaling_factor=self.constants.robot_max_ang_speed_rad_per_s, + max_value=self.controller_config.move_rot.max_value, + normalizing_multiplier=self.constants.robot_max_ang_speed_rad_per_s, ) elif ( - event.code - == self.controller_config[RobotControlType.KICK_POWER][ - HandheldDeviceConfigKeys.CODE - ] + event.code == self.controller_config.chicker_power.event_code ): self.kick_power_accumulator = self.__parse_kick_event_value(event.value) elif ( - event.code - == self.controller_config[RobotControlType.DRIBBLER_SPEED][ - HandheldDeviceConfigKeys.CODE - ] + event.code == self.controller_config.dribbler_speed.event_code ): self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( event.value @@ -319,19 +294,13 @@ def __process_event(self, event: InputEvent): elif ( event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_1][ - HandheldDeviceConfigKeys.CODE - ] + == self.controller_config.primary_dribbler_enable.event_code or event.code - == self.controller_config[RobotControlType.DRIBBLER_ENABLE_2][ - HandheldDeviceConfigKeys.CODE - ] + == self.controller_config.secondary_dribbler_enable.event_code ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( value=event.value, - max_value=self.controller_config[ - RobotControlType.DRIBBLER_ENABLE_2 - ][HandheldDeviceConfigKeys.MAX_VALUE], + max_value=self.controller_config.primary_dribbler_enable.max_value, ) if dribbler_enabled: self.motor_control.dribbler_speed_rpm = ( @@ -341,9 +310,7 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( event.code - == self.controller_config[RobotControlType.KICK][ - HandheldDeviceConfigKeys.CODE - ] + == self.controller_config.kick.event_code and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -353,9 +320,7 @@ def __process_event(self, event: InputEvent): if ( event.code - == self.controller_config[RobotControlType.CHIP][ - HandheldDeviceConfigKeys.CODE - ] + == self.controller_config.chip.event_code and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -365,17 +330,15 @@ def __process_event(self, event: InputEvent): @staticmethod def __parse_move_event_value( - event_value: float, max_value: float, scaling_factor: float + event_value: float, max_value: float, normalizing_multiplier: float ) -> float: - # TODO: rename parameters to better names eg max_raw_value and scaling_multiplier - # TODO can redo logic for better flow probably - normalized = event_value / max_value * scaling_factor - if abs(normalized) < ( - HandheldDeviceConstants.DEADZONE_PERCENTAGE * scaling_factor + relative_value = event_value / max_value + if abs(relative_value) < ( + HandheldDeviceConstants.DEADZONE_PERCENTAGE ): return 0 else: - return normalized + return relative_value * normalizing_multiplier def __parse_dribbler_enabled_event_value( self, value: float, max_value: float From bf8355770a9984628a682f301af21b00105c6011 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 18:16:28 -0700 Subject: [PATCH 098/138] type error --- src/software/thunderscope/constants.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 92625dacbc..97e44ca99f 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -383,10 +383,10 @@ class HandheldDeviceConstants: ), # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button - kick=HDIEField(304), + kick=HDIEField(304, None), # Name: "BTN_Y", Type: EV_KEY # Canonical: "Y" Button - chip=HDIEField(308), + chip=HDIEField(308, None), # Name: "ABS_HAT0X", Type: EV_ABS # Canonical: D-pad X-axis chicker_power=HDIEField( From ddc84d0705e5189afa2ed2a83374e020c772c456 Mon Sep 17 00:00:00 2001 From: boris Date: Tue, 16 Apr 2024 18:16:53 -0700 Subject: [PATCH 099/138] formatting --- src/software/thunderscope/constants.py | 60 +++++++------------ .../handheld_device_manager.py | 33 +++------- 2 files changed, 29 insertions(+), 64 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 97e44ca99f..45425e64cf 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -342,6 +342,7 @@ class HandheldDeviceConfigKeys(Enum): # nomenclature: HDIE stands for Handheld-Device-Input-Event + @dataclass class HDIEField: event_code: int @@ -350,37 +351,28 @@ class HDIEField: @dataclass class HDIEConfig: - move_x: HDIEField - move_y: HDIEField - move_rot: HDIEField - kick: HDIEField - chip: HDIEField - chicker_power: HDIEField - dribbler_speed: HDIEField - primary_dribbler_enable: HDIEField - secondary_dribbler_enable: HDIEField + move_x: HDIEField + move_y: HDIEField + move_rot: HDIEField + kick: HDIEField + chip: HDIEField + chicker_power: HDIEField + dribbler_speed: HDIEField + primary_dribbler_enable: HDIEField + secondary_dribbler_enable: HDIEField class HandheldDeviceConstants: NewXboxConfig = HDIEConfig( # Name: "ABS_X", Type: EV_ABS # Canonical: Left joystick X-axis - move_x=HDIEField( - event_code=0, - max_value=32767.0 - ), + move_x=HDIEField(event_code=0, max_value=32767.0), # Name: "ABS_Y", Type: EV_ABS # Canonical: Left joystick Y-axis - move_y=HDIEField( - event_code=1, - max_value=32767.0 - ), + move_y=HDIEField(event_code=1, max_value=32767.0), # Name: "ABS_RX", Type: EV_ABS # Canonical: Right joystick X-axis - move_rot=HDIEField( - event_code=3, - max_value=32767.0 - ), + move_rot=HDIEField(event_code=3, max_value=32767.0), # Name: "BTN_A", Type: EV_KEY # Canonical: "A" Button kick=HDIEField(304, None), @@ -389,34 +381,22 @@ class HandheldDeviceConstants: chip=HDIEField(308, None), # Name: "ABS_HAT0X", Type: EV_ABS # Canonical: D-pad X-axis - chicker_power=HDIEField( - event_code=16, - max_value=1.0 - ), + chicker_power=HDIEField(event_code=16, max_value=1.0), # Name: "ABS_HAT0Y", Type: EV_ABS # Canonical: D-pad Y-axis - dribbler_speed=HDIEField( - event_code=17, - max_value=1.0 - ), + dribbler_speed=HDIEField(event_code=17, max_value=1.0), # Name: "ABS_Z", Type: EV_ABS # Canonical: Left trigger - primary_dribbler_enable=HDIEField( - event_code=2, - max_value=1023.0 - ), + primary_dribbler_enable=HDIEField(event_code=2, max_value=1023.0), # Name: "ABS_RZ", Type: EV_ABS # Canonical: Right trigger - secondary_dribbler_enable=HDIEField( - event_code=5, - max_value=1023.0 - ) + secondary_dribbler_enable=HDIEField(event_code=5, max_value=1023.0), ) CONTROLLER_NAME_CONFIG_MAP = { - "Microsoft Xbox One X pad": NewXboxConfig, - "Microsoft X-Box One S pad": NewXboxConfig, - "Microsoft X-Box 360 pad": NewXboxConfig, + "Microsoft Xbox One X pad": NewXboxConfig, + "Microsoft X-Box One S pad": NewXboxConfig, + "Microsoft X-Box 360 pad": NewXboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index c82c6f9d6f..d5ccc51300 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -249,9 +249,7 @@ def __process_event(self, event: InputEvent): + ", and with value: " + str(event.value) ) - if ( - event.code == self.controller_config.move_x.event_code - ): + if event.code == self.controller_config.move_x.event_code: # self.motor_control.direct_velocity_control.velocity.x_component_meters self.move_x = self.__parse_move_event_value( event_value=event.value, @@ -260,9 +258,7 @@ def __process_event(self, event: InputEvent): ) self.logger.debug(self.move_x) - if ( - event.code == self.controller_config.move_y.event_code - ): + if event.code == self.controller_config.move_y.event_code: # self.motor_control.direct_velocity_control.velocity.y_component_meters self.move_y = self.__parse_move_event_value( event_value=event.value, @@ -270,9 +266,7 @@ def __process_event(self, event: InputEvent): normalizing_multiplier=self.constants.robot_max_speed_m_per_s, ) - if ( - event.code == self.controller_config.move_rot.event_code - ): + if event.code == self.controller_config.move_rot.event_code: # self.motor_control.direct_velocity_control.angular_velocity.radians_per_second self.ang_vel = self.__parse_move_event_value( event_value=event.value, @@ -280,21 +274,16 @@ def __process_event(self, event: InputEvent): normalizing_multiplier=self.constants.robot_max_ang_speed_rad_per_s, ) - elif ( - event.code == self.controller_config.chicker_power.event_code - ): + elif event.code == self.controller_config.chicker_power.event_code: self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - elif ( - event.code == self.controller_config.dribbler_speed.event_code - ): + elif event.code == self.controller_config.dribbler_speed.event_code: self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( event.value ) elif ( - event.code - == self.controller_config.primary_dribbler_enable.event_code + event.code == self.controller_config.primary_dribbler_enable.event_code or event.code == self.controller_config.secondary_dribbler_enable.event_code ): @@ -309,8 +298,7 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( - event.code - == self.controller_config.kick.event_code + event.code == self.controller_config.kick.event_code and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -319,8 +307,7 @@ def __process_event(self, event: InputEvent): ) if ( - event.code - == self.controller_config.chip.event_code + event.code == self.controller_config.chip.event_code and event.value == 1 ): self.power_control.geneva_slot = 3 @@ -333,9 +320,7 @@ def __parse_move_event_value( event_value: float, max_value: float, normalizing_multiplier: float ) -> float: relative_value = event_value / max_value - if abs(relative_value) < ( - HandheldDeviceConstants.DEADZONE_PERCENTAGE - ): + if abs(relative_value) < (HandheldDeviceConstants.DEADZONE_PERCENTAGE): return 0 else: return relative_value * normalizing_multiplier From 2f89a5bedae8668a1682b3f61ccc11fc3d9b89f2 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 08:51:42 -0700 Subject: [PATCH 100/138] update deadzone perc --- src/software/thunderscope/constants.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 45425e64cf..0e8d34c4d6 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -400,7 +400,7 @@ class HandheldDeviceConstants: } INPUT_DELAY_THRESHOLD = 0.01 - DEADZONE_PERCENTAGE = 0.03 + DEADZONE_PERCENTAGE = 0.10 # TODO: remove, not needed MAX_LINEAR_SPEED_METER_PER_S = 2.0 From fb18730ca010a7e1ccb574e16cd6abe2310a06c4 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 08:52:05 -0700 Subject: [PATCH 101/138] we do a lil cleanup --- .../robot_diagnostics/handheld_device_manager.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index d5ccc51300..484ba4f5f0 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -288,7 +288,7 @@ def __process_event(self, event: InputEvent): == self.controller_config.secondary_dribbler_enable.event_code ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( - value=event.value, + event_value=event.value, max_value=self.controller_config.primary_dribbler_enable.max_value, ) if dribbler_enabled: @@ -320,15 +320,15 @@ def __parse_move_event_value( event_value: float, max_value: float, normalizing_multiplier: float ) -> float: relative_value = event_value / max_value - if abs(relative_value) < (HandheldDeviceConstants.DEADZONE_PERCENTAGE): + if abs(relative_value) < HandheldDeviceConstants.DEADZONE_PERCENTAGE: return 0 else: return relative_value * normalizing_multiplier def __parse_dribbler_enabled_event_value( - self, value: float, max_value: float + self, event_value: float, max_value: float ) -> bool: - return value > (max_value / 2.0) + return event_value / max_value > 0.5 def __parse_dribbler_event_value(self, value: float) -> float: return numpy.clip( From 51e992eab77729106787306b2e29ba9fb0c9816f Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 20:15:28 -0700 Subject: [PATCH 102/138] renamed function to use snake_case --- src/software/field_tests/field_test_fixture.py | 4 ++-- src/software/field_tests/movement_robot_field_test.py | 4 ++-- src/software/field_tests/pivot_kick_field_test.py | 4 ++-- src/software/logger/logger.py | 10 +++++----- src/software/networking/unix/threaded_unix_listener.py | 4 ++-- src/software/simulated_tests/simulated_test_fixture.py | 4 ++-- src/software/simulated_tests/tbots_test_runner.py | 4 ++-- src/software/thunderscope/thread_safe_buffer.py | 4 ++-- 8 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 7e8cac7e37..c69f8cebb9 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -16,7 +16,7 @@ from software.thunderscope.binary_context_managers.simulator import Simulator from software.thunderscope.binary_context_managers.game_controller import Gamecontroller from software.thunderscope.replay.proto_logger import ProtoLogger -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software.thunderscope.thunderscope_config import configure_field_test_view @@ -25,7 +25,7 @@ from software.thunderscope.estop_helpers import get_estop_config from software.py_constants import * -logger = createLogger(__name__) +logger = create_logger(__name__) WORLD_BUFFER_TIMEOUT = 5.0 PROCESS_BUFFER_DELAY_S = 0.01 diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index a904c86095..90c679855f 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -7,12 +7,12 @@ from software.field_tests.field_test_fixture import * from software.simulated_tests.simulated_test_fixture import * -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software.simulated_tests.robot_enters_region import RobotEventuallyEntersRegion from proto.message_translation.tbots_protobuf import create_world_state import math -logger = createLogger(__name__) +logger = create_logger(__name__) # TODO 2908: Support running this test in both simulator or field mode diff --git a/src/software/field_tests/pivot_kick_field_test.py b/src/software/field_tests/pivot_kick_field_test.py index c8e4110f10..ec031d8ff8 100644 --- a/src/software/field_tests/pivot_kick_field_test.py +++ b/src/software/field_tests/pivot_kick_field_test.py @@ -8,11 +8,11 @@ from software.field_tests.field_test_fixture import * from software.simulated_tests.simulated_test_fixture import * -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software.simulated_tests.robot_enters_region import RobotEventuallyEntersRegion from proto.message_translation.tbots_protobuf import create_world_state -logger = createLogger(__name__) +logger = create_logger(__name__) def test_pivot_kick(field_test_runner): diff --git a/src/software/logger/logger.py b/src/software/logger/logger.py index e555223226..4e12721306 100644 --- a/src/software/logger/logger.py +++ b/src/software/logger/logger.py @@ -1,15 +1,15 @@ -import logging +from logging import Logger, basicConfig, getLogger, INFO -logging.basicConfig( - level=logging.INFO, +basicConfig( + level=INFO, format="%(asctime)s - [%(levelname)s] - [%(threadName)s] - %(name)s - (%(filename)s).%(funcName)s(%(lineno)d) - %(message)s", ) -def createLogger(name): +def create_logger(name) -> Logger: """Create a logger given the name of the logger :returns: A Logger """ - return logging.getLogger(name) + return getLogger(name) diff --git a/src/software/networking/unix/threaded_unix_listener.py b/src/software/networking/unix/threaded_unix_listener.py index 49d7c2564b..20f42e22fd 100644 --- a/src/software/networking/unix/threaded_unix_listener.py +++ b/src/software/networking/unix/threaded_unix_listener.py @@ -2,10 +2,10 @@ import queue import socketserver from threading import Thread -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software import py_constants -logger = createLogger(__name__) +logger = create_logger(__name__) class ThreadedUnixListener: diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 104f1a407a..8f17f41fb0 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -26,9 +26,9 @@ from software.thunderscope.thunderscope_config import configure_simulated_test_view from software.thunderscope.replay.proto_logger import ProtoLogger -from software.logger.logger import createLogger +from software.logger.logger import create_logger -logger = createLogger(__name__) +logger = create_logger(__name__) LAUNCH_DELAY_S = 0.1 WORLD_BUFFER_TIMEOUT = 0.5 diff --git a/src/software/simulated_tests/tbots_test_runner.py b/src/software/simulated_tests/tbots_test_runner.py index 2979404463..c273dc11cd 100644 --- a/src/software/simulated_tests/tbots_test_runner.py +++ b/src/software/simulated_tests/tbots_test_runner.py @@ -3,14 +3,14 @@ import pytest from proto.import_all_protos import * -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.time_provider import TimeProvider from software.thunderscope.thunderscope import Thunderscope from proto.ssl_gc_common_pb2 import Team -logger = createLogger(__name__) +logger = create_logger(__name__) class TbotsTestRunner(TimeProvider): diff --git a/src/software/thunderscope/thread_safe_buffer.py b/src/software/thunderscope/thread_safe_buffer.py index 22376d9884..67735ed105 100644 --- a/src/software/thunderscope/thread_safe_buffer.py +++ b/src/software/thunderscope/thread_safe_buffer.py @@ -1,5 +1,5 @@ import queue -from software.logger.logger import createLogger +from software.logger.logger import create_logger from typing import Type, Optional from google.protobuf.message import Message @@ -32,7 +32,7 @@ def __init__( :param log_overrun: False """ - self.logger = createLogger(protobuf_type.DESCRIPTOR.name + " Buffer") + self.logger = create_logger(protobuf_type.DESCRIPTOR.name + " Buffer") self.queue = queue.Queue(buffer_size) self.protobuf_type = protobuf_type self.log_overrun = log_overrun From 73cad9e4fdc69b29fd967bc06bc5063582e96092 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 20:16:32 -0700 Subject: [PATCH 103/138] add logger to robot comms, rename attr --- src/software/thunderscope/BUILD | 1 + src/software/thunderscope/robot_communication.py | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index 58b7ce2632..63ed6bfc5d 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -157,6 +157,7 @@ py_library( ], deps = [ "//software/thunderscope:constants", + "//software/logger:py_logger", ], ) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index dcdd38b5a9..2cbe8e1176 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -7,6 +7,7 @@ from google.protobuf.message import Message from pyqtgraph.Qt import QtCore from proto.import_all_protos import * +from software.logger.logger import create_logger from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -54,12 +55,14 @@ def __init__( self.running = False self.enable_radio = enable_radio - self.fullsystem_primitive_buffer = ThreadSafeBuffer(1, PrimitiveSet) + self.logger = create_logger("RobotCommunication") + + self.fullsystem_primitive_set_buffer = ThreadSafeBuffer(1, PrimitiveSet) self.diagnostics_primitive_buffer = ThreadSafeBuffer(1, Primitive) self.current_proto_unix_io.register_observer( - PrimitiveSet, self.fullsystem_primitive_buffer + PrimitiveSet, self.fullsystem_primitive_set_buffer ) self.current_proto_unix_io.register_observer( @@ -148,7 +151,7 @@ def toggle_keyboard_estop(self) -> None: """ if self.estop_mode == EstopMode.KEYBOARD_ESTOP: self.estop_is_playing = not self.estop_is_playing - print( + self.logger.debug( "Keyboard Estop changed to " + ( f"\x1b[32mPLAY \x1b[0m" @@ -252,7 +255,7 @@ def __run_primitive_set(self) -> None: robot_primitives_map[robot_id] = diagnostics_primitive # get the most recent fullsystem primitives - fullsystem_primitive_set = self.fullsystem_primitive_buffer.get( + fullsystem_primitive_set = self.fullsystem_primitive_set_buffer.get( block=True, timeout=ROBOT_COMMUNICATIONS_TIMEOUT_S ) From 908a52d7d8d8eee523c8df059396f772b793744a Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:43:06 -0700 Subject: [PATCH 104/138] wrapped with enum constructor --- src/software/thunderscope/robot_diagnostics/robot_info.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index afc4ab7fc7..2f06b35f73 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -209,7 +209,7 @@ def create_control_mode_menu( control_mode_menu.currentIndexChanged.connect( lambda mode, robot_id=self.robot_id: self.individual_robot_control_mode_signal.emit( - robot_id, mode + robot_id, IndividualRobotMode(mode) ) ) From 7b9f4747f6a3e675b07d542fad677dd9b5c2e77e Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:43:10 -0700 Subject: [PATCH 105/138] add todo --- src/proto/primitive.proto | 1 + 1 file changed, 1 insertion(+) diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index b679a65127..d592355033 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -92,6 +92,7 @@ message PowerControl }; } + // TODO: Remove this message type - unused & obsolete message GenevaControl { float angle_deg = 1; From 29819ecf3a95021d29c31a9f32d0b191993a43e9 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:43:47 -0700 Subject: [PATCH 106/138] comment groupbox for now, collapses whole ui for some reason --- .../robot_diagnostics/handheld_device_status_view.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index ddb10b4483..08d751918f 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -40,7 +40,6 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N ), } - self.group_box = QGroupBox() # initialize controller refresh button self.handheld_device_reinitialize_button = QPushButton() @@ -60,14 +59,14 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N # set layout and spacing self.hbox_layout.addWidget(self.handheld_device_status) self.hbox_layout.addWidget(self.handheld_device_reinitialize_button) - # self.hbox_layout.setStretch(0, 4) - # self.hbox_layout.setStretch(1, 1) + self.hbox_layout.setStretch(0, 4) + self.hbox_layout.setStretch(1, 1) # set groupbox to contain layout with status and button - self.group_box.setLayout(self.hbox_layout) + # self.group_box.setLayout(self.hbox_layout) # add box to whole widget layout - self.vbox_layout.addWidget(self.group_box) + # self.vbox_layout.addWidget(self.group_box) # set the layout for the whole widget self.setLayout(self.hbox_layout) From 06c63e475821f922b32978c175fe9ac154c725c2 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:48:52 -0700 Subject: [PATCH 107/138] split method into a bunch of smaller ones, renamed a few other methods --- .../drive_and_dribbler_widget.py | 87 ++++++++++--------- 1 file changed, 45 insertions(+), 42 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 17b11b62a9..65777b1ac1 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -34,24 +34,22 @@ def __init__(self) -> None: layout = QVBoxLayout() # Add widgets to layout - layout.addWidget(self.setup_direct_velocity("Drive")) - layout.addWidget(self.setup_dribbler("Dribbler")) + layout.addWidget(self.__setup_direct_velocity("Drive")) + layout.addWidget(self.__setup_dribbler("Dribbler")) self.setLayout(layout) - def set_slider(self, slider: SliderType, value: float): - """ - Sets the x movement slider value - :param slider: the type of slider to set - :param value: the new value to set for the given slider type - :return: None - """ - if slider == SliderType.XVelocitySlider: - self.x_velocity_slider.setValue(value) - elif slider == SliderType.YVelocitySlider: - self.y_velocity_slider.setValue(value) - elif slider == SliderType.AngularVelocitySlider: - self.angular_velocity_slider.setValue(value) + def set_x_velocity_slider(self, value: float): + self.x_velocity_slider.setValue(value) + + def set_y_velocity_slider(self, value: float): + self.y_velocity_slider.setValue(value) + + def set_angular_velocity_slider(self, value: float): + self.angular_velocity_slider.setValue(value) + + def set_dribbler_velocity_slider(self, value: float): + self.dribbler_speed_rpm_slider.setValue(value) def refresh(self, mode: ControlMode) -> None: """ @@ -70,14 +68,16 @@ def refresh(self, mode: ControlMode) -> None: self.motor_control.direct_velocity_control.velocity.x_component_meters = ( self.x_velocity_slider.value() ) + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( self.y_velocity_slider.value() ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( self.angular_velocity_slider.value() ) - def value_change(self, value: float) -> str: + def __value_change_handler(self, value: float) -> str: """ Converts the given float value to a string label :param value: float value to be converted @@ -86,7 +86,7 @@ def value_change(self, value: float) -> str: value_str = "%.2f" % value return value_str - def setup_direct_velocity(self, title: str) -> QGroupBox: + def __setup_direct_velocity(self, title: str) -> QGroupBox: """ Create a widget to control the direct velocity of the robot's motors :param title: the name of the slider @@ -131,17 +131,17 @@ def setup_direct_velocity(self, title: str) -> QGroupBox: # add listener functions for sliders to update label with slider value common_widgets.enable_slider( - self.x_velocity_slider, self.x_velocity_label, self.value_change + self.x_velocity_slider, self.x_velocity_label, self.__value_change_handler ) common_widgets.enable_slider( - self.y_velocity_slider, self.y_velocity_label, self.value_change + self.y_velocity_slider, self.y_velocity_label, self.__value_change_handler ) common_widgets.enable_slider( - self.angular_velocity_slider, self.angular_velocity_label, self.value_change + self.angular_velocity_slider, self.angular_velocity_label, self.__value_change_handler ) self.stop_and_reset_direct = common_widgets.create_push_button("Stop and Reset") - self.stop_and_reset_direct.clicked.connect(self.reset_direct_sliders) + self.stop_and_reset_direct.clicked.connect(self.__reset_direct_sliders) dbox.addLayout(x_layout) dbox.addLayout(y_layout) @@ -154,7 +154,7 @@ def setup_direct_velocity(self, title: str) -> QGroupBox: return group_box - def setup_dribbler(self, title: str) -> QGroupBox: + def __setup_dribbler(self, title: str) -> QGroupBox: """Create a widget to control the dribbler RPM :param title: the name of the slider @@ -180,13 +180,13 @@ def setup_dribbler(self, title: str) -> QGroupBox: common_widgets.enable_slider( self.dribbler_speed_rpm_slider, self.dribbler_speed_rpm_label, - self.value_change, + self.__value_change_handler, ) self.stop_and_reset_dribbler = common_widgets.create_push_button( "Stop and Reset" ) - self.stop_and_reset_dribbler.clicked.connect(self.reset_dribbler_slider) + self.stop_and_reset_dribbler.clicked.connect(self.__reset_dribbler_slider) dbox.addLayout(dribbler_layout) dbox.addWidget( @@ -200,30 +200,33 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: """ Disables or enables all sliders and buttons depending on ControlMode parameter. Sliders are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode - Updates listener functions and stylesheets accordingly :param mode: ControlMode enum parameter """ if mode == ControlMode.DIAGNOSTICS: # disconnect all sliders - self.disconnect_sliders() + self.__disconnect_sliders() # enable all sliders by adding listener to update label with slider value common_widgets.enable_slider( - self.x_velocity_slider, self.x_velocity_label, self.value_change + self.x_velocity_slider, + self.x_velocity_label, + self.__value_change_handler ) common_widgets.enable_slider( - self.y_velocity_slider, self.y_velocity_label, self.value_change + self.y_velocity_slider, + self.y_velocity_label, + self.__value_change_handler ) common_widgets.enable_slider( self.angular_velocity_slider, self.angular_velocity_label, - self.value_change, + self.__value_change_handler, ) common_widgets.enable_slider( self.dribbler_speed_rpm_slider, self.dribbler_speed_rpm_label, - self.value_change, + self.__value_change_handler, ) # enable buttons @@ -232,20 +235,20 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: elif mode == ControlMode.HANDHELD: # reset slider values and disconnect - self.reset_all_sliders() - # self.disconnect_sliders() + self.__reset_all_sliders() + self.__disconnect_sliders() # disable all sliders by adding listener to keep slider value the same - # common_widgets.disable_slider(self.x_velocity_slider) - # common_widgets.disable_slider(self.y_velocity_slider) - # common_widgets.disable_slider(self.angular_velocity_slider) - # common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + common_widgets.disable_slider(self.x_velocity_slider) + common_widgets.disable_slider(self.y_velocity_slider) + common_widgets.disable_slider(self.angular_velocity_slider) + common_widgets.disable_slider(self.dribbler_speed_rpm_slider) # disable buttons common_widgets.disable_button(self.stop_and_reset_dribbler) common_widgets.disable_button(self.stop_and_reset_direct) - def disconnect_sliders(self) -> None: + def __disconnect_sliders(self) -> None: """ Disconnect listener for changing values for all sliders """ @@ -254,22 +257,22 @@ def disconnect_sliders(self) -> None: self.angular_velocity_slider.valueChanged.disconnect() self.dribbler_speed_rpm_slider.valueChanged.disconnect() - def reset_direct_sliders(self) -> None: + def __reset_direct_sliders(self) -> None: """Reset direct sliders back to 0 """ self.x_velocity_slider.setValue(0) self.y_velocity_slider.setValue(0) self.angular_velocity_slider.setValue(0) - def reset_dribbler_slider(self) -> None: + def __reset_dribbler_slider(self) -> None: """ Reset the dribbler slider back to 0 """ self.dribbler_speed_rpm_slider.setValue(0) - def reset_all_sliders(self) -> None: + def __reset_all_sliders(self) -> None: """ Reset all sliders back to 0 """ - self.reset_direct_sliders() - self.reset_dribbler_slider() + self.__reset_direct_sliders() + self.__reset_dribbler_slider() From 96ecf2ae822fed387ed4c7a8308f09398e8dbfd3 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:51:03 -0700 Subject: [PATCH 108/138] refactored logic - added more signals to propagate ui changes, added handlers, simplified some other methods --- .../robot_diagnostics/diagnostics_widget.py | 90 +++++++++++-------- 1 file changed, 54 insertions(+), 36 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index e59efc78e9..5e92f996df 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -2,7 +2,7 @@ from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * from proto.import_all_protos import * -from software.logger.logger import createLogger +from software.logger.logger import create_logger from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget @@ -28,21 +28,27 @@ class DiagnosticsWidget(QWidget): diagnostics_input_mode_signal = pyqtSignal(ControlMode) # signal to indicate that controller should be reinitialized - reinitialize_controller_signal = pyqtSignal() + reinitialize_handheld_device_signal = pyqtSignal() + + # signal to indicate that controller was disconnected + handheld_device_connection_status_signal = pyqtSignal(HandheldDeviceConnectionStatus) def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() - self.logger = createLogger("RobotDiagnostics") + self.logger = create_logger("RobotDiagnostics") self.proto_unix_io = proto_unix_io # default start with diagnostics input self.__control_mode = ControlMode.DIAGNOSTICS + # default start with disconnected controller + self.__handheld_device_status = HandheldDeviceConnectionStatus.DISCONNECTED + # initialize widgets - self.controller_status = HandheldDeviceStatusView( - self.reinitialize_controller_signal + self.handheld_device_status_widget = HandheldDeviceStatusView( + self.reinitialize_handheld_device_signal ) self.drive_dribbler_widget = DriveAndDribblerWidget() self.chicker_widget = ChickerWidget(proto_unix_io) @@ -51,47 +57,53 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) # connect input mode signal with handler - self.diagnostics_input_mode_signal.connect(self.__toggle_control_and_refresh) + self.diagnostics_input_mode_signal.connect(self.__toggle_control) + + # connect handheld device reinitialization signal with handler + self.reinitialize_handheld_device_signal.connect(self.__reinitialize_controller) - # connect controller reinitialization signal with handler - self.reinitialize_controller_signal.connect(self.__reinitialize_controller) + # connect handheld device connection status toggle signal with handler + self.handheld_device_connection_status_signal.connect(self.__toggle_handheld_device_connection_status) # initialize controller - self.controller_handler = HandheldDeviceManager(proto_unix_io, self.logger) + self.controller_handler = HandheldDeviceManager( + self.proto_unix_io, # TODO: proto shouldn't be passed down + self.logger, + self.handheld_device_connection_status_signal + ) # layout for the entire diagnostics tab vbox_layout = QVBoxLayout() - vbox_layout.addWidget(self.controller_status) + vbox_layout.addWidget(self.handheld_device_status_widget) vbox_layout.addWidget(self.diagnostics_control_input_widget) vbox_layout.addWidget(self.drive_dribbler_widget) vbox_layout.addWidget(self.chicker_widget) self.setLayout(vbox_layout) - def __toggle_control_and_refresh(self, mode: ControlMode): + def __toggle_control(self, mode: ControlMode): self.__control_mode = mode + self.controller_handler.refresh(self.__control_mode) - self.drive_dribbler_widget.refresh(mode) - self.chicker_widget.refresh(mode) - self.controller_handler.refresh(mode) + def __toggle_handheld_device_connection_status(self, status: HandheldDeviceConnectionStatus): + self.__handheld_device_status = status def refresh(self): """ Refreshes sub-widgets so that they display the most recent values, as well as the most recent values are available for use. """ - # get the most recent state on whether controller is connected from controller handler - controller_status = self.controller_handler.get_controller_connection_status() # update controller status view with most recent controller status - self.controller_status.refresh(controller_status) + self.handheld_device_status_widget.refresh(self.__handheld_device_status) # update diagnostic ui with most recent recent controller status - self.diagnostics_control_input_widget.refresh(controller_status) + self.diagnostics_control_input_widget.refresh(self.__handheld_device_status) - if controller_status == HandheldDeviceConnectionStatus.DISCONNECTED: - self.__toggle_control_and_refresh(ControlMode.DIAGNOSTICS) + # update all widgets based on current control mode + self.drive_dribbler_widget.refresh(self.__control_mode) + self.chicker_widget.refresh(self.__control_mode) if self.__control_mode == ControlMode.DIAGNOSTICS: diagnostics_primitive = Primitive( @@ -100,32 +112,38 @@ def refresh(self): power_control=self.chicker_widget.power_control, ) ) - + # self.logger.debug(self.drive_dribbler_widget.motor_control) self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) elif self.__control_mode == ControlMode.HANDHELD: - # diagnostics_primitive = ( - # self.controller_handler.get_latest_primitive_controls() - # ) + # get latest diagnostics primitive + diagnostics_primitive = ( + self.controller_handler.get_latest_primitive_controls() + ) + + # send it - self.drive_dribbler_widget.set_slider( - SliderType.XVelocitySlider, self.controller_handler.move_x + # update ui visually + # TODO update dribbler speed and kick power, flash kick and chip buttons + self.drive_dribbler_widget.set_x_velocity_slider( + diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.x_component_meters ) - self.drive_dribbler_widget.set_slider( - SliderType.YVelocitySlider, self.controller_handler.move_y + self.drive_dribbler_widget.set_y_velocity_slider( + diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.y_component_meters ) - self.drive_dribbler_widget.set_slider( - SliderType.DribblerVelocitySlider, self.controller_handler.ang_vel + self.drive_dribbler_widget.set_angular_velocity_slider( + diagnostics_primitive.direct_control.motor_control.direct_velocity_control.angular_velocity. + radians_per_second ) - # if diagnostics_primitive is not None: - # self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - # else: - # self.proto_unix_io.send_proto(Primitive, StopPrimitive()) + self.drive_dribbler_widget.set_dribbler_velocity_slider( + diagnostics_primitive.direct_control.motor_control.dribbler_speed_rpm + ) + + self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) def __reinitialize_controller(self) -> None: - self.controller_handler.clear_controller() - self.controller_handler.initialize_controller() + self.controller_handler.reinitialize_controller() # TODO: investigate why these methods aren't called on user hitting close button def closeEvent(self, event): From 42536254c88de30f61ec6a2c388b068cebcd3428 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:52:37 -0700 Subject: [PATCH 109/138] changed threading logic, added separate chipper distance attr and parser, general simplifications --- .../handheld_device_manager.py | 284 +++++++++--------- 1 file changed, 143 insertions(+), 141 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 484ba4f5f0..24b615fa18 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -1,6 +1,7 @@ import time +from logging import Logger from threading import Thread, Event -from typing import Optional +from typing import Type import evdev.ecodes import numpy @@ -18,6 +19,8 @@ # TODO: function docs lollll +# TODO bug with threading & switching back and forth between control types, +# thread refuses to terminate as usual class HandheldDeviceManager(object): @@ -26,27 +29,30 @@ class HandheldDeviceManager(object): interpreting the device inputs into usable inputs for the robots. """ - def __init__(self, proto_unix_io: ProtoUnixIO, logger): + def __init__( + self, + proto_unix_io: ProtoUnixIO, + logger: Logger, + handheld_device_disconnected_signal: Type[QtCore.pyqtSignal] + ): self.proto_unix_io = proto_unix_io self.logger = logger + self.handheld_device_disconnected_signal = handheld_device_disconnected_signal + self.controller: Optional[InputDevice] = None - self.controller_config = None + self.controller_config: Optional[HDIEConfig] = None # Control primitives that are directly updated with # parsed controller inputs on every iteration of the event loop - self.move_x = 0 - self.move_y = 0 - self.ang_vel = 0 self.motor_control = MotorControl() self.power_control = PowerControl() - self.__initialize_empty_controls() - # These fields are here to temporarily persist the controller's input. # They are read once certain buttons are pressed on the controller, # and the values are inserted into the control primitives above. - self.kick_power_accumulator = 0 - self.dribbler_speed_accumulator = 0 + self.kick_power_accumulator: float = 0.0 + self.chip_distance_accumulator: float = 0.0 + self.dribbler_speed_accumulator: int = 0 self.constants = tbots_cpp.create2021RobotConstants() @@ -54,49 +60,26 @@ def __init__(self, proto_unix_io: ProtoUnixIO, logger): self.__stop_thread_signal_event = Event() # Set-up process that will run the event loop - self.__setup_new_event_listener_process() + self.__setup_new_event_listener_thread() - self.initialize_controller() + self.__initialize_controller() - def get_controller_connection_status(self) -> HandheldDeviceConnectionStatus: - return ( - HandheldDeviceConnectionStatus.CONNECTED - if self.controller is not None - else HandheldDeviceConnectionStatus.DISCONNECTED - ) + def __initialize_default_controls(self): + # default values for motor control + self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 + self.motor_control.direct_velocity_control.velocity.y_component_meters = 0.0 + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = 0.0 + self.motor_control.dribbler_speed_rpm = 0 - def refresh(self, mode: ControlMode): - """ - Refresh this class. - Spawns a new thread that runs the handheld device event - processing function if Control.Mode is Diagnostics, - otherwise, stops and joins the thread, if it is running - :param mode: The current user requested mode for controlling the robot - """ - if mode == ControlMode.DIAGNOSTICS: - if self.__controller_event_loop_handler_process.is_alive(): - self.logger.debug("Terminating controller event handling process") - self.__stop_thread_signal_event.set() - self.__controller_event_loop_handler_process.join() + # default values for power control + self.power_control.geneva_slot = 3 + self.power_control.chicker = ChickerControl() - elif mode == ControlMode.HANDHELD: - self.logger.debug("Setting up new controller event handling process") - self.__setup_new_event_listener_process() - self.__start_event_listener_process() + def reinitialize_controller(self): + self.__clear_controller() + self.__initialize_controller() - def get_latest_primitive_controls(self): - diagnostics_primitive = Primitive( - direct_control=DirectControlPrimitive( - motor_control=self.motor_control, power_control=self.power_control - ) - ) - # TODO: pre-emptive bugfix: need to reset controls, especially power so that - # the control message isn't set to what is essentially auto-kick/chip - self.power_control = PowerControl() - # self.motor_control = MotorControl() - return diagnostics_primitive - - def initialize_controller(self): + def __initialize_controller(self): """ Attempt to initialize a controller. The first controller that is recognized as a valid controller will be used @@ -104,9 +87,9 @@ def initialize_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name - in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + controller is not None + and controller.name + in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() self.controller = controller @@ -116,45 +99,78 @@ def initialize_controller(self): break if ( - self.get_controller_connection_status() - == HandheldDeviceConnectionStatus.CONNECTED + self.__get_handheld_device_connection_status() + == HandheldDeviceConnectionStatus.CONNECTED ): - self.logger.debug( - "Initialized handheld controller " - + '"' - + self.controller.name - + '"' - + " and located at path: " - + self.controller.path - ) + self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.CONNECTED) + self.logger.debug('Successfully initialized handheld!') + self.logger.debug('Device name: \"' + self.controller.name + '\"') + self.logger.debug('Device path: ' + self.controller.path) elif ( - self.get_controller_connection_status() - == HandheldDeviceConnectionStatus.DISCONNECTED + self.__get_handheld_device_connection_status() + == HandheldDeviceConnectionStatus.DISCONNECTED ): + self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.DISCONNECTED) self.logger.debug( "Failed to initialize a handheld controller, check USB connection" ) self.logger.debug("Tried the following available devices:") self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) - def __start_event_listener_process(self): - # start the process for reading controller events - if not self.__controller_event_loop_handler_process.is_alive(): - self.__controller_event_loop_handler_process.start() + def __get_handheld_device_connection_status(self) -> HandheldDeviceConnectionStatus: + return ( + HandheldDeviceConnectionStatus.CONNECTED + if self.controller is not None + else HandheldDeviceConnectionStatus.DISCONNECTED + ) + + def get_latest_primitive_controls(self): + diagnostics_primitive = Primitive( + direct_control=DirectControlPrimitive( + motor_control=self.motor_control, + power_control=self.power_control + ) + ) + # TODO: pre-emptive bugfix: need to reset controls, especially power so that + # the control message isn't set to what is essentially auto-kick/chip + self.power_control.chicker.chip_distance_meters = 0.0 + self.power_control.chicker.kick_speed_m_per_s = 0.0 + + return diagnostics_primitive - def __setup_new_event_listener_process(self): + def refresh(self, mode: ControlMode): """ - initializes a new process that will run the event processing loop + Refresh this class. + Spawns a new thread that runs the handheld device event + processing function if Control.Mode is Diagnostics, + otherwise, stops and joins the thread, if it is running + :param mode: The current user requested mode for controlling the robot """ - - # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Starting controller event loop process") - self.__controller_event_loop_handler_process = Thread( - target=self.__event_loop, daemon=True - ) + if mode == ControlMode.DIAGNOSTICS: + if self.__controller_event_loop_handler_thread.is_alive(): + self.logger.debug("Terminating controller event handling process") + self.__shutdown_handheld_device_event_loop_handler_thread() + elif mode == ControlMode.HANDHELD: + # if not self.__controller_event_loop_handler_thread.is_alive(): + self.logger.debug("Setting up new controller event handling process") + self.__setup_new_event_listener_thread() + self.__start_event_listener_thread() + + def __clear_controller(self): + self.logger.debug( + f"shutting down handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}") + self.__shutdown_handheld_device_event_loop_handler_thread() + self.logger.debug( + f"after shutdown handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}") + self.controller = None + self.controller_config = None + self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.DISCONNECTED) def close(self): + self.__shutdown_handheld_device_event_loop_handler_thread() + + def __shutdown_handheld_device_event_loop_handler_thread(self): """ Shut down the controller event loop :return: @@ -162,7 +178,25 @@ def close(self): # TODO (#3165): Use trace level logging here # self.logger.debug("Shutdown down controller event loop process") self.__stop_thread_signal_event.set() - self.__controller_event_loop_handler_process.join() + if self.__controller_event_loop_handler_thread.is_alive(): + self.__controller_event_loop_handler_thread.join() + self.__setup_new_event_listener_thread() + + def __start_event_listener_thread(self): + # start the process for reading controller events + self.__controller_event_loop_handler_thread.start() + self.logger.debug(self.__controller_event_loop_handler_thread.is_alive()) + + def __setup_new_event_listener_thread(self): + """ + initializes a new process that will run the event processing loop + """ + + # TODO (#3165): Use trace level self.logger here + # self.logger.debug("Starting controller event loop process") + self.__controller_event_loop_handler_thread = Thread( + target=self.__event_loop, daemon=True + ) def __event_loop(self): # TODO (#3165): Use trace level self.logger here @@ -177,17 +211,16 @@ def __event_loop(self): # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are dropped & ignored if ( - event is not None - and time.time() - event.timestamp() - < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD + event is not None + and time.time() - event.timestamp() + < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) - self.send_primitive() time.sleep(0.0005) except OSError as ose: - self.clear_controller() + self.__clear_controller() self.logger.debug( "Caught an OSError while reading handheld controller event loop!" ) @@ -195,39 +228,13 @@ def __event_loop(self): self.logger.debug("Check physical handheld controller USB connection!") return except Exception as e: - self.clear_controller() + self.__clear_controller() self.logger.critical( "Caught an unexpected error while reading handheld controller event loop!" ) self.logger.critical("Error message: " + str(e)) return - def send_primitive(self): - motor_control = MotorControl() - - motor_control.dribbler_speed_rpm = 0 - motor_control.direct_velocity_control.velocity.x_component_meters = self.move_x - motor_control.direct_velocity_control.velocity.y_component_meters = self.move_y - motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.ang_vel - ) - - diagnostics_primitive = Primitive( - direct_control=DirectControlPrimitive( - motor_control=motor_control, power_control=PowerControl() - ) - ) - - self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - - def clear_controller(self): - self.controller = None - - def __initialize_empty_controls(self): - self.move_x = 0.0 - self.move_y = 0.0 - self.ang_vel = 0.0 - def __process_event(self, event: InputEvent): # TODO (#3165): Use trace level self.logger here @@ -241,34 +248,22 @@ def __process_event(self, event: InputEvent): # ) if event.type == ecodes.EV_ABS: - self.logger.debug( - "Processing controller event with type: " - + str(ecodes.bytype[event.type][event.code]) - + ", with code: " - + str(event.code) - + ", and with value: " - + str(event.value) - ) if event.code == self.controller_config.move_x.event_code: - # self.motor_control.direct_velocity_control.velocity.x_component_meters - self.move_x = self.__parse_move_event_value( + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( event_value=event.value, max_value=self.controller_config.move_x.max_value, normalizing_multiplier=self.constants.robot_max_speed_m_per_s, ) - self.logger.debug(self.move_x) if event.code == self.controller_config.move_y.event_code: - # self.motor_control.direct_velocity_control.velocity.y_component_meters - self.move_y = self.__parse_move_event_value( + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( event_value=event.value, max_value=self.controller_config.move_y.max_value, normalizing_multiplier=self.constants.robot_max_speed_m_per_s, ) if event.code == self.controller_config.move_rot.event_code: - # self.motor_control.direct_velocity_control.angular_velocity.radians_per_second - self.ang_vel = self.__parse_move_event_value( + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( event_value=event.value, max_value=self.controller_config.move_rot.max_value, normalizing_multiplier=self.constants.robot_max_ang_speed_rad_per_s, @@ -276,16 +271,17 @@ def __process_event(self, event: InputEvent): elif event.code == self.controller_config.chicker_power.event_code: self.kick_power_accumulator = self.__parse_kick_event_value(event.value) + self.chip_distance_accumulator = self.__parse_chip_event_value(event.value) elif event.code == self.controller_config.dribbler_speed.event_code: - self.dribbler_speed_accumulator = self.__parse_dribbler_event_value( + self.dribbler_speed_accumulator = self.__parse_dribbler_speed_event_value( event.value ) elif ( - event.code == self.controller_config.primary_dribbler_enable.event_code - or event.code - == self.controller_config.secondary_dribbler_enable.event_code + event.code == self.controller_config.primary_dribbler_enable.event_code + or event.code + == self.controller_config.secondary_dribbler_enable.event_code ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( event_value=event.value, @@ -298,8 +294,8 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( - event.code == self.controller_config.kick.event_code - and event.value == 1 + event.code == self.controller_config.kick.event_code + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = ( @@ -307,8 +303,8 @@ def __process_event(self, event: InputEvent): ) if ( - event.code == self.controller_config.chip.event_code - and event.value == 1 + event.code == self.controller_config.chip.event_code + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.chip_distance_meters = ( @@ -317,7 +313,7 @@ def __process_event(self, event: InputEvent): @staticmethod def __parse_move_event_value( - event_value: float, max_value: float, normalizing_multiplier: float + event_value: float, max_value: float, normalizing_multiplier: float ) -> float: relative_value = event_value / max_value if abs(relative_value) < HandheldDeviceConstants.DEADZONE_PERCENTAGE: @@ -326,24 +322,30 @@ def __parse_move_event_value( return relative_value * normalizing_multiplier def __parse_dribbler_enabled_event_value( - self, event_value: float, max_value: float + self, event_value: float, max_value: float ) -> bool: - return event_value / max_value > 0.5 + return (event_value / max_value) > 0.5 - def __parse_dribbler_event_value(self, value: float) -> float: + def __parse_dribbler_speed_event_value(self, value: float) -> float: return numpy.clip( - value * HandheldDeviceConstants.DRIBBLER_STEPPER, - 0, - HandheldDeviceConstants.DRIBBLER_INDEFINITE_SPEED, + a=self.motor_control.dribbler_speed_rpm + value * HandheldDeviceConstants.DRIBBLER_SPEED_STEPPER, + a_min=0, + a_max=HandheldDeviceConstants.DRIBBLER_MAX_SPEED, ) def __parse_kick_event_value(self, value: float) -> float: return numpy.clip( - value * HandheldDeviceConstants.POWER_STEPPER, - HandheldDeviceConstants.MIN_POWER, - HandheldDeviceConstants.MAX_POWER, + a=self.power_control.chicker.kick_speed_m_per_s + value * HandheldDeviceConstants.KICK_POWER_STEPPER, + a_min=HandheldDeviceConstants.MIN_KICK_POWER, + a_max=HandheldDeviceConstants.MAX_KICK_POWER, ) + def __parse_chip_event_value(self, value: float) -> float: + return numpy.clip( + a=value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, + a_min=HandheldDeviceConstants.MIN_CHIP_POWER, + a_max=HandheldDeviceConstants.MAX_CHIP_POWER, + ) # TODO: remove thee after field testing... # { From c9970e908b09f98f4d96e565d43350264562c8bf Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:52:44 -0700 Subject: [PATCH 110/138] updated --- src/software/thunderscope/constants.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 0e8d34c4d6..84a6f46791 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -406,9 +406,14 @@ class HandheldDeviceConstants: MAX_LINEAR_SPEED_METER_PER_S = 2.0 MAX_ANGULAR_SPEED_RAD_PER_S = 20.0 - DRIBBLER_STEPPER = 100.0 - DRIBBLER_INDEFINITE_SPEED = -10000.0 + DRIBBLER_SPEED_STEPPER = 1000.0 + # This is actually considered to be an "indefinite" speed in the robots software backend + DRIBBLER_MAX_SPEED = -10000.0 - POWER_STEPPER = 100.0 - MIN_POWER = 1000.0 - MAX_POWER = 20000.0 + KICK_POWER_STEPPER = 1000.0 + MIN_KICK_POWER = 1000.0 + MAX_KICK_POWER = 20000.0 + + CHIP_DISTANCE_STEPPER = 100.0 + MIN_CHIP_POWER = 0.5 + MAX_CHIP_POWER = 3.0 From d16a339f71e45dcde2ca1ad25aaacf58ba97723a Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 17 Apr 2024 21:54:10 -0700 Subject: [PATCH 111/138] formatting --- src/software/thunderscope/BUILD | 2 +- .../robot_diagnostics/diagnostics_widget.py | 17 ++-- .../drive_and_dribbler_widget.py | 8 +- .../handheld_device_manager.py | 88 +++++++++++-------- .../handheld_device_status_view.py | 1 - 5 files changed, 68 insertions(+), 48 deletions(-) diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index 63ed6bfc5d..3995e2bb23 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -156,8 +156,8 @@ py_library( "//software:py_constants.so", ], deps = [ - "//software/thunderscope:constants", "//software/logger:py_logger", + "//software/thunderscope:constants", ], ) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 5e92f996df..2d4cd88e49 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -31,7 +31,9 @@ class DiagnosticsWidget(QWidget): reinitialize_handheld_device_signal = pyqtSignal() # signal to indicate that controller was disconnected - handheld_device_connection_status_signal = pyqtSignal(HandheldDeviceConnectionStatus) + handheld_device_connection_status_signal = pyqtSignal( + HandheldDeviceConnectionStatus + ) def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() @@ -63,13 +65,15 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.reinitialize_handheld_device_signal.connect(self.__reinitialize_controller) # connect handheld device connection status toggle signal with handler - self.handheld_device_connection_status_signal.connect(self.__toggle_handheld_device_connection_status) + self.handheld_device_connection_status_signal.connect( + self.__toggle_handheld_device_connection_status + ) # initialize controller self.controller_handler = HandheldDeviceManager( self.proto_unix_io, # TODO: proto shouldn't be passed down self.logger, - self.handheld_device_connection_status_signal + self.handheld_device_connection_status_signal, ) # layout for the entire diagnostics tab @@ -86,7 +90,9 @@ def __toggle_control(self, mode: ControlMode): self.__control_mode = mode self.controller_handler.refresh(self.__control_mode) - def __toggle_handheld_device_connection_status(self, status: HandheldDeviceConnectionStatus): + def __toggle_handheld_device_connection_status( + self, status: HandheldDeviceConnectionStatus + ): self.__handheld_device_status = status def refresh(self): @@ -132,8 +138,7 @@ def refresh(self): diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.y_component_meters ) self.drive_dribbler_widget.set_angular_velocity_slider( - diagnostics_primitive.direct_control.motor_control.direct_velocity_control.angular_velocity. - radians_per_second + diagnostics_primitive.direct_control.motor_control.direct_velocity_control.angular_velocity.radians_per_second ) self.drive_dribbler_widget.set_dribbler_velocity_slider( diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 65777b1ac1..51c1013c7a 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -137,7 +137,9 @@ def __setup_direct_velocity(self, title: str) -> QGroupBox: self.y_velocity_slider, self.y_velocity_label, self.__value_change_handler ) common_widgets.enable_slider( - self.angular_velocity_slider, self.angular_velocity_label, self.__value_change_handler + self.angular_velocity_slider, + self.angular_velocity_label, + self.__value_change_handler, ) self.stop_and_reset_direct = common_widgets.create_push_button("Stop and Reset") @@ -211,12 +213,12 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: common_widgets.enable_slider( self.x_velocity_slider, self.x_velocity_label, - self.__value_change_handler + self.__value_change_handler, ) common_widgets.enable_slider( self.y_velocity_slider, self.y_velocity_label, - self.__value_change_handler + self.__value_change_handler, ) common_widgets.enable_slider( self.angular_velocity_slider, diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 24b615fa18..112ddba29d 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -30,10 +30,10 @@ class HandheldDeviceManager(object): """ def __init__( - self, - proto_unix_io: ProtoUnixIO, - logger: Logger, - handheld_device_disconnected_signal: Type[QtCore.pyqtSignal] + self, + proto_unix_io: ProtoUnixIO, + logger: Logger, + handheld_device_disconnected_signal: Type[QtCore.pyqtSignal], ): self.proto_unix_io = proto_unix_io self.logger = logger @@ -68,7 +68,9 @@ def __initialize_default_controls(self): # default values for motor control self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 self.motor_control.direct_velocity_control.velocity.y_component_meters = 0.0 - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = 0.0 + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + 0.0 + ) self.motor_control.dribbler_speed_rpm = 0 # default values for power control @@ -87,9 +89,9 @@ def __initialize_controller(self): for device in list_devices(): controller = InputDevice(device) if ( - controller is not None - and controller.name - in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + controller is not None + and controller.name + in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() self.controller = controller @@ -99,19 +101,23 @@ def __initialize_controller(self): break if ( - self.__get_handheld_device_connection_status() - == HandheldDeviceConnectionStatus.CONNECTED + self.__get_handheld_device_connection_status() + == HandheldDeviceConnectionStatus.CONNECTED ): - self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.CONNECTED) - self.logger.debug('Successfully initialized handheld!') - self.logger.debug('Device name: \"' + self.controller.name + '\"') - self.logger.debug('Device path: ' + self.controller.path) + self.handheld_device_disconnected_signal.emit( + HandheldDeviceConnectionStatus.CONNECTED + ) + self.logger.debug("Successfully initialized handheld!") + self.logger.debug('Device name: "' + self.controller.name + '"') + self.logger.debug("Device path: " + self.controller.path) elif ( - self.__get_handheld_device_connection_status() - == HandheldDeviceConnectionStatus.DISCONNECTED + self.__get_handheld_device_connection_status() + == HandheldDeviceConnectionStatus.DISCONNECTED ): - self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.DISCONNECTED) + self.handheld_device_disconnected_signal.emit( + HandheldDeviceConnectionStatus.DISCONNECTED + ) self.logger.debug( "Failed to initialize a handheld controller, check USB connection" ) @@ -128,8 +134,7 @@ def __get_handheld_device_connection_status(self) -> HandheldDeviceConnectionSta def get_latest_primitive_controls(self): diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( - motor_control=self.motor_control, - power_control=self.power_control + motor_control=self.motor_control, power_control=self.power_control ) ) # TODO: pre-emptive bugfix: need to reset controls, especially power so that @@ -159,13 +164,17 @@ def refresh(self, mode: ControlMode): def __clear_controller(self): self.logger.debug( - f"shutting down handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}") + f"shutting down handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}" + ) self.__shutdown_handheld_device_event_loop_handler_thread() self.logger.debug( - f"after shutdown handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}") + f"after shutdown handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}" + ) self.controller = None self.controller_config = None - self.handheld_device_disconnected_signal.emit(HandheldDeviceConnectionStatus.DISCONNECTED) + self.handheld_device_disconnected_signal.emit( + HandheldDeviceConnectionStatus.DISCONNECTED + ) def close(self): self.__shutdown_handheld_device_event_loop_handler_thread() @@ -211,9 +220,9 @@ def __event_loop(self): # only the events that have occurred very recently are processed, and # any events that occur before switching to handheld mode are dropped & ignored if ( - event is not None - and time.time() - event.timestamp() - < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD + event is not None + and time.time() - event.timestamp() + < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) @@ -271,7 +280,9 @@ def __process_event(self, event: InputEvent): elif event.code == self.controller_config.chicker_power.event_code: self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - self.chip_distance_accumulator = self.__parse_chip_event_value(event.value) + self.chip_distance_accumulator = self.__parse_chip_event_value( + event.value + ) elif event.code == self.controller_config.dribbler_speed.event_code: self.dribbler_speed_accumulator = self.__parse_dribbler_speed_event_value( @@ -279,9 +290,9 @@ def __process_event(self, event: InputEvent): ) elif ( - event.code == self.controller_config.primary_dribbler_enable.event_code - or event.code - == self.controller_config.secondary_dribbler_enable.event_code + event.code == self.controller_config.primary_dribbler_enable.event_code + or event.code + == self.controller_config.secondary_dribbler_enable.event_code ): dribbler_enabled = self.__parse_dribbler_enabled_event_value( event_value=event.value, @@ -294,8 +305,8 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_KEY: if ( - event.code == self.controller_config.kick.event_code - and event.value == 1 + event.code == self.controller_config.kick.event_code + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = ( @@ -303,8 +314,8 @@ def __process_event(self, event: InputEvent): ) if ( - event.code == self.controller_config.chip.event_code - and event.value == 1 + event.code == self.controller_config.chip.event_code + and event.value == 1 ): self.power_control.geneva_slot = 3 self.power_control.chicker.chip_distance_meters = ( @@ -313,7 +324,7 @@ def __process_event(self, event: InputEvent): @staticmethod def __parse_move_event_value( - event_value: float, max_value: float, normalizing_multiplier: float + event_value: float, max_value: float, normalizing_multiplier: float ) -> float: relative_value = event_value / max_value if abs(relative_value) < HandheldDeviceConstants.DEADZONE_PERCENTAGE: @@ -322,20 +333,22 @@ def __parse_move_event_value( return relative_value * normalizing_multiplier def __parse_dribbler_enabled_event_value( - self, event_value: float, max_value: float + self, event_value: float, max_value: float ) -> bool: return (event_value / max_value) > 0.5 def __parse_dribbler_speed_event_value(self, value: float) -> float: return numpy.clip( - a=self.motor_control.dribbler_speed_rpm + value * HandheldDeviceConstants.DRIBBLER_SPEED_STEPPER, + a=self.motor_control.dribbler_speed_rpm + + value * HandheldDeviceConstants.DRIBBLER_SPEED_STEPPER, a_min=0, a_max=HandheldDeviceConstants.DRIBBLER_MAX_SPEED, ) def __parse_kick_event_value(self, value: float) -> float: return numpy.clip( - a=self.power_control.chicker.kick_speed_m_per_s + value * HandheldDeviceConstants.KICK_POWER_STEPPER, + a=self.power_control.chicker.kick_speed_m_per_s + + value * HandheldDeviceConstants.KICK_POWER_STEPPER, a_min=HandheldDeviceConstants.MIN_KICK_POWER, a_max=HandheldDeviceConstants.MAX_KICK_POWER, ) @@ -347,6 +360,7 @@ def __parse_chip_event_value(self, value: float) -> float: a_max=HandheldDeviceConstants.MAX_CHIP_POWER, ) + # TODO: remove thee after field testing... # { # ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 08d751918f..0079ba6550 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -40,7 +40,6 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N ), } - # initialize controller refresh button self.handheld_device_reinitialize_button = QPushButton() self.handheld_device_reinitialize_button.setText( From 0ae3a1011a57c32c411cfca506400bf261e7f2e7 Mon Sep 17 00:00:00 2001 From: boris Date: Thu, 18 Apr 2024 05:31:55 -0700 Subject: [PATCH 112/138] remove unused enum, reorder imports --- .../robot_diagnostics/diagnostics_widget.py | 3 +-- .../drive_and_dribbler_widget.py | 16 +++------------- 2 files changed, 4 insertions(+), 15 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 2d4cd88e49..c5d2de34f4 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -18,8 +18,7 @@ HandheldDeviceManager, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( - DriveAndDribblerWidget, - SliderType, + DriveAndDribblerWidget ) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 51c1013c7a..c3e695c2b2 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -1,22 +1,12 @@ -from enum import Enum - +import time from pyqtgraph.Qt.QtCore import Qt from pyqtgraph.Qt.QtWidgets import * -import time -import software.python_bindings as tbots_cpp +from proto.import_all_protos import * +import software.python_bindings as tbots_cpp from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.common import common_widgets -from proto.import_all_protos import * - - -class SliderType(Enum): - XVelocitySlider = 0 - YVelocitySlider = 1 - AngularVelocitySlider = 2 - DribblerVelocitySlider = 2 - class DriveAndDribblerWidget(QWidget): def __init__(self) -> None: From 5e8b874c8f189aca76bb1db68604c9d4f32e65a0 Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 6 May 2024 00:29:13 -0700 Subject: [PATCH 113/138] docs, added todos for refactoring out exp kick speed into method, variable naming, some slight logic refactor and field testing adjustments from a while ago ig --- .../message_translation/power_frame_msg.hpp | 15 +- .../power_frame_msg_test.cpp | 24 +- src/software/jetson_nano/services/motor.cpp | 2 +- src/software/thunderscope/constants.py | 87 +++---- .../diagnostics_input_widget.py | 26 +- .../robot_diagnostics/diagnostics_widget.py | 77 ++++-- .../drive_and_dribbler_widget.py | 35 +-- .../handheld_device_manager.py | 225 +++++++++++------- .../handheld_device_status_view.py | 53 ++--- 9 files changed, 330 insertions(+), 214 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index f52f92f44e..f2001611cf 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -118,12 +118,12 @@ TbotsProto_PowerStatus inline createNanoPbPowerStatus( /** * Converts a google protobuf power control msg to its nanopb representation * - * @param google_control protobuf message to convert + * @param power_control protobuf message to convert * @return a nanopb power control msg matching provided protobuf */ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( - const TbotsProto::PowerControl& google_control, double kick_coeff, int kick_constant, + const TbotsProto::PowerControl& power_control, double kick_coeff, int kick_constant, int chip_pulse_width) { TbotsProto_PowerPulseControl nanopb_control = @@ -133,15 +133,16 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( kick_constant = std::min(kick_constant, MAX_KICK_CONSTANT); kick_coeff = std::min(kick_coeff, MAX_KICK_COEFFICIENT); - switch (google_control.chicker().chicker_command_case()) + switch (power_control.chicker().chicker_command_case()) { case TbotsProto::PowerControl::ChickerControl::kKickSpeedMPerS: nanopb_control.chicker.which_chicker_command = TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag; + // TODO: refactor kick pulse width calculation out into seperate function nanopb_control.chicker.chicker_command.kick_pulse_width = static_cast( kick_constant * - std::exp(kick_coeff * google_control.chicker().kick_speed_m_per_s())); + std::exp(kick_coeff * power_control.chicker().kick_speed_m_per_s())); break; case TbotsProto::PowerControl::ChickerControl::kChipDistanceMeters: nanopb_control.chicker.which_chicker_command = @@ -151,7 +152,7 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( case TbotsProto::PowerControl::ChickerControl::kAutoChipOrKick: nanopb_control.chicker.which_chicker_command = TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag; - switch (google_control.chicker().auto_chip_or_kick().auto_chip_or_kick_case()) + switch (power_control.chicker().auto_chip_or_kick().auto_chip_or_kick_case()) { case TbotsProto::AutoChipOrKick::kAutokickSpeedMPerS: nanopb_control.chicker.chicker_command.auto_chip_or_kick @@ -160,7 +161,7 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( nanopb_control.chicker.chicker_command.auto_chip_or_kick .auto_chip_or_kick.autokick_pulse_width = static_cast( kick_constant * - std::exp(kick_coeff * google_control.chicker() + std::exp(kick_coeff * power_control.chicker() .auto_chip_or_kick() .autokick_speed_m_per_s())); break; @@ -179,7 +180,7 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( default: break; } - switch (google_control.geneva_slot()) + switch (power_control.geneva_slot()) { case TbotsProto::Geneva::LEFT: nanopb_control.geneva_slot = TbotsProto_Geneva_Slot_LEFT; diff --git a/src/proto/message_translation/power_frame_msg_test.cpp b/src/proto/message_translation/power_frame_msg_test.cpp index 3953d5c6e0..9b1e5a6c35 100644 --- a/src/proto/message_translation/power_frame_msg_test.cpp +++ b/src/proto/message_translation/power_frame_msg_test.cpp @@ -3,26 +3,26 @@ #include #include -TEST(PowerFrameMsgTest, google_to_nanopb) +TEST(PowerFrameMsgTest, proto_to_nanopb) { - auto google_control = TbotsProto::PowerControl(); - google_control.mutable_chicker()->set_chip_distance_meters(3); - auto nanopb_control = createNanoPbPowerPulseControl(google_control, 0, 0, 100); + auto power_control = TbotsProto::PowerControl(); + power_control.mutable_chicker()->set_chip_distance_meters(3); + auto nanopb_control = createNanoPbPowerPulseControl(power_control, 0, 0, 100); EXPECT_EQ(nanopb_control.chicker.which_chicker_command, TbotsProto_PowerPulseControl_ChickerControl_chip_pulse_width_tag); EXPECT_EQ(nanopb_control.chicker.chicker_command.chip_pulse_width, 100); - google_control.mutable_chicker()->set_kick_speed_m_per_s(5); - nanopb_control = createNanoPbPowerPulseControl(google_control, 0.3, 300, 0); + power_control.mutable_chicker()->set_kick_speed_m_per_s(5); + nanopb_control = createNanoPbPowerPulseControl(power_control, 0.3, 300, 0); EXPECT_EQ(nanopb_control.chicker.which_chicker_command, TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag); EXPECT_EQ(nanopb_control.chicker.chicker_command.kick_pulse_width, static_cast(300 * std::exp(0.3 * 5))); - google_control.mutable_chicker() + power_control.mutable_chicker() ->mutable_auto_chip_or_kick() ->set_autokick_speed_m_per_s(4); - nanopb_control = createNanoPbPowerPulseControl(google_control, 0.1, 3, 0); + nanopb_control = createNanoPbPowerPulseControl(power_control, 0.1, 3, 0); EXPECT_EQ(nanopb_control.chicker.which_chicker_command, TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag); EXPECT_EQ( @@ -32,10 +32,10 @@ TEST(PowerFrameMsgTest, google_to_nanopb) .autokick_pulse_width, static_cast(3 * std::exp(4 * 0.1))); - google_control.mutable_chicker() + power_control.mutable_chicker() ->mutable_auto_chip_or_kick() ->set_autokick_speed_m_per_s(6); - nanopb_control = createNanoPbPowerPulseControl(google_control, 10000, 10000, 0); + nanopb_control = createNanoPbPowerPulseControl(power_control, 10000, 10000, 0); EXPECT_EQ(nanopb_control.chicker.which_chicker_command, TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag); EXPECT_EQ( @@ -45,10 +45,10 @@ TEST(PowerFrameMsgTest, google_to_nanopb) .autokick_pulse_width, static_cast(MAX_KICK_CONSTANT * std::exp(6 * MAX_KICK_COEFFICIENT))); - google_control.mutable_chicker() + power_control.mutable_chicker() ->mutable_auto_chip_or_kick() ->set_autochip_distance_meters(2); - nanopb_control = createNanoPbPowerPulseControl(google_control, 0, 0, 200); + nanopb_control = createNanoPbPowerPulseControl(power_control, 0, 0, 200); EXPECT_EQ(nanopb_control.chicker.which_chicker_command, TbotsProto_PowerPulseControl_ChickerControl_auto_chip_or_kick_tag); EXPECT_EQ( diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index b1c095a1a1..e02596358f 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -540,8 +540,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor TbotsProto::MotorControl_DirectVelocityControl direct_velocity = motor.direct_velocity_control(); EuclideanSpace_t target_euclidean_velocity = { - -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), + direct_velocity.velocity().y_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; target_wheel_velocities = diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 84a6f46791..0e27d3b15b 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -340,80 +340,87 @@ class HandheldDeviceConfigKeys(Enum): MAX_VALUE = 2 -# nomenclature: HDIE stands for Handheld-Device-Input-Event +# nomenclature: + +@dataclass +class HDKeyEvent: + """ + This dataclass holds the code for a key input event + """ + event_code: int @dataclass -class HDIEField: +class HDAbsEvent: + """ + This dataclass holds the code and max value for an abs input event + + """ event_code: int - max_value: Optional[float] + max_value: float @dataclass class HDIEConfig: - move_x: HDIEField - move_y: HDIEField - move_rot: HDIEField - kick: HDIEField - chip: HDIEField - chicker_power: HDIEField - dribbler_speed: HDIEField - primary_dribbler_enable: HDIEField - secondary_dribbler_enable: HDIEField + move_x: HDAbsEvent + move_y: HDAbsEvent + move_rot: HDAbsEvent + kick: HDKeyEvent + chip: HDKeyEvent + chicker_power: HDAbsEvent + dribbler_speed: HDAbsEvent + primary_dribbler_enable: HDAbsEvent + secondary_dribbler_enable: HDAbsEvent class HandheldDeviceConstants: - NewXboxConfig = HDIEConfig( - # Name: "ABS_X", Type: EV_ABS + XboxConfig = HDIEConfig( + # Name: "ABS_X" # Canonical: Left joystick X-axis - move_x=HDIEField(event_code=0, max_value=32767.0), - # Name: "ABS_Y", Type: EV_ABS + move_x=HDAbsEvent(event_code=0, max_value=32767.0), + # Name: "ABS_Y" # Canonical: Left joystick Y-axis - move_y=HDIEField(event_code=1, max_value=32767.0), - # Name: "ABS_RX", Type: EV_ABS + move_y=HDAbsEvent(event_code=1, max_value=32767.0), + # Name: "ABS_RX" # Canonical: Right joystick X-axis - move_rot=HDIEField(event_code=3, max_value=32767.0), - # Name: "BTN_A", Type: EV_KEY + move_rot=HDAbsEvent(event_code=3, max_value=32767.0), + # Name: "BTN_A" # Canonical: "A" Button - kick=HDIEField(304, None), - # Name: "BTN_Y", Type: EV_KEY + kick=HDKeyEvent(event_code=304), + # Name: "BTN_Y" # Canonical: "Y" Button - chip=HDIEField(308, None), - # Name: "ABS_HAT0X", Type: EV_ABS + chip=HDKeyEvent(event_code=308), + # Name: "ABS_HAT0X # Canonical: D-pad X-axis - chicker_power=HDIEField(event_code=16, max_value=1.0), + chicker_power=HDAbsEvent(event_code=16, max_value=1.0), # Name: "ABS_HAT0Y", Type: EV_ABS # Canonical: D-pad Y-axis - dribbler_speed=HDIEField(event_code=17, max_value=1.0), + dribbler_speed=HDAbsEvent(event_code=17, max_value=1.0), # Name: "ABS_Z", Type: EV_ABS # Canonical: Left trigger - primary_dribbler_enable=HDIEField(event_code=2, max_value=1023.0), + primary_dribbler_enable=HDAbsEvent(event_code=2, max_value=1023.0), # Name: "ABS_RZ", Type: EV_ABS # Canonical: Right trigger - secondary_dribbler_enable=HDIEField(event_code=5, max_value=1023.0), + secondary_dribbler_enable=HDAbsEvent(event_code=5, max_value=1023.0), ) CONTROLLER_NAME_CONFIG_MAP = { - "Microsoft Xbox One X pad": NewXboxConfig, - "Microsoft X-Box One S pad": NewXboxConfig, - "Microsoft X-Box 360 pad": NewXboxConfig, + "Microsoft Xbox One X pad": XboxConfig, + "Microsoft X-Box One S pad": XboxConfig, + "Microsoft X-Box 360 pad": XboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 DEADZONE_PERCENTAGE = 0.10 - # TODO: remove, not needed - MAX_LINEAR_SPEED_METER_PER_S = 2.0 - MAX_ANGULAR_SPEED_RAD_PER_S = 20.0 - - DRIBBLER_SPEED_STEPPER = 1000.0 + DRIBBLER_RPM_STEPPER = 1000 # This is actually considered to be an "indefinite" speed in the robots software backend - DRIBBLER_MAX_SPEED = -10000.0 + DRIBBLER_MAX_RPM = 10000 KICK_POWER_STEPPER = 1000.0 MIN_KICK_POWER = 1000.0 MAX_KICK_POWER = 20000.0 - CHIP_DISTANCE_STEPPER = 100.0 - MIN_CHIP_POWER = 0.5 - MAX_CHIP_POWER = 3.0 + CHIP_DISTANCE_STEPPER = 0.25 + MIN_CHIP_POWER = 0.25 + MAX_CHIP_POWER = 5.0 diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 346a4ca4e8..71b9134a00 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -31,10 +31,12 @@ class DiagnosticsInputToggleWidget(QWidget): def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> None: """ Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and XBox control - :param on_control_mode_switch_callback The callback to use for handling changes in input mode + :param diagnostics_input_mode_signal The signal to emit when the input mode changes """ super(DiagnosticsInputToggleWidget, self).__init__() + self.__control_mode = ControlMode.DIAGNOSTICS + self.diagnostics_input_mode_signal = diagnostics_input_mode_signal vbox_layout = QVBoxLayout() @@ -55,6 +57,7 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No lambda: self.diagnostics_input_mode_signal.emit(ControlMode.HANDHELD) ) + # default to diagnostics input, and disable handheld self.handheld_control_button.setEnabled(False) self.diagnostics_control_button.setChecked(True) @@ -63,6 +66,21 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No self.setLayout(vbox_layout) def refresh(self, status: HandheldDeviceConnectionStatus) -> None: - self.handheld_control_button.setEnabled( - status == HandheldDeviceConnectionStatus.CONNECTED - ) + """ + Refresh this widget. + If the controller is connected: + - enables the handheld button + If the controller is disconnected: + - disables the handheld control button + - sets the diagnostics button to checked + - emits diagnostics input change signal + + :param status: + """ + if status == HandheldDeviceConnectionStatus.CONNECTED: + self.handheld_control_button.setEnabled(True) + + elif status == HandheldDeviceConnectionStatus.DISCONNECTED: + self.diagnostics_control_button.setChecked(True) + self.handheld_control_button.setEnabled(False) + self.diagnostics_input_mode_signal.emit(ControlMode.DIAGNOSTICS) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index c5d2de34f4..b5123b55d8 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,3 +1,5 @@ +from typing import Union, Literal, Tuple + from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * @@ -21,8 +23,28 @@ DriveAndDribblerWidget ) +# All possible states that the diagnostics control mode and handheld device connection status can be in. +DiagnosticsControlState = Union[ + Tuple[Literal[ControlMode.DIAGNOSTICS], Literal[HandheldDeviceConnectionStatus.CONNECTED]], + Tuple[Literal[ControlMode.DIAGNOSTICS], Literal[HandheldDeviceConnectionStatus.DISCONNECTED]], + Tuple[Literal[ControlMode.HANDHELD], Literal[HandheldDeviceConnectionStatus.CONNECTED]] +] + +Bruh = Union[Tuple[int, None]] + class DiagnosticsWidget(QWidget): + """ + The DiagnosticsWidget contains all widgets related to diagnostics: + - HandheldDeviceStatusWidget + - DiagnosticsInputWidget + - DriveAndDribblerWidget + - ChickerWidget + + This widget is also responsible for controlling the diagnostics inout, + either from the DriverAndDribblerWidget or the HandheldDeviceManager, + depending on the user selected choice from the DiagnosticsWidget UI + """ # signal to indicate if manual controls should be disabled based on enum mode parameter diagnostics_input_mode_signal = pyqtSignal(ControlMode) @@ -41,10 +63,18 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io + # default start with diagnostics-disconnected + self.__state: DiagnosticsControlState = ( + ControlMode.HANDHELD, + HandheldDeviceConnectionStatus.DISCONNECTED + ) + # default start with diagnostics input + # TODO: remove self.__control_mode = ControlMode.DIAGNOSTICS # default start with disconnected controller + # TODO: remove self.__handheld_device_status = HandheldDeviceConnectionStatus.DISCONNECTED # initialize widgets @@ -58,18 +88,20 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) # connect input mode signal with handler - self.diagnostics_input_mode_signal.connect(self.__toggle_control) + self.diagnostics_input_mode_signal.connect(self.__control_mode_update_handler) # connect handheld device reinitialization signal with handler - self.reinitialize_handheld_device_signal.connect(self.__reinitialize_controller) + self.reinitialize_handheld_device_signal.connect( + lambda: self.handheld_device_handler.reinitialize_controller() + ) # connect handheld device connection status toggle signal with handler self.handheld_device_connection_status_signal.connect( - self.__toggle_handheld_device_connection_status + self.__handheld_device_connection_status_update_handler ) # initialize controller - self.controller_handler = HandheldDeviceManager( + self.handheld_device_handler = HandheldDeviceManager( self.proto_unix_io, # TODO: proto shouldn't be passed down self.logger, self.handheld_device_connection_status_signal, @@ -85,13 +117,27 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) - def __toggle_control(self, mode: ControlMode): + def bruh(self) -> Bruh: + return ( + ControlMode.HANDHELD, + HandheldDeviceConnectionStatus.DISCONNECTED + ) + + def __control_mode_update_handler(self, mode: ControlMode) -> None: + """ + Handler for managing a control mode update + :param mode: The new mode that is being used + """ self.__control_mode = mode - self.controller_handler.refresh(self.__control_mode) + self.handheld_device_handler.refresh(self.__control_mode) - def __toggle_handheld_device_connection_status( + def __handheld_device_connection_status_update_handler( self, status: HandheldDeviceConnectionStatus ): + """ + Handler for propagating + :param status: The new mode that is being used + """ self.__handheld_device_status = status def refresh(self): @@ -114,6 +160,7 @@ def refresh(self): diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( motor_control=self.drive_dribbler_widget.motor_control, + # TODO remove proto unix io from chicker widget power_control=self.chicker_widget.power_control, ) ) @@ -123,12 +170,10 @@ def refresh(self): elif self.__control_mode == ControlMode.HANDHELD: # get latest diagnostics primitive diagnostics_primitive = ( - self.controller_handler.get_latest_primitive_controls() + self.handheld_device_handler.get_latest_primitive_controls() ) - # send it - - # update ui visually + # update drive and dribbler visually. # TODO update dribbler speed and kick power, flash kick and chip buttons self.drive_dribbler_widget.set_x_velocity_slider( diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.x_component_meters @@ -144,23 +189,21 @@ def refresh(self): diagnostics_primitive.direct_control.motor_control.dribbler_speed_rpm ) + # send handheld diagnostics primitive self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - def __reinitialize_controller(self) -> None: - self.controller_handler.reinitialize_controller() - # TODO: investigate why these methods aren't called on user hitting close button def closeEvent(self, event): self.logger.info("test") - self.controller_handler.close() + self.handheld_device_handler.close() event.accept() def close(self, event): self.logger.info("test") - self.controller_handler.close() + self.handheld_device_handler.close() event.accept() def __exit__(self, event): self.logger.info("test") - self.controller_handler.close() + self.handheld_device_handler.close() event.accept() diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index c3e695c2b2..b4fe0a8793 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -8,6 +8,7 @@ from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.common import common_widgets + class DriveAndDribblerWidget(QWidget): def __init__(self) -> None: """ @@ -24,8 +25,8 @@ def __init__(self) -> None: layout = QVBoxLayout() # Add widgets to layout - layout.addWidget(self.__setup_direct_velocity("Drive")) - layout.addWidget(self.__setup_dribbler("Dribbler")) + layout.addWidget(self.__setup_direct_velocity()) + layout.addWidget(self.__setup_dribbler()) self.setLayout(layout) @@ -72,17 +73,15 @@ def __value_change_handler(self, value: float) -> str: Converts the given float value to a string label :param value: float value to be converted """ - value = float(value) - value_str = "%.2f" % value - return value_str + return "%.2f" % float(value) - def __setup_direct_velocity(self, title: str) -> QGroupBox: + def __setup_direct_velocity(self) -> QGroupBox: """ Create a widget to control the direct velocity of the robot's motors :param title: the name of the slider """ - group_box = QGroupBox(title) + group_box = QGroupBox("Drive") dbox = QVBoxLayout() ( @@ -121,10 +120,14 @@ def __setup_direct_velocity(self, title: str) -> QGroupBox: # add listener functions for sliders to update label with slider value common_widgets.enable_slider( - self.x_velocity_slider, self.x_velocity_label, self.__value_change_handler + self.x_velocity_slider, + self.x_velocity_label, + self.__value_change_handler ) common_widgets.enable_slider( - self.y_velocity_slider, self.y_velocity_label, self.__value_change_handler + self.y_velocity_slider, + self.y_velocity_label, + self.__value_change_handler ) common_widgets.enable_slider( self.angular_velocity_slider, @@ -146,14 +149,14 @@ def __setup_direct_velocity(self, title: str) -> QGroupBox: return group_box - def __setup_dribbler(self, title: str) -> QGroupBox: + def __setup_dribbler(self) -> QGroupBox: """Create a widget to control the dribbler RPM :param title: the name of the slider """ - group_box = QGroupBox(title) + group_box = QGroupBox("Dribbler") dbox = QVBoxLayout() ( @@ -228,13 +231,13 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: elif mode == ControlMode.HANDHELD: # reset slider values and disconnect self.__reset_all_sliders() - self.__disconnect_sliders() + # self.__disconnect_sliders() # disable all sliders by adding listener to keep slider value the same - common_widgets.disable_slider(self.x_velocity_slider) - common_widgets.disable_slider(self.y_velocity_slider) - common_widgets.disable_slider(self.angular_velocity_slider) - common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + # common_widgets.disable_slider(self.x_velocity_slider) + # common_widgets.disable_slider(self.y_velocity_slider) + # common_widgets.disable_slider(self.angular_velocity_slider) + # common_widgets.disable_slider(self.dribbler_speed_rpm_slider) # disable buttons common_widgets.disable_button(self.stop_and_reset_dribbler) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 112ddba29d..daea82e5af 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -18,15 +18,11 @@ from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -# TODO: function docs lollll -# TODO bug with threading & switching back and forth between control types, -# thread refuses to terminate as usual - - class HandheldDeviceManager(object): """ - This class is responsible for reading from a handheld controller device and - interpreting the device inputs into usable inputs for the robots. + This class is responsible for managing the connection between a computer and a handheld device to control robots. + This class relies on the `evdev` python package, in order to implement parsing and control flow for device events. + More info & docs can be found here: https://python-evdev.readthedocs.io/en/latest/apidoc.html """ def __init__( @@ -47,12 +43,15 @@ def __init__( self.motor_control = MotorControl() self.power_control = PowerControl() + self.__initialize_default_control_values() + # These fields are here to temporarily persist the controller's input. # They are read once certain buttons are pressed on the controller, # and the values are inserted into the control primitives above. self.kick_power_accumulator: float = 0.0 self.chip_distance_accumulator: float = 0.0 self.dribbler_speed_accumulator: int = 0 + self.dribbler_running: bool = False self.constants = tbots_cpp.create2021RobotConstants() @@ -64,27 +63,35 @@ def __init__( self.__initialize_controller() - def __initialize_default_controls(self): + def __initialize_default_control_values(self) -> None: + """ + This method sets all required fields in the control protos to the minimum default value + """ # default values for motor control - self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 - self.motor_control.direct_velocity_control.velocity.y_component_meters = 0.0 - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - 0.0 - ) + self.motor_control.direct_velocity_control.velocity\ + .x_component_meters = 0.0 + self.motor_control.direct_velocity_control.velocity\ + .y_component_meters = 0.0 + self.motor_control.direct_velocity_control.angular_velocity\ + .radians_per_second = 0.0 self.motor_control.dribbler_speed_rpm = 0 # default values for power control self.power_control.geneva_slot = 3 - self.power_control.chicker = ChickerControl() - def reinitialize_controller(self): - self.__clear_controller() + def reinitialize_controller(self) -> None: + """ + Reinitialize controller + """ self.__initialize_controller() - def __initialize_controller(self): + def __initialize_controller(self) -> None: """ - Attempt to initialize a controller. - The first controller that is recognized as a valid controller will be used + Attempt to initialize a a connection to a handheld device, + which may or may not be successful. + The first controller that is recognized as a valid controller will be used. + Valid handheld devices are any devices whose name has a matching HDIEConfig + defined in constants.py """ for device in list_devices(): controller = InputDevice(device) @@ -155,61 +162,68 @@ def refresh(self, mode: ControlMode): if mode == ControlMode.DIAGNOSTICS: if self.__controller_event_loop_handler_thread.is_alive(): self.logger.debug("Terminating controller event handling process") - self.__shutdown_handheld_device_event_loop_handler_thread() + self.__shutdown_event_listener_thread() elif mode == ControlMode.HANDHELD: - # if not self.__controller_event_loop_handler_thread.is_alive(): - self.logger.debug("Setting up new controller event handling process") - self.__setup_new_event_listener_thread() - self.__start_event_listener_thread() - - def __clear_controller(self): - self.logger.debug( - f"shutting down handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}" - ) - self.__shutdown_handheld_device_event_loop_handler_thread() - self.logger.debug( - f"after shutdown handler thread, is_alive = {self.__controller_event_loop_handler_thread.is_alive()}" - ) + if not self.__controller_event_loop_handler_thread.is_alive(): + self.logger.debug("Setting up new controller event handling process") + self.__setup_new_event_listener_thread() + self.__start_event_listener_thread() + + def __clear_controller(self) -> None: + """ + Clears controller & config field by setting to null, + and emits a disconnected notification signal. + """ self.controller = None self.controller_config = None self.handheld_device_disconnected_signal.emit( HandheldDeviceConnectionStatus.DISCONNECTED ) - def close(self): - self.__shutdown_handheld_device_event_loop_handler_thread() + def close(self) -> None: + """ + Shuts down the thread running the event processing loop. + """ + self.__shutdown_event_listener_thread() - def __shutdown_handheld_device_event_loop_handler_thread(self): + def __shutdown_event_listener_thread(self) -> None: """ - Shut down the controller event loop - :return: + Shut down the event processing loop by setting the stop event flag, + and then joins the handling thread, if it is alive. """ # TODO (#3165): Use trace level logging here # self.logger.debug("Shutdown down controller event loop process") self.__stop_thread_signal_event.set() if self.__controller_event_loop_handler_thread.is_alive(): self.__controller_event_loop_handler_thread.join() - self.__setup_new_event_listener_thread() + self.__stop_thread_signal_event.clear() - def __start_event_listener_thread(self): - # start the process for reading controller events + def __start_event_listener_thread(self) -> None: + """ + Starts the thread that runs the event processing loop. + """ self.__controller_event_loop_handler_thread.start() - self.logger.debug(self.__controller_event_loop_handler_thread.is_alive()) def __setup_new_event_listener_thread(self): """ - initializes a new process that will run the event processing loop + Initializes a new thread that will run the event processing loop """ - # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Starting controller event loop process") + # self.logger.debug("Initializing new controller event loop thread") self.__controller_event_loop_handler_thread = Thread( target=self.__event_loop, daemon=True ) - def __event_loop(self): + def __event_loop(self) -> None: + """ + This is the method that contains the event processing loop + that is called runs in the event handling thread. + An infinite while loop reads and processes events one by one, using the evdev API. + Old events are skipped if they exceed a threshold, and any caught errors + cause the manager to revert to a disconnected handheld device state. + """ # TODO (#3165): Use trace level self.logger here - self.logger.debug("Starting handheld controller event handling loop") + # self.logger.debug("Starting handheld controller event handling loop") try: while True: if self.__stop_thread_signal_event.is_set(): @@ -244,7 +258,12 @@ def __event_loop(self): self.logger.critical("Error message: " + str(e)) return - def __process_event(self, event: InputEvent): + def __process_event(self, event: InputEvent) -> None: + """ + Processes the given device event. Sets corresponding motor & power control values + based on the event type, using the current config set in self.handheld_device_config + :param event: The event to process. + """ # TODO (#3165): Use trace level self.logger here # self.logger.debug( @@ -258,57 +277,53 @@ def __process_event(self, event: InputEvent): if event.type == ecodes.EV_ABS: if event.code == self.controller_config.move_x.event_code: - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config.move_x.max_value, - normalizing_multiplier=self.constants.robot_max_speed_m_per_s, + self.__interpret_move_event_value( + event.value, + self.controller_config.move_x.max_value, + self.constants.robot_max_speed_m_per_s, ) if event.code == self.controller_config.move_y.event_code: - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config.move_y.max_value, - normalizing_multiplier=self.constants.robot_max_speed_m_per_s, + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__interpret_move_event_value( + -event.value, + self.controller_config.move_y.max_value, + self.constants.robot_max_speed_m_per_s, ) if event.code == self.controller_config.move_rot.event_code: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__parse_move_event_value( - event_value=event.value, - max_value=self.controller_config.move_rot.max_value, - normalizing_multiplier=self.constants.robot_max_ang_speed_rad_per_s, + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__interpret_move_event_value( + event.value, + self.controller_config.move_rot.max_value, + self.constants.robot_max_ang_speed_rad_per_s, ) elif event.code == self.controller_config.chicker_power.event_code: - self.kick_power_accumulator = self.__parse_kick_event_value(event.value) - self.chip_distance_accumulator = self.__parse_chip_event_value( + self.kick_power_accumulator = self.__interpret_kick_event_value( + event.value + ) + self.chip_distance_accumulator = self.__interpret_chip_event_value( event.value ) elif event.code == self.controller_config.dribbler_speed.event_code: - self.dribbler_speed_accumulator = self.__parse_dribbler_speed_event_value( + self.dribbler_speed_accumulator = self.__interpret_dribbler_speed_event_value( event.value ) elif ( - event.code == self.controller_config.primary_dribbler_enable.event_code - or event.code - == self.controller_config.secondary_dribbler_enable.event_code + event.code == self.controller_config.primary_dribbler_enable.event_code or + event.code == self.controller_config.secondary_dribbler_enable.event_code ): - dribbler_enabled = self.__parse_dribbler_enabled_event_value( - event_value=event.value, - max_value=self.controller_config.primary_dribbler_enable.max_value, + self.dribbler_running = self.__interpret_dribbler_enabled_event_value( + event.value, + self.controller_config.primary_dribbler_enable.max_value, ) - if dribbler_enabled: - self.motor_control.dribbler_speed_rpm = ( - self.dribbler_speed_accumulator - ) if event.type == ecodes.EV_KEY: if ( event.code == self.controller_config.kick.event_code and event.value == 1 ): - self.power_control.geneva_slot = 3 self.power_control.chicker.kick_speed_m_per_s = ( self.kick_power_accumulator ) @@ -317,45 +332,79 @@ def __process_event(self, event: InputEvent): event.code == self.controller_config.chip.event_code and event.value == 1 ): - self.power_control.geneva_slot = 3 self.power_control.chicker.chip_distance_meters = ( - self.kick_power_accumulator + self.chip_distance_accumulator ) + if self.dribbler_running: + self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator + else: + self.motor_control.dribbler_speed_rpm = 0 + @staticmethod - def __parse_move_event_value( + def __interpret_move_event_value( event_value: float, max_value: float, normalizing_multiplier: float ) -> float: + """ + Parse the event_value that corresponds to movement control + :param event_value: the value for the current event being interpreted + :param max_value: max value for this type of event event type + :param normalizing_multiplier: multiplier for converting between + :return: The interpreted value that can be set a value for a field in robot movement control + """ relative_value = event_value / max_value if abs(relative_value) < HandheldDeviceConstants.DEADZONE_PERCENTAGE: return 0 else: return relative_value * normalizing_multiplier - def __parse_dribbler_enabled_event_value( - self, event_value: float, max_value: float + @staticmethod + def __interpret_dribbler_enabled_event_value( + event_value: float, max_value: float ) -> bool: + """ + Interpret the event_value that corresponds to controlling whether the dribbler is enabled + :param event_value: the value for the current event being interpreted + :param max_value: max value for this type of event event type + :return: The interpreted value that can be set a value for a field in robot control + """ return (event_value / max_value) > 0.5 - def __parse_dribbler_speed_event_value(self, value: float) -> float: + def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: + """ + Interprets the event value that corresponds to controlling the dribbler speed. + :param event_value: the value for the current event being interpreted + :return: the interpreted value to be used for the new dribbler speed on the robot + """ return numpy.clip( - a=self.motor_control.dribbler_speed_rpm - + value * HandheldDeviceConstants.DRIBBLER_SPEED_STEPPER, - a_min=0, - a_max=HandheldDeviceConstants.DRIBBLER_MAX_SPEED, + a=self.dribbler_speed_accumulator + - event_value * HandheldDeviceConstants.DRIBBLER_RPM_STEPPER, + a_min=-HandheldDeviceConstants.DRIBBLER_MAX_RPM, + a_max=HandheldDeviceConstants.DRIBBLER_MAX_RPM, ) - def __parse_kick_event_value(self, value: float) -> float: + def __interpret_kick_event_value(self, event_value: float) -> float: + """ + Interprets the event value that corresponds to controlling the dribbler speed. + :param event_value: the value for the current event being interpreted + :return: the interpreted value to be used for the kick power on the robot + """ return numpy.clip( a=self.power_control.chicker.kick_speed_m_per_s - + value * HandheldDeviceConstants.KICK_POWER_STEPPER, + + event_value * HandheldDeviceConstants.KICK_POWER_STEPPER, a_min=HandheldDeviceConstants.MIN_KICK_POWER, a_max=HandheldDeviceConstants.MAX_KICK_POWER, ) - def __parse_chip_event_value(self, value: float) -> float: + def __interpret_chip_event_value(self, value: float) -> float: + """ + Interprets the event value that corresponds to controlling the chip distance. + :param value: the value for the current event being interpreted + :return: the interpreted value to be used for the chip distance on the robot + """ return numpy.clip( - a=value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, + a=self.power_control.chicker.chip_distance_meters + + value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, a_min=HandheldDeviceConstants.MIN_CHIP_POWER, a_max=HandheldDeviceConstants.MAX_CHIP_POWER, ) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 0079ba6550..3c01055ccb 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -18,12 +18,14 @@ class HandheldDeviceConnectionStatus(Enum): class HandheldDeviceStatusView(QWidget): - """Class to show whether a handheld controller is connected to thunderscope and initialized, - or no controller is connected at all. - """ - def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> None: - super().__init__() + """ + Initialize the HandheldDeviceStatusView widget. + This widget sjows the user the current state of the connection with a handheld device, + as well as a button that attempts to reinitialize a controller object when clicked + :param reinitialize_controller_signal: The signal to use for the reinitialize button + """ + super(HandheldDeviceStatusView, self).__init__() self.reinitialize_controller_signal = reinitialize_controller_signal @@ -49,31 +51,21 @@ def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> N self.reinitialize_controller_signal ) - # layout for the whole widget - self.vbox_layout = QVBoxLayout() - - # layout for controller button & status - self.hbox_layout = QHBoxLayout() - - # set layout and spacing - self.hbox_layout.addWidget(self.handheld_device_status) - self.hbox_layout.addWidget(self.handheld_device_reinitialize_button) - self.hbox_layout.setStretch(0, 4) - self.hbox_layout.setStretch(1, 1) + hbox_layout = QHBoxLayout() - # set groupbox to contain layout with status and button - # self.group_box.setLayout(self.hbox_layout) + hbox_layout.addWidget(self.handheld_device_status) + hbox_layout.addWidget(self.handheld_device_reinitialize_button) + hbox_layout.setStretch(0, 4) + hbox_layout.setStretch(1, 1) - # add box to whole widget layout - # self.vbox_layout.addWidget(self.group_box) - - # set the layout for the whole widget - self.setLayout(self.hbox_layout) self.set_view_state(HandheldDeviceConnectionStatus.DISCONNECTED) + self.setLayout(hbox_layout) - def set_view_state( - self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED - ): + def set_view_state(self, connection_state: HandheldDeviceConnectionStatus) -> None: + """ + Sets the label to display the correct status depending on the connection state + :param connection_state: The state to use + """ self.handheld_device_status.setText( self.status_label_view_map[connection_state][0] ) @@ -81,7 +73,10 @@ def set_view_state( self.status_label_view_map[connection_state][1] ) - def refresh(self, connected=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: - """Refresh the label + def refresh(self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: + """ + Refreshes this widget. + The status label will reflect to the user the current state of the controller connection + :param connection_state: The new state of the controller connection """ - self.set_view_state(connected) + self.set_view_state(connection_state) From c0d0b924bb7dd34db1ae6419cff01c08c45d8220 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 8 May 2024 19:01:38 -0700 Subject: [PATCH 114/138] turns out python type hinting isnt really that hintful :/ --- .../robot_diagnostics/diagnostics_widget.py | 23 ------------------- .../handheld_device_manager.py | 3 +-- 2 files changed, 1 insertion(+), 25 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index b5123b55d8..1f97a34c8e 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -23,15 +23,6 @@ DriveAndDribblerWidget ) -# All possible states that the diagnostics control mode and handheld device connection status can be in. -DiagnosticsControlState = Union[ - Tuple[Literal[ControlMode.DIAGNOSTICS], Literal[HandheldDeviceConnectionStatus.CONNECTED]], - Tuple[Literal[ControlMode.DIAGNOSTICS], Literal[HandheldDeviceConnectionStatus.DISCONNECTED]], - Tuple[Literal[ControlMode.HANDHELD], Literal[HandheldDeviceConnectionStatus.CONNECTED]] -] - -Bruh = Union[Tuple[int, None]] - class DiagnosticsWidget(QWidget): """ @@ -63,18 +54,10 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - # default start with diagnostics-disconnected - self.__state: DiagnosticsControlState = ( - ControlMode.HANDHELD, - HandheldDeviceConnectionStatus.DISCONNECTED - ) - # default start with diagnostics input - # TODO: remove self.__control_mode = ControlMode.DIAGNOSTICS # default start with disconnected controller - # TODO: remove self.__handheld_device_status = HandheldDeviceConnectionStatus.DISCONNECTED # initialize widgets @@ -117,12 +100,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(vbox_layout) - def bruh(self) -> Bruh: - return ( - ControlMode.HANDHELD, - HandheldDeviceConnectionStatus.DISCONNECTED - ) - def __control_mode_update_handler(self, mode: ControlMode) -> None: """ Handler for managing a control mode update diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index daea82e5af..e4f60075f4 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -83,6 +83,7 @@ def reinitialize_controller(self) -> None: """ Reinitialize controller """ + self.__clear_controller() self.__initialize_controller() def __initialize_controller(self) -> None: @@ -144,8 +145,6 @@ def get_latest_primitive_controls(self): motor_control=self.motor_control, power_control=self.power_control ) ) - # TODO: pre-emptive bugfix: need to reset controls, especially power so that - # the control message isn't set to what is essentially auto-kick/chip self.power_control.chicker.chip_distance_meters = 0.0 self.power_control.chicker.kick_speed_m_per_s = 0.0 From d8a99e6303d4085a03ebc3111ee0c60044054aa5 Mon Sep 17 00:00:00 2001 From: boris Date: Wed, 8 May 2024 22:27:57 -0700 Subject: [PATCH 115/138] moved proto sending logic down a level --- .../message_translation/power_frame_msg.hpp | 2 +- .../thunderscope/robot_communication.py | 19 +++- .../robot_diagnostics/chicker_widget.py | 82 +++++++++------- .../robot_diagnostics/diagnostics_widget.py | 34 ++----- .../drive_and_dribbler_widget.py | 20 ++-- .../handheld_device_manager.py | 95 ++++++++++--------- 6 files changed, 140 insertions(+), 112 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index f2001611cf..12edc135e9 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -138,7 +138,7 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( case TbotsProto::PowerControl::ChickerControl::kKickSpeedMPerS: nanopb_control.chicker.which_chicker_command = TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag; - // TODO: refactor kick pulse width calculation out into seperate function + // TODO (#3193): refactor kick pulse width calculation out into seperate function nanopb_control.chicker.chicker_command.kick_pulse_width = static_cast( kick_constant * diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 2cbe8e1176..4eadac068f 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -59,14 +59,19 @@ def __init__( self.fullsystem_primitive_set_buffer = ThreadSafeBuffer(1, PrimitiveSet) - self.diagnostics_primitive_buffer = ThreadSafeBuffer(1, Primitive) + self.motor_control_primitive_buffer = ThreadSafeBuffer(1, MotorControl) + self.power_control_primitive_buffer = ThreadSafeBuffer(1, PowerControl) self.current_proto_unix_io.register_observer( PrimitiveSet, self.fullsystem_primitive_set_buffer ) self.current_proto_unix_io.register_observer( - Primitive, self.diagnostics_primitive_buffer + MotorControl, self.motor_control_primitive_buffer + ) + + self.current_proto_unix_io.register_observer( + PowerControl, self.power_control_primitive_buffer ) self.send_estop_state_thread = threading.Thread( @@ -241,7 +246,15 @@ def __run_primitive_set(self) -> None: robot_primitives_map = {} # get the most recent diagnostics primitive - diagnostics_primitive = self.diagnostics_primitive_buffer.get(block=False) + motor_control = self.motor_control_primitive_buffer.get(block=False) + power_control = self.power_control_primitive_buffer.get(block=False) + + diagnostics_primitive = Primitive( + direct_control=DirectControlPrimitive( + motor_control=motor_control, + power_control=power_control, + ) + ) # filter for diagnostics controlled robots diagnostics_robots = list( diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 4a22970dd8..0da2067cf4 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -18,7 +18,10 @@ class ChickerCommandMode(Enum): class ChickerWidget(QWidget): - def __init__(self, proto_unix_io: ProtoUnixIO) -> None: + def __init__( + self, + proto_unix_io + ) -> None: """Handles the robot diagnostics input to create a PowerControl message to be sent to the robots. @@ -35,23 +38,32 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(ChickerWidget, self).__init__() self.proto_unix_io = proto_unix_io - self.power_control = PowerControl() # initial values - self.power_value = 1 + self.__initialize_default_power_control_values() vbox_layout = QVBoxLayout() self.setLayout(vbox_layout) - # Initializing power slider for kicking & chipping + # Initializing power slider for kicking + ( + self.kick_power_slider_layout, + self.kick_power_slider, + self.kick_power_label, + ) = common_widgets.create_slider( + "Power (m/s)", 1, 10, 1 + ) + vbox_layout.addLayout(self.kick_power_slider_layout) + + # Initializing distance slider for chipping ( - self.power_slider_layout, - self.power_slider, - self.power_label, + self.chip_distance_slider_layout, + self.chip_distance_slider, + self.chip_distance_label, ) = common_widgets.create_slider( - "Power (m/s) (Chipper power is fixed)", 1, 10, 1 + "Chip Distance (m)", 1, 10, 1 ) - vbox_layout.addLayout(self.power_slider_layout) + vbox_layout.addLayout(self.kick_power_slider_layout) # Initializing kick & chip buttons self.push_button_box, self.push_buttons = common_widgets.create_buttons( @@ -61,7 +73,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.chip_button = self.push_buttons[1] # set buttons to be initially enabled - # TODO: don't need boolean flags, refactor them out self.kick_chip_buttons_enable = True vbox_layout.addWidget(self.push_button_box) @@ -137,7 +148,7 @@ def enable_kick_chip_buttons(self) -> None: # clears the proto buffer when buttons are re-enabled # just to start fresh and clear any unwanted protos - self.clear_proto_buffer() + self.__initialize_default_power_control_values() def set_should_enable_buttons(self, enable: bool) -> None: """ @@ -161,32 +172,31 @@ def send_command(self, command: ChickerCommandMode) -> None: """ # gets slider values - power_value = self.power_slider.value() + kick_power_value = self.kick_power_slider.value() + chip_distance_value = self.chip_distance_slider.value() - power_control = PowerControl() - power_control.geneva_slot = 3 - - # sends kick, chip, autokick, or autchip primitive + # sends kick, chip, autokick, or autochip primitive if command == ChickerCommandMode.KICK: - power_control.chicker.kick_speed_m_per_s = power_value + self.power_control.chicker.kick_speed_m_per_s = kick_power_value elif command == ChickerCommandMode.CHIP: - power_control.chicker.chip_distance_meters = power_value + self.power_control.chicker.chip_distance_meters = chip_distance_value elif command == ChickerCommandMode.AUTOKICK: - power_control.chicker.auto_chip_or_kick.autokick_speed_m_per_s = power_value + self.power_control.chicker.auto_chip_or_kick.autokick_speed_m_per_s = ( + kick_power_value + ) elif command == ChickerCommandMode.AUTOCHIP: - power_control.chicker.auto_chip_or_kick.autochip_distance_meters = ( - power_value + self.power_control.chicker.auto_chip_or_kick.autochip_distance_meters = ( + chip_distance_value ) - # update the proto to be sent - self.power_control = power_control + self.proto_unix_io.send_proto(PowerControl, self.power_control) # clears the proto buffer for kick or chip commands # so only one kick / chip is sent if command == ChickerCommandMode.KICK or command == ChickerCommandMode.CHIP: - self.clear_proto_buffer() + self.__initialize_default_power_control_values() - def clear_proto_buffer(self) -> None: + def __initialize_default_power_control_values(self) -> None: """ Sends an empty proto to the proto unix io buffer This is due to a bug in robot_communication where if a new PowerControl message is not sent, @@ -195,16 +205,16 @@ def clear_proto_buffer(self) -> None: If buffer is full, blocks execution until buffer has space """ self.power_control = PowerControl() + self.power_control.geneva_slot = 3 - def change_button_state(self, button: QPushButton, enable: bool) -> None: + def refresh_button_state(self, button: QPushButton) -> None: """Change button color and clickable state. :param button: button to change the state of - :param enable: bool: if True: enable this button, if False: disable :returns: None """ - if enable: + if self.kick_chip_buttons_enable: button.setStyleSheet("background-color: White") button.setEnabled(True) else: @@ -221,13 +231,17 @@ def refresh(self, mode: ControlMode) -> None: # Update this widgets accessibility to the user based on the ControlMode parameter self.update_widget_accessibility(mode) - # get power value slider value and set the label to that value - power_value = self.power_slider.value() - self.power_label.setText(str(power_value)) + # get kick power value slider value and set the label to that value + kick_power_value = self.kick_power_slider.value() + self.kick_power_label.setText(str(kick_power_value)) + + # get chip distance value slider value and set the label to that value + chip_distance_value = self.chip_distance_slider.value() + self.chip_distance_label.setText(str(chip_distance_value)) - # refreshes button state based on enable boolean - self.change_button_state(self.kick_button, self.kick_chip_buttons_enable) - self.change_button_state(self.chip_button, self.kick_chip_buttons_enable) + # refresh button state to reflect to user current status + self.refresh_button_state(self.kick_button) + self.refresh_button_state(self.chip_button) # If auto is enabled, we want to populate the autochip or kick message if self.auto_kick_button.isChecked(): diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 1f97a34c8e..5db908d750 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -64,8 +64,8 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.handheld_device_status_widget = HandheldDeviceStatusView( self.reinitialize_handheld_device_signal ) - self.drive_dribbler_widget = DriveAndDribblerWidget() - self.chicker_widget = ChickerWidget(proto_unix_io) + self.drive_dribbler_widget = DriveAndDribblerWidget(self.proto_unix_io) + self.chicker_widget = ChickerWidget(self.proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( self.diagnostics_input_mode_signal ) @@ -85,8 +85,8 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # initialize controller self.handheld_device_handler = HandheldDeviceManager( - self.proto_unix_io, # TODO: proto shouldn't be passed down self.logger, + self.proto_unix_io, self.handheld_device_connection_status_signal, ) @@ -134,41 +134,27 @@ def refresh(self): self.chicker_widget.refresh(self.__control_mode) if self.__control_mode == ControlMode.DIAGNOSTICS: - diagnostics_primitive = Primitive( - direct_control=DirectControlPrimitive( - motor_control=self.drive_dribbler_widget.motor_control, - # TODO remove proto unix io from chicker widget - power_control=self.chicker_widget.power_control, - ) - ) - # self.logger.debug(self.drive_dribbler_widget.motor_control) - self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) + self.logger.debug(self.chicker_widget.power_control) elif self.__control_mode == ControlMode.HANDHELD: - # get latest diagnostics primitive - diagnostics_primitive = ( - self.handheld_device_handler.get_latest_primitive_controls() - ) + self.logger.debug(self.handheld_device_handler.power_control) # update drive and dribbler visually. - # TODO update dribbler speed and kick power, flash kick and chip buttons + # TODO kick power, flash kick and chip buttons self.drive_dribbler_widget.set_x_velocity_slider( - diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.x_component_meters + self.handheld_device_handler.motor_control.direct_velocity_control.velocity.x_component_meters ) self.drive_dribbler_widget.set_y_velocity_slider( - diagnostics_primitive.direct_control.motor_control.direct_velocity_control.velocity.y_component_meters + self.handheld_device_handler.motor_control.direct_velocity_control.velocity.y_component_meters ) self.drive_dribbler_widget.set_angular_velocity_slider( - diagnostics_primitive.direct_control.motor_control.direct_velocity_control.angular_velocity.radians_per_second + self.handheld_device_handler.motor_control.direct_velocity_control.angular_velocity.radians_per_second ) self.drive_dribbler_widget.set_dribbler_velocity_slider( - diagnostics_primitive.direct_control.motor_control.dribbler_speed_rpm + self.handheld_device_handler.motor_control.dribbler_speed_rpm ) - # send handheld diagnostics primitive - self.proto_unix_io.send_proto(Primitive, diagnostics_primitive) - # TODO: investigate why these methods aren't called on user hitting close button def closeEvent(self, event): self.logger.info("test") diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index b4fe0a8793..0db2477532 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -4,19 +4,26 @@ from proto.import_all_protos import * import software.python_bindings as tbots_cpp + +from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.common import common_widgets class DriveAndDribblerWidget(QWidget): - def __init__(self) -> None: + def __init__( + self, + proto_unix_io: ProtoUnixIO + ) -> None: """ Initialize the widget to control the robot's motors """ super(DriveAndDribblerWidget, self).__init__() + self.proto_unix_io = proto_unix_io + self.motor_control = MotorControl() self.input_a = time.time() @@ -68,6 +75,9 @@ def refresh(self, mode: ControlMode) -> None: self.angular_velocity_slider.value() ) + if mode == ControlMode.DIAGNOSTICS: + self.proto_unix_io.send_proto(MotorControl, self.motor_control) + def __value_change_handler(self, value: float) -> str: """ Converts the given float value to a string label @@ -78,7 +88,6 @@ def __value_change_handler(self, value: float) -> str: def __setup_direct_velocity(self) -> QGroupBox: """ Create a widget to control the direct velocity of the robot's motors - :param title: the name of the slider """ group_box = QGroupBox("Drive") @@ -150,12 +159,9 @@ def __setup_direct_velocity(self) -> QGroupBox: return group_box def __setup_dribbler(self) -> QGroupBox: - """Create a widget to control the dribbler RPM - - :param title: the name of the slider - """ - + Create a widget to control the dribbler RPM + """ group_box = QGroupBox("Dribbler") dbox = QVBoxLayout() diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index e4f60075f4..58af75be1e 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -27,23 +27,20 @@ class HandheldDeviceManager(object): def __init__( self, - proto_unix_io: ProtoUnixIO, logger: Logger, + proto_unix_io: ProtoUnixIO, handheld_device_disconnected_signal: Type[QtCore.pyqtSignal], ): - self.proto_unix_io = proto_unix_io self.logger = logger + self.proto_unix_io = proto_unix_io self.handheld_device_disconnected_signal = handheld_device_disconnected_signal - self.controller: Optional[InputDevice] = None - self.controller_config: Optional[HDIEConfig] = None + self.handheld_device: Optional[InputDevice] = None + self.handheld_device_config: Optional[HDIEConfig] = None - # Control primitives that are directly updated with - # parsed controller inputs on every iteration of the event loop - self.motor_control = MotorControl() - self.power_control = PowerControl() - - self.__initialize_default_control_values() + # initialize proto fields for motor and power control; set default values + self.__initialize_default_motor_control_values() + self.__initialize_default_power_control_values() # These fields are here to temporarily persist the controller's input. # They are read once certain buttons are pressed on the controller, @@ -63,11 +60,12 @@ def __init__( self.__initialize_controller() - def __initialize_default_control_values(self) -> None: + def __initialize_default_motor_control_values(self) -> None: """ - This method sets all required fields in the control protos to the minimum default value + This method sets all required fields in the motor + control proto to the default/minimum default value """ - # default values for motor control + self.motor_control = MotorControl() self.motor_control.direct_velocity_control.velocity\ .x_component_meters = 0.0 self.motor_control.direct_velocity_control.velocity\ @@ -76,7 +74,14 @@ def __initialize_default_control_values(self) -> None: .radians_per_second = 0.0 self.motor_control.dribbler_speed_rpm = 0 + def __initialize_default_power_control_values(self) -> None: + """ + This method sets all required fields in the power + control proto to the default/minimum default value + """ + # default values for power control + self.power_control = PowerControl() self.power_control.geneva_slot = 3 def reinitialize_controller(self) -> None: @@ -102,8 +107,8 @@ def __initialize_controller(self) -> None: in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() - self.controller = controller - self.controller_config: HDIEConfig = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ + self.handheld_device = controller + self.handheld_device_config: HDIEConfig = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ controller.name ] break @@ -116,8 +121,8 @@ def __initialize_controller(self) -> None: HandheldDeviceConnectionStatus.CONNECTED ) self.logger.debug("Successfully initialized handheld!") - self.logger.debug('Device name: "' + self.controller.name + '"') - self.logger.debug("Device path: " + self.controller.path) + self.logger.debug('Device name: "' + self.handheld_device.name + '"') + self.logger.debug("Device path: " + self.handheld_device.path) elif ( self.__get_handheld_device_connection_status() @@ -133,22 +138,22 @@ def __initialize_controller(self) -> None: self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __get_handheld_device_connection_status(self) -> HandheldDeviceConnectionStatus: + """ + Get and return the latest connection status, based on the `controller` field + :return: the current status of the handheld device conneciton + """ return ( HandheldDeviceConnectionStatus.CONNECTED - if self.controller is not None + if self.handheld_device is not None else HandheldDeviceConnectionStatus.DISCONNECTED ) - def get_latest_primitive_controls(self): - diagnostics_primitive = Primitive( - direct_control=DirectControlPrimitive( - motor_control=self.motor_control, power_control=self.power_control - ) - ) - self.power_control.chicker.chip_distance_meters = 0.0 - self.power_control.chicker.kick_speed_m_per_s = 0.0 + def __send_proto_command_latest_primitive_controls(self) -> None: + self.proto_unix_io.send_proto(MotorControl, self.motor_control) + self.proto_unix_io.send_proto(PowerControl, self.power_control) - return diagnostics_primitive + # reset power control + self.__initialize_default_power_control_values() def refresh(self, mode: ControlMode): """ @@ -168,13 +173,16 @@ def refresh(self, mode: ControlMode): self.__setup_new_event_listener_thread() self.__start_event_listener_thread() + self.proto_unix_io.send_proto(MotorControl, self.motor_control) + self.proto_unix_io.send_proto(PowerControl, self.power_control) + def __clear_controller(self) -> None: """ Clears controller & config field by setting to null, and emits a disconnected notification signal. """ - self.controller = None - self.controller_config = None + self.handheld_device = None + self.handheld_device_config = None self.handheld_device_disconnected_signal.emit( HandheldDeviceConnectionStatus.DISCONNECTED ) @@ -227,7 +235,7 @@ def __event_loop(self) -> None: while True: if self.__stop_thread_signal_event.is_set(): return - event = self.controller.read_one() + event = self.handheld_device.read_one() # All events accumulate into a file and will be read back eventually, # even if handheld mode is disabled. This time based recency check ensures that # only the events that have occurred very recently are processed, and @@ -238,6 +246,7 @@ def __event_loop(self) -> None: < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD ): self.__process_event(event) + self.__send_proto_command_latest_primitive_controls() time.sleep(0.0005) @@ -275,28 +284,28 @@ def __process_event(self, event: InputEvent) -> None: # ) if event.type == ecodes.EV_ABS: - if event.code == self.controller_config.move_x.event_code: + if event.code == self.handheld_device_config.move_x.event_code: self.__interpret_move_event_value( event.value, - self.controller_config.move_x.max_value, + self.handheld_device_config.move_x.max_value, self.constants.robot_max_speed_m_per_s, ) - if event.code == self.controller_config.move_y.event_code: + if event.code == self.handheld_device_config.move_y.event_code: self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__interpret_move_event_value( -event.value, - self.controller_config.move_y.max_value, + self.handheld_device_config.move_y.max_value, self.constants.robot_max_speed_m_per_s, ) - if event.code == self.controller_config.move_rot.event_code: + if event.code == self.handheld_device_config.move_rot.event_code: self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__interpret_move_event_value( event.value, - self.controller_config.move_rot.max_value, + self.handheld_device_config.move_rot.max_value, self.constants.robot_max_ang_speed_rad_per_s, ) - elif event.code == self.controller_config.chicker_power.event_code: + elif event.code == self.handheld_device_config.chicker_power.event_code: self.kick_power_accumulator = self.__interpret_kick_event_value( event.value ) @@ -304,23 +313,23 @@ def __process_event(self, event: InputEvent) -> None: event.value ) - elif event.code == self.controller_config.dribbler_speed.event_code: + elif event.code == self.handheld_device_config.dribbler_speed.event_code: self.dribbler_speed_accumulator = self.__interpret_dribbler_speed_event_value( event.value ) elif ( - event.code == self.controller_config.primary_dribbler_enable.event_code or - event.code == self.controller_config.secondary_dribbler_enable.event_code + event.code == self.handheld_device_config.primary_dribbler_enable.event_code or + event.code == self.handheld_device_config.secondary_dribbler_enable.event_code ): self.dribbler_running = self.__interpret_dribbler_enabled_event_value( event.value, - self.controller_config.primary_dribbler_enable.max_value, + self.handheld_device_config.primary_dribbler_enable.max_value, ) if event.type == ecodes.EV_KEY: if ( - event.code == self.controller_config.kick.event_code + event.code == self.handheld_device_config.kick.event_code and event.value == 1 ): self.power_control.chicker.kick_speed_m_per_s = ( @@ -328,7 +337,7 @@ def __process_event(self, event: InputEvent) -> None: ) if ( - event.code == self.controller_config.chip.event_code + event.code == self.handheld_device_config.chip.event_code and event.value == 1 ): self.power_control.chicker.chip_distance_meters = ( From 8a7ef7854cd44509ab1a97132294343608616d82 Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 20 May 2024 15:19:46 -0700 Subject: [PATCH 116/138] layouting and ui changes on diagnostics tab - all controls now wrapped in group box with with proper titles. bunch of field testing updates, --- src/proto/primitive.proto | 3 +- src/software/thunderscope/constants.py | 68 +++---- .../thunderscope/robot_communication.py | 2 + .../thunderscope/robot_diagnostics/BUILD | 8 +- .../robot_diagnostics/chicker_widget.py | 76 ++++++-- .../diagnostics_input_widget.py | 13 +- .../robot_diagnostics/diagnostics_widget.py | 76 ++++---- .../drive_and_dribbler_widget.py | 4 + .../robot_diagnostics/estop_view.py | 2 + .../handheld_device_manager.py | 168 +++++++----------- .../handheld_device_status_view.py | 34 ++-- 11 files changed, 235 insertions(+), 219 deletions(-) diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index d592355033..af2abdd633 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -92,13 +92,14 @@ message PowerControl }; } - // TODO: Remove this message type - unused & obsolete + // TODO (3194): Remove this message type message GenevaControl { float angle_deg = 1; float rotation_speed_rpm = 2; } + // TODO (3194): Remove this message type ChickerControl chicker = 1; Geneva.Slot geneva_slot = 2; } diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 0e27d3b15b..f089140acb 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -343,17 +343,17 @@ class HandheldDeviceConfigKeys(Enum): # nomenclature: @dataclass -class HDKeyEvent: +class DeviceKeyEvent: """ - This dataclass holds the code for a key input event + This dataclass holds the code for a "EV_KEY" input event """ event_code: int @dataclass -class HDAbsEvent: +class DeviceAbsEvent: """ - This dataclass holds the code and max value for an abs input event + This dataclass holds the code and max value for an "EV_ABS" input event """ event_code: int @@ -361,66 +361,66 @@ class HDAbsEvent: @dataclass -class HDIEConfig: - move_x: HDAbsEvent - move_y: HDAbsEvent - move_rot: HDAbsEvent - kick: HDKeyEvent - chip: HDKeyEvent - chicker_power: HDAbsEvent - dribbler_speed: HDAbsEvent - primary_dribbler_enable: HDAbsEvent - secondary_dribbler_enable: HDAbsEvent +class DeviceConfig: + move_x: DeviceAbsEvent + move_y: DeviceAbsEvent + move_rot: DeviceAbsEvent + kick: DeviceKeyEvent + chip: DeviceKeyEvent + chicker_power: DeviceAbsEvent + dribbler_speed: DeviceAbsEvent + primary_dribbler_enable: DeviceAbsEvent + secondary_dribbler_enable: DeviceAbsEvent class HandheldDeviceConstants: - XboxConfig = HDIEConfig( + XboxConfig = DeviceConfig( # Name: "ABS_X" # Canonical: Left joystick X-axis - move_x=HDAbsEvent(event_code=0, max_value=32767.0), + move_x=DeviceAbsEvent(event_code=0, max_value=32767.0), # Name: "ABS_Y" # Canonical: Left joystick Y-axis - move_y=HDAbsEvent(event_code=1, max_value=32767.0), + move_y=DeviceAbsEvent(event_code=1, max_value=32767.0), # Name: "ABS_RX" # Canonical: Right joystick X-axis - move_rot=HDAbsEvent(event_code=3, max_value=32767.0), + move_rot=DeviceAbsEvent(event_code=3, max_value=32767.0), # Name: "BTN_A" # Canonical: "A" Button - kick=HDKeyEvent(event_code=304), + kick=DeviceKeyEvent(event_code=304), # Name: "BTN_Y" # Canonical: "Y" Button - chip=HDKeyEvent(event_code=308), + chip=DeviceKeyEvent(event_code=308), # Name: "ABS_HAT0X # Canonical: D-pad X-axis - chicker_power=HDAbsEvent(event_code=16, max_value=1.0), - # Name: "ABS_HAT0Y", Type: EV_ABS + chicker_power=DeviceAbsEvent(event_code=16, max_value=1.0), + # Name: "ABS_HAT0Y" # Canonical: D-pad Y-axis - dribbler_speed=HDAbsEvent(event_code=17, max_value=1.0), - # Name: "ABS_Z", Type: EV_ABS + dribbler_speed=DeviceAbsEvent(event_code=17, max_value=1.0), + # Name: "ABS_Z" # Canonical: Left trigger - primary_dribbler_enable=HDAbsEvent(event_code=2, max_value=1023.0), - # Name: "ABS_RZ", Type: EV_ABS + primary_dribbler_enable=DeviceAbsEvent(event_code=2, max_value=1023.0), + # Name: "ABS_RZ" # Canonical: Right trigger - secondary_dribbler_enable=HDAbsEvent(event_code=5, max_value=1023.0), + secondary_dribbler_enable=DeviceAbsEvent(event_code=5, max_value=1023.0), ) - CONTROLLER_NAME_CONFIG_MAP = { + HANDHELD_DEVICE_NAME_CONFIG_MAP = { "Microsoft Xbox One X pad": XboxConfig, "Microsoft X-Box One S pad": XboxConfig, "Microsoft X-Box 360 pad": XboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 - DEADZONE_PERCENTAGE = 0.10 + DEADZONE_PERCENTAGE = 0.05 DRIBBLER_RPM_STEPPER = 1000 # This is actually considered to be an "indefinite" speed in the robots software backend DRIBBLER_MAX_RPM = 10000 - KICK_POWER_STEPPER = 1000.0 - MIN_KICK_POWER = 1000.0 - MAX_KICK_POWER = 20000.0 + KICK_POWER_STEPPER = 1 + MIN_KICK_POWER = 1 + MAX_KICK_POWER = 10 - CHIP_DISTANCE_STEPPER = 0.25 - MIN_CHIP_POWER = 0.25 + CHIP_DISTANCE_STEPPER = 0.5 + MIN_CHIP_POWER = 0.5 MAX_CHIP_POWER = 5.0 diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 4eadac068f..c394bf6354 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -123,6 +123,8 @@ def setup_for_fullsystem(self) -> None: """ Sets up a listener for SSL vision and referee data, and connects all robots to fullsystem as default """ + + # set all robots to AI control in the control mode map self.robot_control_mode_map.update( (robot_id, IndividualRobotMode.AI) for robot_id in self.robot_control_mode_map.keys() diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 91a62fe9ac..2a4c4bc7a6 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -24,7 +24,7 @@ py_library( ) py_library( - name = "controller", + name = "handheld_device_manager", srcs = ["handheld_device_manager.py"], deps = [ "//proto:software_py_proto", @@ -36,7 +36,7 @@ py_library( ) py_library( - name = "controller_status_view", + name = "handheld_device_status_view", srcs = ["handheld_device_status_view.py"], deps = [ "//software/thunderscope:constants", @@ -53,8 +53,8 @@ py_library( "//software/thunderscope:thread_safe_buffer", "//software/thunderscope:thunderscope_types", "//software/thunderscope/robot_diagnostics:chicker", - "//software/thunderscope/robot_diagnostics:controller", - "//software/thunderscope/robot_diagnostics:controller_status_view", + "//software/thunderscope/robot_diagnostics:handheld_device_manager", + "//software/thunderscope/robot_diagnostics:handheld_device_status_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", requirement("pyqtgraph"), diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 0da2067cf4..e9e7a4032c 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -5,8 +5,8 @@ from proto.import_all_protos import * from enum import Enum import software.thunderscope.common.common_widgets as common_widgets +from software.thunderscope.constants import HandheldDeviceConstants from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -37,13 +37,16 @@ def __init__( super(ChickerWidget, self).__init__() - self.proto_unix_io = proto_unix_io + self.proto_unix_io: ProtoUnixIO = proto_unix_io # initial values self.__initialize_default_power_control_values() - vbox_layout = QVBoxLayout() - self.setLayout(vbox_layout) + chicker_widget_vbox_layout = QVBoxLayout() + self.setLayout(chicker_widget_vbox_layout) + + kick_chip_sliders_hbox_layout = QHBoxLayout() + kick_chip_sliders_hbox_layout.setContentsMargins(0, 0, 0, 0) # Initializing power slider for kicking ( @@ -51,31 +54,50 @@ def __init__( self.kick_power_slider, self.kick_power_label, ) = common_widgets.create_slider( - "Power (m/s)", 1, 10, 1 + "Power (m/s)", + HandheldDeviceConstants.MIN_KICK_POWER, + HandheldDeviceConstants.MAX_KICK_POWER, + HandheldDeviceConstants.KICK_POWER_STEPPER ) - vbox_layout.addLayout(self.kick_power_slider_layout) + + kick_chip_sliders_hbox_layout.addLayout(self.kick_power_slider_layout) # Initializing distance slider for chipping ( self.chip_distance_slider_layout, self.chip_distance_slider, self.chip_distance_label, - ) = common_widgets.create_slider( - "Chip Distance (m)", 1, 10, 1 + ) = common_widgets.create_float_slider( + "Chip Distance (m)", + 1, + HandheldDeviceConstants.MIN_CHIP_POWER, + HandheldDeviceConstants.MAX_CHIP_POWER, + HandheldDeviceConstants.CHIP_DISTANCE_STEPPER ) - vbox_layout.addLayout(self.kick_power_slider_layout) + + kick_chip_sliders_hbox_layout.addLayout(self.chip_distance_slider_layout) + + kick_chip_sliders_box = QGroupBox() + kick_chip_sliders_box.setLayout(kick_chip_sliders_hbox_layout) + kick_chip_sliders_box.setTitle("Kick Power and Chip Distance") + + chicker_widget_vbox_layout.addWidget(kick_chip_sliders_box) # Initializing kick & chip buttons - self.push_button_box, self.push_buttons = common_widgets.create_buttons( + self.kick_chip_buttons_box, self.kick_chip_buttons = common_widgets.create_buttons( ["Kick", "Chip"] ) - self.kick_button = self.push_buttons[0] - self.chip_button = self.push_buttons[1] + + self.kick_chip_buttons_box.setTitle("Single Kick and Chip") + self.kick_chip_buttons_box.layout().setContentsMargins(0, 0, 0, 0) + + self.kick_button = self.kick_chip_buttons[0] + self.chip_button = self.kick_chip_buttons[1] # set buttons to be initially enabled self.kick_chip_buttons_enable = True - vbox_layout.addWidget(self.push_button_box) + chicker_widget_vbox_layout.addWidget(self.kick_chip_buttons_box) # Initializing auto kick & chip buttons self.button_clickable_map = { @@ -84,12 +106,15 @@ def __init__( "auto_chip": True, } self.radio_buttons_group = QButtonGroup() - self.radio_button_box, self.radio_buttons = common_widgets.create_radio( + self.auto_kick_chip_buttons_box, self.auto_kick_chip_buttons = common_widgets.create_radio( ["No Auto", "Auto Kick", "Auto Chip"], self.radio_buttons_group ) - self.no_auto_button = self.radio_buttons[0] - self.auto_kick_button = self.radio_buttons[1] - self.auto_chip_button = self.radio_buttons[2] + self.auto_kick_chip_buttons_box.setTitle("Auto Kick and Chip") + self.auto_kick_chip_buttons_box.layout().setContentsMargins(0, 0, 0, 0) + + self.no_auto_button = self.auto_kick_chip_buttons[0] + self.auto_kick_button = self.auto_kick_chip_buttons[1] + self.auto_chip_button = self.auto_kick_chip_buttons[2] # Set no auto button to be selected by default on launch self.no_auto_button.setChecked(True) @@ -114,7 +139,21 @@ def __init__( lambda: self.set_should_enable_buttons(False) ) - vbox_layout.addWidget(self.radio_button_box) + chicker_widget_vbox_layout.addWidget(self.auto_kick_chip_buttons_box) + + def set_kick_power_slider_value(self, value: float) -> None: + """ + Set the kick power slider to a specific value. + :param value: The value to set for the slider + """ + self.kick_power_slider.setValue(value) + + def set_chip_distance_slider_value(self, value: float) -> None: + """ + Set the chip distance slider to a specific value. + :param value: the value to set for the slider + """ + self.chip_distance_slider.setValue(value) def send_command_and_timeout(self, command: ChickerCommandMode) -> None: """ @@ -206,6 +245,7 @@ def __initialize_default_power_control_values(self) -> None: """ self.power_control = PowerControl() self.power_control.geneva_slot = 3 + self.proto_unix_io.send_proto(PowerControl, self.power_control, True) def refresh_button_state(self, button: QPushButton) -> None: """Change button color and clickable state. diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 71b9134a00..fcdad51171 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -39,7 +39,7 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No self.diagnostics_input_mode_signal = diagnostics_input_mode_signal - vbox_layout = QVBoxLayout() + diagnostics_input_widget_vbox_layout = QVBoxLayout() self.connect_options_group = QButtonGroup() @@ -47,6 +47,9 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No ["Diagnostics Control", "Handheld Control"], self.connect_options_group ) + self.connect_options_box.layout().setContentsMargins(0, 0, 0, 0) + self.connect_options_box.setTitle("Diagnostics Input") + self.diagnostics_control_button = self.connect_options[ControlMode.DIAGNOSTICS] self.handheld_control_button = self.connect_options[ControlMode.HANDHELD] @@ -61,16 +64,16 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No self.handheld_control_button.setEnabled(False) self.diagnostics_control_button.setChecked(True) - vbox_layout.addWidget(self.connect_options_box) + diagnostics_input_widget_vbox_layout.addWidget(self.connect_options_box) - self.setLayout(vbox_layout) + self.setLayout(diagnostics_input_widget_vbox_layout) def refresh(self, status: HandheldDeviceConnectionStatus) -> None: """ Refresh this widget. - If the controller is connected: + If the handheld device is connected: - enables the handheld button - If the controller is disconnected: + If the handheld device is disconnected: - disables the handheld control button - sets the diagnostics button to checked - emits diagnostics input change signal diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 5db908d750..050df5b0f9 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -24,7 +24,7 @@ ) -class DiagnosticsWidget(QWidget): +class DiagnosticsWidget(QScrollArea): """ The DiagnosticsWidget contains all widgets related to diagnostics: - HandheldDeviceStatusWidget @@ -39,10 +39,10 @@ class DiagnosticsWidget(QWidget): # signal to indicate if manual controls should be disabled based on enum mode parameter diagnostics_input_mode_signal = pyqtSignal(ControlMode) - # signal to indicate that controller should be reinitialized + # signal to indicate that handheld device should be reinitialized reinitialize_handheld_device_signal = pyqtSignal() - # signal to indicate that controller was disconnected + # signal to indicate that handheld device was disconnected handheld_device_connection_status_signal = pyqtSignal( HandheldDeviceConnectionStatus ) @@ -75,7 +75,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # connect handheld device reinitialization signal with handler self.reinitialize_handheld_device_signal.connect( - lambda: self.handheld_device_handler.reinitialize_controller() + lambda: self.handheld_device_handler.reinitialize_handheld_device() ) # connect handheld device connection status toggle signal with handler @@ -91,14 +91,21 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) # layout for the entire diagnostics tab - vbox_layout = QVBoxLayout() - - vbox_layout.addWidget(self.handheld_device_status_widget) - vbox_layout.addWidget(self.diagnostics_control_input_widget) - vbox_layout.addWidget(self.drive_dribbler_widget) - vbox_layout.addWidget(self.chicker_widget) - - self.setLayout(vbox_layout) + diagnostics_widget_vbox_layout = QVBoxLayout() + + diagnostics_widget_vbox_layout.addWidget(self.handheld_device_status_widget) + diagnostics_widget_vbox_layout.addWidget(self.diagnostics_control_input_widget) + diagnostics_widget_vbox_layout.addWidget(self.drive_dribbler_widget) + diagnostics_widget_vbox_layout.addWidget(self.chicker_widget) + + # for a QScrollArea, widgets cannot be added to it directly + # doing so causes no scrolling to happen, and all the components get smaller + # instead, widgets are added to the layout which is set for a container + # the container is set as the current QScrollArea's widget + self.container = QFrame(self) + self.container.setLayout(diagnostics_widget_vbox_layout) + self.setWidget(self.container) + self.setWidgetResizable(True) def __control_mode_update_handler(self, mode: ControlMode) -> None: """ @@ -109,7 +116,7 @@ def __control_mode_update_handler(self, mode: ControlMode) -> None: self.handheld_device_handler.refresh(self.__control_mode) def __handheld_device_connection_status_update_handler( - self, status: HandheldDeviceConnectionStatus + self, status: HandheldDeviceConnectionStatus ): """ Handler for propagating @@ -119,8 +126,9 @@ def __handheld_device_connection_status_update_handler( def refresh(self): """ - Refreshes sub-widgets so that they display the most recent values, as well as - the most recent values are available for use. + Refreshes sub-widgets so that they display the most recent status values. + If in handheld mode, then also visually updates driver, dribbler and chicker sliders + to the values currently being set by the handheld device. """ # update controller status view with most recent controller status @@ -133,40 +141,26 @@ def refresh(self): self.drive_dribbler_widget.refresh(self.__control_mode) self.chicker_widget.refresh(self.__control_mode) - if self.__control_mode == ControlMode.DIAGNOSTICS: - self.logger.debug(self.chicker_widget.power_control) - - elif self.__control_mode == ControlMode.HANDHELD: - self.logger.debug(self.handheld_device_handler.power_control) + if self.__control_mode == ControlMode.HANDHELD: + # visually update driver, dribbler, kick power and chip distance sliders, + # based on the controller values + motor_control = self.handheld_device_handler.motor_control + kick_power = self.handheld_device_handler.kick_power_accumulator + chip_distance = self.handheld_device_handler.chip_distance_accumulator - # update drive and dribbler visually. - # TODO kick power, flash kick and chip buttons self.drive_dribbler_widget.set_x_velocity_slider( - self.handheld_device_handler.motor_control.direct_velocity_control.velocity.x_component_meters + motor_control.direct_velocity_control.velocity.x_component_meters ) self.drive_dribbler_widget.set_y_velocity_slider( - self.handheld_device_handler.motor_control.direct_velocity_control.velocity.y_component_meters + motor_control.direct_velocity_control.velocity.y_component_meters ) self.drive_dribbler_widget.set_angular_velocity_slider( - self.handheld_device_handler.motor_control.direct_velocity_control.angular_velocity.radians_per_second + motor_control.direct_velocity_control.angular_velocity.radians_per_second ) self.drive_dribbler_widget.set_dribbler_velocity_slider( - self.handheld_device_handler.motor_control.dribbler_speed_rpm + motor_control.dribbler_speed_rpm ) - # TODO: investigate why these methods aren't called on user hitting close button - def closeEvent(self, event): - self.logger.info("test") - self.handheld_device_handler.close() - event.accept() - - def close(self, event): - self.logger.info("test") - self.handheld_device_handler.close() - event.accept() - - def __exit__(self, event): - self.logger.info("test") - self.handheld_device_handler.close() - event.accept() + self.chicker_widget.set_kick_power_slider_value(kick_power) + self.chicker_widget.set_chip_distance_slider_value(chip_distance) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 0db2477532..dd0932ae09 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -93,6 +93,8 @@ def __setup_direct_velocity(self) -> QGroupBox: group_box = QGroupBox("Drive") dbox = QVBoxLayout() + dbox.setContentsMargins(0, 0, 0, 0) + ( x_layout, self.x_velocity_slider, @@ -165,6 +167,8 @@ def __setup_dribbler(self) -> QGroupBox: group_box = QGroupBox("Dribbler") dbox = QVBoxLayout() + dbox.setContentsMargins(0, 0, 0, 0) + ( dribbler_layout, self.dribbler_speed_rpm_slider, diff --git a/src/software/thunderscope/robot_diagnostics/estop_view.py b/src/software/thunderscope/robot_diagnostics/estop_view.py index 3363964793..fd2a289d61 100644 --- a/src/software/thunderscope/robot_diagnostics/estop_view.py +++ b/src/software/thunderscope/robot_diagnostics/estop_view.py @@ -1,6 +1,7 @@ import pyqtgraph as pg from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * +from pyqtgraph.Qt.QtCore import * from software.py_constants import * from proto.import_all_protos import * import software.thunderscope.common.common_widgets as common_widgets @@ -17,6 +18,7 @@ def __init__(self) -> None: self.estop_state_buffer = ThreadSafeBuffer(1, EstopState) + self.setAlignment(Qt.AlignmentFlag.AlignCenter) self.setText("Disconnected") self.setStyleSheet("background-color: blue") diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 58af75be1e..c91b28cefe 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -36,29 +36,29 @@ def __init__( self.handheld_device_disconnected_signal = handheld_device_disconnected_signal self.handheld_device: Optional[InputDevice] = None - self.handheld_device_config: Optional[HDIEConfig] = None + self.handheld_device_config: Optional[DeviceConfig] = None # initialize proto fields for motor and power control; set default values self.__initialize_default_motor_control_values() self.__initialize_default_power_control_values() - # These fields are here to temporarily persist the controller's input. - # They are read once certain buttons are pressed on the controller, - # and the values are inserted into the control primitives above. - self.kick_power_accumulator: float = 0.0 - self.chip_distance_accumulator: float = 0.0 + # The following fields are here to temporarily persist device input. + # They are read once certain buttons are pressed on the handheld device, + # and the values are set for the control primitives above. + self.kick_power_accumulator: float = HandheldDeviceConstants.MIN_KICK_POWER + self.chip_distance_accumulator: float = HandheldDeviceConstants.MIN_CHIP_POWER self.dribbler_speed_accumulator: int = 0 self.dribbler_running: bool = False self.constants = tbots_cpp.create2021RobotConstants() - # event that is used to stop the controller event loop + # event that is used to stop the handheld device event loop self.__stop_thread_signal_event = Event() # Set-up process that will run the event loop self.__setup_new_event_listener_thread() - self.__initialize_controller() + self.__initialize_handheld_device() def __initialize_default_motor_control_values(self) -> None: """ @@ -84,32 +84,32 @@ def __initialize_default_power_control_values(self) -> None: self.power_control = PowerControl() self.power_control.geneva_slot = 3 - def reinitialize_controller(self) -> None: + def reinitialize_handheld_device(self) -> None: """ - Reinitialize controller + Reinitialize handheld_device """ - self.__clear_controller() - self.__initialize_controller() + self.__clear_handheld_device() + self.__initialize_handheld_device() - def __initialize_controller(self) -> None: + def __initialize_handheld_device(self) -> None: """ Attempt to initialize a a connection to a handheld device, which may or may not be successful. - The first controller that is recognized as a valid controller will be used. + The first handheld device that is recognized as a valid will be used. Valid handheld devices are any devices whose name has a matching HDIEConfig defined in constants.py """ for device in list_devices(): - controller = InputDevice(device) + handheld_device = InputDevice(device) if ( - controller is not None - and controller.name - in HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP + handheld_device is not None + and handheld_device.name + in HandheldDeviceConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP ): self.__stop_thread_signal_event.clear() - self.handheld_device = controller - self.handheld_device_config: HDIEConfig = HandheldDeviceConstants.CONTROLLER_NAME_CONFIG_MAP[ - controller.name + self.handheld_device = handheld_device + self.handheld_device_config: DeviceConfig = HandheldDeviceConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP[ + handheld_device.name ] break @@ -132,15 +132,15 @@ def __initialize_controller(self) -> None: HandheldDeviceConnectionStatus.DISCONNECTED ) self.logger.debug( - "Failed to initialize a handheld controller, check USB connection" + "Failed to initialize a handheld device, check USB connection" ) self.logger.debug("Tried the following available devices:") self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __get_handheld_device_connection_status(self) -> HandheldDeviceConnectionStatus: """ - Get and return the latest connection status, based on the `controller` field - :return: the current status of the handheld device conneciton + Get and return the latest connection status, based on the `handheld device` field + :return: the current status of the handheld device connection """ return ( HandheldDeviceConnectionStatus.CONNECTED @@ -164,21 +164,21 @@ def refresh(self, mode: ControlMode): :param mode: The current user requested mode for controlling the robot """ if mode == ControlMode.DIAGNOSTICS: - if self.__controller_event_loop_handler_thread.is_alive(): - self.logger.debug("Terminating controller event handling process") + if self.__handheld_device_event_loop_thread.is_alive(): + self.logger.debug("Terminating handheld device event handling process") self.__shutdown_event_listener_thread() elif mode == ControlMode.HANDHELD: - if not self.__controller_event_loop_handler_thread.is_alive(): - self.logger.debug("Setting up new controller event handling process") + if not self.__handheld_device_event_loop_thread.is_alive(): + self.logger.debug("Setting up new handheld device event handling process") self.__setup_new_event_listener_thread() self.__start_event_listener_thread() self.proto_unix_io.send_proto(MotorControl, self.motor_control) self.proto_unix_io.send_proto(PowerControl, self.power_control) - def __clear_controller(self) -> None: + def __clear_handheld_device(self) -> None: """ - Clears controller & config field by setting to null, + Clears handheld device & config field by setting to null, and emits a disconnected notification signal. """ self.handheld_device = None @@ -187,37 +187,31 @@ def __clear_controller(self) -> None: HandheldDeviceConnectionStatus.DISCONNECTED ) - def close(self) -> None: - """ - Shuts down the thread running the event processing loop. - """ - self.__shutdown_event_listener_thread() - def __shutdown_event_listener_thread(self) -> None: """ Shut down the event processing loop by setting the stop event flag, and then joins the handling thread, if it is alive. """ # TODO (#3165): Use trace level logging here - # self.logger.debug("Shutdown down controller event loop process") + # self.logger.debug("Shutdown down handheld device event loop process") self.__stop_thread_signal_event.set() - if self.__controller_event_loop_handler_thread.is_alive(): - self.__controller_event_loop_handler_thread.join() + if self.__handheld_device_event_loop_thread.is_alive(): + self.__handheld_device_event_loop_thread.join() self.__stop_thread_signal_event.clear() def __start_event_listener_thread(self) -> None: """ Starts the thread that runs the event processing loop. """ - self.__controller_event_loop_handler_thread.start() + self.__handheld_device_event_loop_thread.start() def __setup_new_event_listener_thread(self): """ Initializes a new thread that will run the event processing loop """ # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Initializing new controller event loop thread") - self.__controller_event_loop_handler_thread = Thread( + # self.logger.debug("Initializing new handheld device event loop thread") + self.__handheld_device_event_loop_thread = Thread( target=self.__event_loop, daemon=True ) @@ -230,7 +224,7 @@ def __event_loop(self) -> None: cause the manager to revert to a disconnected handheld device state. """ # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Starting handheld controller event handling loop") + # self.logger.debug("Starting handheld handheld device event handling loop") try: while True: if self.__stop_thread_signal_event.is_set(): @@ -251,17 +245,17 @@ def __event_loop(self) -> None: time.sleep(0.0005) except OSError as ose: - self.__clear_controller() + self.__clear_handheld_device() self.logger.debug( - "Caught an OSError while reading handheld controller event loop!" + "Caught an OSError while reading handheld handheld device event loop!" ) self.logger.debug("Error message: " + str(ose)) - self.logger.debug("Check physical handheld controller USB connection!") + self.logger.debug("Check physical handheld handheld device USB connection!") return except Exception as e: - self.__clear_controller() + self.__clear_handheld_device() self.logger.critical( - "Caught an unexpected error while reading handheld controller event loop!" + "Caught an unexpected error while reading handheld handheld device event loop!" ) self.logger.critical("Error message: " + str(e)) return @@ -275,7 +269,7 @@ def __process_event(self, event: InputEvent) -> None: # TODO (#3165): Use trace level self.logger here # self.logger.debug( - # "Processing controller event with type: " + # "Processing handheld device event with type: " # + str(ecodes.bytype[event.type][event.code]) # + ", with code: " # + str(event.code) @@ -285,24 +279,30 @@ def __process_event(self, event: InputEvent) -> None: if event.type == ecodes.EV_ABS: if event.code == self.handheld_device_config.move_x.event_code: - self.__interpret_move_event_value( - event.value, - self.handheld_device_config.move_x.max_value, - self.constants.robot_max_speed_m_per_s, + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( + self.__interpret_move_event_value( + event.value, + self.handheld_device_config.move_x.max_value, + self.constants.robot_max_speed_m_per_s, + ) ) if event.code == self.handheld_device_config.move_y.event_code: - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__interpret_move_event_value( - -event.value, - self.handheld_device_config.move_y.max_value, - self.constants.robot_max_speed_m_per_s, + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + self.__interpret_move_event_value( + -event.value, + self.handheld_device_config.move_y.max_value, + self.constants.robot_max_speed_m_per_s, + ) ) if event.code == self.handheld_device_config.move_rot.event_code: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__interpret_move_event_value( - event.value, - self.handheld_device_config.move_rot.max_value, - self.constants.robot_max_ang_speed_rad_per_s, + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.__interpret_move_event_value( + -event.value, + self.handheld_device_config.move_rot.max_value, + self.constants.robot_max_ang_speed_rad_per_s, + ) ) elif event.code == self.handheld_device_config.chicker_power.event_code: @@ -398,59 +398,21 @@ def __interpret_kick_event_value(self, event_value: float) -> float: :return: the interpreted value to be used for the kick power on the robot """ return numpy.clip( - a=self.power_control.chicker.kick_speed_m_per_s + a=self.kick_power_accumulator + event_value * HandheldDeviceConstants.KICK_POWER_STEPPER, a_min=HandheldDeviceConstants.MIN_KICK_POWER, a_max=HandheldDeviceConstants.MAX_KICK_POWER, ) - def __interpret_chip_event_value(self, value: float) -> float: + def __interpret_chip_event_value(self, event_value: float) -> float: """ Interprets the event value that corresponds to controlling the chip distance. :param value: the value for the current event being interpreted :return: the interpreted value to be used for the chip distance on the robot """ return numpy.clip( - a=self.power_control.chicker.chip_distance_meters - + value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, + a=self.chip_distance_accumulator + + event_value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, a_min=HandheldDeviceConstants.MIN_CHIP_POWER, a_max=HandheldDeviceConstants.MAX_CHIP_POWER, ) - - -# TODO: remove thee after field testing... -# { -# ('EV_SYN', 0): [('SYN_REPORT', 0), ('SYN_CONFIG', 1), ('SYN_DROPPED', 3), ('?', 21)], -# ('EV_KEY', 1): [ -# ('KEY_RECORD', 167), -# (['BTN_A', 'BTN_GAMEPAD', 'BTN_SOUTH'], 304), -# (['BTN_B', 'BTN_EAST'], 305), -# (['BTN_NORTH', 'BTN_X'], 307), -# (['BTN_WEST', 'BTN_Y'], 308), -# ('BTN_TL', 310), -# ('BTN_TR', 311), -# ('BTN_SELECT', 314), -# ('BTN_START', 315), -# ('BTN_MODE', 316), -# ('BTN_THUMBL', 317), -# ('BTN_THUMBR', 318) -# ], -# ('EV_ABS', 3): [ -# (('ABS_X', 0), AbsInfo(value=1242, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_Y', 1), AbsInfo(value=425, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_Z', 2), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), -# (('ABS_RX', 3), AbsInfo(value=-418, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_RY', 4), AbsInfo(value=-485, min=-32768, max=32767, fuzz=16, flat=128, resolution=0)), -# (('ABS_RZ', 5), AbsInfo(value=0, min=0, max=1023, fuzz=0, flat=0, resolution=0)), -# (('ABS_HAT0X', 16), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)), -# (('ABS_HAT0Y', 17), AbsInfo(value=0, min=-1, max=1, fuzz=0, flat=0, resolution=0)) -# ], -# ('EV_FF', 21): [ -# (['FF_EFFECT_MIN', 'FF_RUMBLE'], 80), -# ('FF_PERIODIC', 81), -# (['FF_SQUARE', 'FF_WAVEFORM_MIN'], 88), -# ('FF_TRIANGLE', 89), -# ('FF_SINE', 90), -# (['FF_GAIN', 'FF_MAX_EFFECTS'], 96) -# ] -# } diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 3c01055ccb..86b01ba767 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -18,48 +18,56 @@ class HandheldDeviceConnectionStatus(Enum): class HandheldDeviceStatusView(QWidget): - def __init__(self, reinitialize_controller_signal: Type[QtCore.pyqtSignal]) -> None: + def __init__(self, reinitialize_handheld_device_signal: Type[QtCore.pyqtSignal]) -> None: """ Initialize the HandheldDeviceStatusView widget. - This widget sjows the user the current state of the connection with a handheld device, - as well as a button that attempts to reinitialize a controller object when clicked - :param reinitialize_controller_signal: The signal to use for the reinitialize button + This widget shows the user the current state of the connection with a handheld device, + as well as a button that attempts to reinitialize a handheld device object when clicked + :param reinitialize_handheld_device_signal: The signal to use for the reinitialize button """ super(HandheldDeviceStatusView, self).__init__() - self.reinitialize_controller_signal = reinitialize_controller_signal + self.reinitialize_handheld_device_signal = reinitialize_handheld_device_signal self.handheld_device_status = QLabel() + self.handheld_device_status.setAlignment(Qt.AlignmentFlag.AlignCenter) self.status_label_view_map: dict[HandheldDeviceConnectionStatus, (str, str)] = { HandheldDeviceConnectionStatus.CONNECTED: ( - "Handheld Controller is Connected & Initialized", + "Handheld Device is Connected", "background-color: green", ), HandheldDeviceConnectionStatus.DISCONNECTED: ( - "No Handheld Controller is Connected...", + "Handheld Device is Disconnected...", "background-color: red", ), } - # initialize controller refresh button + # initialize device refresh button self.handheld_device_reinitialize_button = QPushButton() self.handheld_device_reinitialize_button.setText( - "Re-initialize Handheld Controller" + "Re-initialize Handheld Device" ) self.handheld_device_reinitialize_button.clicked.connect( - self.reinitialize_controller_signal + self.reinitialize_handheld_device_signal ) + box = QGroupBox() hbox_layout = QHBoxLayout() + hbox_layout.setContentsMargins(0, 0, 0, 0) + box.setTitle("Handheld Device Status and Re-initialization") + widget_layout = QVBoxLayout() hbox_layout.addWidget(self.handheld_device_status) hbox_layout.addWidget(self.handheld_device_reinitialize_button) hbox_layout.setStretch(0, 4) hbox_layout.setStretch(1, 1) + box.setLayout(hbox_layout) + widget_layout.addWidget(box) + self.set_view_state(HandheldDeviceConnectionStatus.DISCONNECTED) - self.setLayout(hbox_layout) + self.setLayout(widget_layout) def set_view_state(self, connection_state: HandheldDeviceConnectionStatus) -> None: """ @@ -76,7 +84,7 @@ def set_view_state(self, connection_state: HandheldDeviceConnectionStatus) -> No def refresh(self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: """ Refreshes this widget. - The status label will reflect to the user the current state of the controller connection - :param connection_state: The new state of the controller connection + The status label will reflect to the user the current state of the handheld device connection + :param connection_state: The new state of the handheld device connection """ self.set_view_state(connection_state) From bafd463361d576a0d3a7ad24b625390aeaed16d9 Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 20 May 2024 15:22:49 -0700 Subject: [PATCH 117/138] formatting --- .../message_translation/power_frame_msg.hpp | 3 +- src/software/thunderscope/constants.py | 4 +- .../thunderscope/robot_communication.py | 3 +- .../thunderscope/robot_diagnostics/BUILD | 4 +- .../robot_diagnostics/chicker_widget.py | 21 ++++---- .../robot_diagnostics/diagnostics_widget.py | 7 ++- .../drive_and_dribbler_widget.py | 13 ++--- .../handheld_device_manager.py | 53 +++++++++---------- .../handheld_device_status_view.py | 8 ++- 9 files changed, 56 insertions(+), 60 deletions(-) diff --git a/src/proto/message_translation/power_frame_msg.hpp b/src/proto/message_translation/power_frame_msg.hpp index 12edc135e9..692cfc16ed 100644 --- a/src/proto/message_translation/power_frame_msg.hpp +++ b/src/proto/message_translation/power_frame_msg.hpp @@ -138,7 +138,8 @@ TbotsProto_PowerPulseControl inline createNanoPbPowerPulseControl( case TbotsProto::PowerControl::ChickerControl::kKickSpeedMPerS: nanopb_control.chicker.which_chicker_command = TbotsProto_PowerPulseControl_ChickerControl_kick_pulse_width_tag; - // TODO (#3193): refactor kick pulse width calculation out into seperate function + // TODO (#3193): refactor kick pulse width calculation out into seperate + // function nanopb_control.chicker.chicker_command.kick_pulse_width = static_cast( kick_constant * diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index f089140acb..81c21e7476 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -1,5 +1,4 @@ from dataclasses import dataclass -from typing import Optional from pyqtgraph.Qt import QtCore, QtGui from proto.import_all_protos import * @@ -342,11 +341,13 @@ class HandheldDeviceConfigKeys(Enum): # nomenclature: + @dataclass class DeviceKeyEvent: """ This dataclass holds the code for a "EV_KEY" input event """ + event_code: int @@ -356,6 +357,7 @@ class DeviceAbsEvent: This dataclass holds the code and max value for an "EV_ABS" input event """ + event_code: int max_value: float diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index c394bf6354..d539d07505 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -253,8 +253,7 @@ def __run_primitive_set(self) -> None: diagnostics_primitive = Primitive( direct_control=DirectControlPrimitive( - motor_control=motor_control, - power_control=power_control, + motor_control=motor_control, power_control=power_control, ) ) diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 2a4c4bc7a6..24d6273f05 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -53,10 +53,10 @@ py_library( "//software/thunderscope:thread_safe_buffer", "//software/thunderscope:thunderscope_types", "//software/thunderscope/robot_diagnostics:chicker", - "//software/thunderscope/robot_diagnostics:handheld_device_manager", - "//software/thunderscope/robot_diagnostics:handheld_device_status_view", "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", + "//software/thunderscope/robot_diagnostics:handheld_device_manager", + "//software/thunderscope/robot_diagnostics:handheld_device_status_view", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index e9e7a4032c..d35256684e 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -18,10 +18,7 @@ class ChickerCommandMode(Enum): class ChickerWidget(QWidget): - def __init__( - self, - proto_unix_io - ) -> None: + def __init__(self, proto_unix_io) -> None: """Handles the robot diagnostics input to create a PowerControl message to be sent to the robots. @@ -57,7 +54,7 @@ def __init__( "Power (m/s)", HandheldDeviceConstants.MIN_KICK_POWER, HandheldDeviceConstants.MAX_KICK_POWER, - HandheldDeviceConstants.KICK_POWER_STEPPER + HandheldDeviceConstants.KICK_POWER_STEPPER, ) kick_chip_sliders_hbox_layout.addLayout(self.kick_power_slider_layout) @@ -72,7 +69,7 @@ def __init__( 1, HandheldDeviceConstants.MIN_CHIP_POWER, HandheldDeviceConstants.MAX_CHIP_POWER, - HandheldDeviceConstants.CHIP_DISTANCE_STEPPER + HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, ) kick_chip_sliders_hbox_layout.addLayout(self.chip_distance_slider_layout) @@ -84,9 +81,10 @@ def __init__( chicker_widget_vbox_layout.addWidget(kick_chip_sliders_box) # Initializing kick & chip buttons - self.kick_chip_buttons_box, self.kick_chip_buttons = common_widgets.create_buttons( - ["Kick", "Chip"] - ) + ( + self.kick_chip_buttons_box, + self.kick_chip_buttons, + ) = common_widgets.create_buttons(["Kick", "Chip"]) self.kick_chip_buttons_box.setTitle("Single Kick and Chip") self.kick_chip_buttons_box.layout().setContentsMargins(0, 0, 0, 0) @@ -106,7 +104,10 @@ def __init__( "auto_chip": True, } self.radio_buttons_group = QButtonGroup() - self.auto_kick_chip_buttons_box, self.auto_kick_chip_buttons = common_widgets.create_radio( + ( + self.auto_kick_chip_buttons_box, + self.auto_kick_chip_buttons, + ) = common_widgets.create_radio( ["No Auto", "Auto Kick", "Auto Chip"], self.radio_buttons_group ) self.auto_kick_chip_buttons_box.setTitle("Auto Kick and Chip") diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 050df5b0f9..f9eb2406e4 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,5 +1,3 @@ -from typing import Union, Literal, Tuple - from pyqtgraph.Qt import QtCore, QtGui from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * @@ -20,7 +18,7 @@ HandheldDeviceManager, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( - DriveAndDribblerWidget + DriveAndDribblerWidget, ) @@ -36,6 +34,7 @@ class DiagnosticsWidget(QScrollArea): either from the DriverAndDribblerWidget or the HandheldDeviceManager, depending on the user selected choice from the DiagnosticsWidget UI """ + # signal to indicate if manual controls should be disabled based on enum mode parameter diagnostics_input_mode_signal = pyqtSignal(ControlMode) @@ -116,7 +115,7 @@ def __control_mode_update_handler(self, mode: ControlMode) -> None: self.handheld_device_handler.refresh(self.__control_mode) def __handheld_device_connection_status_update_handler( - self, status: HandheldDeviceConnectionStatus + self, status: HandheldDeviceConnectionStatus ): """ Handler for propagating diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index dd0932ae09..aa55ac67fc 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -12,10 +12,7 @@ class DriveAndDribblerWidget(QWidget): - def __init__( - self, - proto_unix_io: ProtoUnixIO - ) -> None: + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """ Initialize the widget to control the robot's motors """ @@ -131,14 +128,10 @@ def __setup_direct_velocity(self) -> QGroupBox: # add listener functions for sliders to update label with slider value common_widgets.enable_slider( - self.x_velocity_slider, - self.x_velocity_label, - self.__value_change_handler + self.x_velocity_slider, self.x_velocity_label, self.__value_change_handler ) common_widgets.enable_slider( - self.y_velocity_slider, - self.y_velocity_label, - self.__value_change_handler + self.y_velocity_slider, self.y_velocity_label, self.__value_change_handler ) common_widgets.enable_slider( self.angular_velocity_slider, diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index c91b28cefe..2198558a9e 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -66,12 +66,11 @@ def __initialize_default_motor_control_values(self) -> None: control proto to the default/minimum default value """ self.motor_control = MotorControl() - self.motor_control.direct_velocity_control.velocity\ - .x_component_meters = 0.0 - self.motor_control.direct_velocity_control.velocity\ - .y_component_meters = 0.0 - self.motor_control.direct_velocity_control.angular_velocity\ - .radians_per_second = 0.0 + self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 + self.motor_control.direct_velocity_control.velocity.y_component_meters = 0.0 + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + 0.0 + ) self.motor_control.dribbler_speed_rpm = 0 def __initialize_default_power_control_values(self) -> None: @@ -169,7 +168,9 @@ def refresh(self, mode: ControlMode): self.__shutdown_event_listener_thread() elif mode == ControlMode.HANDHELD: if not self.__handheld_device_event_loop_thread.is_alive(): - self.logger.debug("Setting up new handheld device event handling process") + self.logger.debug( + "Setting up new handheld device event handling process" + ) self.__setup_new_event_listener_thread() self.__start_event_listener_thread() @@ -279,30 +280,24 @@ def __process_event(self, event: InputEvent) -> None: if event.type == ecodes.EV_ABS: if event.code == self.handheld_device_config.move_x.event_code: - self.motor_control.direct_velocity_control.velocity.x_component_meters = ( - self.__interpret_move_event_value( - event.value, - self.handheld_device_config.move_x.max_value, - self.constants.robot_max_speed_m_per_s, - ) + self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__interpret_move_event_value( + event.value, + self.handheld_device_config.move_x.max_value, + self.constants.robot_max_speed_m_per_s, ) if event.code == self.handheld_device_config.move_y.event_code: - self.motor_control.direct_velocity_control.velocity.y_component_meters = ( - self.__interpret_move_event_value( - -event.value, - self.handheld_device_config.move_y.max_value, - self.constants.robot_max_speed_m_per_s, - ) + self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__interpret_move_event_value( + -event.value, + self.handheld_device_config.move_y.max_value, + self.constants.robot_max_speed_m_per_s, ) if event.code == self.handheld_device_config.move_rot.event_code: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - self.__interpret_move_event_value( - -event.value, - self.handheld_device_config.move_rot.max_value, - self.constants.robot_max_ang_speed_rad_per_s, - ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__interpret_move_event_value( + -event.value, + self.handheld_device_config.move_rot.max_value, + self.constants.robot_max_ang_speed_rad_per_s, ) elif event.code == self.handheld_device_config.chicker_power.event_code: @@ -319,8 +314,10 @@ def __process_event(self, event: InputEvent) -> None: ) elif ( - event.code == self.handheld_device_config.primary_dribbler_enable.event_code or - event.code == self.handheld_device_config.secondary_dribbler_enable.event_code + event.code + == self.handheld_device_config.primary_dribbler_enable.event_code + or event.code + == self.handheld_device_config.secondary_dribbler_enable.event_code ): self.dribbler_running = self.__interpret_dribbler_enabled_event_value( event.value, @@ -368,7 +365,7 @@ def __interpret_move_event_value( @staticmethod def __interpret_dribbler_enabled_event_value( - event_value: float, max_value: float + event_value: float, max_value: float ) -> bool: """ Interpret the event_value that corresponds to controlling whether the dribbler is enabled diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 86b01ba767..aaf7da60bf 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -18,7 +18,9 @@ class HandheldDeviceConnectionStatus(Enum): class HandheldDeviceStatusView(QWidget): - def __init__(self, reinitialize_handheld_device_signal: Type[QtCore.pyqtSignal]) -> None: + def __init__( + self, reinitialize_handheld_device_signal: Type[QtCore.pyqtSignal] + ) -> None: """ Initialize the HandheldDeviceStatusView widget. This widget shows the user the current state of the connection with a handheld device, @@ -81,7 +83,9 @@ def set_view_state(self, connection_state: HandheldDeviceConnectionStatus) -> No self.status_label_view_map[connection_state][1] ) - def refresh(self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED) -> None: + def refresh( + self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED + ) -> None: """ Refreshes this widget. The status label will reflect to the user the current state of the handheld device connection From 5edb7805a0062b421c10ea63a3efd028033a401e Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 20 May 2024 15:36:24 -0700 Subject: [PATCH 118/138] small cleanup --- .../thunderscope/robot_diagnostics/handheld_device_manager.py | 2 +- src/software/thunderscope/thunderscope_main.py | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 2198558a9e..c2f85ca007 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -351,7 +351,7 @@ def __interpret_move_event_value( event_value: float, max_value: float, normalizing_multiplier: float ) -> float: """ - Parse the event_value that corresponds to movement control + Interpret the event_value that corresponds to movement control :param event_value: the value for the current event being interpreted :param max_value: max value for this type of event event type :param normalizing_multiplier: multiplier for converting between diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 36937c3a18..e6c4184941 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -38,8 +38,6 @@ if __name__ == "__main__": - logging.getLogger().setLevel(logging.DEBUG) - # Setup parser parser = argparse.ArgumentParser( description="Thunderscope: Run with no arguments to run AI vs AI" From 6c8391bb9b4508b0d173e300b1b307c9465ac6f0 Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 20 May 2024 15:36:52 -0700 Subject: [PATCH 119/138] formatting --- src/software/thunderscope/thunderscope_main.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index e6c4184941..bfe27acfde 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -1,6 +1,5 @@ import argparse import contextlib -import logging import os import sys import threading From e686a59364b6ec6714a378eec79f739ca0e8faff Mon Sep 17 00:00:00 2001 From: boris Date: Mon, 20 May 2024 15:42:00 -0700 Subject: [PATCH 120/138] pr comments --- src/software/thunderscope/constants.py | 2 ++ .../robot_diagnostics/handheld_device_manager.py | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 81c21e7476..3ed5a0b365 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -413,6 +413,8 @@ class HandheldDeviceConstants: } INPUT_DELAY_THRESHOLD = 0.01 + EVENT_LOOP_SLEEP_DURATION = 0.0005 + BUTTON_PRESSED_THRESHOLD = 0.5 DEADZONE_PERCENTAGE = 0.05 DRIBBLER_RPM_STEPPER = 1000 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index c2f85ca007..056e5a25de 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -243,7 +243,7 @@ def __event_loop(self) -> None: self.__process_event(event) self.__send_proto_command_latest_primitive_controls() - time.sleep(0.0005) + time.sleep(HandheldDeviceConstants.EVENT_LOOP_SLEEP_DURATION) except OSError as ose: self.__clear_handheld_device() @@ -373,7 +373,9 @@ def __interpret_dribbler_enabled_event_value( :param max_value: max value for this type of event event type :return: The interpreted value that can be set a value for a field in robot control """ - return (event_value / max_value) > 0.5 + return ( + event_value / max_value + ) > HandheldDeviceConstants.BUTTON_PRESSED_THRESHOLD def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: """ From 20d9aace66b94f0e362814440ef75b9e42117eeb Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 2 Sep 2024 15:56:07 -0700 Subject: [PATCH 121/138] Revert accidental change to pyqt version --- environment_setup/ubuntu20_requirements.txt | 4 ++-- environment_setup/ubuntu22_requirements.txt | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 7736b9bb2d..0ebbd0e385 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,8 +1,8 @@ protobuf==3.20.2 pyqtgraph==0.13.7 pyqtdarktheme==2.1.0 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 python-Levenshtein==0.12.2 diff --git a/environment_setup/ubuntu22_requirements.txt b/environment_setup/ubuntu22_requirements.txt index 7736b9bb2d..0ebbd0e385 100644 --- a/environment_setup/ubuntu22_requirements.txt +++ b/environment_setup/ubuntu22_requirements.txt @@ -1,8 +1,8 @@ protobuf==3.20.2 pyqtgraph==0.13.7 pyqtdarktheme==2.1.0 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 python-Levenshtein==0.12.2 From 4696dbaff4d17046dec26104d1e4c4adf0df7fc4 Mon Sep 17 00:00:00 2001 From: williamckha Date: Fri, 6 Sep 2024 21:32:36 -0700 Subject: [PATCH 122/138] Fix bugs --- src/software/thunderscope/constants.py | 27 ++----------------- .../robot_diagnostics/chicker_widget.py | 2 +- .../robot_diagnostics/diagnostics_widget.py | 10 +++---- .../drive_and_dribbler_widget.py | 6 ++--- .../handheld_device_manager.py | 5 ++-- 5 files changed, 13 insertions(+), 37 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 8946406b8b..b5b9a281b4 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -302,27 +302,6 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 -class RobotControlType(Enum): - MOVE_X = 1 - MOVE_Y = 2 - ROTATE = 3 - KICK = 4 - CHIP = 5 - KICK_POWER = 6 - DRIBBLER_SPEED = 7 - DRIBBLER_ENABLE_1 = 8 - DRIBBLER_ENABLE_2 = 9 - - -# TODO: come up with less ambiguous/weird name -class HandheldDeviceConfigKeys(Enum): - CODE = 1 - MAX_VALUE = 2 - - -# nomenclature: - - @dataclass class DeviceKeyEvent: """This dataclass holds the code for a "EV_KEY" input event""" @@ -384,8 +363,8 @@ class HandheldDeviceConstants: HANDHELD_DEVICE_NAME_CONFIG_MAP = { "Microsoft Xbox One X pad": XboxConfig, - "Microsoft X-Box One S pad": XboxConfig, - "Microsoft X-Box 360 pad": XboxConfig, + "Microsoft Xbox One S pad": XboxConfig, + "Microsoft Xbox 360 pad": XboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 @@ -394,8 +373,6 @@ class HandheldDeviceConstants: DEADZONE_PERCENTAGE = 0.05 DRIBBLER_RPM_STEPPER = 1000 - # This is actually considered to be an "indefinite" speed in the robots software backend - DRIBBLER_MAX_RPM = 10000 KICK_POWER_STEPPER = 1 MIN_KICK_POWER = 1 diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 08a00c7e3a..79bc005fff 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -66,7 +66,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: 1, HandheldDeviceConstants.MIN_CHIP_POWER, HandheldDeviceConstants.MAX_CHIP_POWER, - HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, + int(HandheldDeviceConstants.CHIP_DISTANCE_STEPPER), ) kick_chip_sliders_hbox_layout.addLayout(self.chip_distance_slider_layout) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index fd6e4de14e..603de0fa32 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -70,11 +70,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # connect input mode signal with handler self.diagnostics_input_mode_signal.connect(self.__control_mode_update_handler) - # connect handheld device reinitialization signal with handler - self.reinitialize_handheld_device_signal.connect( - lambda: self.handheld_device_handler.reinitialize_handheld_device() - ) - # connect handheld device connection status toggle signal with handler self.handheld_device_connection_status_signal.connect( self.__handheld_device_connection_status_update_handler @@ -87,6 +82,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.handheld_device_connection_status_signal, ) + # connect handheld device reinitialization signal with handler + self.reinitialize_handheld_device_signal.connect( + self.handheld_device_handler.reinitialize_handheld_device + ) + # layout for the entire diagnostics tab diagnostics_widget_vbox_layout = QVBoxLayout() diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index b18033439c..fc7f627752 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -127,7 +127,7 @@ def __setup_direct_velocity(self) -> QGroupBox: self.__value_change_handler, ) - self.stop_and_reset_direct = common_widgets.create_push_button("Stop and Reset") + self.stop_and_reset_direct = QPushButton("Stop and Reset") self.stop_and_reset_direct.clicked.connect(self.__reset_direct_sliders) dbox.addLayout(x_layout) @@ -167,9 +167,7 @@ def __setup_dribbler(self) -> QGroupBox: self.__value_change_handler, ) - self.stop_and_reset_dribbler = common_widgets.create_push_button( - "Stop and Reset" - ) + self.stop_and_reset_dribbler = QPushButton("Stop and Reset") self.stop_and_reset_dribbler.clicked.connect(self.__reset_dribbler_slider) dbox.addLayout(dribbler_layout) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index e436afae62..469c0666fa 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -5,6 +5,7 @@ import numpy from evdev import InputDevice, ecodes, list_devices, InputEvent +from pyqtgraph.Qt import QtCore import software.python_bindings as tbots_cpp from proto.import_all_protos import * @@ -364,8 +365,8 @@ def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: return numpy.clip( a=self.dribbler_speed_accumulator - event_value * HandheldDeviceConstants.DRIBBLER_RPM_STEPPER, - a_min=-HandheldDeviceConstants.DRIBBLER_MAX_RPM, - a_max=HandheldDeviceConstants.DRIBBLER_MAX_RPM, + a_min=self.constants.indefinite_dribbler_speed_rpm, + a_max=-self.constants.indefinite_dribbler_speed_rpm, ) def __interpret_kick_event_value(self, event_value: float) -> float: From 58049f5983862f8999496f1a5f069e2a29b36058 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 21 Sep 2024 12:02:38 -0700 Subject: [PATCH 123/138] made it work but untested kind of --- src/software/thunderscope/common/common_widgets.py | 2 +- src/software/thunderscope/constants.py | 4 ++-- .../robot_diagnostics/handheld_device_manager.py | 5 +++-- src/software/thunderscope/robot_diagnostics/robot_info.py | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/software/thunderscope/common/common_widgets.py b/src/software/thunderscope/common/common_widgets.py index 8068578ba0..225eb1cf3f 100644 --- a/src/software/thunderscope/common/common_widgets.py +++ b/src/software/thunderscope/common/common_widgets.py @@ -145,7 +145,7 @@ def setValue(self, value: float) -> None: :param value: the float value to set """ - super(ColorProgressBar, self).setValue(value * self.decimals) + super(ColorProgressBar, self).setValue(int(value * self.decimals)) # clamp percent to make sure it's between 0% and 100% percent = self.getPercentage() diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index b5b9a281b4..867d08acdf 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -363,14 +363,14 @@ class HandheldDeviceConstants: HANDHELD_DEVICE_NAME_CONFIG_MAP = { "Microsoft Xbox One X pad": XboxConfig, - "Microsoft Xbox One S pad": XboxConfig, + "Microsoft X-Box One S pad": XboxConfig, "Microsoft Xbox 360 pad": XboxConfig, } INPUT_DELAY_THRESHOLD = 0.01 EVENT_LOOP_SLEEP_DURATION = 0.0005 BUTTON_PRESSED_THRESHOLD = 0.5 - DEADZONE_PERCENTAGE = 0.05 + DEADZONE_PERCENTAGE = 0.20 DRIBBLER_RPM_STEPPER = 1000 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 469c0666fa..6ddb2c704e 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -96,6 +96,7 @@ def __initialize_handheld_device(self) -> None: and handheld_device.name in HandheldDeviceConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP ): + print("I've been executed!") self.__stop_thread_signal_event.clear() self.handheld_device = handheld_device self.handheld_device_config: DeviceConfig = ( @@ -334,7 +335,7 @@ def __interpret_move_event_value( ) -> float: """Interpret the event_value that corresponds to movement control :param event_value: the value for the current event being interpreted - :param max_value: max value for this type of event event type + :param max_value: max value for this type event type :param normalizing_multiplier: multiplier for converting between :return: The interpreted value that can be set a value for a field in robot movement control """ @@ -350,7 +351,7 @@ def __interpret_dribbler_enabled_event_value( ) -> bool: """Interpret the event_value that corresponds to controlling whether the dribbler is enabled :param event_value: the value for the current event being interpreted - :param max_value: max value for this type of event event type + :param max_value: max value for this event type :return: The interpreted value that can be set a value for a field in robot control """ return ( diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index a5a86a6b41..7030d472d9 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -308,7 +308,7 @@ def update(self, robot_status: RobotStatus, round_trip_time: RobotStatistic): self.__update_ui(robot_status, round_trip_time) - QtCore.QTimer.singleShot(DISCONNECT_DURATION_MS, self.disconnect_robot) + QtCore.QTimer.singleShot(int(DISCONNECT_DURATION_MS), self.disconnect_robot) def disconnect_robot(self) -> None: """Calculates the time between the last robot status and now From 7deff554d200d2e8df29d48c40a5402831523531 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 22 Sep 2024 18:54:58 -0700 Subject: [PATCH 124/138] Fix thread safety problems, refactoring --- src/software/thunderscope/constants.py | 10 +- .../thunderscope/robot_communication.py | 2 +- .../robot_diagnostics/chicker_widget.py | 19 +- .../diagnostics_input_widget.py | 32 +- .../robot_diagnostics/diagnostics_widget.py | 121 +++--- .../drive_and_dribbler_widget.py | 4 - .../handheld_device_manager.py | 352 +++++++----------- .../handheld_device_status_view.py | 45 +-- .../robot_diagnostics/motor_fault_view.py | 1 - .../thunderscope/widget_setup_functions.py | 8 +- 10 files changed, 220 insertions(+), 374 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 867d08acdf..919044030b 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -326,11 +326,10 @@ class DeviceConfig: chip: DeviceKeyEvent chicker_power: DeviceAbsEvent dribbler_speed: DeviceAbsEvent - primary_dribbler_enable: DeviceAbsEvent - secondary_dribbler_enable: DeviceAbsEvent + dribbler_enable: DeviceAbsEvent -class HandheldDeviceConstants: +class DiagnosticsConstants: XboxConfig = DeviceConfig( # Name: "ABS_X" # Canonical: Left joystick X-axis @@ -353,12 +352,9 @@ class HandheldDeviceConstants: # Name: "ABS_HAT0Y" # Canonical: D-pad Y-axis dribbler_speed=DeviceAbsEvent(event_code=17, max_value=1.0), - # Name: "ABS_Z" - # Canonical: Left trigger - primary_dribbler_enable=DeviceAbsEvent(event_code=2, max_value=1023.0), # Name: "ABS_RZ" # Canonical: Right trigger - secondary_dribbler_enable=DeviceAbsEvent(event_code=5, max_value=1023.0), + dribbler_enable=DeviceAbsEvent(event_code=5, max_value=1023.0), ) HANDHELD_DEVICE_NAME_CONFIG_MAP = { diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index f00bdec153..c17d5a612d 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -19,7 +19,7 @@ ) -class RobotCommunication(object): +class RobotCommunication: """Communicate with the robots""" def __init__( diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 79bc005fff..eb7a03c26c 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -4,7 +4,7 @@ from proto.import_all_protos import * from enum import Enum import software.thunderscope.common.common_widgets as common_widgets -from software.thunderscope.constants import HandheldDeviceConstants +from software.thunderscope.constants import DiagnosticsConstants from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.proto_unix_io import ProtoUnixIO @@ -33,14 +33,12 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io: ProtoUnixIO = proto_unix_io - # initial values self.__initialize_default_power_control_values() chicker_widget_vbox_layout = QVBoxLayout() self.setLayout(chicker_widget_vbox_layout) kick_chip_sliders_hbox_layout = QHBoxLayout() - kick_chip_sliders_hbox_layout.setContentsMargins(0, 0, 0, 0) # Initializing power slider for kicking ( @@ -49,9 +47,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.kick_power_label, ) = common_widgets.create_slider( "Power (m/s)", - HandheldDeviceConstants.MIN_KICK_POWER, - HandheldDeviceConstants.MAX_KICK_POWER, - HandheldDeviceConstants.KICK_POWER_STEPPER, + DiagnosticsConstants.MIN_KICK_POWER, + DiagnosticsConstants.MAX_KICK_POWER, + DiagnosticsConstants.KICK_POWER_STEPPER, ) kick_chip_sliders_hbox_layout.addLayout(self.kick_power_slider_layout) @@ -64,9 +62,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) = common_widgets.create_float_slider( "Chip Distance (m)", 1, - HandheldDeviceConstants.MIN_CHIP_POWER, - HandheldDeviceConstants.MAX_CHIP_POWER, - int(HandheldDeviceConstants.CHIP_DISTANCE_STEPPER), + DiagnosticsConstants.MIN_CHIP_POWER, + DiagnosticsConstants.MAX_CHIP_POWER, + int(DiagnosticsConstants.CHIP_DISTANCE_STEPPER), ) kick_chip_sliders_hbox_layout.addLayout(self.chip_distance_slider_layout) @@ -84,7 +82,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) = common_widgets.create_buttons(["Kick", "Chip"]) self.kick_chip_buttons_box.setTitle("Single Kick and Chip") - self.kick_chip_buttons_box.layout().setContentsMargins(0, 0, 0, 0) self.kick_button = self.kick_chip_buttons[0] self.chip_button = self.kick_chip_buttons[1] @@ -108,7 +105,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ["No Auto", "Auto Kick", "Auto Chip"], self.radio_buttons_group ) self.auto_kick_chip_buttons_box.setTitle("Auto Kick and Chip") - self.auto_kick_chip_buttons_box.layout().setContentsMargins(0, 0, 0, 0) self.no_auto_button = self.auto_kick_chip_buttons[0] self.auto_kick_button = self.auto_kick_chip_buttons[1] @@ -158,7 +154,6 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: :param command: Command to send. One of ChickerCommandMode.KICK or ChickerCommandMode.CHIP """ - # if button is enabled if self.kick_chip_buttons_enable: # send kick primitive self.send_command(command) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 9b79550cf6..94899637a6 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -1,5 +1,3 @@ -from typing import Type - from pyqtgraph.Qt import QtCore from pyqtgraph.Qt.QtWidgets import * from software.py_constants import * @@ -25,18 +23,14 @@ class DiagnosticsInputToggleWidget(QWidget): Disables Manual controls in the other two modes """ - def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> None: + control_mode_changed_signal = QtCore.pyqtSignal(ControlMode) + + def __init__(self) -> None: """Initialises a new Fullsystem Connect Widget to allow switching between Diagnostics and Xbox control - - :param diagnostics_input_mode_signal: The signal to emit when the input mode changes """ super(DiagnosticsInputToggleWidget, self).__init__() - self.__control_mode = ControlMode.DIAGNOSTICS - - self.diagnostics_input_mode_signal = diagnostics_input_mode_signal - diagnostics_input_widget_vbox_layout = QVBoxLayout() self.connect_options_group = QButtonGroup() @@ -45,17 +39,16 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No ["Diagnostics Control", "Handheld Control"], self.connect_options_group ) - self.connect_options_box.layout().setContentsMargins(0, 0, 0, 0) self.connect_options_box.setTitle("Diagnostics Input") self.diagnostics_control_button = self.connect_options[ControlMode.DIAGNOSTICS] self.handheld_control_button = self.connect_options[ControlMode.HANDHELD] self.diagnostics_control_button.clicked.connect( - lambda: self.diagnostics_input_mode_signal.emit(ControlMode.DIAGNOSTICS) + lambda: self.control_mode_changed_signal.emit(ControlMode.DIAGNOSTICS) ) self.handheld_control_button.clicked.connect( - lambda: self.diagnostics_input_mode_signal.emit(ControlMode.HANDHELD) + lambda: self.control_mode_changed_signal.emit(ControlMode.HANDHELD) ) # default to diagnostics input, and disable handheld @@ -66,8 +59,8 @@ def __init__(self, diagnostics_input_mode_signal: Type[QtCore.pyqtSignal]) -> No self.setLayout(diagnostics_input_widget_vbox_layout) - def refresh(self, status: HandheldDeviceConnectionStatus) -> None: - """Refresh this widget. + def update(self, status: HandheldDeviceConnectionStatus) -> None: + """Update this widget. If the handheld device is connected: - enables the handheld button @@ -76,7 +69,7 @@ def refresh(self, status: HandheldDeviceConnectionStatus) -> None: - sets the diagnostics button to checked - emits diagnostics input change signal - :param status: + :param status: the current handheld device connection status """ if status == HandheldDeviceConnectionStatus.CONNECTED: self.handheld_control_button.setEnabled(True) @@ -84,4 +77,11 @@ def refresh(self, status: HandheldDeviceConnectionStatus) -> None: elif status == HandheldDeviceConnectionStatus.DISCONNECTED: self.diagnostics_control_button.setChecked(True) self.handheld_control_button.setEnabled(False) - self.diagnostics_input_mode_signal.emit(ControlMode.DIAGNOSTICS) + self.control_mode_changed_signal.emit(ControlMode.DIAGNOSTICS) + + def get_control_mode(self) -> ControlMode: + return ( + ControlMode.DIAGNOSTICS + if self.diagnostics_control_button.isChecked() + else ControlMode.HANDHELD + ) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 603de0fa32..215acbb731 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -33,17 +33,6 @@ class DiagnosticsWidget(QScrollArea): depending on the user selected choice from the DiagnosticsWidget UI """ - # signal to indicate if manual controls should be disabled based on enum mode parameter - diagnostics_input_mode_signal = pyqtSignal(ControlMode) - - # signal to indicate that handheld device should be reinitialized - reinitialize_handheld_device_signal = pyqtSignal() - - # signal to indicate that handheld device was disconnected - handheld_device_connection_status_signal = pyqtSignal( - HandheldDeviceConnectionStatus - ) - def __init__(self, proto_unix_io: ProtoUnixIO) -> None: super(DiagnosticsWidget, self).__init__() @@ -51,45 +40,31 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - # default start with diagnostics input - self.__control_mode = ControlMode.DIAGNOSTICS - - # default start with disconnected controller - self.__handheld_device_status = HandheldDeviceConnectionStatus.DISCONNECTED - - # initialize widgets - self.handheld_device_status_widget = HandheldDeviceStatusView( - self.reinitialize_handheld_device_signal - ) + self.handheld_device_status_widget = HandheldDeviceStatusView() self.drive_dribbler_widget = DriveAndDribblerWidget(self.proto_unix_io) self.chicker_widget = ChickerWidget(self.proto_unix_io) - self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget( - self.diagnostics_input_mode_signal - ) - - # connect input mode signal with handler - self.diagnostics_input_mode_signal.connect(self.__control_mode_update_handler) + self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget() - # connect handheld device connection status toggle signal with handler - self.handheld_device_connection_status_signal.connect( - self.__handheld_device_connection_status_update_handler - ) - - # initialize controller self.handheld_device_handler = HandheldDeviceManager( self.logger, self.proto_unix_io, - self.handheld_device_connection_status_signal, ) - # connect handheld device reinitialization signal with handler - self.reinitialize_handheld_device_signal.connect( + self.handheld_device_handler.handheld_device_connection_status_signal.connect( + self.__handheld_device_connection_status_signal_handler + ) + + self.diagnostics_control_input_widget.control_mode_changed_signal.connect( + self.__control_mode_changed_signal_handler + ) + + self.handheld_device_status_widget.reinitialize_handheld_device_signal.connect( self.handheld_device_handler.reinitialize_handheld_device ) - # layout for the entire diagnostics tab - diagnostics_widget_vbox_layout = QVBoxLayout() + self.handheld_device_handler.reinitialize_handheld_device() + diagnostics_widget_vbox_layout = QVBoxLayout() diagnostics_widget_vbox_layout.addWidget(self.handheld_device_status_widget) diagnostics_widget_vbox_layout.addWidget(self.diagnostics_control_input_widget) diagnostics_widget_vbox_layout.addWidget(self.drive_dribbler_widget) @@ -104,56 +79,50 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setWidget(self.container) self.setWidgetResizable(True) - def __control_mode_update_handler(self, mode: ControlMode) -> None: + def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: """Handler for managing a control mode update :param mode: The new mode that is being used """ - self.__control_mode = mode - self.handheld_device_handler.refresh(self.__control_mode) + self.handheld_device_handler.set_enabled(mode == ControlMode.HANDHELD) - def __handheld_device_connection_status_update_handler( + def __handheld_device_connection_status_signal_handler( self, status: HandheldDeviceConnectionStatus ): """Handler for propagating :param status: The new mode that is being used """ - self.__handheld_device_status = status + self.handheld_device_status_widget.update(status) + self.diagnostics_control_input_widget.update(status) def refresh(self): """Refreshes sub-widgets so that they display the most recent status values. If in handheld mode, then also visually updates driver, dribbler and chicker sliders to the values currently being set by the handheld device. """ - # update controller status view with most recent controller status - self.handheld_device_status_widget.refresh(self.__handheld_device_status) - - # update diagnostic ui with most recent recent controller status - self.diagnostics_control_input_widget.refresh(self.__handheld_device_status) - - # update all widgets based on current control mode - self.drive_dribbler_widget.refresh(self.__control_mode) - self.chicker_widget.refresh(self.__control_mode) - - if self.__control_mode == ControlMode.HANDHELD: - # visually update driver, dribbler, kick power and chip distance sliders, - # based on the controller values - motor_control = self.handheld_device_handler.motor_control - kick_power = self.handheld_device_handler.kick_power_accumulator - chip_distance = self.handheld_device_handler.chip_distance_accumulator - - self.drive_dribbler_widget.set_x_velocity_slider( - motor_control.direct_velocity_control.velocity.x_component_meters - ) - self.drive_dribbler_widget.set_y_velocity_slider( - motor_control.direct_velocity_control.velocity.y_component_meters - ) - self.drive_dribbler_widget.set_angular_velocity_slider( - motor_control.direct_velocity_control.angular_velocity.radians_per_second - ) - - self.drive_dribbler_widget.set_dribbler_velocity_slider( - motor_control.dribbler_speed_rpm - ) - - self.chicker_widget.set_kick_power_slider_value(kick_power) - self.chicker_widget.set_chip_distance_slider_value(chip_distance) + control_mode = self.diagnostics_control_input_widget.get_control_mode() + + self.drive_dribbler_widget.refresh(control_mode) + self.chicker_widget.refresh(control_mode) + + if control_mode == ControlMode.HANDHELD: + with self.handheld_device_handler.lock: + motor_control = self.handheld_device_handler.motor_control + kick_power = self.handheld_device_handler.kick_power + chip_distance = self.handheld_device_handler.chip_distance + + self.drive_dribbler_widget.set_x_velocity_slider( + motor_control.direct_velocity_control.velocity.x_component_meters + ) + self.drive_dribbler_widget.set_y_velocity_slider( + motor_control.direct_velocity_control.velocity.y_component_meters + ) + self.drive_dribbler_widget.set_angular_velocity_slider( + motor_control.direct_velocity_control.angular_velocity.radians_per_second + ) + + self.drive_dribbler_widget.set_dribbler_velocity_slider( + motor_control.dribbler_speed_rpm + ) + + self.chicker_widget.set_kick_power_slider_value(kick_power) + self.chicker_widget.set_chip_distance_slider_value(chip_distance) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index fc7f627752..34e9568929 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -78,8 +78,6 @@ def __setup_direct_velocity(self) -> QGroupBox: group_box = QGroupBox("Drive") dbox = QVBoxLayout() - dbox.setContentsMargins(0, 0, 0, 0) - ( x_layout, self.x_velocity_slider, @@ -146,8 +144,6 @@ def __setup_dribbler(self) -> QGroupBox: group_box = QGroupBox("Dribbler") dbox = QVBoxLayout() - dbox.setContentsMargins(0, 0, 0, 0) - ( dribbler_layout, self.dribbler_speed_rpm_slider, diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 6ddb2c704e..e8d06b3f73 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -1,7 +1,6 @@ import time from logging import Logger -from threading import Thread, Event -from typing import Type +from threading import Thread, RLock import numpy from evdev import InputDevice, ecodes, list_devices, InputEvent @@ -10,54 +9,55 @@ import software.python_bindings as tbots_cpp from proto.import_all_protos import * from software.thunderscope.constants import * -from software.thunderscope.constants import HandheldDeviceConstants +from software.thunderscope.constants import DiagnosticsConstants from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( HandheldDeviceConnectionStatus, ) -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode -class HandheldDeviceManager(object): +class HandheldDeviceManager(QtCore.QObject): """This class is responsible for managing the connection between a computer and a handheld device to control robots. This class relies on the `evdev` python package, in order to implement parsing and control flow for device events. More info & docs can be found here: https://python-evdev.readthedocs.io/en/latest/apidoc.html """ + handheld_device_connection_status_signal = QtCore.pyqtSignal( + HandheldDeviceConnectionStatus + ) + def __init__( self, logger: Logger, proto_unix_io: ProtoUnixIO, - handheld_device_disconnected_signal: Type[QtCore.pyqtSignal], ): + super().__init__() + + self.lock = RLock() + self.logger = logger self.proto_unix_io = proto_unix_io - self.handheld_device_disconnected_signal = handheld_device_disconnected_signal + + self.enabled = False self.handheld_device: Optional[InputDevice] = None self.handheld_device_config: Optional[DeviceConfig] = None - # initialize proto fields for motor and power control; set default values self.__initialize_default_motor_control_values() self.__initialize_default_power_control_values() # The following fields are here to temporarily persist device input. # They are read once certain buttons are pressed on the handheld device, # and the values are set for the control primitives above. - self.kick_power_accumulator: float = HandheldDeviceConstants.MIN_KICK_POWER - self.chip_distance_accumulator: float = HandheldDeviceConstants.MIN_CHIP_POWER - self.dribbler_speed_accumulator: int = 0 + self.kick_power: float = DiagnosticsConstants.MIN_KICK_POWER + self.chip_distance: float = DiagnosticsConstants.MIN_CHIP_POWER + self.dribbler_speed: int = 0 self.dribbler_running: bool = False self.constants = tbots_cpp.create2021RobotConstants() - # event that is used to stop the handheld device event loop - self.__stop_thread_signal_event = Event() - - # Set-up process that will run the event loop - self.__setup_new_event_listener_thread() - - self.__initialize_handheld_device() + self.event_loop_thread = Thread(target=self.__event_loop, daemon=True) + self.event_loop_thread.start() def __initialize_default_motor_control_values(self) -> None: """This method sets all required fields in the motor @@ -73,162 +73,94 @@ def __initialize_default_power_control_values(self) -> None: """This method sets all required fields in the power control proto to the default/minimum default value """ - # default values for power control self.power_control = PowerControl() self.power_control.geneva_slot = 3 + def set_enabled(self, enabled) -> None: + with self.lock: + self.enabled = enabled + def reinitialize_handheld_device(self) -> None: """Reinitialize handheld_device""" self.__clear_handheld_device() self.__initialize_handheld_device() def __initialize_handheld_device(self) -> None: - """Attempt to initialize a a connection to a handheld device, - which may or may not be successful. - The first handheld device that is recognized as a valid will be used. - Valid handheld devices are any devices whose name has a matching HDIEConfig - defined in constants.py + """Attempt to initialize a connection to a handheld device. + The first handheld device that is recognized as valid will be used. + Valid handheld devices are any devices whose name has a matching DeviceConfig + in DiagnosticsConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP. """ + device_config_map = DiagnosticsConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP + for device in list_devices(): handheld_device = InputDevice(device) if ( handheld_device is not None - and handheld_device.name - in HandheldDeviceConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP + and handheld_device.name in device_config_map ): - print("I've been executed!") - self.__stop_thread_signal_event.clear() - self.handheld_device = handheld_device - self.handheld_device_config: DeviceConfig = ( - HandheldDeviceConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP[ + with self.lock: + self.handheld_device = handheld_device + self.handheld_device_config = device_config_map[ handheld_device.name ] - ) - break - - if ( - self.__get_handheld_device_connection_status() - == HandheldDeviceConnectionStatus.CONNECTED - ): - self.handheld_device_disconnected_signal.emit( - HandheldDeviceConnectionStatus.CONNECTED - ) - self.logger.debug("Successfully initialized handheld!") - self.logger.debug('Device name: "' + self.handheld_device.name + '"') - self.logger.debug("Device path: " + self.handheld_device.path) - - elif ( - self.__get_handheld_device_connection_status() - == HandheldDeviceConnectionStatus.DISCONNECTED - ): - self.handheld_device_disconnected_signal.emit( - HandheldDeviceConnectionStatus.DISCONNECTED - ) - self.logger.debug( - "Failed to initialize a handheld device, check USB connection" - ) - self.logger.debug("Tried the following available devices:") - self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) - def __get_handheld_device_connection_status(self) -> HandheldDeviceConnectionStatus: - """Get and return the latest connection status, based on the `handheld device` field - :return: the current status of the handheld device connection - """ - return ( - HandheldDeviceConnectionStatus.CONNECTED - if self.handheld_device is not None - else HandheldDeviceConnectionStatus.DISCONNECTED - ) - - def __send_proto_command_latest_primitive_controls(self) -> None: - self.proto_unix_io.send_proto(MotorControl, self.motor_control) - self.proto_unix_io.send_proto(PowerControl, self.power_control) - - # reset power control - self.__initialize_default_power_control_values() - - def refresh(self, mode: ControlMode): - """Refresh this class. - Spawns a new thread that runs the handheld device event - processing function if Control.Mode is Diagnostics, - otherwise, stops and joins the thread, if it is running - :param mode: The current user requested mode for controlling the robot - """ - if mode == ControlMode.DIAGNOSTICS: - if self.__handheld_device_event_loop_thread.is_alive(): - self.logger.debug("Terminating handheld device event handling process") - self.__shutdown_event_listener_thread() - elif mode == ControlMode.HANDHELD: - if not self.__handheld_device_event_loop_thread.is_alive(): - self.logger.debug( - "Setting up new handheld device event handling process" + self.handheld_device_connection_status_signal.emit( + HandheldDeviceConnectionStatus.CONNECTED ) - self.__setup_new_event_listener_thread() - self.__start_event_listener_thread() + self.logger.debug("Successfully initialized handheld!") + self.logger.debug('Device name: "' + self.handheld_device.name + '"') + self.logger.debug("Device path: " + self.handheld_device.path) + + return - self.proto_unix_io.send_proto(MotorControl, self.motor_control) - self.proto_unix_io.send_proto(PowerControl, self.power_control) + self.__clear_handheld_device() + self.logger.debug("Failed to initialize handheld device, check USB connection") + self.logger.debug("Tried the following available devices:") + self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __clear_handheld_device(self) -> None: """Clears handheld device & config field by setting to null, and emits a disconnected notification signal. """ - self.handheld_device = None - self.handheld_device_config = None - self.handheld_device_disconnected_signal.emit( - HandheldDeviceConnectionStatus.DISCONNECTED - ) + with self.lock: + self.handheld_device = None + self.handheld_device_config = None - def __shutdown_event_listener_thread(self) -> None: - """Shut down the event processing loop by setting the stop event flag, - and then joins the handling thread, if it is alive. - """ - # TODO (#3165): Use trace level logging here - # self.logger.debug("Shutdown down handheld device event loop process") - self.__stop_thread_signal_event.set() - if self.__handheld_device_event_loop_thread.is_alive(): - self.__handheld_device_event_loop_thread.join() - self.__stop_thread_signal_event.clear() - - def __start_event_listener_thread(self) -> None: - """Starts the thread that runs the event processing loop.""" - self.__handheld_device_event_loop_thread.start() - - def __setup_new_event_listener_thread(self): - """Initializes a new thread that will run the event processing loop""" - # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Initializing new handheld device event loop thread") - self.__handheld_device_event_loop_thread = Thread( - target=self.__event_loop, daemon=True + self.handheld_device_connection_status_signal.emit( + HandheldDeviceConnectionStatus.DISCONNECTED ) def __event_loop(self) -> None: - """This is the method that contains the event processing loop - that is called runs in the event handling thread. - An infinite while loop reads and processes events one by one, using the evdev API. - Old events are skipped if they exceed a threshold, and any caught errors - cause the manager to revert to a disconnected handheld device state. - """ - # TODO (#3165): Use trace level self.logger here - # self.logger.debug("Starting handheld handheld device event handling loop") + while True: + with self.lock: + self.__read_and_process_event() + time.sleep(DiagnosticsConstants.EVENT_LOOP_SLEEP_DURATION) + + def __read_and_process_event(self) -> None: + if not self.handheld_device: + return + try: - while True: - if self.__stop_thread_signal_event.is_set(): - return - event = self.handheld_device.read_one() - # All events accumulate into a file and will be read back eventually, - # even if handheld mode is disabled. This time based recency check ensures that - # only the events that have occurred very recently are processed, and - # any events that occur before switching to handheld mode are dropped & ignored - if ( - event is not None - and time.time() - event.timestamp() - < HandheldDeviceConstants.INPUT_DELAY_THRESHOLD - ): + event = self.handheld_device.read_one() + + # All events accumulate into a file and will be read back eventually, + # even if handheld mode is disabled. This time based recency check ensures that + # only the events that have occurred very recently are processed, and + # any events that occur before switching to handheld mode are dropped & ignored + if self.enabled and event: + time_since_event = time.time() - event.timestamp() + if time_since_event < DiagnosticsConstants.INPUT_DELAY_THRESHOLD: self.__process_event(event) - self.__send_proto_command_latest_primitive_controls() - time.sleep(HandheldDeviceConstants.EVENT_LOOP_SLEEP_DURATION) + motor_control = MotorControl() + power_control = PowerControl() + motor_control.CopyFrom(self.motor_control) + power_control.CopyFrom(self.power_control) + self.proto_unix_io.send_proto(MotorControl, motor_control) + self.proto_unix_io.send_proto(PowerControl, power_control) + + self.__initialize_default_power_control_values() except OSError as ose: self.__clear_handheld_device() @@ -237,159 +169,139 @@ def __event_loop(self) -> None: ) self.logger.debug("Error message: " + str(ose)) self.logger.debug("Check physical handheld handheld device USB connection!") - return + except Exception as e: self.__clear_handheld_device() self.logger.critical( "Caught an unexpected error while reading handheld handheld device event loop!" ) self.logger.critical("Error message: " + str(e)) - return def __process_event(self, event: InputEvent) -> None: """Processes the given device event. Sets corresponding motor & power control values based on the event type, using the current config set in self.handheld_device_config :param event: The event to process. """ - # TODO (#3165): Use trace level self.logger here - # self.logger.debug( - # "Processing handheld device event with type: " - # + str(ecodes.bytype[event.type][event.code]) - # + ", with code: " - # + str(event.code) - # + ", and with value: " - # + str(event.value) - # ) + config = self.handheld_device_config if event.type == ecodes.EV_ABS: - if event.code == self.handheld_device_config.move_x.event_code: - self.motor_control.direct_velocity_control.velocity.x_component_meters = self.__interpret_move_event_value( + if event.code == config.move_x.event_code: + velocity = self.motor_control.direct_velocity_control.velocity + velocity.x_component_meters = self.__interpret_move_event_value( event.value, - self.handheld_device_config.move_x.max_value, + config.move_x.max_value, self.constants.robot_max_speed_m_per_s, ) - if event.code == self.handheld_device_config.move_y.event_code: - self.motor_control.direct_velocity_control.velocity.y_component_meters = self.__interpret_move_event_value( + elif event.code == config.move_y.event_code: + velocity = self.motor_control.direct_velocity_control.velocity + velocity.y_component_meters = self.__interpret_move_event_value( -event.value, - self.handheld_device_config.move_y.max_value, + config.move_y.max_value, self.constants.robot_max_speed_m_per_s, ) - if event.code == self.handheld_device_config.move_rot.event_code: - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.__interpret_move_event_value( + elif event.code == config.move_rot.event_code: + angular_velocity = ( + self.motor_control.direct_velocity_control.angular_velocity + ) + angular_velocity.radians_per_second = self.__interpret_move_event_value( -event.value, - self.handheld_device_config.move_rot.max_value, + config.move_rot.max_value, self.constants.robot_max_ang_speed_rad_per_s, ) - elif event.code == self.handheld_device_config.chicker_power.event_code: - self.kick_power_accumulator = self.__interpret_kick_event_value( - event.value - ) - self.chip_distance_accumulator = self.__interpret_chip_event_value( - event.value - ) + elif event.code == config.chicker_power.event_code: + self.kick_power = self.__interpret_kick_event_value(event.value) + self.chip_distance = self.__interpret_chip_event_value(event.value) - elif event.code == self.handheld_device_config.dribbler_speed.event_code: - self.dribbler_speed_accumulator = ( - self.__interpret_dribbler_speed_event_value(event.value) + elif event.code == config.dribbler_speed.event_code: + self.dribbler_speed = self.__interpret_dribbler_speed_event_value( + event.value ) - elif ( - event.code - == self.handheld_device_config.primary_dribbler_enable.event_code - or event.code - == self.handheld_device_config.secondary_dribbler_enable.event_code - ): + elif event.code == config.dribbler_enable.event_code: self.dribbler_running = self.__interpret_dribbler_enabled_event_value( event.value, - self.handheld_device_config.primary_dribbler_enable.max_value, + config.dribbler_enable.max_value, ) - if event.type == ecodes.EV_KEY: - if ( - event.code == self.handheld_device_config.kick.event_code - and event.value == 1 - ): - self.power_control.chicker.kick_speed_m_per_s = ( - self.kick_power_accumulator - ) + elif event.type == ecodes.EV_KEY: + if event.code == config.kick.event_code and event.value == 1: + self.power_control.chicker.kick_speed_m_per_s = self.kick_power - if ( - event.code == self.handheld_device_config.chip.event_code - and event.value == 1 - ): - self.power_control.chicker.chip_distance_meters = ( - self.chip_distance_accumulator - ) + elif event.code == config.chip.event_code and event.value == 1: + self.power_control.chicker.chip_distance_meters = self.chip_distance - if self.dribbler_running: - self.motor_control.dribbler_speed_rpm = self.dribbler_speed_accumulator - else: - self.motor_control.dribbler_speed_rpm = 0 + self.motor_control.dribbler_speed_rpm = ( + self.dribbler_speed if self.dribbler_running else 0 + ) - @staticmethod def __interpret_move_event_value( - event_value: float, max_value: float, normalizing_multiplier: float + self, event_value: float, max_value: float, normalizing_multiplier: float ) -> float: """Interpret the event_value that corresponds to movement control + :param event_value: the value for the current event being interpreted :param max_value: max value for this type event type - :param normalizing_multiplier: multiplier for converting between + :param normalizing_multiplier: multiplier for converting between the event value and the + robot movement control value :return: The interpreted value that can be set a value for a field in robot movement control """ relative_value = event_value / max_value - if abs(relative_value) < HandheldDeviceConstants.DEADZONE_PERCENTAGE: + if abs(relative_value) < DiagnosticsConstants.DEADZONE_PERCENTAGE: return 0 else: return relative_value * normalizing_multiplier - @staticmethod def __interpret_dribbler_enabled_event_value( - event_value: float, max_value: float + self, event_value: float, max_value: float ) -> bool: """Interpret the event_value that corresponds to controlling whether the dribbler is enabled + :param event_value: the value for the current event being interpreted :param max_value: max value for this event type :return: The interpreted value that can be set a value for a field in robot control """ - return ( - event_value / max_value - ) > HandheldDeviceConstants.BUTTON_PRESSED_THRESHOLD + button_percent_pressed = event_value / max_value + return button_percent_pressed > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: """Interprets the event value that corresponds to controlling the dribbler speed. + :param event_value: the value for the current event being interpreted :return: the interpreted value to be used for the new dribbler speed on the robot """ - return numpy.clip( - a=self.dribbler_speed_accumulator - - event_value * HandheldDeviceConstants.DRIBBLER_RPM_STEPPER, - a_min=self.constants.indefinite_dribbler_speed_rpm, - a_max=-self.constants.indefinite_dribbler_speed_rpm, + return int( + numpy.clip( + a=self.dribbler_speed + - event_value * DiagnosticsConstants.DRIBBLER_RPM_STEPPER, + a_min=self.constants.indefinite_dribbler_speed_rpm, + a_max=-self.constants.indefinite_dribbler_speed_rpm, + ) ) def __interpret_kick_event_value(self, event_value: float) -> float: """Interprets the event value that corresponds to controlling the dribbler speed. + :param event_value: the value for the current event being interpreted :return: the interpreted value to be used for the kick power on the robot """ return numpy.clip( - a=self.kick_power_accumulator - + event_value * HandheldDeviceConstants.KICK_POWER_STEPPER, - a_min=HandheldDeviceConstants.MIN_KICK_POWER, - a_max=HandheldDeviceConstants.MAX_KICK_POWER, + a=self.kick_power + event_value * DiagnosticsConstants.KICK_POWER_STEPPER, + a_min=DiagnosticsConstants.MIN_KICK_POWER, + a_max=DiagnosticsConstants.MAX_KICK_POWER, ) def __interpret_chip_event_value(self, event_value: float) -> float: """Interprets the event value that corresponds to controlling the chip distance. + :param value: the value for the current event being interpreted :return: the interpreted value to be used for the chip distance on the robot """ return numpy.clip( - a=self.chip_distance_accumulator - + event_value * HandheldDeviceConstants.CHIP_DISTANCE_STEPPER, - a_min=HandheldDeviceConstants.MIN_CHIP_POWER, - a_max=HandheldDeviceConstants.MAX_CHIP_POWER, + a=self.chip_distance + + event_value * DiagnosticsConstants.CHIP_DISTANCE_STEPPER, + a_min=DiagnosticsConstants.MIN_CHIP_POWER, + a_max=DiagnosticsConstants.MAX_CHIP_POWER, ) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 40494b5f82..512a3fd821 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -1,5 +1,4 @@ from enum import Enum -from typing import Type from proto.import_all_protos import * @@ -16,22 +15,19 @@ class HandheldDeviceConnectionStatus(Enum): class HandheldDeviceStatusView(QWidget): - def __init__( - self, reinitialize_handheld_device_signal: Type[QtCore.pyqtSignal] - ) -> None: + reinitialize_handheld_device_signal = QtCore.pyqtSignal() + + def __init__(self) -> None: """Initialize the HandheldDeviceStatusView widget. This widget shows the user the current state of the connection with a handheld device, as well as a button that attempts to reinitialize a handheld device object when clicked - :param reinitialize_handheld_device_signal: The signal to use for the reinitialize button """ super(HandheldDeviceStatusView, self).__init__() - self.reinitialize_handheld_device_signal = reinitialize_handheld_device_signal - self.handheld_device_status = QLabel() self.handheld_device_status.setAlignment(Qt.AlignmentFlag.AlignCenter) - self.status_label_view_map: dict[HandheldDeviceConnectionStatus, (str, str)] = { + self.status_label_view_map = { HandheldDeviceConnectionStatus.CONNECTED: ( "Handheld Device is Connected", "background-color: green", @@ -42,19 +38,15 @@ def __init__( ), } - # initialize device refresh button self.handheld_device_reinitialize_button = QPushButton() - self.handheld_device_reinitialize_button.setText( - "Re-initialize Handheld Device" - ) + self.handheld_device_reinitialize_button.setText("Detect Handheld Device") self.handheld_device_reinitialize_button.clicked.connect( self.reinitialize_handheld_device_signal ) box = QGroupBox() hbox_layout = QHBoxLayout() - hbox_layout.setContentsMargins(0, 0, 0, 0) - box.setTitle("Handheld Device Status and Re-initialization") + box.setTitle("Handheld Device Status") widget_layout = QVBoxLayout() hbox_layout.addWidget(self.handheld_device_status) @@ -64,26 +56,15 @@ def __init__( box.setLayout(hbox_layout) widget_layout.addWidget(box) - - self.set_view_state(HandheldDeviceConnectionStatus.DISCONNECTED) self.setLayout(widget_layout) - def set_view_state(self, connection_state: HandheldDeviceConnectionStatus) -> None: + self.update(HandheldDeviceConnectionStatus.DISCONNECTED) + + def update(self, connection_state: HandheldDeviceConnectionStatus) -> None: """Sets the label to display the correct status depending on the connection state - :param connection_state: The state to use - """ - self.handheld_device_status.setText( - self.status_label_view_map[connection_state][0] - ) - self.handheld_device_status.setStyleSheet( - self.status_label_view_map[connection_state][1] - ) - def refresh( - self, connection_state=HandheldDeviceConnectionStatus.DISCONNECTED - ) -> None: - """Refreshes this widget. - The status label will reflect to the user the current state of the handheld device connection - :param connection_state: The new state of the handheld device connection + :param connection_state: The state to use """ - self.set_view_state(connection_state) + text, color = self.status_label_view_map[connection_state] + self.handheld_device_status.setText(text) + self.handheld_device_status.setStyleSheet(color) diff --git a/src/software/thunderscope/robot_diagnostics/motor_fault_view.py b/src/software/thunderscope/robot_diagnostics/motor_fault_view.py index 2133b3e0bd..96e2afcaa4 100644 --- a/src/software/thunderscope/robot_diagnostics/motor_fault_view.py +++ b/src/software/thunderscope/robot_diagnostics/motor_fault_view.py @@ -56,7 +56,6 @@ def __init__(self) -> None: self.layout.addWidget(self.motor_fault_display) - self.layout.setContentsMargins(0, 0, 0, 0) self.setLayout(self.layout) def event(self, event: QEvent) -> bool: diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index 8f3036d96f..cdff828fe8 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -321,7 +321,7 @@ def setup_robot_error_log_view_widget(proto_unix_io: ProtoUnixIO) -> RobotErrorL return robot_error_log -def setup_estop_view(proto_unix_io) -> EstopView: +def setup_estop_view(proto_unix_io: ProtoUnixIO) -> EstopView: """Setup the estop view widget :param proto_unix_io: The proto unix io object for the full system @@ -342,10 +342,8 @@ def setup_diagnostics_widget( - ControllerStatusViewWidget :param proto_unix_io: The proto unix io object - :returns: The diagnostics widget that contains - the control input switch, drive & dribbler sliders, - chicker control and controller handler - + :returns: The diagnostics widget that contains the control input switch, + drive & dribbler sliders, chicker control and controller handler """ diagnostics_widget = DiagnosticsWidget(proto_unix_io) From 7672ffab4e8c7fca7ccfd6c8512f0cbf2f7ffd7e Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 22 Sep 2024 21:36:01 -0700 Subject: [PATCH 125/138] Fix bugs --- .../thunderscope/common/common_widgets.py | 30 ++++--- .../robot_diagnostics/chicker_widget.py | 16 ++-- .../robot_diagnostics/diagnostics_widget.py | 2 + .../drive_and_dribbler_widget.py | 83 ++++++------------- .../handheld_device_manager.py | 22 +++-- 5 files changed, 71 insertions(+), 82 deletions(-) diff --git a/src/software/thunderscope/common/common_widgets.py b/src/software/thunderscope/common/common_widgets.py index 225eb1cf3f..87deecdc17 100644 --- a/src/software/thunderscope/common/common_widgets.py +++ b/src/software/thunderscope/common/common_widgets.py @@ -380,13 +380,23 @@ def set_table_data( table.setHorizontalHeaderLabels(horizontal_headers) +def disconnect_signal(signal: QtCore.Signal): + """Helper function to disconnect all connections for a Qt signal. + Suppresses TypeErrors thrown by Signal.disconnect() if there are no connections. + """ + try: + signal.disconnect() + except TypeError: + pass + + def enable_button(button): - """Disables the given button and sets the button UI to reflect that + """Enables the given button and sets the button UI to reflect that visually to the user. :param button: button to change the state of """ - button.setStyleSheet("background-color: White") + button.setStyleSheet("") button.setEnabled(True) @@ -396,20 +406,20 @@ def disable_button(button): :param button: button to change the state of """ - button.setStyleSheet("background-color: Grey") + button.setStyleSheet("background-color: grey") button.setEnabled(False) def disable_slider(slider): """Disables a slider by getting the current value and setting the slider to that - value upon every value change + value every time the slider is moved This results in slider value not changing even when slider is moved :param slider: slider widget to be disabled """ old_val = slider.value() - slider.valueChanged.connect(lambda: slider.setValue(old_val)) + slider.sliderMoved.connect(lambda new_val: slider.setValue(old_val)) slider.setStyleSheet( "QSlider::sub-page:horizontal" "{" @@ -423,15 +433,13 @@ def disable_slider(slider): ) -def enable_slider(slider, label, get_value): - """Enables a slider by connecting a function to update label upon value change +def enable_slider(slider): + """Enables a slider that was disabled with disable_slider :param slider: slider widget to be enabled - :param label: label widget corresponding to the slider - :param get_value: function to translate slider value into label text """ - slider.valueChanged.connect(lambda: label.setText(get_value(slider.value()))) - slider.setStyleSheet("QSlider::groove:horizontal" "{" "border-width: 0px" "}") + disconnect_signal(slider.sliderMoved) + slider.setStyleSheet("") def disable_radio_button(button_group): diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index eb7a03c26c..49a6ab9038 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -160,7 +160,7 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: self.disable_kick_chip_buttons() # set and start timer to re-enable buttons after 3 seconds - QTimer.singleShot(CHICKER_TIMEOUT, self.enable_kick_chip_buttons) + QTimer.singleShot(int(CHICKER_TIMEOUT), self.enable_kick_chip_buttons) def disable_kick_chip_buttons(self) -> None: """Disables the buttons""" @@ -236,10 +236,10 @@ def refresh_button_state(self, button: QPushButton) -> None: """ if self.kick_chip_buttons_enable: - button.setStyleSheet("background-color: White") + button.setStyleSheet("") button.setEnabled(True) else: - button.setStyleSheet("background-color: Grey") + button.setStyleSheet("background-color: grey") button.setEnabled(False) def update_widget_accessibility(self, mode: ControlMode): @@ -247,10 +247,14 @@ def update_widget_accessibility(self, mode: ControlMode): self.auto_chip_button.setEnabled(mode == ControlMode.DIAGNOSTICS) self.set_should_enable_buttons(mode == ControlMode.DIAGNOSTICS) - def refresh(self, mode: ControlMode) -> None: - # Update this widgets accessibility to the user based on the ControlMode parameter - self.update_widget_accessibility(mode) + if mode == ControlMode.DIAGNOSTICS: + common_widgets.enable_slider(self.kick_power_slider) + common_widgets.enable_slider(self.chip_distance_slider) + elif mode == ControlMode.HANDHELD: + common_widgets.disable_slider(self.kick_power_slider) + common_widgets.disable_slider(self.chip_distance_slider) + def refresh(self, mode: ControlMode) -> None: # get kick power value slider value and set the label to that value kick_power_value = self.kick_power_slider.value() self.kick_power_label.setText(str(kick_power_value)) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 215acbb731..a2ec37b83c 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -84,6 +84,8 @@ def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: :param mode: The new mode that is being used """ self.handheld_device_handler.set_enabled(mode == ControlMode.HANDHELD) + self.drive_dribbler_widget.update_widget_accessibility(mode) + self.chicker_widget.update_widget_accessibility(mode) def __handheld_device_connection_status_signal_handler( self, status: HandheldDeviceConnectionStatus diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 34e9568929..98909b5e56 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -1,4 +1,3 @@ -import time from pyqtgraph.Qt.QtCore import Qt from pyqtgraph.Qt.QtWidgets import * from proto.import_all_protos import * @@ -13,15 +12,13 @@ class DriveAndDribblerWidget(QWidget): def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """Initialize the widget to control the robot's motors""" - super(DriveAndDribblerWidget, self).__init__() + super().__init__() self.proto_unix_io = proto_unix_io self.motor_control = MotorControl() - self.input_a = time.time() self.constants = tbots_cpp.create2021RobotConstants() - QWidget.__init__(self) layout = QVBoxLayout() # Add widgets to layout @@ -44,12 +41,8 @@ def set_dribbler_velocity_slider(self, value: float): def refresh(self, mode: ControlMode) -> None: """Refresh the widget's sliders - Enable or disable this widgets sliders and buttons based on mode value Collect motor control values and persist in the primitive field """ - # Update this widgets accessibility to the user based on the ControlMode parameter - self.update_widget_accessibility(mode) - self.motor_control.dribbler_speed_rpm = int( self.dribbler_speed_rpm_slider.value() ) @@ -113,16 +106,20 @@ def __setup_direct_velocity(self) -> QGroupBox: ) # add listener functions for sliders to update label with slider value - common_widgets.enable_slider( - self.x_velocity_slider, self.x_velocity_label, self.__value_change_handler + self.x_velocity_slider.valueChanged.connect( + lambda: self.x_velocity_label.setText( + self.__value_change_handler(self.x_velocity_slider.value()) + ) ) - common_widgets.enable_slider( - self.y_velocity_slider, self.y_velocity_label, self.__value_change_handler + self.y_velocity_slider.valueChanged.connect( + lambda: self.y_velocity_label.setText( + self.__value_change_handler(self.y_velocity_slider.value()) + ) ) - common_widgets.enable_slider( - self.angular_velocity_slider, - self.angular_velocity_label, - self.__value_change_handler, + self.angular_velocity_slider.valueChanged.connect( + lambda: self.angular_velocity_label.setText( + self.__value_change_handler(self.angular_velocity_slider.value()) + ) ) self.stop_and_reset_direct = QPushButton("Stop and Reset") @@ -156,11 +153,10 @@ def __setup_dribbler(self) -> QGroupBox: 1, ) - # add listener function to update label with slider value - common_widgets.enable_slider( - self.dribbler_speed_rpm_slider, - self.dribbler_speed_rpm_label, - self.__value_change_handler, + self.dribbler_speed_rpm_slider.valueChanged.connect( + lambda: self.dribbler_speed_rpm_label.setText( + self.__value_change_handler(self.dribbler_speed_rpm_slider.value()) + ) ) self.stop_and_reset_dribbler = QPushButton("Stop and Reset") @@ -178,60 +174,33 @@ def update_widget_accessibility(self, mode: ControlMode) -> None: """Disables or enables all sliders and buttons depending on ControlMode parameter. Sliders are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode Updates listener functions and stylesheets accordingly + :param mode: ControlMode enum parameter """ if mode == ControlMode.DIAGNOSTICS: - # disconnect all sliders - self.__disconnect_sliders() - # enable all sliders by adding listener to update label with slider value - common_widgets.enable_slider( - self.x_velocity_slider, - self.x_velocity_label, - self.__value_change_handler, - ) - common_widgets.enable_slider( - self.y_velocity_slider, - self.y_velocity_label, - self.__value_change_handler, - ) - common_widgets.enable_slider( - self.angular_velocity_slider, - self.angular_velocity_label, - self.__value_change_handler, - ) - common_widgets.enable_slider( - self.dribbler_speed_rpm_slider, - self.dribbler_speed_rpm_label, - self.__value_change_handler, - ) + common_widgets.enable_slider(self.x_velocity_slider) + common_widgets.enable_slider(self.y_velocity_slider) + common_widgets.enable_slider(self.angular_velocity_slider) + common_widgets.enable_slider(self.dribbler_speed_rpm_slider) # enable buttons common_widgets.enable_button(self.stop_and_reset_dribbler) common_widgets.enable_button(self.stop_and_reset_direct) elif mode == ControlMode.HANDHELD: - # reset slider values and disconnect self.__reset_all_sliders() - # self.__disconnect_sliders() # disable all sliders by adding listener to keep slider value the same - # common_widgets.disable_slider(self.x_velocity_slider) - # common_widgets.disable_slider(self.y_velocity_slider) - # common_widgets.disable_slider(self.angular_velocity_slider) - # common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + common_widgets.disable_slider(self.x_velocity_slider) + common_widgets.disable_slider(self.y_velocity_slider) + common_widgets.disable_slider(self.angular_velocity_slider) + common_widgets.disable_slider(self.dribbler_speed_rpm_slider) # disable buttons common_widgets.disable_button(self.stop_and_reset_dribbler) common_widgets.disable_button(self.stop_and_reset_direct) - def __disconnect_sliders(self) -> None: - """Disconnect listener for changing values for all sliders""" - self.x_velocity_slider.valueChanged.disconnect() - self.y_velocity_slider.valueChanged.disconnect() - self.angular_velocity_slider.valueChanged.disconnect() - self.dribbler_speed_rpm_slider.valueChanged.disconnect() - def __reset_direct_sliders(self) -> None: """Reset direct sliders back to 0""" self.x_velocity_slider.setValue(0) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index e8d06b3f73..1f0f110d5c 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -60,8 +60,8 @@ def __init__( self.event_loop_thread.start() def __initialize_default_motor_control_values(self) -> None: - """This method sets all required fields in the motor - control proto to the default/minimum default value + """This method sets all required fields in the motor control proto to + the default/minimum default value """ self.motor_control = MotorControl() self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 @@ -70,18 +70,23 @@ def __initialize_default_motor_control_values(self) -> None: self.motor_control.dribbler_speed_rpm = 0 def __initialize_default_power_control_values(self) -> None: - """This method sets all required fields in the power - control proto to the default/minimum default value + """This method sets all required fields in the power control proto to + the default/minimum default value """ self.power_control = PowerControl() self.power_control.geneva_slot = 3 def set_enabled(self, enabled) -> None: + """Enable or disable the HandheldDeviceManager from processing input events. + + :param enabled: whether to enable (True) or disable (False) the + HandheldDeviceManager from processing input events + """ with self.lock: self.enabled = enabled def reinitialize_handheld_device(self) -> None: - """Reinitialize handheld_device""" + """Attempt to reinitialize a connection to a handheld device.""" self.__clear_handheld_device() self.__initialize_handheld_device() @@ -120,7 +125,7 @@ def __initialize_handheld_device(self) -> None: self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __clear_handheld_device(self) -> None: - """Clears handheld device & config field by setting to null, + """Clears handheld device and config field by setting them to None, and emits a disconnected notification signal. """ with self.lock: @@ -178,9 +183,10 @@ def __read_and_process_event(self) -> None: self.logger.critical("Error message: " + str(e)) def __process_event(self, event: InputEvent) -> None: - """Processes the given device event. Sets corresponding motor & power control values + """Processes the given device event. Sets corresponding motor and power control values based on the event type, using the current config set in self.handheld_device_config - :param event: The event to process. + + :param event: The input event to process """ config = self.handheld_device_config From 00f6a30a203fb3590af2afc60c4366e12c778fdd Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 22 Sep 2024 22:54:37 -0700 Subject: [PATCH 126/138] Nits --- src/software/jetson_nano/services/motor.cpp | 2 +- .../robot_diagnostics/chicker_widget.py | 36 ++++++------- .../diagnostics_input_widget.py | 21 ++++---- .../robot_diagnostics/diagnostics_widget.py | 50 +++++++++-------- .../drive_and_dribbler_widget.py | 53 +++++++++++++------ .../handheld_device_manager.py | 5 ++ .../handheld_device_status_view.py | 8 +-- 7 files changed, 103 insertions(+), 72 deletions(-) diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/jetson_nano/services/motor.cpp index e02596358f..b1c095a1a1 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/jetson_nano/services/motor.cpp @@ -540,8 +540,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor TbotsProto::MotorControl_DirectVelocityControl direct_velocity = motor.direct_velocity_control(); EuclideanSpace_t target_euclidean_velocity = { + -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), - direct_velocity.velocity().y_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; target_wheel_velocities = diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 49a6ab9038..944aebe181 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -92,11 +92,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: chicker_widget_vbox_layout.addWidget(self.kick_chip_buttons_box) # Initializing auto kick & chip buttons - self.button_clickable_map = { - "no_auto": True, - "auto_kick": True, - "auto_chip": True, - } self.radio_buttons_group = QButtonGroup() ( self.auto_kick_chip_buttons_box, @@ -137,20 +132,21 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def set_kick_power_slider_value(self, value: float) -> None: """Set the kick power slider to a specific value. + :param value: The value to set for the slider """ self.kick_power_slider.setValue(value) def set_chip_distance_slider_value(self, value: float) -> None: """Set the chip distance slider to a specific value. + :param value: the value to set for the slider """ self.chip_distance_slider.setValue(value) def send_command_and_timeout(self, command: ChickerCommandMode) -> None: - """If buttons are enabled, sends a Kick command and disables buttons - - Attaches a callback to re-enable buttons after 3 seconds + """If the kick/chip buttons are enabled, sends a kick/chip command and disables the buttons. + The buttons will be re-enabled after CHICKER_TIMEOUT. :param command: Command to send. One of ChickerCommandMode.KICK or ChickerCommandMode.CHIP """ @@ -159,7 +155,7 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: self.send_command(command) self.disable_kick_chip_buttons() - # set and start timer to re-enable buttons after 3 seconds + # set and start timer to re-enable buttons after CHICKER_TIMEOUT QTimer.singleShot(int(CHICKER_TIMEOUT), self.enable_kick_chip_buttons) def disable_kick_chip_buttons(self) -> None: @@ -232,17 +228,18 @@ def refresh_button_state(self, button: QPushButton) -> None: """Change button color and clickable state. :param button: button to change the state of - :returns: None - """ - if self.kick_chip_buttons_enable: - button.setStyleSheet("") - button.setEnabled(True) - else: - button.setStyleSheet("background-color: grey") - button.setEnabled(False) + button.setEnabled(self.kick_chip_buttons_enable) + button.setStyleSheet( + "" if self.kick_chip_buttons_enable else "background-color: grey" + ) def update_widget_accessibility(self, mode: ControlMode): + """Disables or enables all sliders and buttons depending on the given control mode. + Sliders and buttons are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode + + :param mode: the current control mode + """ self.auto_kick_button.setEnabled(mode == ControlMode.DIAGNOSTICS) self.auto_chip_button.setEnabled(mode == ControlMode.DIAGNOSTICS) self.set_should_enable_buttons(mode == ControlMode.DIAGNOSTICS) @@ -254,7 +251,10 @@ def update_widget_accessibility(self, mode: ControlMode): common_widgets.disable_slider(self.kick_power_slider) common_widgets.disable_slider(self.chip_distance_slider) - def refresh(self, mode: ControlMode) -> None: + def refresh(self) -> None: + """Update the currently persisted PowerControl proto based on the widget's + slider values and sends out autokick/autochip commands if they are enabled + """ # get kick power value slider value and set the label to that value kick_power_value = self.kick_power_slider.value() self.kick_power_label.setText(str(kick_power_value)) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 94899637a6..1044c44ac5 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -10,26 +10,21 @@ class ControlMode(IntEnum): - """Enum for the 2 modes of control (Manual and Xbox)""" + """Enum representing the available control modes in Robot Diagnostics""" DIAGNOSTICS = 0 HANDHELD = 1 class DiagnosticsInputToggleWidget(QWidget): - """Class to allow the user to switch between Manual, Xbox, and Fullsystem control - through Thunderscope UI - - Disables Manual controls in the other two modes - """ + """Widget for switching between manual (Diagnostics) and Xbox (Handheld) control""" control_mode_changed_signal = QtCore.pyqtSignal(ControlMode) + """Signal emitted when the control mode changes""" def __init__(self) -> None: - """Initialises a new Fullsystem Connect Widget to allow switching - between Diagnostics and Xbox control - """ - super(DiagnosticsInputToggleWidget, self).__init__() + """Initialise the DiagnosticsInputToggleWidget""" + super().__init__() diagnostics_input_widget_vbox_layout = QVBoxLayout() @@ -60,7 +55,7 @@ def __init__(self) -> None: self.setLayout(diagnostics_input_widget_vbox_layout) def update(self, status: HandheldDeviceConnectionStatus) -> None: - """Update this widget. + """Update this widget with the current handheld device connection status. If the handheld device is connected: - enables the handheld button @@ -80,6 +75,10 @@ def update(self, status: HandheldDeviceConnectionStatus) -> None: self.control_mode_changed_signal.emit(ControlMode.DIAGNOSTICS) def get_control_mode(self) -> ControlMode: + """Get the currently selected control mode. + + :returns: the currently selected control mode + """ return ( ControlMode.DIAGNOSTICS if self.diagnostics_control_button.isChecked() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index a2ec37b83c..59caa1dcb9 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -22,18 +22,18 @@ class DiagnosticsWidget(QScrollArea): - """The DiagnosticsWidget contains all widgets related to diagnostics: + """The DiagnosticsWidget contains all widgets related to Robot Diagnostics: - HandheldDeviceStatusWidget - DiagnosticsInputWidget - DriveAndDribblerWidget - ChickerWidget - - This widget is also responsible for controlling the diagnostics inout, - either from the DriverAndDribblerWidget or the HandheldDeviceManager, - depending on the user selected choice from the DiagnosticsWidget UI """ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: + """Initialize the DiagnosticsWidget + + :param proto_unix_io: proto unix io to configure the diagnostics widgets with + """ super(DiagnosticsWidget, self).__init__() self.logger = create_logger("RobotDiagnostics") @@ -45,12 +45,12 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.chicker_widget = ChickerWidget(self.proto_unix_io) self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget() - self.handheld_device_handler = HandheldDeviceManager( + self.handheld_device_manager = HandheldDeviceManager( self.logger, self.proto_unix_io, ) - self.handheld_device_handler.handheld_device_connection_status_signal.connect( + self.handheld_device_manager.handheld_device_connection_status_signal.connect( self.__handheld_device_connection_status_signal_handler ) @@ -59,10 +59,10 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) self.handheld_device_status_widget.reinitialize_handheld_device_signal.connect( - self.handheld_device_handler.reinitialize_handheld_device + self.handheld_device_manager.reinitialize_handheld_device ) - self.handheld_device_handler.reinitialize_handheld_device() + self.handheld_device_manager.reinitialize_handheld_device() diagnostics_widget_vbox_layout = QVBoxLayout() diagnostics_widget_vbox_layout.addWidget(self.handheld_device_status_widget) @@ -80,37 +80,41 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setWidgetResizable(True) def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: - """Handler for managing a control mode update - :param mode: The new mode that is being used + """Handler for the control_mode_changed_signal emitted by DiagnosticsControlInputWidget + + :param mode: the currently selected control mode """ - self.handheld_device_handler.set_enabled(mode == ControlMode.HANDHELD) + self.handheld_device_manager.set_enabled(mode == ControlMode.HANDHELD) self.drive_dribbler_widget.update_widget_accessibility(mode) self.chicker_widget.update_widget_accessibility(mode) def __handheld_device_connection_status_signal_handler( self, status: HandheldDeviceConnectionStatus - ): - """Handler for propagating - :param status: The new mode that is being used + ) -> None: + """Handler for the handheld_device_connection_status_signal emitted by HandheldDeviceManager + + :param status: the new handheld device connection status """ self.handheld_device_status_widget.update(status) self.diagnostics_control_input_widget.update(status) - def refresh(self): + def refresh(self) -> None: """Refreshes sub-widgets so that they display the most recent status values. If in handheld mode, then also visually updates driver, dribbler and chicker sliders to the values currently being set by the handheld device. """ control_mode = self.diagnostics_control_input_widget.get_control_mode() - self.drive_dribbler_widget.refresh(control_mode) - self.chicker_widget.refresh(control_mode) + self.chicker_widget.refresh() + + if control_mode == ControlMode.DIAGNOSTICS: + self.drive_dribbler_widget.refresh() - if control_mode == ControlMode.HANDHELD: - with self.handheld_device_handler.lock: - motor_control = self.handheld_device_handler.motor_control - kick_power = self.handheld_device_handler.kick_power - chip_distance = self.handheld_device_handler.chip_distance + elif control_mode == ControlMode.HANDHELD: + with self.handheld_device_manager.lock: + motor_control = self.handheld_device_manager.motor_control + kick_power = self.handheld_device_manager.kick_power + chip_distance = self.handheld_device_manager.chip_distance self.drive_dribbler_widget.set_x_velocity_slider( motor_control.direct_velocity_control.velocity.x_component_meters diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 98909b5e56..50c02e1164 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -27,21 +27,37 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setLayout(layout) - def set_x_velocity_slider(self, value: float): + def set_x_velocity_slider(self, value: float) -> None: + """Set the value of the slider controlling the robot's X velocity + + :param value: the value to set the slider to in m/s + """ self.x_velocity_slider.setValue(value) - def set_y_velocity_slider(self, value: float): + def set_y_velocity_slider(self, value: float) -> None: + """Set the value of the slider controlling the robot's Y velocity + + :param value: the value to set the slider to in m/s + """ self.y_velocity_slider.setValue(value) - def set_angular_velocity_slider(self, value: float): + def set_angular_velocity_slider(self, value: float) -> None: + """Set the value of the slider controlling the robot's angular velocity + + :param value: the value to set the slider to in rad/s + """ self.angular_velocity_slider.setValue(value) - def set_dribbler_velocity_slider(self, value: float): + def set_dribbler_velocity_slider(self, value: float) -> None: + """Set the value of the slider controlling the robot's dribbler velocity + + :param value: the value to set the slider to in RPM + """ self.dribbler_speed_rpm_slider.setValue(value) - def refresh(self, mode: ControlMode) -> None: - """Refresh the widget's sliders - Collect motor control values and persist in the primitive field + def refresh(self) -> None: + """Update the currently persisted MotorControl proto based on the widget's slider values + and sends out the proto """ self.motor_control.dribbler_speed_rpm = int( self.dribbler_speed_rpm_slider.value() @@ -57,17 +73,21 @@ def refresh(self, mode: ControlMode) -> None: self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.angular_velocity_slider.value() - if mode == ControlMode.DIAGNOSTICS: - self.proto_unix_io.send_proto(MotorControl, self.motor_control) + self.proto_unix_io.send_proto(MotorControl, self.motor_control) def __value_change_handler(self, value: float) -> str: - """Converts the given float value to a string label + """Convert the given float value to a string label + :param value: float value to be converted """ return "%.2f" % float(value) def __setup_direct_velocity(self) -> QGroupBox: - """Create a widget to control the direct velocity of the robot's motors""" + """Create a widget to control the direct velocity of the robot's motors + + :returns: a QGroupBox containing sliders and controls for controlling the + direct velocity of the robot's motors + """ group_box = QGroupBox("Drive") dbox = QVBoxLayout() @@ -137,7 +157,11 @@ def __setup_direct_velocity(self) -> QGroupBox: return group_box def __setup_dribbler(self) -> QGroupBox: - """Create a widget to control the dribbler RPM""" + """Create a widget to control the dribbler speed + + :returns: a QGroupBox containing a slider and controls for controlling the + robot's dribbler speed + """ group_box = QGroupBox("Dribbler") dbox = QVBoxLayout() @@ -171,11 +195,10 @@ def __setup_dribbler(self) -> QGroupBox: return group_box def update_widget_accessibility(self, mode: ControlMode) -> None: - """Disables or enables all sliders and buttons depending on ControlMode parameter. + """Disables or enables all sliders and buttons depending on the given control mode. Sliders are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode - Updates listener functions and stylesheets accordingly - :param mode: ControlMode enum parameter + :param mode: the current control mode """ if mode == ControlMode.DIAGNOSTICS: # enable all sliders by adding listener to update label with slider value diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 1f0f110d5c..0b7fbcbd03 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -31,6 +31,11 @@ def __init__( logger: Logger, proto_unix_io: ProtoUnixIO, ): + """Initialize the HandheldDeviceManager + + :param logger: the logger to use + :param proto_unix_io: the proto unix io to use for sending protos + """ super().__init__() self.lock = RLock() diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 512a3fd821..3a09624a12 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -60,11 +60,11 @@ def __init__(self) -> None: self.update(HandheldDeviceConnectionStatus.DISCONNECTED) - def update(self, connection_state: HandheldDeviceConnectionStatus) -> None: - """Sets the label to display the correct status depending on the connection state + def update(self, connection_status: HandheldDeviceConnectionStatus) -> None: + """Sets the label to display the correct status depending on the connection status - :param connection_state: The state to use + :param connection_status: the connection status to display """ - text, color = self.status_label_view_map[connection_state] + text, color = self.status_label_view_map[connection_status] self.handheld_device_status.setText(text) self.handheld_device_status.setStyleSheet(color) From 2c23948cd402ed628ec32b611b851395bb4b2dc6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Mon, 23 Sep 2024 06:02:59 +0000 Subject: [PATCH 127/138] [pre-commit.ci lite] apply automatic fixes --- .../robot_diagnostics/chicker_widget.py | 4 ++-- .../diagnostics_input_widget.py | 4 ++-- .../robot_diagnostics/diagnostics_widget.py | 14 +++++++------- .../drive_and_dribbler_widget.py | 16 ++++++++-------- .../robot_diagnostics/handheld_device_manager.py | 2 +- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 944aebe181..a53f98bc52 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -252,8 +252,8 @@ def update_widget_accessibility(self, mode: ControlMode): common_widgets.disable_slider(self.chip_distance_slider) def refresh(self) -> None: - """Update the currently persisted PowerControl proto based on the widget's - slider values and sends out autokick/autochip commands if they are enabled + """Update the currently persisted PowerControl proto based on the widget's + slider values and sends out autokick/autochip commands if they are enabled """ # get kick power value slider value and set the label to that value kick_power_value = self.kick_power_slider.value() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 1044c44ac5..abafd0aaae 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -55,7 +55,7 @@ def __init__(self) -> None: self.setLayout(diagnostics_input_widget_vbox_layout) def update(self, status: HandheldDeviceConnectionStatus) -> None: - """Update this widget with the current handheld device connection status. + """Update this widget with the current handheld device connection status. If the handheld device is connected: - enables the handheld button @@ -76,7 +76,7 @@ def update(self, status: HandheldDeviceConnectionStatus) -> None: def get_control_mode(self) -> ControlMode: """Get the currently selected control mode. - + :returns: the currently selected control mode """ return ( diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 59caa1dcb9..a4513e4b12 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -23,15 +23,15 @@ class DiagnosticsWidget(QScrollArea): """The DiagnosticsWidget contains all widgets related to Robot Diagnostics: - - HandheldDeviceStatusWidget - - DiagnosticsInputWidget - - DriveAndDribblerWidget - - ChickerWidget + - HandheldDeviceStatusWidget + - DiagnosticsInputWidget + - DriveAndDribblerWidget + - ChickerWidget """ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """Initialize the DiagnosticsWidget - + :param proto_unix_io: proto unix io to configure the diagnostics widgets with """ super(DiagnosticsWidget, self).__init__() @@ -82,7 +82,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: """Handler for the control_mode_changed_signal emitted by DiagnosticsControlInputWidget - :param mode: the currently selected control mode + :param mode: the currently selected control mode """ self.handheld_device_manager.set_enabled(mode == ControlMode.HANDHELD) self.drive_dribbler_widget.update_widget_accessibility(mode) @@ -92,7 +92,7 @@ def __handheld_device_connection_status_signal_handler( self, status: HandheldDeviceConnectionStatus ) -> None: """Handler for the handheld_device_connection_status_signal emitted by HandheldDeviceManager - + :param status: the new handheld device connection status """ self.handheld_device_status_widget.update(status) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 50c02e1164..0977194767 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -29,34 +29,34 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def set_x_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's X velocity - + :param value: the value to set the slider to in m/s """ self.x_velocity_slider.setValue(value) def set_y_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's Y velocity - + :param value: the value to set the slider to in m/s """ self.y_velocity_slider.setValue(value) def set_angular_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's angular velocity - + :param value: the value to set the slider to in rad/s """ self.angular_velocity_slider.setValue(value) def set_dribbler_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's dribbler velocity - + :param value: the value to set the slider to in RPM """ self.dribbler_speed_rpm_slider.setValue(value) def refresh(self) -> None: - """Update the currently persisted MotorControl proto based on the widget's slider values + """Update the currently persisted MotorControl proto based on the widget's slider values and sends out the proto """ self.motor_control.dribbler_speed_rpm = int( @@ -84,8 +84,8 @@ def __value_change_handler(self, value: float) -> str: def __setup_direct_velocity(self) -> QGroupBox: """Create a widget to control the direct velocity of the robot's motors - - :returns: a QGroupBox containing sliders and controls for controlling the + + :returns: a QGroupBox containing sliders and controls for controlling the direct velocity of the robot's motors """ group_box = QGroupBox("Drive") @@ -158,7 +158,7 @@ def __setup_direct_velocity(self) -> QGroupBox: def __setup_dribbler(self) -> QGroupBox: """Create a widget to control the dribbler speed - + :returns: a QGroupBox containing a slider and controls for controlling the robot's dribbler speed """ diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 0b7fbcbd03..73071b47df 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -32,7 +32,7 @@ def __init__( proto_unix_io: ProtoUnixIO, ): """Initialize the HandheldDeviceManager - + :param logger: the logger to use :param proto_unix_io: the proto unix io to use for sending protos """ From 5c282b1b314871180e44acd840cf64bb46a81123 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sun, 22 Sep 2024 23:20:29 -0700 Subject: [PATCH 128/138] More nits --- .../robot_diagnostics/chicker_widget.py | 4 +- .../diagnostics_input_widget.py | 4 +- .../robot_diagnostics/diagnostics_widget.py | 26 +++++----- .../drive_and_dribbler_widget.py | 16 +++---- .../handheld_device_manager.py | 38 +++++++++++---- .../handheld_device_status_view.py | 47 +++++++++---------- 6 files changed, 78 insertions(+), 57 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 944aebe181..a53f98bc52 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -252,8 +252,8 @@ def update_widget_accessibility(self, mode: ControlMode): common_widgets.disable_slider(self.chip_distance_slider) def refresh(self) -> None: - """Update the currently persisted PowerControl proto based on the widget's - slider values and sends out autokick/autochip commands if they are enabled + """Update the currently persisted PowerControl proto based on the widget's + slider values and sends out autokick/autochip commands if they are enabled """ # get kick power value slider value and set the label to that value kick_power_value = self.kick_power_slider.value() diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py index 1044c44ac5..abafd0aaae 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py @@ -55,7 +55,7 @@ def __init__(self) -> None: self.setLayout(diagnostics_input_widget_vbox_layout) def update(self, status: HandheldDeviceConnectionStatus) -> None: - """Update this widget with the current handheld device connection status. + """Update this widget with the current handheld device connection status. If the handheld device is connected: - enables the handheld button @@ -76,7 +76,7 @@ def update(self, status: HandheldDeviceConnectionStatus) -> None: def get_control_mode(self) -> ControlMode: """Get the currently selected control mode. - + :returns: the currently selected control mode """ return ( diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 59caa1dcb9..584b557e4d 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -7,7 +7,6 @@ from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( HandheldDeviceStatusView, - HandheldDeviceConnectionStatus, ) from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( DiagnosticsInputToggleWidget, @@ -15,6 +14,7 @@ ) from software.thunderscope.robot_diagnostics.handheld_device_manager import ( HandheldDeviceManager, + HandheldDeviceConnectionChangedEvent, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, @@ -23,15 +23,15 @@ class DiagnosticsWidget(QScrollArea): """The DiagnosticsWidget contains all widgets related to Robot Diagnostics: - - HandheldDeviceStatusWidget - - DiagnosticsInputWidget - - DriveAndDribblerWidget - - ChickerWidget + - HandheldDeviceStatusWidget + - DiagnosticsInputWidget + - DriveAndDribblerWidget + - ChickerWidget """ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """Initialize the DiagnosticsWidget - + :param proto_unix_io: proto unix io to configure the diagnostics widgets with """ super(DiagnosticsWidget, self).__init__() @@ -82,21 +82,23 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: """Handler for the control_mode_changed_signal emitted by DiagnosticsControlInputWidget - :param mode: the currently selected control mode + :param mode: the currently selected control mode """ self.handheld_device_manager.set_enabled(mode == ControlMode.HANDHELD) self.drive_dribbler_widget.update_widget_accessibility(mode) self.chicker_widget.update_widget_accessibility(mode) def __handheld_device_connection_status_signal_handler( - self, status: HandheldDeviceConnectionStatus + self, event: HandheldDeviceConnectionChangedEvent ) -> None: """Handler for the handheld_device_connection_status_signal emitted by HandheldDeviceManager - - :param status: the new handheld device connection status + + :param event: the signal event containing the new handheld device connection status """ - self.handheld_device_status_widget.update(status) - self.diagnostics_control_input_widget.update(status) + self.handheld_device_status_widget.update( + event.connection_status, event.device_name + ) + self.diagnostics_control_input_widget.update(event.connection_status) def refresh(self) -> None: """Refreshes sub-widgets so that they display the most recent status values. diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 50c02e1164..0977194767 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -29,34 +29,34 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: def set_x_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's X velocity - + :param value: the value to set the slider to in m/s """ self.x_velocity_slider.setValue(value) def set_y_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's Y velocity - + :param value: the value to set the slider to in m/s """ self.y_velocity_slider.setValue(value) def set_angular_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's angular velocity - + :param value: the value to set the slider to in rad/s """ self.angular_velocity_slider.setValue(value) def set_dribbler_velocity_slider(self, value: float) -> None: """Set the value of the slider controlling the robot's dribbler velocity - + :param value: the value to set the slider to in RPM """ self.dribbler_speed_rpm_slider.setValue(value) def refresh(self) -> None: - """Update the currently persisted MotorControl proto based on the widget's slider values + """Update the currently persisted MotorControl proto based on the widget's slider values and sends out the proto """ self.motor_control.dribbler_speed_rpm = int( @@ -84,8 +84,8 @@ def __value_change_handler(self, value: float) -> str: def __setup_direct_velocity(self) -> QGroupBox: """Create a widget to control the direct velocity of the robot's motors - - :returns: a QGroupBox containing sliders and controls for controlling the + + :returns: a QGroupBox containing sliders and controls for controlling the direct velocity of the robot's motors """ group_box = QGroupBox("Drive") @@ -158,7 +158,7 @@ def __setup_direct_velocity(self) -> QGroupBox: def __setup_dribbler(self) -> QGroupBox: """Create a widget to control the dribbler speed - + :returns: a QGroupBox containing a slider and controls for controlling the robot's dribbler speed """ diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 0b7fbcbd03..1f3cce727a 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -15,6 +15,17 @@ HandheldDeviceConnectionStatus, ) +from dataclasses import dataclass + + +@dataclass +class HandheldDeviceConnectionChangedEvent: + connection_status: HandheldDeviceConnectionStatus + """The new handheld device connection status""" + + device_name: str | None + """The name of the connected handheld device""" + class HandheldDeviceManager(QtCore.QObject): """This class is responsible for managing the connection between a computer and a handheld device to control robots. @@ -23,8 +34,9 @@ class HandheldDeviceManager(QtCore.QObject): """ handheld_device_connection_status_signal = QtCore.pyqtSignal( - HandheldDeviceConnectionStatus + HandheldDeviceConnectionChangedEvent ) + """Signal emitted when the handheld device connection status changes""" def __init__( self, @@ -32,7 +44,7 @@ def __init__( proto_unix_io: ProtoUnixIO, ): """Initialize the HandheldDeviceManager - + :param logger: the logger to use :param proto_unix_io: the proto unix io to use for sending protos """ @@ -115,12 +127,18 @@ def __initialize_handheld_device(self) -> None: handheld_device.name ] - self.handheld_device_connection_status_signal.emit( - HandheldDeviceConnectionStatus.CONNECTED - ) - self.logger.debug("Successfully initialized handheld!") - self.logger.debug('Device name: "' + self.handheld_device.name + '"') - self.logger.debug("Device path: " + self.handheld_device.path) + self.handheld_device_connection_status_signal.emit( + HandheldDeviceConnectionChangedEvent( + HandheldDeviceConnectionStatus.CONNECTED, + self.handheld_device.name, + ) + ) + + self.logger.debug("Successfully initialized handheld!") + self.logger.debug( + 'Device name: "' + self.handheld_device.name + '"' + ) + self.logger.debug("Device path: " + self.handheld_device.path) return @@ -138,7 +156,9 @@ def __clear_handheld_device(self) -> None: self.handheld_device_config = None self.handheld_device_connection_status_signal.emit( - HandheldDeviceConnectionStatus.DISCONNECTED + HandheldDeviceConnectionChangedEvent( + HandheldDeviceConnectionStatus.DISCONNECTED, None + ) ) def __event_loop(self) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 3a09624a12..64aaa2fbdc 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -10,37 +10,30 @@ class HandheldDeviceConnectionStatus(Enum): + """Enum representing whether a handheld device is connected or disconnected""" + CONNECTED = 1 DISCONNECTED = 2 class HandheldDeviceStatusView(QWidget): + """Widget that shows the user the current state of the connection with a handheld device, + as well as a button that attempts to detect and reinitialize a handheld device when pressed + """ + reinitialize_handheld_device_signal = QtCore.pyqtSignal() + """Signal emitted when the "Detect Handheld Device" button is pressed""" def __init__(self) -> None: - """Initialize the HandheldDeviceStatusView widget. - This widget shows the user the current state of the connection with a handheld device, - as well as a button that attempts to reinitialize a handheld device object when clicked - """ + """Initialize the HandheldDeviceStatusView""" super(HandheldDeviceStatusView, self).__init__() self.handheld_device_status = QLabel() self.handheld_device_status.setAlignment(Qt.AlignmentFlag.AlignCenter) - self.status_label_view_map = { - HandheldDeviceConnectionStatus.CONNECTED: ( - "Handheld Device is Connected", - "background-color: green", - ), - HandheldDeviceConnectionStatus.DISCONNECTED: ( - "Handheld Device is Disconnected...", - "background-color: red", - ), - } - - self.handheld_device_reinitialize_button = QPushButton() - self.handheld_device_reinitialize_button.setText("Detect Handheld Device") - self.handheld_device_reinitialize_button.clicked.connect( + self.detect_handheld_device_button = QPushButton() + self.detect_handheld_device_button.setText("Detect Handheld Device") + self.detect_handheld_device_button.clicked.connect( self.reinitialize_handheld_device_signal ) @@ -50,7 +43,7 @@ def __init__(self) -> None: widget_layout = QVBoxLayout() hbox_layout.addWidget(self.handheld_device_status) - hbox_layout.addWidget(self.handheld_device_reinitialize_button) + hbox_layout.addWidget(self.detect_handheld_device_button) hbox_layout.setStretch(0, 4) hbox_layout.setStretch(1, 1) @@ -58,13 +51,19 @@ def __init__(self) -> None: widget_layout.addWidget(box) self.setLayout(widget_layout) - self.update(HandheldDeviceConnectionStatus.DISCONNECTED) + self.update(HandheldDeviceConnectionStatus.DISCONNECTED, None) - def update(self, connection_status: HandheldDeviceConnectionStatus) -> None: + def update( + self, connection_status: HandheldDeviceConnectionStatus, device_name: str | None + ) -> None: """Sets the label to display the correct status depending on the connection status :param connection_status: the connection status to display + :param device_name: the name of the handheld device that was connected """ - text, color = self.status_label_view_map[connection_status] - self.handheld_device_status.setText(text) - self.handheld_device_status.setStyleSheet(color) + if connection_status == HandheldDeviceConnectionStatus.CONNECTED: + self.handheld_device_status.setText(f"Connected: {device_name}") + self.handheld_device_status.setStyleSheet("background-color: green") + elif connection_status == HandheldDeviceConnectionStatus.DISCONNECTED: + self.handheld_device_status.setText("Disconnected") + self.handheld_device_status.setStyleSheet("background-color: red") From c9e4770e1a243f335aefa4ee223c8387c8d0b585 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 23 Sep 2024 20:51:02 -0700 Subject: [PATCH 129/138] Fix bugs with chicker widget buttons enabling/disabling at wrong times --- .../robot_diagnostics/chicker_widget.py | 110 ++++++------------ 1 file changed, 35 insertions(+), 75 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index a53f98bc52..62b150b407 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -38,9 +38,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: chicker_widget_vbox_layout = QVBoxLayout() self.setLayout(chicker_widget_vbox_layout) - kick_chip_sliders_hbox_layout = QHBoxLayout() - - # Initializing power slider for kicking ( self.kick_power_slider_layout, self.kick_power_slider, @@ -52,9 +49,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: DiagnosticsConstants.KICK_POWER_STEPPER, ) - kick_chip_sliders_hbox_layout.addLayout(self.kick_power_slider_layout) - - # Initializing distance slider for chipping ( self.chip_distance_slider_layout, self.chip_distance_slider, @@ -67,6 +61,16 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: int(DiagnosticsConstants.CHIP_DISTANCE_STEPPER), ) + self.kick_power_slider.valueChanged.connect( + lambda: self.kick_power_label.setText(str(self.kick_power_slider.value())) + ) + + self.chip_distance_slider.valueChanged.connect( + lambda: self.chip_distance_label.setText(str(self.chip_distance_slider.value())) + ) + + kick_chip_sliders_hbox_layout = QHBoxLayout() + kick_chip_sliders_hbox_layout.addLayout(self.kick_power_slider_layout) kick_chip_sliders_hbox_layout.addLayout(self.chip_distance_slider_layout) kick_chip_sliders_box = QGroupBox() @@ -86,9 +90,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.kick_button = self.kick_chip_buttons[0] self.chip_button = self.kick_chip_buttons[1] - # set buttons to be initially enabled - self.kick_chip_buttons_enable = True - chicker_widget_vbox_layout.addWidget(self.kick_chip_buttons_box) # Initializing auto kick & chip buttons @@ -107,9 +108,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # Set no auto button to be selected by default on launch self.no_auto_button.setChecked(True) - self.no_auto_selected = True - # Initialize on-click handlers for kick & chip buttons. + self.no_auto_button.toggled.connect( + self.update_kick_chip_buttons_accessibility + ) + self.kick_button.clicked.connect( lambda: self.send_command_and_timeout(ChickerCommandMode.KICK) ) @@ -117,17 +120,6 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: lambda: self.send_command_and_timeout(ChickerCommandMode.CHIP) ) - # Initialize on-click handlers for no auto, auto kick and auto chip buttons. - self.no_auto_button.toggled.connect( - lambda: self.set_should_enable_buttons(True) - ) - self.auto_kick_button.toggled.connect( - lambda: self.set_should_enable_buttons(False) - ) - self.auto_chip_button.toggled.connect( - lambda: self.set_should_enable_buttons(False) - ) - chicker_widget_vbox_layout.addWidget(self.auto_kick_chip_buttons_box) def set_kick_power_slider_value(self, value: float) -> None: @@ -150,38 +142,23 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: :param command: Command to send. One of ChickerCommandMode.KICK or ChickerCommandMode.CHIP """ - if self.kick_chip_buttons_enable: - # send kick primitive + if self.kick_button.isEnabled() and self.chip_button.isEnabled(): + common_widgets.disable_button(self.kick_button) + common_widgets.disable_button(self.chip_button) + self.send_command(command) - self.disable_kick_chip_buttons() # set and start timer to re-enable buttons after CHICKER_TIMEOUT - QTimer.singleShot(int(CHICKER_TIMEOUT), self.enable_kick_chip_buttons) - - def disable_kick_chip_buttons(self) -> None: - """Disables the buttons""" - self.kick_chip_buttons_enable = False - - def enable_kick_chip_buttons(self) -> None: - """If buttons should be enabled, enables them""" - if self.no_auto_selected: - self.kick_chip_buttons_enable = True - - # clears the proto buffer when buttons are re-enabled - # just to start fresh and clear any unwanted protos - self.__initialize_default_power_control_values() + QTimer.singleShot(int(CHICKER_TIMEOUT), self.update_kick_chip_buttons_accessibility) - def set_should_enable_buttons(self, enable: bool) -> None: - """Changes if buttons are clickable or not based on boolean parameter - - :param enable: boolean to indicate whether buttons should be made clickable or not - """ - self.no_auto_selected = enable - - if enable: - self.enable_kick_chip_buttons() + def update_kick_chip_buttons_accessibility(self) -> None: + """Enables or disables the kick/chip buttons depending on whether autokick/autochip is on""" + if self.no_auto_button.isChecked(): + common_widgets.enable_button(self.kick_button) + common_widgets.enable_button(self.chip_button) else: - self.disable_kick_chip_buttons() + common_widgets.disable_button(self.kick_button) + common_widgets.disable_button(self.chip_button) def send_command(self, command: ChickerCommandMode) -> None: """Sends a [auto]kick or [auto]chip primitive @@ -224,49 +201,32 @@ def __initialize_default_power_control_values(self) -> None: self.power_control.geneva_slot = 3 self.proto_unix_io.send_proto(PowerControl, self.power_control, True) - def refresh_button_state(self, button: QPushButton) -> None: - """Change button color and clickable state. - - :param button: button to change the state of - """ - button.setEnabled(self.kick_chip_buttons_enable) - button.setStyleSheet( - "" if self.kick_chip_buttons_enable else "background-color: grey" - ) - def update_widget_accessibility(self, mode: ControlMode): """Disables or enables all sliders and buttons depending on the given control mode. Sliders and buttons are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode :param mode: the current control mode """ - self.auto_kick_button.setEnabled(mode == ControlMode.DIAGNOSTICS) - self.auto_chip_button.setEnabled(mode == ControlMode.DIAGNOSTICS) - self.set_should_enable_buttons(mode == ControlMode.DIAGNOSTICS) - if mode == ControlMode.DIAGNOSTICS: common_widgets.enable_slider(self.kick_power_slider) common_widgets.enable_slider(self.chip_distance_slider) + common_widgets.enable_button(self.no_auto_button) + common_widgets.enable_button(self.auto_kick_button) + common_widgets.enable_button(self.auto_chip_button) + self.update_kick_chip_buttons_accessibility() elif mode == ControlMode.HANDHELD: common_widgets.disable_slider(self.kick_power_slider) common_widgets.disable_slider(self.chip_distance_slider) + common_widgets.disable_button(self.no_auto_button) + common_widgets.disable_button(self.auto_kick_button) + common_widgets.disable_button(self.auto_chip_button) + common_widgets.disable_button(self.kick_button) + common_widgets.disable_button(self.chip_button) def refresh(self) -> None: """Update the currently persisted PowerControl proto based on the widget's slider values and sends out autokick/autochip commands if they are enabled """ - # get kick power value slider value and set the label to that value - kick_power_value = self.kick_power_slider.value() - self.kick_power_label.setText(str(kick_power_value)) - - # get chip distance value slider value and set the label to that value - chip_distance_value = self.chip_distance_slider.value() - self.chip_distance_label.setText(str(chip_distance_value)) - - # refresh button state to reflect to user current status - self.refresh_button_state(self.kick_button) - self.refresh_button_state(self.chip_button) - # If auto is enabled, we want to populate the autochip or kick message if self.auto_kick_button.isChecked(): self.send_command(ChickerCommandMode.AUTOKICK) From 749db91d86b416d33264ebd3b2859943395c39db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Tue, 24 Sep 2024 03:58:30 +0000 Subject: [PATCH 130/138] [pre-commit.ci lite] apply automatic fixes --- .../thunderscope/robot_diagnostics/chicker_widget.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 62b150b407..12b7e7ea4a 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -66,7 +66,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) self.chip_distance_slider.valueChanged.connect( - lambda: self.chip_distance_label.setText(str(self.chip_distance_slider.value())) + lambda: self.chip_distance_label.setText( + str(self.chip_distance_slider.value()) + ) ) kick_chip_sliders_hbox_layout = QHBoxLayout() @@ -109,9 +111,7 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: # Set no auto button to be selected by default on launch self.no_auto_button.setChecked(True) - self.no_auto_button.toggled.connect( - self.update_kick_chip_buttons_accessibility - ) + self.no_auto_button.toggled.connect(self.update_kick_chip_buttons_accessibility) self.kick_button.clicked.connect( lambda: self.send_command_and_timeout(ChickerCommandMode.KICK) @@ -149,7 +149,9 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: self.send_command(command) # set and start timer to re-enable buttons after CHICKER_TIMEOUT - QTimer.singleShot(int(CHICKER_TIMEOUT), self.update_kick_chip_buttons_accessibility) + QTimer.singleShot( + int(CHICKER_TIMEOUT), self.update_kick_chip_buttons_accessibility + ) def update_kick_chip_buttons_accessibility(self) -> None: """Enables or disables the kick/chip buttons depending on whether autokick/autochip is on""" From 99b7c0a4cda1c3ac83aca9c51df085407edfa954 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 23 Sep 2024 21:14:56 -0700 Subject: [PATCH 131/138] Nits --- .../robot_diagnostics/chicker_widget.py | 34 +++++++++---------- .../robot_diagnostics/diagnostics_widget.py | 9 +++-- .../drive_and_dribbler_widget.py | 9 ++++- .../handheld_device_manager.py | 2 +- .../handheld_device_status_view.py | 4 +++ 5 files changed, 36 insertions(+), 22 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index 62b150b407..47f60b7a1f 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -17,17 +17,20 @@ class ChickerCommandMode(Enum): class ChickerWidget(QWidget): - def __init__(self, proto_unix_io: ProtoUnixIO) -> None: - """Handles the robot diagnostics input to create a PowerControl message - to be sent to the robots. + """Handles the robot diagnostics input to create a PowerControl message + to be sent to the robots. + + NOTE: The powerboards run in regulation mode, which means that they are + always charged and do not need to be explicitly charged. - NOTE: The powerboards run in regulation mode, which means that they are - always charged and do not need to be explicitly charged. + The powerboard also has an internal cooldown, so spamming kick or chip + will not work until the capacitors charge up and the cooldown is over. + """ - The powerboard also has an internal cooldown, so spamming kick or chip - will not work until the capacitors charge up and the cooldown is over. + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: + """Initialize the ChickerWidget - :param proto_unix_io: proto_unix_io object to send messages to the robot + :param proto_unix_io: ProtoUnixIO to send messages to the robot """ super().__init__() @@ -137,7 +140,7 @@ def set_chip_distance_slider_value(self, value: float) -> None: self.chip_distance_slider.setValue(value) def send_command_and_timeout(self, command: ChickerCommandMode) -> None: - """If the kick/chip buttons are enabled, sends a kick/chip command and disables the buttons. + """If the kick/chip buttons are enabled, send a kick/chip command and disables the buttons. The buttons will be re-enabled after CHICKER_TIMEOUT. :param command: Command to send. One of ChickerCommandMode.KICK or ChickerCommandMode.CHIP @@ -152,7 +155,7 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: QTimer.singleShot(int(CHICKER_TIMEOUT), self.update_kick_chip_buttons_accessibility) def update_kick_chip_buttons_accessibility(self) -> None: - """Enables or disables the kick/chip buttons depending on whether autokick/autochip is on""" + """Enable or disable the kick/chip buttons depending on whether autokick/autochip is on""" if self.no_auto_button.isChecked(): common_widgets.enable_button(self.kick_button) common_widgets.enable_button(self.chip_button) @@ -161,7 +164,7 @@ def update_kick_chip_buttons_accessibility(self) -> None: common_widgets.disable_button(self.chip_button) def send_command(self, command: ChickerCommandMode) -> None: - """Sends a [auto]kick or [auto]chip primitive + """Send a [auto]kick or [auto]chip primitive :param command: enum int value to indicate what primitive to send """ @@ -191,7 +194,7 @@ def send_command(self, command: ChickerCommandMode) -> None: self.__initialize_default_power_control_values() def __initialize_default_power_control_values(self) -> None: - """Sends an empty proto to the proto unix io buffer + """Send an empty proto to the proto unix io buffer. This is due to a bug in robot_communication where if a new PowerControl message is not sent, The previous, cached message is resent to the robot repeatedly which we don't want for kick/chip So sending an empty message overwrites the cache and prevents spamming commands @@ -202,7 +205,7 @@ def __initialize_default_power_control_values(self) -> None: self.proto_unix_io.send_proto(PowerControl, self.power_control, True) def update_widget_accessibility(self, mode: ControlMode): - """Disables or enables all sliders and buttons depending on the given control mode. + """Enable or disable all sliders and buttons depending on the given control mode. Sliders and buttons are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode :param mode: the current control mode @@ -224,10 +227,7 @@ def update_widget_accessibility(self, mode: ControlMode): common_widgets.disable_button(self.chip_button) def refresh(self) -> None: - """Update the currently persisted PowerControl proto based on the widget's - slider values and sends out autokick/autochip commands if they are enabled - """ - # If auto is enabled, we want to populate the autochip or kick message + """Send autokick/autochip commands if enabled""" if self.auto_kick_button.isChecked(): self.send_command(ChickerCommandMode.AUTOKICK) elif self.auto_chip_button.isChecked(): diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 584b557e4d..7e99164a99 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -23,16 +23,19 @@ class DiagnosticsWidget(QScrollArea): """The DiagnosticsWidget contains all widgets related to Robot Diagnostics: + - HandheldDeviceStatusWidget - DiagnosticsInputWidget - DriveAndDribblerWidget - ChickerWidget + """ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """Initialize the DiagnosticsWidget - :param proto_unix_io: proto unix io to configure the diagnostics widgets with + :param proto_unix_io: ProtoUnixIO (for sending messages to the robot) + to configure the diagnostics widgets with """ super(DiagnosticsWidget, self).__init__() @@ -101,8 +104,8 @@ def __handheld_device_connection_status_signal_handler( self.diagnostics_control_input_widget.update(event.connection_status) def refresh(self) -> None: - """Refreshes sub-widgets so that they display the most recent status values. - If in handheld mode, then also visually updates driver, dribbler and chicker sliders + """Refresh sub-widgets so that they display the most recent status values. + If in handheld mode, visually update driver, dribbler and chicker sliders to the values currently being set by the handheld device. """ control_mode = self.diagnostics_control_input_widget.get_control_mode() diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 0977194767..e1c904661b 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -10,8 +10,15 @@ class DriveAndDribblerWidget(QWidget): + """Handles the robot diagnostics input to create a MotorControl message + to be sent to the robots. + """ + def __init__(self, proto_unix_io: ProtoUnixIO) -> None: - """Initialize the widget to control the robot's motors""" + """Initialize the DriveAndDribblerWidget + + :param proto_unix_io: ProtoUnixIO to send messages to the robot + """ super().__init__() self.proto_unix_io = proto_unix_io diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index 1f3cce727a..bd3fff08df 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -46,7 +46,7 @@ def __init__( """Initialize the HandheldDeviceManager :param logger: the logger to use - :param proto_unix_io: the proto unix io to use for sending protos + :param proto_unix_io: ProtoUnixIO to send messages to the robot """ super().__init__() diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 64aaa2fbdc..59de7a6e50 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -33,6 +33,10 @@ def __init__(self) -> None: self.detect_handheld_device_button = QPushButton() self.detect_handheld_device_button.setText("Detect Handheld Device") + self.detect_handheld_device_button.setSizePolicy( + QSizePolicy.Policy.Expanding, + QSizePolicy.Policy.Expanding, + ) self.detect_handheld_device_button.clicked.connect( self.reinitialize_handheld_device_signal ) From c30ba4aa9470e88283623015dcb3e200596750d5 Mon Sep 17 00:00:00 2001 From: williamckha Date: Mon, 23 Sep 2024 21:15:22 -0700 Subject: [PATCH 132/138] Formatting --- .../robot_diagnostics/handheld_device_status_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py index 59de7a6e50..4c157f3b12 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py @@ -34,7 +34,7 @@ def __init__(self) -> None: self.detect_handheld_device_button = QPushButton() self.detect_handheld_device_button.setText("Detect Handheld Device") self.detect_handheld_device_button.setSizePolicy( - QSizePolicy.Policy.Expanding, + QSizePolicy.Policy.Expanding, QSizePolicy.Policy.Expanding, ) self.detect_handheld_device_button.clicked.connect( From ecdd19ef0ef55cad31c94a737795ec3d448e3e14 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 24 Sep 2024 20:10:53 -0700 Subject: [PATCH 133/138] Add docstrings --- .../handheld_device_manager.py | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index bd3fff08df..e95dfe5dc9 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -28,9 +28,10 @@ class HandheldDeviceConnectionChangedEvent: class HandheldDeviceManager(QtCore.QObject): - """This class is responsible for managing the connection between a computer and a handheld device to control robots. - This class relies on the `evdev` python package, in order to implement parsing and control flow for device events. - More info & docs can be found here: https://python-evdev.readthedocs.io/en/latest/apidoc.html + """This class is responsible for managing the connection between a computer and a + handheld device to control robots. This class relies on the `evdev` python package + in order to implement parsing and control flow for device events. + See: https://python-evdev.readthedocs.io/en/latest/apidoc.html """ handheld_device_connection_status_signal = QtCore.pyqtSignal( @@ -77,8 +78,8 @@ def __init__( self.event_loop_thread.start() def __initialize_default_motor_control_values(self) -> None: - """This method sets all required fields in the motor control proto to - the default/minimum default value + """Set all required fields in the motor control proto to their + default/minimum values """ self.motor_control = MotorControl() self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 @@ -87,8 +88,8 @@ def __initialize_default_motor_control_values(self) -> None: self.motor_control.dribbler_speed_rpm = 0 def __initialize_default_power_control_values(self) -> None: - """This method sets all required fields in the power control proto to - the default/minimum default value + """Set all required fields in the power control proto to their + default/minimum values """ self.power_control = PowerControl() self.power_control.geneva_slot = 3 @@ -148,8 +149,8 @@ def __initialize_handheld_device(self) -> None: self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) def __clear_handheld_device(self) -> None: - """Clears handheld device and config field by setting them to None, - and emits a disconnected notification signal. + """Clear the handheld device and config fields by setting them to None, + and emit a disconnected notification signal. """ with self.lock: self.handheld_device = None @@ -162,12 +163,14 @@ def __clear_handheld_device(self) -> None: ) def __event_loop(self) -> None: + """Loop that continuously reads and processes events from the connected handheld device.""" while True: with self.lock: self.__read_and_process_event() time.sleep(DiagnosticsConstants.EVENT_LOOP_SLEEP_DURATION) def __read_and_process_event(self) -> None: + """Try reading an event from the connected handheld device and process it.""" if not self.handheld_device: return @@ -208,7 +211,7 @@ def __read_and_process_event(self) -> None: self.logger.critical("Error message: " + str(e)) def __process_event(self, event: InputEvent) -> None: - """Processes the given device event. Sets corresponding motor and power control values + """Process the given device event. Set the corresponding motor and power control values based on the event type, using the current config set in self.handheld_device_config :param event: The input event to process @@ -298,7 +301,7 @@ def __interpret_dribbler_enabled_event_value( return button_percent_pressed > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: - """Interprets the event value that corresponds to controlling the dribbler speed. + """Interpret the event value that corresponds to controlling the dribbler speed. :param event_value: the value for the current event being interpreted :return: the interpreted value to be used for the new dribbler speed on the robot @@ -313,7 +316,7 @@ def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: ) def __interpret_kick_event_value(self, event_value: float) -> float: - """Interprets the event value that corresponds to controlling the dribbler speed. + """Interpret the event value that corresponds to controlling the dribbler speed. :param event_value: the value for the current event being interpreted :return: the interpreted value to be used for the kick power on the robot @@ -325,7 +328,7 @@ def __interpret_kick_event_value(self, event_value: float) -> float: ) def __interpret_chip_event_value(self, event_value: float) -> float: - """Interprets the event value that corresponds to controlling the chip distance. + """Interpret the event value that corresponds to controlling the chip distance. :param value: the value for the current event being interpreted :return: the interpreted value to be used for the chip distance on the robot From c0be885f731fe5d4a5b7f65d749318496027de89 Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 28 Sep 2024 16:41:04 -0700 Subject: [PATCH 134/138] Add docs --- src/software/thunderscope/robot_communication.py | 4 ++-- .../robot_diagnostics/handheld_device_manager.py | 9 ++++++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index c17d5a612d..45279bdc39 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -99,7 +99,7 @@ def __init__( # TODO: (#3174): move estop state management out of robot_communication self.estop_mode = estop_mode self.estop_path = estop_path - self.estop_buadrate = estop_baudrate + self.estop_baudrate = estop_baudrate # initialising the estop # tries to access a plugged in estop. if not found, throws an exception @@ -114,7 +114,7 @@ def __init__( if self.estop_mode == EstopMode.PHYSICAL_ESTOP: try: self.estop_reader = tbots_cpp.ThreadedEstopReader( - self.estop_path, self.estop_buadrate + self.estop_path, self.estop_baudrate ) except Exception: raise Exception(f"Invalid Estop found at location {self.estop_path}") diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py index e95dfe5dc9..8095e682ff 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py @@ -170,7 +170,11 @@ def __event_loop(self) -> None: time.sleep(DiagnosticsConstants.EVENT_LOOP_SLEEP_DURATION) def __read_and_process_event(self) -> None: - """Try reading an event from the connected handheld device and process it.""" + """Try reading an event from the connected handheld device and process it. + + This method is not thread-safe. Callers must acquire self.lock beforehand + to guarantee exclusive access to the HandheldDeviceManager's mutable state. + """ if not self.handheld_device: return @@ -214,6 +218,9 @@ def __process_event(self, event: InputEvent) -> None: """Process the given device event. Set the corresponding motor and power control values based on the event type, using the current config set in self.handheld_device_config + This method is not thread-safe. Callers must acquire self.lock beforehand + to guarantee exclusive access to the HandheldDeviceManager's mutable state. + :param event: The input event to process """ config = self.handheld_device_config From 8f2b080d619c643ecc01692533d6c7097e8fd06b Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 8 Oct 2024 20:58:07 -0700 Subject: [PATCH 135/138] Major refactor --- src/software/thunderscope/BUILD | 1 - src/software/thunderscope/constants.py | 67 +--- .../thunderscope/robot_communication.py | 9 +- .../thunderscope/robot_diagnostics/BUILD | 30 +- .../robot_diagnostics/chicker_widget.py | 193 +++++----- .../diagnostics_input_widget.py | 86 ----- .../robot_diagnostics/diagnostics_widget.py | 133 +++---- .../drive_and_dribbler_widget.py | 192 ++++------ .../robot_diagnostics/handheld_controller.py | 86 +++++ .../handheld_controller_widget.py | 205 +++++++++++ .../handheld_device_manager.py | 348 ------------------ .../handheld_device_status_view.py | 73 ---- .../thunderscope/widget_setup_functions.py | 1 - 13 files changed, 533 insertions(+), 891 deletions(-) delete mode 100644 src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py create mode 100644 src/software/thunderscope/robot_diagnostics/handheld_controller.py create mode 100644 src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py delete mode 100644 src/software/thunderscope/robot_diagnostics/handheld_device_manager.py delete mode 100644 src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index 0697c11c4f..9eb386f592 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -101,7 +101,6 @@ py_library( "//software/thunderscope/play:refereeinfo_widget", "//software/thunderscope/replay:proto_player", "//software/thunderscope/robot_diagnostics:chicker", - "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:diagnostics_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", "//software/thunderscope/robot_diagnostics:estop_view", diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index 919044030b..a8f967eace 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -1,5 +1,3 @@ -from dataclasses import dataclass - from pyqtgraph.Qt import QtGui from OpenGL.GL import * from proto.import_all_protos import * @@ -302,69 +300,16 @@ class TrailValues: DEFAULT_TRAIL_SAMPLING_RATE = 0 -@dataclass -class DeviceKeyEvent: - """This dataclass holds the code for a "EV_KEY" input event""" - - event_code: int - - -@dataclass -class DeviceAbsEvent: - """This dataclass holds the code and max value for an "EV_ABS" input event""" - - event_code: int - max_value: float - - -@dataclass -class DeviceConfig: - move_x: DeviceAbsEvent - move_y: DeviceAbsEvent - move_rot: DeviceAbsEvent - kick: DeviceKeyEvent - chip: DeviceKeyEvent - chicker_power: DeviceAbsEvent - dribbler_speed: DeviceAbsEvent - dribbler_enable: DeviceAbsEvent - - class DiagnosticsConstants: - XboxConfig = DeviceConfig( - # Name: "ABS_X" - # Canonical: Left joystick X-axis - move_x=DeviceAbsEvent(event_code=0, max_value=32767.0), - # Name: "ABS_Y" - # Canonical: Left joystick Y-axis - move_y=DeviceAbsEvent(event_code=1, max_value=32767.0), - # Name: "ABS_RX" - # Canonical: Right joystick X-axis - move_rot=DeviceAbsEvent(event_code=3, max_value=32767.0), - # Name: "BTN_A" - # Canonical: "A" Button - kick=DeviceKeyEvent(event_code=304), - # Name: "BTN_Y" - # Canonical: "Y" Button - chip=DeviceKeyEvent(event_code=308), - # Name: "ABS_HAT0X - # Canonical: D-pad X-axis - chicker_power=DeviceAbsEvent(event_code=16, max_value=1.0), - # Name: "ABS_HAT0Y" - # Canonical: D-pad Y-axis - dribbler_speed=DeviceAbsEvent(event_code=17, max_value=1.0), - # Name: "ABS_RZ" - # Canonical: Right trigger - dribbler_enable=DeviceAbsEvent(event_code=5, max_value=1023.0), - ) + """Constants for Robot Diagnostics""" - HANDHELD_DEVICE_NAME_CONFIG_MAP = { - "Microsoft Xbox One X pad": XboxConfig, - "Microsoft X-Box One S pad": XboxConfig, - "Microsoft Xbox 360 pad": XboxConfig, + # Device names of the controllers supported for controlling robots + SUPPORTED_CONTROLLERS = { + "Microsoft Xbox One X pad", + "Microsoft X-Box One S pad", + "Microsoft Xbox 360 pad", } - INPUT_DELAY_THRESHOLD = 0.01 - EVENT_LOOP_SLEEP_DURATION = 0.0005 BUTTON_PRESSED_THRESHOLD = 0.5 DEADZONE_PERCENTAGE = 0.20 diff --git a/src/software/thunderscope/robot_communication.py b/src/software/thunderscope/robot_communication.py index 45279bdc39..712f7fb81b 100644 --- a/src/software/thunderscope/robot_communication.py +++ b/src/software/thunderscope/robot_communication.py @@ -221,7 +221,8 @@ def __should_send_packet(self) -> bool: ) def __run_primitive_set(self) -> None: - """Forward PrimitiveSet protos from fullsystem to the robots. + """Forward PrimitiveSet protos from Fullsystem and MotorControl/PowerControl + protos from Robot Diagnostics to the robots. For AI protos, blocks for 10ms if no proto is available, and then returns a cached proto @@ -230,12 +231,6 @@ def __run_primitive_set(self) -> None: If the emergency stop is tripped, the PrimitiveSet will not be sent so that the robots timeout and stop. - - NOTE: If disconnect_fullsystem_from_robots is called, then the packets - will not be forwarded to the robots. - - send_override_primitive_set can be used to send a primitive set, which - is useful to dip in and out of robot diagnostics. """ while self.running: # map of robot id to diagnostics/fullsystem primitive map diff --git a/src/software/thunderscope/robot_diagnostics/BUILD b/src/software/thunderscope/robot_diagnostics/BUILD index 24d6273f05..dcd80f7406 100644 --- a/src/software/thunderscope/robot_diagnostics/BUILD +++ b/src/software/thunderscope/robot_diagnostics/BUILD @@ -24,21 +24,15 @@ py_library( ) py_library( - name = "handheld_device_manager", - srcs = ["handheld_device_manager.py"], - deps = [ - "//proto:software_py_proto", - "//software/logger:py_logger", - "//software/thunderscope:constants", - "//software/thunderscope:thread_safe_buffer", - requirement("pyqtgraph"), - ], + name = "handheld_controller", + srcs = ["handheld_controller.py"], ) py_library( - name = "handheld_device_status_view", - srcs = ["handheld_device_status_view.py"], + name = "handheld_controller_widget", + srcs = ["handheld_controller_widget.py"], deps = [ + ":handheld_controller", "//software/thunderscope:constants", requirement("pyqtgraph"), ], @@ -53,20 +47,8 @@ py_library( "//software/thunderscope:thread_safe_buffer", "//software/thunderscope:thunderscope_types", "//software/thunderscope/robot_diagnostics:chicker", - "//software/thunderscope/robot_diagnostics:diagnostics_input_widget", "//software/thunderscope/robot_diagnostics:drive_and_dribbler_widget", - "//software/thunderscope/robot_diagnostics:handheld_device_manager", - "//software/thunderscope/robot_diagnostics:handheld_device_status_view", - requirement("pyqtgraph"), - ], -) - -py_library( - name = "diagnostics_input_widget", - srcs = ["diagnostics_input_widget.py"], - deps = [ - "//software/thunderscope:constants", - "//software/thunderscope:thread_safe_buffer", + "//software/thunderscope/robot_diagnostics:handheld_controller_widget", requirement("pyqtgraph"), ], ) diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index d6b6fb3b49..8ae421d3f8 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -5,11 +5,12 @@ from enum import Enum import software.thunderscope.common.common_widgets as common_widgets from software.thunderscope.constants import DiagnosticsConstants -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.proto_unix_io import ProtoUnixIO class ChickerCommandMode(Enum): + """The types of chicker commands that we can send to the robots""" + KICK = 1 CHIP = 2 AUTOKICK = 3 @@ -17,8 +18,10 @@ class ChickerCommandMode(Enum): class ChickerWidget(QWidget): - """Handles the robot diagnostics input to create a PowerControl message - to be sent to the robots. + """This widget provides an interface to send PowerControl messages for + kicking and chipping actions to the robot. It has sliders for adjusting + the kick power and chip distance, as well as buttons for sending single + kick/chip commands or turning on autokick/autochip. NOTE: The powerboards run in regulation mode, which means that they are always charged and do not need to be explicitly charged. @@ -36,7 +39,9 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io: ProtoUnixIO = proto_unix_io - self.__initialize_default_power_control_values() + self.enabled = True + + self.kick_chip_locked = False chicker_widget_vbox_layout = QVBoxLayout() self.setLayout(chicker_widget_vbox_layout) @@ -65,13 +70,11 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: ) self.kick_power_slider.valueChanged.connect( - lambda: self.kick_power_label.setText(str(self.kick_power_slider.value())) + lambda new_value: self.kick_power_label.setText(str(new_value)) ) - self.chip_distance_slider.valueChanged.connect( - lambda: self.chip_distance_label.setText( - str(self.chip_distance_slider.value()) - ) + self.chip_distance_slider.floatValueChanged.connect( + lambda new_value: self.chip_distance_label.setText(str(new_value)) ) kick_chip_sliders_hbox_layout = QHBoxLayout() @@ -111,10 +114,10 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.auto_kick_button = self.auto_kick_chip_buttons[1] self.auto_chip_button = self.auto_kick_chip_buttons[2] - # Set no auto button to be selected by default on launch self.no_auto_button.setChecked(True) - - self.no_auto_button.toggled.connect(self.update_kick_chip_buttons_accessibility) + self.no_auto_button.toggled.connect( + self.__update_kick_chip_buttons_accessibility + ) self.kick_button.clicked.connect( lambda: self.send_command_and_timeout(ChickerCommandMode.KICK) @@ -125,112 +128,116 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: chicker_widget_vbox_layout.addWidget(self.auto_kick_chip_buttons_box) - def set_kick_power_slider_value(self, value: float) -> None: - """Set the kick power slider to a specific value. - - :param value: The value to set for the slider - """ - self.kick_power_slider.setValue(value) - - def set_chip_distance_slider_value(self, value: float) -> None: - """Set the chip distance slider to a specific value. - - :param value: the value to set for the slider - """ - self.chip_distance_slider.setValue(value) - def send_command_and_timeout(self, command: ChickerCommandMode) -> None: - """If the kick/chip buttons are enabled, send a kick/chip command and disables the buttons. - The buttons will be re-enabled after CHICKER_TIMEOUT. + """If the kick/chip buttons are enabled and unlocked, send a kick/chip command + and then disable and lock the buttons. The buttons will be re-enabled and unlocked + after CHICKER_TIMEOUT. - :param command: Command to send. One of ChickerCommandMode.KICK or ChickerCommandMode.CHIP + :param command: the chicker command to send; should be either + ChickerCommandMode.KICK or ChickerCommandMode.CHIP """ - if self.kick_button.isEnabled() and self.chip_button.isEnabled(): + if not self.kick_chip_locked: + self.kick_chip_locked = True + common_widgets.disable_button(self.kick_button) common_widgets.disable_button(self.chip_button) self.send_command(command) - # set and start timer to re-enable buttons after CHICKER_TIMEOUT - QTimer.singleShot( - int(CHICKER_TIMEOUT), self.update_kick_chip_buttons_accessibility - ) + def unlock_kick_chip(): + self.kick_chip_locked = False + self.__update_kick_chip_buttons_accessibility() - def update_kick_chip_buttons_accessibility(self) -> None: - """Enable or disable the kick/chip buttons depending on whether autokick/autochip is on""" - if self.no_auto_button.isChecked(): - common_widgets.enable_button(self.kick_button) - common_widgets.enable_button(self.chip_button) - else: - common_widgets.disable_button(self.kick_button) - common_widgets.disable_button(self.chip_button) + QTimer.singleShot(int(CHICKER_TIMEOUT), unlock_kick_chip) def send_command(self, command: ChickerCommandMode) -> None: - """Send a [auto]kick or [auto]chip primitive + """Send a [auto]kick or [auto]chip primitive with the currently set + kick power or chip distance - :param command: enum int value to indicate what primitive to send + :param command: the type of chicker command to send """ - # gets slider values - kick_power_value = self.kick_power_slider.value() - chip_distance_value = self.chip_distance_slider.value() + # Get slider values + kick_power = self.kick_power_slider.value() + chip_dist = self.chip_distance_slider.value() + + # Send kick, chip, autokick, or autochip primitive + power_control = PowerControl() + power_control.geneva_slot = 3 - # sends kick, chip, autokick, or autochip primitive if command == ChickerCommandMode.KICK: - self.power_control.chicker.kick_speed_m_per_s = kick_power_value + power_control.chicker.kick_speed_m_per_s = kick_power elif command == ChickerCommandMode.CHIP: - self.power_control.chicker.chip_distance_meters = chip_distance_value + power_control.chicker.chip_distance_meters = chip_dist elif command == ChickerCommandMode.AUTOKICK: - self.power_control.chicker.auto_chip_or_kick.autokick_speed_m_per_s = ( - kick_power_value - ) + power_control.chicker.auto_chip_or_kick.autokick_speed_m_per_s = kick_power elif command == ChickerCommandMode.AUTOCHIP: - self.power_control.chicker.auto_chip_or_kick.autochip_distance_meters = ( - chip_distance_value - ) + power_control.chicker.auto_chip_or_kick.autochip_distance_meters = chip_dist - self.proto_unix_io.send_proto(PowerControl, self.power_control) + self.proto_unix_io.send_proto(PowerControl, power_control) - # clears the proto buffer for kick or chip commands - # so only one kick / chip is sent if command == ChickerCommandMode.KICK or command == ChickerCommandMode.CHIP: - self.__initialize_default_power_control_values() - - def __initialize_default_power_control_values(self) -> None: - """Send an empty proto to the proto unix io buffer. - This is due to a bug in robot_communication where if a new PowerControl message is not sent, - The previous, cached message is resent to the robot repeatedly which we don't want for kick/chip - So sending an empty message overwrites the cache and prevents spamming commands - If buffer is full, blocks execution until buffer has space - """ - self.power_control = PowerControl() - self.power_control.geneva_slot = 3 - self.proto_unix_io.send_proto(PowerControl, self.power_control, True) - - def update_widget_accessibility(self, mode: ControlMode): - """Enable or disable all sliders and buttons depending on the given control mode. - Sliders and buttons are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode - - :param mode: the current control mode + # We need to send an empty message to the PowerControl proto buffer to prevent the + # spamming of kick/chip commands. This is because if no new messages are received in the + # buffer, the last sent message will be repeatedly resent to the robot, which we don't + # want for kick/chip. + power_control = PowerControl() + power_control.geneva_slot = 3 + self.proto_unix_io.send_proto(PowerControl, power_control, True) + + def enable(self) -> None: + """Enable all sliders and buttons in the ChickerWidget""" + if self.enabled: + return + + self.enabled = True + + common_widgets.enable_slider(self.kick_power_slider) + common_widgets.enable_slider(self.chip_distance_slider) + common_widgets.enable_button(self.no_auto_button) + common_widgets.enable_button(self.auto_kick_button) + common_widgets.enable_button(self.auto_chip_button) + self.__update_kick_chip_buttons_accessibility() + + def disable(self) -> None: + """Disable all sliders and buttons in the ChickerWidget""" + if not self.enabled: + return + + self.enabled = False + + common_widgets.disable_slider(self.kick_power_slider) + common_widgets.disable_slider(self.chip_distance_slider) + common_widgets.disable_button(self.no_auto_button) + common_widgets.disable_button(self.auto_kick_button) + common_widgets.disable_button(self.auto_chip_button) + common_widgets.disable_button(self.kick_button) + common_widgets.disable_button(self.chip_button) + + def override_slider_values(self, kick_power: int, chip_distance: float) -> None: + """Set the widget's sliders to match the given values. + + :param kick_power: the value to set the kick power slider to + :param chip_distance: the value to set the chip distance slider to """ - if mode == ControlMode.DIAGNOSTICS: - common_widgets.enable_slider(self.kick_power_slider) - common_widgets.enable_slider(self.chip_distance_slider) - common_widgets.enable_button(self.no_auto_button) - common_widgets.enable_button(self.auto_kick_button) - common_widgets.enable_button(self.auto_chip_button) - self.update_kick_chip_buttons_accessibility() - elif mode == ControlMode.HANDHELD: - common_widgets.disable_slider(self.kick_power_slider) - common_widgets.disable_slider(self.chip_distance_slider) - common_widgets.disable_button(self.no_auto_button) - common_widgets.disable_button(self.auto_kick_button) - common_widgets.disable_button(self.auto_chip_button) - common_widgets.disable_button(self.kick_button) - common_widgets.disable_button(self.chip_button) + self.kick_power_slider.setValue(kick_power) + self.chip_distance_slider.setValue(chip_distance) def refresh(self) -> None: - """Send autokick/autochip commands if enabled""" + """Send out autokick/autochip commands if enabled""" if self.auto_kick_button.isChecked(): self.send_command(ChickerCommandMode.AUTOKICK) elif self.auto_chip_button.isChecked(): self.send_command(ChickerCommandMode.AUTOCHIP) + + def __update_kick_chip_buttons_accessibility(self) -> None: + """Enable or disable the kick/chip buttons depending on whether autokick/autochip is on""" + if ( + self.enabled + and not self.kick_chip_locked + and self.no_auto_button.isChecked() + ): + common_widgets.enable_button(self.kick_button) + common_widgets.enable_button(self.chip_button) + else: + common_widgets.disable_button(self.kick_button) + common_widgets.disable_button(self.chip_button) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py deleted file mode 100644 index abafd0aaae..0000000000 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_input_widget.py +++ /dev/null @@ -1,86 +0,0 @@ -from pyqtgraph.Qt import QtCore -from pyqtgraph.Qt.QtWidgets import * -from software.py_constants import * -import software.thunderscope.common.common_widgets as common_widgets -from enum import IntEnum - -from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( - HandheldDeviceConnectionStatus, -) - - -class ControlMode(IntEnum): - """Enum representing the available control modes in Robot Diagnostics""" - - DIAGNOSTICS = 0 - HANDHELD = 1 - - -class DiagnosticsInputToggleWidget(QWidget): - """Widget for switching between manual (Diagnostics) and Xbox (Handheld) control""" - - control_mode_changed_signal = QtCore.pyqtSignal(ControlMode) - """Signal emitted when the control mode changes""" - - def __init__(self) -> None: - """Initialise the DiagnosticsInputToggleWidget""" - super().__init__() - - diagnostics_input_widget_vbox_layout = QVBoxLayout() - - self.connect_options_group = QButtonGroup() - - self.connect_options_box, self.connect_options = common_widgets.create_radio( - ["Diagnostics Control", "Handheld Control"], self.connect_options_group - ) - - self.connect_options_box.setTitle("Diagnostics Input") - - self.diagnostics_control_button = self.connect_options[ControlMode.DIAGNOSTICS] - self.handheld_control_button = self.connect_options[ControlMode.HANDHELD] - - self.diagnostics_control_button.clicked.connect( - lambda: self.control_mode_changed_signal.emit(ControlMode.DIAGNOSTICS) - ) - self.handheld_control_button.clicked.connect( - lambda: self.control_mode_changed_signal.emit(ControlMode.HANDHELD) - ) - - # default to diagnostics input, and disable handheld - self.handheld_control_button.setEnabled(False) - self.diagnostics_control_button.setChecked(True) - - diagnostics_input_widget_vbox_layout.addWidget(self.connect_options_box) - - self.setLayout(diagnostics_input_widget_vbox_layout) - - def update(self, status: HandheldDeviceConnectionStatus) -> None: - """Update this widget with the current handheld device connection status. - - If the handheld device is connected: - - enables the handheld button - If the handheld device is disconnected: - - disables the handheld control button - - sets the diagnostics button to checked - - emits diagnostics input change signal - - :param status: the current handheld device connection status - """ - if status == HandheldDeviceConnectionStatus.CONNECTED: - self.handheld_control_button.setEnabled(True) - - elif status == HandheldDeviceConnectionStatus.DISCONNECTED: - self.diagnostics_control_button.setChecked(True) - self.handheld_control_button.setEnabled(False) - self.control_mode_changed_signal.emit(ControlMode.DIAGNOSTICS) - - def get_control_mode(self) -> ControlMode: - """Get the currently selected control mode. - - :returns: the currently selected control mode - """ - return ( - ControlMode.DIAGNOSTICS - if self.diagnostics_control_button.isChecked() - else ControlMode.HANDHELD - ) diff --git a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py index 7e99164a99..bd23d99b00 100644 --- a/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py +++ b/src/software/thunderscope/robot_diagnostics/diagnostics_widget.py @@ -1,20 +1,14 @@ from pyqtgraph.Qt.QtWidgets import * from pyqtgraph.Qt.QtCore import * from proto.import_all_protos import * -from software.logger.logger import create_logger from software.thunderscope.proto_unix_io import ProtoUnixIO -from software.thunderscope.robot_diagnostics.chicker_widget import ChickerWidget -from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( - HandheldDeviceStatusView, +from software.thunderscope.robot_diagnostics.chicker_widget import ( + ChickerWidget, + ChickerCommandMode, ) -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ( - DiagnosticsInputToggleWidget, - ControlMode, -) -from software.thunderscope.robot_diagnostics.handheld_device_manager import ( - HandheldDeviceManager, - HandheldDeviceConnectionChangedEvent, +from software.thunderscope.robot_diagnostics.handheld_controller_widget import ( + HandheldControllerWidget, ) from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( DriveAndDribblerWidget, @@ -22,10 +16,10 @@ class DiagnosticsWidget(QScrollArea): - """The DiagnosticsWidget contains all widgets related to Robot Diagnostics: + """The DiagnosticsWidget contains all widgets related to manually + controlling robots in Robot Diagnostics: - - HandheldDeviceStatusWidget - - DiagnosticsInputWidget + - HandheldControllerWidget - DriveAndDribblerWidget - ChickerWidget @@ -39,37 +33,35 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: """ super(DiagnosticsWidget, self).__init__() - self.logger = create_logger("RobotDiagnostics") - self.proto_unix_io = proto_unix_io - self.handheld_device_status_widget = HandheldDeviceStatusView() self.drive_dribbler_widget = DriveAndDribblerWidget(self.proto_unix_io) self.chicker_widget = ChickerWidget(self.proto_unix_io) - self.diagnostics_control_input_widget = DiagnosticsInputToggleWidget() - self.handheld_device_manager = HandheldDeviceManager( - self.logger, - self.proto_unix_io, + self.handheld_controller_widget = HandheldControllerWidget() + self.handheld_controller_widget.kick_button_pressed.connect( + lambda: self.chicker_widget.send_command_and_timeout( + ChickerCommandMode.KICK + ) ) - - self.handheld_device_manager.handheld_device_connection_status_signal.connect( - self.__handheld_device_connection_status_signal_handler + self.handheld_controller_widget.chip_button_pressed.connect( + lambda: self.chicker_widget.send_command_and_timeout( + ChickerCommandMode.CHIP + ) ) - self.diagnostics_control_input_widget.control_mode_changed_signal.connect( - self.__control_mode_changed_signal_handler + self.handheld_controller_widget.setSizePolicy( + QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Minimum ) - - self.handheld_device_status_widget.reinitialize_handheld_device_signal.connect( - self.handheld_device_manager.reinitialize_handheld_device + self.drive_dribbler_widget.setSizePolicy( + QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding + ) + self.chicker_widget.setSizePolicy( + QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding ) - - self.handheld_device_manager.reinitialize_handheld_device() diagnostics_widget_vbox_layout = QVBoxLayout() - diagnostics_widget_vbox_layout.addWidget(self.handheld_device_status_widget) - diagnostics_widget_vbox_layout.addWidget(self.diagnostics_control_input_widget) + diagnostics_widget_vbox_layout.addWidget(self.handheld_controller_widget) diagnostics_widget_vbox_layout.addWidget(self.drive_dribbler_widget) diagnostics_widget_vbox_layout.addWidget(self.chicker_widget) @@ -82,58 +74,29 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.setWidget(self.container) self.setWidgetResizable(True) - def __control_mode_changed_signal_handler(self, mode: ControlMode) -> None: - """Handler for the control_mode_changed_signal emitted by DiagnosticsControlInputWidget - - :param mode: the currently selected control mode - """ - self.handheld_device_manager.set_enabled(mode == ControlMode.HANDHELD) - self.drive_dribbler_widget.update_widget_accessibility(mode) - self.chicker_widget.update_widget_accessibility(mode) - - def __handheld_device_connection_status_signal_handler( - self, event: HandheldDeviceConnectionChangedEvent - ) -> None: - """Handler for the handheld_device_connection_status_signal emitted by HandheldDeviceManager - - :param event: the signal event containing the new handheld device connection status - """ - self.handheld_device_status_widget.update( - event.connection_status, event.device_name - ) - self.diagnostics_control_input_widget.update(event.connection_status) - def refresh(self) -> None: - """Refresh sub-widgets so that they display the most recent status values. - If in handheld mode, visually update driver, dribbler and chicker sliders - to the values currently being set by the handheld device. - """ - control_mode = self.diagnostics_control_input_widget.get_control_mode() + """Call the refresh function on all sub-widgets. + If controller input is enabled in the HandheldControllerWidget, visually update the + DriveAndDribblerWidget and the ChickerWidget to display the motor and chicker values + currently set by the handheld controller. + """ + self.handheld_controller_widget.refresh() + + if self.handheld_controller_widget.controller_input_enabled(): + self.chicker_widget.disable() + self.drive_dribbler_widget.disable() + + self.drive_dribbler_widget.override_slider_values( + self.handheld_controller_widget.motor_control + ) + self.chicker_widget.override_slider_values( + self.handheld_controller_widget.kick_power, + self.handheld_controller_widget.chip_distance, + ) + else: + self.chicker_widget.enable() + self.drive_dribbler_widget.enable() + + self.drive_dribbler_widget.refresh() self.chicker_widget.refresh() - - if control_mode == ControlMode.DIAGNOSTICS: - self.drive_dribbler_widget.refresh() - - elif control_mode == ControlMode.HANDHELD: - with self.handheld_device_manager.lock: - motor_control = self.handheld_device_manager.motor_control - kick_power = self.handheld_device_manager.kick_power - chip_distance = self.handheld_device_manager.chip_distance - - self.drive_dribbler_widget.set_x_velocity_slider( - motor_control.direct_velocity_control.velocity.x_component_meters - ) - self.drive_dribbler_widget.set_y_velocity_slider( - motor_control.direct_velocity_control.velocity.y_component_meters - ) - self.drive_dribbler_widget.set_angular_velocity_slider( - motor_control.direct_velocity_control.angular_velocity.radians_per_second - ) - - self.drive_dribbler_widget.set_dribbler_velocity_slider( - motor_control.dribbler_speed_rpm - ) - - self.chicker_widget.set_kick_power_slider_value(kick_power) - self.chicker_widget.set_chip_distance_slider_value(chip_distance) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index e1c904661b..174328fc03 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -5,13 +5,15 @@ import software.python_bindings as tbots_cpp from software.thunderscope.proto_unix_io import ProtoUnixIO -from software.thunderscope.robot_diagnostics.diagnostics_input_widget import ControlMode from software.thunderscope.common import common_widgets class DriveAndDribblerWidget(QWidget): - """Handles the robot diagnostics input to create a MotorControl message - to be sent to the robots. + """This widget provides an interface for controlling our robots' + drive and dribbler functionalities. It has sliders for manipulating + the direct velocity of the robot's motors as well as the speed of the + dribbler. The slider values are used to create and send MotorControl + messages to the robots. """ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: @@ -23,81 +25,84 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.proto_unix_io = proto_unix_io - self.motor_control = MotorControl() - self.constants = tbots_cpp.create2021RobotConstants() - layout = QVBoxLayout() - # Add widgets to layout - layout.addWidget(self.__setup_direct_velocity()) - layout.addWidget(self.__setup_dribbler()) + self.enabled = True + layout = QVBoxLayout() + layout.addWidget(self.__setup_direct_velocity_widgets()) + layout.addWidget(self.__setup_dribbler_widgets()) self.setLayout(layout) - def set_x_velocity_slider(self, value: float) -> None: - """Set the value of the slider controlling the robot's X velocity + def enable(self) -> None: + """Enable all sliders and buttons in the DriveAndDribblerWidget""" + if self.enabled: + return - :param value: the value to set the slider to in m/s - """ - self.x_velocity_slider.setValue(value) + self.enabled = True - def set_y_velocity_slider(self, value: float) -> None: - """Set the value of the slider controlling the robot's Y velocity + common_widgets.enable_slider(self.x_velocity_slider) + common_widgets.enable_slider(self.y_velocity_slider) + common_widgets.enable_slider(self.angular_velocity_slider) + common_widgets.enable_slider(self.dribbler_speed_rpm_slider) + common_widgets.enable_button(self.stop_and_reset_dribbler) + common_widgets.enable_button(self.stop_and_reset_direct) - :param value: the value to set the slider to in m/s - """ - self.y_velocity_slider.setValue(value) + def disable(self) -> None: + """Disable all sliders and buttons in the DriveAndDribblerWidget""" + if not self.enabled: + return - def set_angular_velocity_slider(self, value: float) -> None: - """Set the value of the slider controlling the robot's angular velocity + self.enabled = False - :param value: the value to set the slider to in rad/s - """ - self.angular_velocity_slider.setValue(value) + self.__reset_all_sliders() + common_widgets.disable_slider(self.x_velocity_slider) + common_widgets.disable_slider(self.y_velocity_slider) + common_widgets.disable_slider(self.angular_velocity_slider) + common_widgets.disable_slider(self.dribbler_speed_rpm_slider) + common_widgets.disable_button(self.stop_and_reset_dribbler) + common_widgets.disable_button(self.stop_and_reset_direct) - def set_dribbler_velocity_slider(self, value: float) -> None: - """Set the value of the slider controlling the robot's dribbler velocity + def override_slider_values(self, motor_control: MotorControl) -> None: + """Set the widget's sliders to match the values in the given MotorControl - :param value: the value to set the slider to in RPM + :param motor_control: the MotorControl values to override the widget's sliders with """ - self.dribbler_speed_rpm_slider.setValue(value) + self.x_velocity_slider.setValue( + motor_control.direct_velocity_control.velocity.x_component_meters + ) + self.y_velocity_slider.setValue( + motor_control.direct_velocity_control.velocity.y_component_meters + ) + self.angular_velocity_slider.setValue( + motor_control.direct_velocity_control.angular_velocity.radians_per_second + ) + self.dribbler_speed_rpm_slider.setValue(motor_control.dribbler_speed_rpm) def refresh(self) -> None: - """Update the currently persisted MotorControl proto based on the widget's slider values - and sends out the proto + """Send out a MotorControl proto with the currently set direct velocity and + dribbler speed values """ - self.motor_control.dribbler_speed_rpm = int( - self.dribbler_speed_rpm_slider.value() - ) - - self.motor_control.direct_velocity_control.velocity.x_component_meters = ( + motor_control = MotorControl() + motor_control.direct_velocity_control.velocity.x_component_meters = ( self.x_velocity_slider.value() ) - - self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + motor_control.direct_velocity_control.velocity.y_component_meters = ( self.y_velocity_slider.value() ) + motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + self.angular_velocity_slider.value() + ) + motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.angular_velocity_slider.value() - - self.proto_unix_io.send_proto(MotorControl, self.motor_control) - - def __value_change_handler(self, value: float) -> str: - """Convert the given float value to a string label - - :param value: float value to be converted - """ - return "%.2f" % float(value) + self.proto_unix_io.send_proto(MotorControl, motor_control) - def __setup_direct_velocity(self) -> QGroupBox: + def __setup_direct_velocity_widgets(self) -> QGroupBox: """Create a widget to control the direct velocity of the robot's motors :returns: a QGroupBox containing sliders and controls for controlling the direct velocity of the robot's motors """ - group_box = QGroupBox("Drive") - dbox = QVBoxLayout() - ( x_layout, self.x_velocity_slider, @@ -132,46 +137,38 @@ def __setup_direct_velocity(self) -> QGroupBox: 1, ) - # add listener functions for sliders to update label with slider value - self.x_velocity_slider.valueChanged.connect( - lambda: self.x_velocity_label.setText( - self.__value_change_handler(self.x_velocity_slider.value()) - ) + self.x_velocity_slider.floatValueChanged.connect( + lambda new_value: self.x_velocity_label.setText("%.2f" % new_value) ) - self.y_velocity_slider.valueChanged.connect( - lambda: self.y_velocity_label.setText( - self.__value_change_handler(self.y_velocity_slider.value()) - ) + self.y_velocity_slider.floatValueChanged.connect( + lambda new_value: self.y_velocity_label.setText("%.2f" % new_value) ) - self.angular_velocity_slider.valueChanged.connect( - lambda: self.angular_velocity_label.setText( - self.__value_change_handler(self.angular_velocity_slider.value()) - ) + self.angular_velocity_slider.floatValueChanged.connect( + lambda new_value: self.angular_velocity_label.setText("%.2f" % new_value) ) self.stop_and_reset_direct = QPushButton("Stop and Reset") self.stop_and_reset_direct.clicked.connect(self.__reset_direct_sliders) - dbox.addLayout(x_layout) - dbox.addLayout(y_layout) - dbox.addLayout(dps_layout) - dbox.addWidget( + vbox = QVBoxLayout() + vbox.addLayout(x_layout) + vbox.addLayout(y_layout) + vbox.addLayout(dps_layout) + vbox.addWidget( self.stop_and_reset_direct, alignment=Qt.AlignmentFlag.AlignCenter ) - group_box.setLayout(dbox) + group_box = QGroupBox("Drive") + group_box.setLayout(vbox) return group_box - def __setup_dribbler(self) -> QGroupBox: + def __setup_dribbler_widgets(self) -> QGroupBox: """Create a widget to control the dribbler speed :returns: a QGroupBox containing a slider and controls for controlling the robot's dribbler speed """ - group_box = QGroupBox("Dribbler") - dbox = QVBoxLayout() - ( dribbler_layout, self.dribbler_speed_rpm_slider, @@ -184,61 +181,32 @@ def __setup_dribbler(self) -> QGroupBox: 1, ) - self.dribbler_speed_rpm_slider.valueChanged.connect( - lambda: self.dribbler_speed_rpm_label.setText( - self.__value_change_handler(self.dribbler_speed_rpm_slider.value()) - ) + self.dribbler_speed_rpm_slider.floatValueChanged.connect( + lambda new_value: self.dribbler_speed_rpm_label.setText("%.2f" % new_value) ) self.stop_and_reset_dribbler = QPushButton("Stop and Reset") self.stop_and_reset_dribbler.clicked.connect(self.__reset_dribbler_slider) - dbox.addLayout(dribbler_layout) - dbox.addWidget( + vbox = QVBoxLayout() + vbox.addLayout(dribbler_layout) + vbox.addWidget( self.stop_and_reset_dribbler, alignment=Qt.AlignmentFlag.AlignCenter ) - group_box.setLayout(dbox) - - return group_box - def update_widget_accessibility(self, mode: ControlMode) -> None: - """Disables or enables all sliders and buttons depending on the given control mode. - Sliders are enabled in DIAGNOSTICS mode, and disabled in HANDHELD mode + group_box = QGroupBox("Dribbler") + group_box.setLayout(vbox) - :param mode: the current control mode - """ - if mode == ControlMode.DIAGNOSTICS: - # enable all sliders by adding listener to update label with slider value - common_widgets.enable_slider(self.x_velocity_slider) - common_widgets.enable_slider(self.y_velocity_slider) - common_widgets.enable_slider(self.angular_velocity_slider) - common_widgets.enable_slider(self.dribbler_speed_rpm_slider) - - # enable buttons - common_widgets.enable_button(self.stop_and_reset_dribbler) - common_widgets.enable_button(self.stop_and_reset_direct) - - elif mode == ControlMode.HANDHELD: - self.__reset_all_sliders() - - # disable all sliders by adding listener to keep slider value the same - common_widgets.disable_slider(self.x_velocity_slider) - common_widgets.disable_slider(self.y_velocity_slider) - common_widgets.disable_slider(self.angular_velocity_slider) - common_widgets.disable_slider(self.dribbler_speed_rpm_slider) - - # disable buttons - common_widgets.disable_button(self.stop_and_reset_dribbler) - common_widgets.disable_button(self.stop_and_reset_direct) + return group_box def __reset_direct_sliders(self) -> None: - """Reset direct sliders back to 0""" + """Reset the direct velocity sliders back to 0""" self.x_velocity_slider.setValue(0) self.y_velocity_slider.setValue(0) self.angular_velocity_slider.setValue(0) def __reset_dribbler_slider(self) -> None: - """Reset the dribbler slider back to 0""" + """Reset the dribbler speed slider back to 0""" self.dribbler_speed_rpm_slider.setValue(0) def __reset_all_sliders(self) -> None: diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller.py b/src/software/thunderscope/robot_diagnostics/handheld_controller.py new file mode 100644 index 0000000000..c8e1bc2121 --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller.py @@ -0,0 +1,86 @@ +import numpy + +from evdev import InputDevice, ecodes +from threading import Thread + + +class HandheldController: + """Represents a handheld game controller or input device that can be used + to manually control our robots. + """ + + def __init__(self, path: str): + """Initialize a HandheldController that reads input events from + the specified device path. + + :param path: the device input path + """ + self.input_device = InputDevice(path) + self.input_values: dict[int, float] = {} + + capabilities = self.input_device.capabilities() + self.supported_keys = set(capabilities[ecodes.EV_KEY]) + self.abs_info = dict(capabilities[ecodes.EV_ABS]) + + self.thread = Thread(target=self.__read_input, daemon=True) + self.thread.start() + + def name(self) -> str: + """Get the device name of the controller. + + :return: the device name + """ + return self.input_device.name + + def connected(self) -> bool: + """Check whether the controller is currently connected. + + :return: true if the controller is connected, false otherwise + """ + return self.input_device is not None + + def key_down(self, key_code: int) -> bool: + """Check whether the given key on the controller is currently pressed down. + + :param key_code: the EV_KEY code of the key to check + :return: true if the key is currently pressed down, false otherwise + """ + if key_code not in self.supported_keys: + return False + + return bool(self.input_values.get(key_code, 0)) + + def abs_value(self, abs_code: int) -> float: + """Get the current value of an absolute axis input on the controller, + normalized to a range of [-1, 1]. + + :param abs_code: the EV_ABS code of the axis to check + :return: the value of the axis input, normalized to [-1, 1] + """ + if abs_code not in self.abs_info: + return 0 + + value_min = self.abs_info[abs_code].min + value_max = self.abs_info[abs_code].max + value = self.input_values.get(abs_code, 0) + + if value >= 0: + return numpy.interp(value, (0, value_max), (0, 1)) + else: + return numpy.interp(value, (value_min, 0), (-1, 0)) + + def close(self) -> None: + """Close the connection to the controller.""" + if self.input_device is not None: + self.input_values.clear() + self.input_device.close() + self.input_device = None + + def __read_input(self) -> None: + """Endless loop that reads input events from the controller.""" + try: + for event in self.input_device.read_loop(): + if event.type == ecodes.EV_KEY or event.type == ecodes.EV_ABS: + self.input_values[event.code] = event.value + except: + self.close() diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py new file mode 100644 index 0000000000..8772ced375 --- /dev/null +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -0,0 +1,205 @@ +import numpy + +import evdev +from evdev import ecodes + +from proto.import_all_protos import * +from pyqtgraph.Qt.QtWidgets import * +from pyqtgraph.Qt import QtCore + + +import software.python_bindings as tbots_cpp + +from software.py_constants import * +from software.thunderscope.constants import DiagnosticsConstants +from software.thunderscope.robot_diagnostics.handheld_controller import ( + HandheldController, +) + + +class HandheldControllerWidget(QWidget): + """This widget lets the user detect and use a connected handheld controller + to manually control our robots. The user can toggle whether controller input + is enabled or disabled. + """ + + kick_button_pressed = QtCore.pyqtSignal() + """Signal emitted when the kick button is pressed on the controller""" + + chip_button_pressed = QtCore.pyqtSignal() + """Signal emitted when the chip button is pressed on the controller""" + + def __init__(self) -> None: + """Initialize the HandheldControllerWidget.""" + super().__init__() + + self.constants = tbots_cpp.create2021RobotConstants() + + self.handheld_controller: HandheldController | None = None + + self.last_d_pad_axis_x_value = 0 + self.last_d_pad_axis_y_value = 0 + self.last_btn_a_value = False + self.last_btn_b_value = False + + self.motor_control = MotorControl() + self.dribbler_speed = 0 + self.kick_power = DiagnosticsConstants.MIN_KICK_POWER + self.chip_distance = DiagnosticsConstants.MIN_CHIP_POWER + + widget_layout = QVBoxLayout() + widget_layout.addWidget(self.__create_widgets()) + self.setLayout(widget_layout) + + self.detect_controller() + + def detect_controller(self) -> None: + """Scan through the list of currently connected devices for a supported + handheld controller and, if one is found, set it as the device to accept + controller inputs from. + """ + self.handheld_controller = None + + for path in evdev.list_devices(): + device = evdev.InputDevice(path) + if device.name in DiagnosticsConstants.SUPPORTED_CONTROLLERS: + self.handheld_controller = HandheldController(path) + break + + self.__update_controller_status() + + def controller_input_enabled(self) -> bool: + """Check whether controller input is enabled. + + :return: true if controller input is enabled, false otherwise + """ + return self.enable_input_checkbox.isChecked() + + def refresh(self) -> None: + if self.handheld_controller is None: + return + + if not self.handheld_controller.connected(): + self.handheld_controller = None + self.__update_controller_status() + return + + if self.enable_input_checkbox.isChecked(): + self.__read_controller_inputs() + + def __create_widgets(self) -> QGroupBox: + """Create the widgets that make up the HandheldControllerWidget UI. + + :return: a QGroupBox containing: + - a QLabel for displaying the current controller connection status + - a QPushButton that tries detecting a controller when pressed + - a QCheckBox that controls whether controller input is enabled + """ + self.controller_status_label = QLabel() + self.controller_status_label.setAlignment(QtCore.Qt.AlignmentFlag.AlignCenter) + + self.detect_controller_button = QPushButton("Detect Controller") + self.detect_controller_button.clicked.connect(self.detect_controller) + + self.enable_input_checkbox = QCheckBox("Enable Input") + self.enable_input_checkbox.setChecked(False) + self.enable_input_checkbox.setEnabled(True) + + grid_layout = QGridLayout() + grid_layout.addWidget(self.controller_status_label, 0, 0, 2, 1) + grid_layout.addWidget(self.detect_controller_button, 0, 1) + grid_layout.addWidget( + self.enable_input_checkbox, 1, 1, QtCore.Qt.AlignmentFlag.AlignHCenter + ) + grid_layout.setColumnStretch(0, 4) + grid_layout.setColumnStretch(1, 1) + + box = QGroupBox() + box.setTitle("Controller Status") + box.setLayout(grid_layout) + + return box + + def __read_controller_inputs(self) -> None: + def with_deadzone(abs_value: float) -> float: + return ( + abs_value + if abs(abs_value) >= DiagnosticsConstants.DEADZONE_PERCENTAGE + else 0 + ) + + move_axis_x = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_X)) + move_axis_y = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_Y)) + move_axis_rot = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_RX)) + + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( + move_axis_x * self.constants.robot_max_speed_m_per_s + ) + self.motor_control.direct_velocity_control.velocity.y_component_meters = ( + -move_axis_y * self.constants.robot_max_speed_m_per_s + ) + self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( + -move_axis_rot * self.constants.robot_max_ang_speed_rad_per_s + ) + + d_pad_axis_x = self.handheld_controller.abs_value(ecodes.ABS_HAT0X) + d_pad_axis_y = self.handheld_controller.abs_value(ecodes.ABS_HAT0Y) + + if d_pad_axis_x != self.last_d_pad_axis_x_value: + self.last_d_pad_axis_x_value = d_pad_axis_x + self.kick_power = numpy.clip( + a=self.kick_power + + int(d_pad_axis_x) * DiagnosticsConstants.KICK_POWER_STEPPER, + a_min=DiagnosticsConstants.MIN_KICK_POWER, + a_max=DiagnosticsConstants.MAX_KICK_POWER, + ) + self.chip_distance = numpy.clip( + a=self.chip_distance + + d_pad_axis_x * DiagnosticsConstants.CHIP_DISTANCE_STEPPER, + a_min=DiagnosticsConstants.MIN_CHIP_POWER, + a_max=DiagnosticsConstants.MAX_CHIP_POWER, + ) + + if d_pad_axis_y != self.last_d_pad_axis_y_value: + self.last_d_pad_axis_y_value = d_pad_axis_y + self.dribbler_speed = int( + numpy.clip( + a=self.dribbler_speed + - d_pad_axis_y * DiagnosticsConstants.DRIBBLER_RPM_STEPPER, + a_min=self.constants.indefinite_dribbler_speed_rpm, + a_max=-self.constants.indefinite_dribbler_speed_rpm, + ) + ) + + right_trigger_value = self.handheld_controller.abs_value(ecodes.ABS_RZ) + if right_trigger_value > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD: + self.motor_control.dribbler_speed_rpm = self.dribbler_speed + else: + self.motor_control.dribbler_speed_rpm = 0 + + btn_a = self.handheld_controller.key_down(ecodes.BTN_A) + btn_b = self.handheld_controller.key_down(ecodes.BTN_B) + + if btn_a != self.last_btn_a_value: + self.last_btn_a_value = btn_a + if btn_a: + self.kick_button_pressed.emit() + + if btn_b != self.last_btn_b_value: + self.last_btn_b_value = btn_b + if btn_b: + self.chip_button_pressed.emit() + + def __update_controller_status(self) -> None: + """Update the widget to display the current controller connection status.""" + if self.handheld_controller and self.handheld_controller.connected(): + self.controller_status_label.setText( + f"Connected: {self.handheld_controller.name()}" + ) + self.controller_status_label.setStyleSheet("background-color: green") + self.enable_input_checkbox.setEnabled(True) + else: + self.controller_status_label.setText("Disconnected") + self.controller_status_label.setStyleSheet("background-color: red") + self.enable_input_checkbox.setChecked(False) + self.enable_input_checkbox.setEnabled(False) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py b/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py deleted file mode 100644 index 8095e682ff..0000000000 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_manager.py +++ /dev/null @@ -1,348 +0,0 @@ -import time -from logging import Logger -from threading import Thread, RLock - -import numpy -from evdev import InputDevice, ecodes, list_devices, InputEvent -from pyqtgraph.Qt import QtCore - -import software.python_bindings as tbots_cpp -from proto.import_all_protos import * -from software.thunderscope.constants import * -from software.thunderscope.constants import DiagnosticsConstants -from software.thunderscope.proto_unix_io import ProtoUnixIO -from software.thunderscope.robot_diagnostics.handheld_device_status_view import ( - HandheldDeviceConnectionStatus, -) - -from dataclasses import dataclass - - -@dataclass -class HandheldDeviceConnectionChangedEvent: - connection_status: HandheldDeviceConnectionStatus - """The new handheld device connection status""" - - device_name: str | None - """The name of the connected handheld device""" - - -class HandheldDeviceManager(QtCore.QObject): - """This class is responsible for managing the connection between a computer and a - handheld device to control robots. This class relies on the `evdev` python package - in order to implement parsing and control flow for device events. - See: https://python-evdev.readthedocs.io/en/latest/apidoc.html - """ - - handheld_device_connection_status_signal = QtCore.pyqtSignal( - HandheldDeviceConnectionChangedEvent - ) - """Signal emitted when the handheld device connection status changes""" - - def __init__( - self, - logger: Logger, - proto_unix_io: ProtoUnixIO, - ): - """Initialize the HandheldDeviceManager - - :param logger: the logger to use - :param proto_unix_io: ProtoUnixIO to send messages to the robot - """ - super().__init__() - - self.lock = RLock() - - self.logger = logger - self.proto_unix_io = proto_unix_io - - self.enabled = False - - self.handheld_device: Optional[InputDevice] = None - self.handheld_device_config: Optional[DeviceConfig] = None - - self.__initialize_default_motor_control_values() - self.__initialize_default_power_control_values() - - # The following fields are here to temporarily persist device input. - # They are read once certain buttons are pressed on the handheld device, - # and the values are set for the control primitives above. - self.kick_power: float = DiagnosticsConstants.MIN_KICK_POWER - self.chip_distance: float = DiagnosticsConstants.MIN_CHIP_POWER - self.dribbler_speed: int = 0 - self.dribbler_running: bool = False - - self.constants = tbots_cpp.create2021RobotConstants() - - self.event_loop_thread = Thread(target=self.__event_loop, daemon=True) - self.event_loop_thread.start() - - def __initialize_default_motor_control_values(self) -> None: - """Set all required fields in the motor control proto to their - default/minimum values - """ - self.motor_control = MotorControl() - self.motor_control.direct_velocity_control.velocity.x_component_meters = 0.0 - self.motor_control.direct_velocity_control.velocity.y_component_meters = 0.0 - self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = 0.0 - self.motor_control.dribbler_speed_rpm = 0 - - def __initialize_default_power_control_values(self) -> None: - """Set all required fields in the power control proto to their - default/minimum values - """ - self.power_control = PowerControl() - self.power_control.geneva_slot = 3 - - def set_enabled(self, enabled) -> None: - """Enable or disable the HandheldDeviceManager from processing input events. - - :param enabled: whether to enable (True) or disable (False) the - HandheldDeviceManager from processing input events - """ - with self.lock: - self.enabled = enabled - - def reinitialize_handheld_device(self) -> None: - """Attempt to reinitialize a connection to a handheld device.""" - self.__clear_handheld_device() - self.__initialize_handheld_device() - - def __initialize_handheld_device(self) -> None: - """Attempt to initialize a connection to a handheld device. - The first handheld device that is recognized as valid will be used. - Valid handheld devices are any devices whose name has a matching DeviceConfig - in DiagnosticsConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP. - """ - device_config_map = DiagnosticsConstants.HANDHELD_DEVICE_NAME_CONFIG_MAP - - for device in list_devices(): - handheld_device = InputDevice(device) - if ( - handheld_device is not None - and handheld_device.name in device_config_map - ): - with self.lock: - self.handheld_device = handheld_device - self.handheld_device_config = device_config_map[ - handheld_device.name - ] - - self.handheld_device_connection_status_signal.emit( - HandheldDeviceConnectionChangedEvent( - HandheldDeviceConnectionStatus.CONNECTED, - self.handheld_device.name, - ) - ) - - self.logger.debug("Successfully initialized handheld!") - self.logger.debug( - 'Device name: "' + self.handheld_device.name + '"' - ) - self.logger.debug("Device path: " + self.handheld_device.path) - - return - - self.__clear_handheld_device() - self.logger.debug("Failed to initialize handheld device, check USB connection") - self.logger.debug("Tried the following available devices:") - self.logger.debug(list(map(lambda d: InputDevice(d).name, list_devices()))) - - def __clear_handheld_device(self) -> None: - """Clear the handheld device and config fields by setting them to None, - and emit a disconnected notification signal. - """ - with self.lock: - self.handheld_device = None - self.handheld_device_config = None - - self.handheld_device_connection_status_signal.emit( - HandheldDeviceConnectionChangedEvent( - HandheldDeviceConnectionStatus.DISCONNECTED, None - ) - ) - - def __event_loop(self) -> None: - """Loop that continuously reads and processes events from the connected handheld device.""" - while True: - with self.lock: - self.__read_and_process_event() - time.sleep(DiagnosticsConstants.EVENT_LOOP_SLEEP_DURATION) - - def __read_and_process_event(self) -> None: - """Try reading an event from the connected handheld device and process it. - - This method is not thread-safe. Callers must acquire self.lock beforehand - to guarantee exclusive access to the HandheldDeviceManager's mutable state. - """ - if not self.handheld_device: - return - - try: - event = self.handheld_device.read_one() - - # All events accumulate into a file and will be read back eventually, - # even if handheld mode is disabled. This time based recency check ensures that - # only the events that have occurred very recently are processed, and - # any events that occur before switching to handheld mode are dropped & ignored - if self.enabled and event: - time_since_event = time.time() - event.timestamp() - if time_since_event < DiagnosticsConstants.INPUT_DELAY_THRESHOLD: - self.__process_event(event) - - motor_control = MotorControl() - power_control = PowerControl() - motor_control.CopyFrom(self.motor_control) - power_control.CopyFrom(self.power_control) - self.proto_unix_io.send_proto(MotorControl, motor_control) - self.proto_unix_io.send_proto(PowerControl, power_control) - - self.__initialize_default_power_control_values() - - except OSError as ose: - self.__clear_handheld_device() - self.logger.debug( - "Caught an OSError while reading handheld handheld device event loop!" - ) - self.logger.debug("Error message: " + str(ose)) - self.logger.debug("Check physical handheld handheld device USB connection!") - - except Exception as e: - self.__clear_handheld_device() - self.logger.critical( - "Caught an unexpected error while reading handheld handheld device event loop!" - ) - self.logger.critical("Error message: " + str(e)) - - def __process_event(self, event: InputEvent) -> None: - """Process the given device event. Set the corresponding motor and power control values - based on the event type, using the current config set in self.handheld_device_config - - This method is not thread-safe. Callers must acquire self.lock beforehand - to guarantee exclusive access to the HandheldDeviceManager's mutable state. - - :param event: The input event to process - """ - config = self.handheld_device_config - - if event.type == ecodes.EV_ABS: - if event.code == config.move_x.event_code: - velocity = self.motor_control.direct_velocity_control.velocity - velocity.x_component_meters = self.__interpret_move_event_value( - event.value, - config.move_x.max_value, - self.constants.robot_max_speed_m_per_s, - ) - - elif event.code == config.move_y.event_code: - velocity = self.motor_control.direct_velocity_control.velocity - velocity.y_component_meters = self.__interpret_move_event_value( - -event.value, - config.move_y.max_value, - self.constants.robot_max_speed_m_per_s, - ) - - elif event.code == config.move_rot.event_code: - angular_velocity = ( - self.motor_control.direct_velocity_control.angular_velocity - ) - angular_velocity.radians_per_second = self.__interpret_move_event_value( - -event.value, - config.move_rot.max_value, - self.constants.robot_max_ang_speed_rad_per_s, - ) - - elif event.code == config.chicker_power.event_code: - self.kick_power = self.__interpret_kick_event_value(event.value) - self.chip_distance = self.__interpret_chip_event_value(event.value) - - elif event.code == config.dribbler_speed.event_code: - self.dribbler_speed = self.__interpret_dribbler_speed_event_value( - event.value - ) - - elif event.code == config.dribbler_enable.event_code: - self.dribbler_running = self.__interpret_dribbler_enabled_event_value( - event.value, - config.dribbler_enable.max_value, - ) - - elif event.type == ecodes.EV_KEY: - if event.code == config.kick.event_code and event.value == 1: - self.power_control.chicker.kick_speed_m_per_s = self.kick_power - - elif event.code == config.chip.event_code and event.value == 1: - self.power_control.chicker.chip_distance_meters = self.chip_distance - - self.motor_control.dribbler_speed_rpm = ( - self.dribbler_speed if self.dribbler_running else 0 - ) - - def __interpret_move_event_value( - self, event_value: float, max_value: float, normalizing_multiplier: float - ) -> float: - """Interpret the event_value that corresponds to movement control - - :param event_value: the value for the current event being interpreted - :param max_value: max value for this type event type - :param normalizing_multiplier: multiplier for converting between the event value and the - robot movement control value - :return: The interpreted value that can be set a value for a field in robot movement control - """ - relative_value = event_value / max_value - if abs(relative_value) < DiagnosticsConstants.DEADZONE_PERCENTAGE: - return 0 - else: - return relative_value * normalizing_multiplier - - def __interpret_dribbler_enabled_event_value( - self, event_value: float, max_value: float - ) -> bool: - """Interpret the event_value that corresponds to controlling whether the dribbler is enabled - - :param event_value: the value for the current event being interpreted - :param max_value: max value for this event type - :return: The interpreted value that can be set a value for a field in robot control - """ - button_percent_pressed = event_value / max_value - return button_percent_pressed > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD - - def __interpret_dribbler_speed_event_value(self, event_value: float) -> int: - """Interpret the event value that corresponds to controlling the dribbler speed. - - :param event_value: the value for the current event being interpreted - :return: the interpreted value to be used for the new dribbler speed on the robot - """ - return int( - numpy.clip( - a=self.dribbler_speed - - event_value * DiagnosticsConstants.DRIBBLER_RPM_STEPPER, - a_min=self.constants.indefinite_dribbler_speed_rpm, - a_max=-self.constants.indefinite_dribbler_speed_rpm, - ) - ) - - def __interpret_kick_event_value(self, event_value: float) -> float: - """Interpret the event value that corresponds to controlling the dribbler speed. - - :param event_value: the value for the current event being interpreted - :return: the interpreted value to be used for the kick power on the robot - """ - return numpy.clip( - a=self.kick_power + event_value * DiagnosticsConstants.KICK_POWER_STEPPER, - a_min=DiagnosticsConstants.MIN_KICK_POWER, - a_max=DiagnosticsConstants.MAX_KICK_POWER, - ) - - def __interpret_chip_event_value(self, event_value: float) -> float: - """Interpret the event value that corresponds to controlling the chip distance. - - :param value: the value for the current event being interpreted - :return: the interpreted value to be used for the chip distance on the robot - """ - return numpy.clip( - a=self.chip_distance - + event_value * DiagnosticsConstants.CHIP_DISTANCE_STEPPER, - a_min=DiagnosticsConstants.MIN_CHIP_POWER, - a_max=DiagnosticsConstants.MAX_CHIP_POWER, - ) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py b/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py deleted file mode 100644 index 4c157f3b12..0000000000 --- a/src/software/thunderscope/robot_diagnostics/handheld_device_status_view.py +++ /dev/null @@ -1,73 +0,0 @@ -from enum import Enum - - -from proto.import_all_protos import * -from pyqtgraph.Qt import QtCore -from pyqtgraph.Qt.QtWidgets import * -from pyqtgraph.Qt.QtCore import * - -from software.py_constants import * - - -class HandheldDeviceConnectionStatus(Enum): - """Enum representing whether a handheld device is connected or disconnected""" - - CONNECTED = 1 - DISCONNECTED = 2 - - -class HandheldDeviceStatusView(QWidget): - """Widget that shows the user the current state of the connection with a handheld device, - as well as a button that attempts to detect and reinitialize a handheld device when pressed - """ - - reinitialize_handheld_device_signal = QtCore.pyqtSignal() - """Signal emitted when the "Detect Handheld Device" button is pressed""" - - def __init__(self) -> None: - """Initialize the HandheldDeviceStatusView""" - super(HandheldDeviceStatusView, self).__init__() - - self.handheld_device_status = QLabel() - self.handheld_device_status.setAlignment(Qt.AlignmentFlag.AlignCenter) - - self.detect_handheld_device_button = QPushButton() - self.detect_handheld_device_button.setText("Detect Handheld Device") - self.detect_handheld_device_button.setSizePolicy( - QSizePolicy.Policy.Expanding, - QSizePolicy.Policy.Expanding, - ) - self.detect_handheld_device_button.clicked.connect( - self.reinitialize_handheld_device_signal - ) - - box = QGroupBox() - hbox_layout = QHBoxLayout() - box.setTitle("Handheld Device Status") - widget_layout = QVBoxLayout() - - hbox_layout.addWidget(self.handheld_device_status) - hbox_layout.addWidget(self.detect_handheld_device_button) - hbox_layout.setStretch(0, 4) - hbox_layout.setStretch(1, 1) - - box.setLayout(hbox_layout) - widget_layout.addWidget(box) - self.setLayout(widget_layout) - - self.update(HandheldDeviceConnectionStatus.DISCONNECTED, None) - - def update( - self, connection_status: HandheldDeviceConnectionStatus, device_name: str | None - ) -> None: - """Sets the label to display the correct status depending on the connection status - - :param connection_status: the connection status to display - :param device_name: the name of the handheld device that was connected - """ - if connection_status == HandheldDeviceConnectionStatus.CONNECTED: - self.handheld_device_status.setText(f"Connected: {device_name}") - self.handheld_device_status.setStyleSheet("background-color: green") - elif connection_status == HandheldDeviceConnectionStatus.DISCONNECTED: - self.handheld_device_status.setText("Disconnected") - self.handheld_device_status.setStyleSheet("background-color: red") diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index cdff828fe8..a9110c2baa 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -338,7 +338,6 @@ def setup_diagnostics_widget( """Setup the diagnostics widget, which contains the following sub-widgets: - ChickerWidget - DriveAndDribblerWidget - - DiagnosticsInputWidget - ControllerStatusViewWidget :param proto_unix_io: The proto unix io object From 2cf5afcfa25ca6a5ffe8dbd84fc84b69b4b43fef Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 8 Oct 2024 21:16:36 -0700 Subject: [PATCH 136/138] Update doc --- .../thunderscope/widget_setup_functions.py | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/software/thunderscope/widget_setup_functions.py b/src/software/thunderscope/widget_setup_functions.py index a9110c2baa..ae82fc89a4 100644 --- a/src/software/thunderscope/widget_setup_functions.py +++ b/src/software/thunderscope/widget_setup_functions.py @@ -37,9 +37,6 @@ from software.thunderscope.play.playinfo_widget import PlayInfoWidget from software.thunderscope.play.refereeinfo_widget import RefereeInfoWidget from software.thunderscope.robot_diagnostics.diagnostics_widget import DiagnosticsWidget -from software.thunderscope.robot_diagnostics.drive_and_dribbler_widget import ( - DriveAndDribblerWidget, -) from software.thunderscope.robot_diagnostics.robot_view import RobotView from software.thunderscope.robot_diagnostics.robot_error_log import RobotErrorLog from software.thunderscope.robot_diagnostics.estop_view import EstopView @@ -332,18 +329,12 @@ def setup_estop_view(proto_unix_io: ProtoUnixIO) -> EstopView: return estop_view -def setup_diagnostics_widget( - proto_unix_io: ProtoUnixIO, -) -> DriveAndDribblerWidget: - """Setup the diagnostics widget, which contains the following sub-widgets: - - ChickerWidget - - DriveAndDribblerWidget - - ControllerStatusViewWidget +def setup_diagnostics_widget(proto_unix_io: ProtoUnixIO) -> DiagnosticsWidget: + """Set up the diagnostics widget that provides an interface for manually + controlling our robots - :param proto_unix_io: The proto unix io object - :returns: The diagnostics widget that contains the control input switch, - drive & dribbler sliders, chicker control and controller handler + :param proto_unix_io: ProtoUnixIO for sending messages to the robot + :returns: the diagnostics widget """ diagnostics_widget = DiagnosticsWidget(proto_unix_io) - return diagnostics_widget From 0e1c0f1eaa50a2aaa82a1d768bca8283bdd55222 Mon Sep 17 00:00:00 2001 From: williamckha Date: Tue, 8 Oct 2024 21:24:48 -0700 Subject: [PATCH 137/138] Update doc --- .../robot_diagnostics/handheld_controller_widget.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py index 8772ced375..9ef48f4089 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -121,6 +121,8 @@ def __create_widgets(self) -> QGroupBox: return box def __read_controller_inputs(self) -> None: + """Read and interpret the current controller input values.""" + def with_deadzone(abs_value: float) -> float: return ( abs_value From b4d1b4612062af207e1ec0eddae0bc118e8e5cdf Mon Sep 17 00:00:00 2001 From: williamckha Date: Sat, 2 Nov 2024 13:59:19 -0700 Subject: [PATCH 138/138] Press left trigger to drive slowly --- src/software/thunderscope/constants.py | 2 ++ .../handheld_controller_widget.py | 26 ++++++++++++------- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/software/thunderscope/constants.py b/src/software/thunderscope/constants.py index a8f967eace..cdba7dd82c 100644 --- a/src/software/thunderscope/constants.py +++ b/src/software/thunderscope/constants.py @@ -313,6 +313,8 @@ class DiagnosticsConstants: BUTTON_PRESSED_THRESHOLD = 0.5 DEADZONE_PERCENTAGE = 0.20 + SPEED_SLOWDOWN_FACTOR = 0.25 + DRIBBLER_RPM_STEPPER = 1000 KICK_POWER_STEPPER = 1 diff --git a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py index 9ef48f4089..ba9473509e 100644 --- a/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py +++ b/src/software/thunderscope/robot_diagnostics/handheld_controller_widget.py @@ -130,18 +130,25 @@ def with_deadzone(abs_value: float) -> float: else 0 ) - move_axis_x = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_X)) - move_axis_y = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_Y)) + move_axis_x = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_Y)) + move_axis_y = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_X)) move_axis_rot = with_deadzone(self.handheld_controller.abs_value(ecodes.ABS_RX)) + left_trigger_value = self.handheld_controller.abs_value(ecodes.ABS_Z) + speed_factor = ( + DiagnosticsConstants.SPEED_SLOWDOWN_FACTOR + if left_trigger_value > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD + else 1 + ) + self.motor_control.direct_velocity_control.velocity.x_component_meters = ( - move_axis_x * self.constants.robot_max_speed_m_per_s + -move_axis_x * self.constants.robot_max_speed_m_per_s * speed_factor ) self.motor_control.direct_velocity_control.velocity.y_component_meters = ( - -move_axis_y * self.constants.robot_max_speed_m_per_s + -move_axis_y * self.constants.robot_max_speed_m_per_s * speed_factor ) self.motor_control.direct_velocity_control.angular_velocity.radians_per_second = ( - -move_axis_rot * self.constants.robot_max_ang_speed_rad_per_s + -move_axis_rot * self.constants.robot_max_ang_speed_rad_per_s * speed_factor ) d_pad_axis_x = self.handheld_controller.abs_value(ecodes.ABS_HAT0X) @@ -174,10 +181,11 @@ def with_deadzone(abs_value: float) -> float: ) right_trigger_value = self.handheld_controller.abs_value(ecodes.ABS_RZ) - if right_trigger_value > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD: - self.motor_control.dribbler_speed_rpm = self.dribbler_speed - else: - self.motor_control.dribbler_speed_rpm = 0 + self.motor_control.dribbler_speed_rpm = ( + self.dribbler_speed + if right_trigger_value > DiagnosticsConstants.BUTTON_PRESSED_THRESHOLD + else 0 + ) btn_a = self.handheld_controller.key_down(ecodes.BTN_A) btn_b = self.handheld_controller.key_down(ecodes.BTN_B)