From 5e43559721a29608e71720f6fd696f13604a4b40 Mon Sep 17 00:00:00 2001 From: Jonathan Liu Date: Sun, 8 Oct 2023 20:58:51 -0700 Subject: [PATCH 1/2] add auto dock --- src/main/cpp/Drive/AutoDock.cpp | 207 ++++++++++++++++++++++++ src/main/include/Drive/AutoDock.h | 56 +++++++ src/main/include/Drive/DriveConstants.h | 11 ++ 3 files changed, 274 insertions(+) create mode 100644 src/main/cpp/Drive/AutoDock.cpp create mode 100644 src/main/include/Drive/AutoDock.h diff --git a/src/main/cpp/Drive/AutoDock.cpp b/src/main/cpp/Drive/AutoDock.cpp new file mode 100644 index 0000000..807106c --- /dev/null +++ b/src/main/cpp/Drive/AutoDock.cpp @@ -0,0 +1,207 @@ +#include "Drive/AutoDock.h" + +#include +#include + +#include "Drive/DriveConstants.h" + +/** + * Constructor + * + * @param isRed If robot is on red side +*/ +AutoDock::AutoDock(bool isRed) + : m_isRed{isRed}, m_curState{NOT_DOCKING}, m_kTilt{AutoConstants::KTILT}, m_preDockSpeed{AutoConstants::PRE_DOCK_SPEED}, + m_maxDockSpeed{AutoConstants::MAX_DOCK_SPEED}, m_preDockAng{AutoConstants::PRE_DOCK_ANG}, + m_dockAng{AutoConstants::DOCK_ANG}, m_dockedTol{AutoConstants::DOCKED_TOL}, m_roll{0}, m_pitch{0}, m_yaw{0} {} + +/** + * Gets current state + * + * @returns current state +*/ +AutoDock::State AutoDock::GetState() { + return m_curState; +} + +/** + * Gets current wheel velocity + * + * @returns wheel velocity +*/ +vec::Vector2D AutoDock::GetVel() { + if (m_curState == NOT_DOCKING || m_curState == DOCKED) { + return {0, 0}; + } + + return m_outputVel; +} + +/** + * Updates odometry + * + * @param r Roll in rad + * @param p pitch in rad + * @param y Yaw in rad + * + * @note parameters should account for roll and pitch offsets (based on how navx is mounted) +*/ +void AutoDock::UpdateOdom(double r, double p, double y) { + m_roll = r; + m_pitch = p; + m_yaw = y; +} + +/** + * Sets tilt constant of porportionality + * + * @param kTilt tilt constant +*/ +void AutoDock::SetkTilt(double kTilt) { + m_kTilt = kTilt; +} + +/** + * Sets pre-dock approach speed + * + * @param preDockSpeed pre-dock approach speed +*/ +void AutoDock::SetPreDockSpeed(double preDockSpeed) { + m_preDockSpeed = preDockSpeed; +} + +/** + * Sets maximum docking speed + * + * This speed is used for the robot to actually get from ground fully + * onto charge station + * + * @param maxDockSpeed maximum docking sped +*/ +void AutoDock::SetMaxDockSpeed(double maxDockSpeed) { + m_maxDockSpeed = maxDockSpeed; +} + +/** + * Starts docking +*/ +void AutoDock::Start() { + if (m_curState != NOT_DOCKING) { + return; + } + + m_curState = PRE_DOCK; +} + +/** + * Resets docking state +*/ +void AutoDock::Reset() { + m_curState = NOT_DOCKING; +} + +/** + * Sets pre-dock angle + * + * This is angle where robot is touching and lined up against charge station, + * ready to go max docking speed to get onto chaarge station + * + * @param preDockAng pre-dock angle +*/ +void AutoDock::SetPreDockAng(double preDockAng) { + m_preDockAng = preDockAng; +} + +/** + * Sets dock angle + * + * This is the angle where robot is fully on charge station + * + * @param dockAng Dock angle +*/ +void AutoDock::SetDockAng(double dockAng) { + m_dockAng = dockAng; +} + +/** + * Sets dock tolerance + * + * This is the angle below which robot is considered fully docked + * and can lock wheels + * + * @param dockedTol +*/ +void AutoDock::SetDockedTol(double dockedTol) { + m_dockedTol = dockedTol; +} + +/** + * Sets side + * + * @param isRed true if robot on red side, false if on blue +*/ +void AutoDock::SetSide(bool isRed) { + m_isRed = isRed; +} + +/** + * Periodic function +*/ +void AutoDock::Periodic() { + double tilt = GetTilt(); + double sign = m_isRed ? 1 : -1; + + switch (m_curState) + { + case NOT_DOCKING: + m_outputVel = {0, 0}; + + break; + case PRE_DOCK: + if (std::abs(tilt) > m_preDockAng) { + m_curState = TOUCH_STN; + } + + m_outputVel = {sign * m_preDockSpeed, 0}; + + break; + case TOUCH_STN: + if (std::abs(tilt) > m_dockAng) { + m_curState = ON_STN; + } + + m_outputVel = {sign * m_maxDockSpeed, 0}; + + break; + case ON_STN: + if (std::abs(tilt) < m_dockedTol) { + m_curState = DOCKED; + } + + m_outputVel = {sign * m_kTilt * std::abs(tilt)}; + + break; + case DOCKED: + m_outputVel = {0, 0}; + break; + default: + break; + } +} + +/** + * Gets tilt from roll, pitch, yaw + * + * Credit: https://www.reddit.com/r/askmath/comments/107g72d/tilt_angle_when_pitch_roll_is_known/ + * + * I'm not sure if this works + * + * @returns Tilt +*/ +double AutoDock::GetTilt() { + // double x = -std::sin(m_yaw) * std::cos(m_roll); + // double y = std::cos(m_yaw) * std::cos(m_pitch) - std::sin(m_yaw) * std::sin(m_pitch) * std::sin(m_roll); + double z = std::cos(m_yaw) * std::sin(m_pitch) + std::sin(m_yaw) * std::cos(m_pitch) * std::sin(m_roll); + + return std::asin(std::clamp(z, -1.0, 1.0)); +} diff --git a/src/main/include/Drive/AutoDock.h b/src/main/include/Drive/AutoDock.h new file mode 100644 index 0000000..4b1cff4 --- /dev/null +++ b/src/main/include/Drive/AutoDock.h @@ -0,0 +1,56 @@ +#pragma once + +#include "Util/thirdparty/simplevectors.hpp" + +namespace vec = svector; + +/** + * Autodocking + * + * Assumes autodocking from opposite side of drivers +*/ +class AutoDock { +public: + enum State { + NOT_DOCKING, + PRE_DOCK, + TOUCH_STN, + ON_STN, + DOCKED + }; + + AutoDock(bool isRed = false); + + State GetState(); + vec::Vector2D GetVel(); + + void UpdateOdom(double r, double p, double y); + void SetkTilt(double kTilt); + void SetPreDockSpeed(double preDockSpeed); + void SetMaxDockSpeed(double maxDockSpeed); + void SetPreDockAng(double preDockAng); + void SetDockAng(double dockAng); + void SetDockedTol(double dockedTol); + void SetSide(bool isRed); + void Start(); + void Reset(); + void Periodic(); + +private: + State m_curState; + + bool m_isRed; + + double m_kTilt; + double m_preDockSpeed; + double m_maxDockSpeed; + + double m_preDockAng; + double m_dockAng; + double m_dockedTol; + + double m_roll, m_pitch, m_yaw; + vec::Vector2D m_outputVel; + + double GetTilt(); +}; \ No newline at end of file diff --git a/src/main/include/Drive/DriveConstants.h b/src/main/include/Drive/DriveConstants.h index 4cd1760..a0718ad 100644 --- a/src/main/include/Drive/DriveConstants.h +++ b/src/main/include/Drive/DriveConstants.h @@ -138,4 +138,15 @@ namespace OdometryConstants { const double POS_STD_DEV = 0.1; const double MEASURE_STD_DEV = 0.1; const double CAMERA_TRUST_K = -10.0; +} + +namespace AutoConstants { + const double PRE_DOCK_SPEED = 0; + const double MAX_DOCK_SPEED = 0; + + const double PRE_DOCK_ANG = 5 * M_PI / 180; + const double DOCK_ANG = 12 * M_PI / 180; + const double DOCKED_TOL = 12 * M_PI / 180; + + const double KTILT = 0; } \ No newline at end of file From 1b6f0d8e3c81d9caebccf691a3de9b9f50c7fd8a Mon Sep 17 00:00:00 2001 From: Jonathan Liu Date: Mon, 9 Oct 2023 09:09:51 -0700 Subject: [PATCH 2/2] fix wrnngs --- src/main/cpp/Drive/AutoDock.cpp | 8 ++++---- src/main/include/Drive/AutoDock.h | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/cpp/Drive/AutoDock.cpp b/src/main/cpp/Drive/AutoDock.cpp index 807106c..9c5bef0 100644 --- a/src/main/cpp/Drive/AutoDock.cpp +++ b/src/main/cpp/Drive/AutoDock.cpp @@ -11,7 +11,7 @@ * @param isRed If robot is on red side */ AutoDock::AutoDock(bool isRed) - : m_isRed{isRed}, m_curState{NOT_DOCKING}, m_kTilt{AutoConstants::KTILT}, m_preDockSpeed{AutoConstants::PRE_DOCK_SPEED}, + : m_curState{NOT_DOCKING}, m_isRed{isRed}, m_kTilt{AutoConstants::KTILT}, m_preDockSpeed{AutoConstants::PRE_DOCK_SPEED}, m_maxDockSpeed{AutoConstants::MAX_DOCK_SPEED}, m_preDockAng{AutoConstants::PRE_DOCK_ANG}, m_dockAng{AutoConstants::DOCK_ANG}, m_dockedTol{AutoConstants::DOCKED_TOL}, m_roll{0}, m_pitch{0}, m_yaw{0} {} @@ -20,7 +20,7 @@ AutoDock::AutoDock(bool isRed) * * @returns current state */ -AutoDock::State AutoDock::GetState() { +AutoDock::State AutoDock::GetState() const { return m_curState; } @@ -29,7 +29,7 @@ AutoDock::State AutoDock::GetState() { * * @returns wheel velocity */ -vec::Vector2D AutoDock::GetVel() { +vec::Vector2D AutoDock::GetVel() const { if (m_curState == NOT_DOCKING || m_curState == DOCKED) { return {0, 0}; } @@ -198,7 +198,7 @@ void AutoDock::Periodic() { * * @returns Tilt */ -double AutoDock::GetTilt() { +double AutoDock::GetTilt() const { // double x = -std::sin(m_yaw) * std::cos(m_roll); // double y = std::cos(m_yaw) * std::cos(m_pitch) - std::sin(m_yaw) * std::sin(m_pitch) * std::sin(m_roll); double z = std::cos(m_yaw) * std::sin(m_pitch) + std::sin(m_yaw) * std::cos(m_pitch) * std::sin(m_roll); diff --git a/src/main/include/Drive/AutoDock.h b/src/main/include/Drive/AutoDock.h index 4b1cff4..be24436 100644 --- a/src/main/include/Drive/AutoDock.h +++ b/src/main/include/Drive/AutoDock.h @@ -21,8 +21,8 @@ class AutoDock { AutoDock(bool isRed = false); - State GetState(); - vec::Vector2D GetVel(); + State GetState() const; + vec::Vector2D GetVel() const; void UpdateOdom(double r, double p, double y); void SetkTilt(double kTilt); @@ -52,5 +52,5 @@ class AutoDock { double m_roll, m_pitch, m_yaw; vec::Vector2D m_outputVel; - double GetTilt(); + double GetTilt() const; }; \ No newline at end of file