From 38a3d5bb7d432939cf8e0dc766b7ccd1ea0dde10 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 28 Jan 2023 14:17:18 -0500 Subject: [PATCH 001/100] Add arm.h w/ some IK math and arm length consts Also add armPose and point header files Yo sontraen onidgon - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 49 +++++++++++++++++++++++ src/main/include/Constants.h | 6 ++- src/main/include/subsystems/arm/arm.h | 21 ++++++++++ src/main/include/subsystems/arm/armPose.h | 8 ++++ src/main/include/subsystems/arm/point.h | 7 ++++ 5 files changed, 90 insertions(+), 1 deletion(-) create mode 100644 src/main/cpp/subsystems/arm/arm.cpp create mode 100644 src/main/include/subsystems/arm/arm.h create mode 100644 src/main/include/subsystems/arm/armPose.h create mode 100644 src/main/include/subsystems/arm/point.h diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp new file mode 100644 index 0000000..95c0c79 --- /dev/null +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -0,0 +1,49 @@ +#include "Mandatory.h" +#include "subsystems/arm/arm.h" + +#include + +Point ArmSubsystem::getElbowPos(float angShoulder) { + Point pt( + k_bicepLenInches * std::cos(angShoulder), + k_bicepLenInches * std::sin(angShoulder) + ); + + return pt; +} + +Point ArmSubsystem::getWristPos(float shoulderAng, float elbowAng) { + Point elbowPos = getElbowPos(shoulderAng); + Point pt( + k_forearmLenInches * std::cos(shoulderAng + elbowAng), + k_forearmLenInches * std::sin(shoulderAng + elbowAng) + ); + + return Point(elbowPos.x + pt.x, elbowPos.y + pt.y); +} + +ArmPose ArmSubsystem::getIKJointPoses(Point pt) { + float targetLen = std::sqrt((pt.x * pt.x + pt.y * pt.y)); // Line from shoulder to target + float targetToXAxisAng = std::atan2(pt.y, pt.x); + + float targetToBicepAng = std::acos( + (k_forearmLenInches * k_forearmLenInches + - k_bicepLenInches * k_bicepLenInches + + targetLen * targetLen) + / (2.0 * k_forearmLenInches * targetLen) + ); + + // Solve IK: + float bicepToXAxisAng = targetToBicepAng + targetToXAxisAng; + + float bicepToForearmAng = + (std::numbers::pi / 2.0) + - std::acos( + (k_forearmLenInches * k_forearmLenInches + - k_bicepLenInches * k_bicepLenInches + + targetLen * targetLen) + / (2.0 * k_forearmLenInches * targetLen)); + + return ArmPose(bicepToXAxisAng, bicepToForearmAng); + +} diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f587b8b..faa1c94 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -1,2 +1,6 @@ #pragma once -#include "Interfaces.h" \ No newline at end of file +#include "Interfaces.h" + +// ARM PARAMS: +const float k_forearmLenInches = 24; // Update to correct value +const float k_bicepLenInches = 24;// Update to correct value \ No newline at end of file diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h new file mode 100644 index 0000000..8e5eb82 --- /dev/null +++ b/src/main/include/subsystems/arm/arm.h @@ -0,0 +1,21 @@ +#pragma once + +#include "armPose.h" +#include "point.h" + +#include + +#include + +class ArmSubsystem : public frc2::SubsystemBase { + public: + + Point getElbowPos(float angShoulder); + + Point getWristPos(float shoulderAng, float elbowAng); + + ArmPose getIKJointPoses(Point pt); + +private: + +}; \ No newline at end of file diff --git a/src/main/include/subsystems/arm/armPose.h b/src/main/include/subsystems/arm/armPose.h new file mode 100644 index 0000000..4af1184 --- /dev/null +++ b/src/main/include/subsystems/arm/armPose.h @@ -0,0 +1,8 @@ + +class ArmPose { + public: + ArmPose(float shoulderAng, float elbowAng) : shoulderAng(shoulderAng), elbowAng(elbowAng) {} + + float shoulderAng; + float elbowAng; +}; diff --git a/src/main/include/subsystems/arm/point.h b/src/main/include/subsystems/arm/point.h new file mode 100644 index 0000000..97b8200 --- /dev/null +++ b/src/main/include/subsystems/arm/point.h @@ -0,0 +1,7 @@ +class Point { + public: + Point(float x, float y) : x(x), y(y) {} + + float x; + float y; +}; \ No newline at end of file From ab6549df67690f7df3a6fee0bee45acd3395382f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 17:56:59 -0500 Subject: [PATCH 002/100] Add #include for cmath Heeyaw - Emmie J --- src/main/cpp/subsystems/arm/arm.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 95c0c79..e9e1318 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -2,6 +2,7 @@ #include "subsystems/arm/arm.h" #include +#include Point ArmSubsystem::getElbowPos(float angShoulder) { Point pt( From 89b32c6053094b305e97c362768adccd29938d06 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 18:17:35 -0500 Subject: [PATCH 003/100] Make Point class take Z param; update 2D points 2D points use 0.0 as their Z axis values VrOOOOoOoOommM - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 8 +++++--- src/main/include/subsystems/arm/point.h | 3 ++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index e9e1318..3b39b4f 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -7,7 +7,8 @@ Point ArmSubsystem::getElbowPos(float angShoulder) { Point pt( k_bicepLenInches * std::cos(angShoulder), - k_bicepLenInches * std::sin(angShoulder) + k_bicepLenInches * std::sin(angShoulder), + 0.0 ); return pt; @@ -17,10 +18,11 @@ Point ArmSubsystem::getWristPos(float shoulderAng, float elbowAng) { Point elbowPos = getElbowPos(shoulderAng); Point pt( k_forearmLenInches * std::cos(shoulderAng + elbowAng), - k_forearmLenInches * std::sin(shoulderAng + elbowAng) + k_forearmLenInches * std::sin(shoulderAng + elbowAng), + 0.0 ); - return Point(elbowPos.x + pt.x, elbowPos.y + pt.y); + return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, 0.0); } ArmPose ArmSubsystem::getIKJointPoses(Point pt) { diff --git a/src/main/include/subsystems/arm/point.h b/src/main/include/subsystems/arm/point.h index 97b8200..9c7cfba 100644 --- a/src/main/include/subsystems/arm/point.h +++ b/src/main/include/subsystems/arm/point.h @@ -1,7 +1,8 @@ class Point { public: - Point(float x, float y) : x(x), y(y) {} + Point(float x, float y, float z) : x(x), y(y), z(z) {} float x; float y; + float z; }; \ No newline at end of file From a26cbf15ee806a7ca88a57bbdd010ec7e9a811ac Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 18:49:51 -0500 Subject: [PATCH 004/100] Rename "get*Pos" Functions to "calc*Pos" for clarity Amazing - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 3b39b4f..ca8133d 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -4,7 +4,7 @@ #include #include -Point ArmSubsystem::getElbowPos(float angShoulder) { +Point ArmSubsystem::calcElbowPos(float angShoulder) { Point pt( k_bicepLenInches * std::cos(angShoulder), k_bicepLenInches * std::sin(angShoulder), @@ -14,8 +14,8 @@ Point ArmSubsystem::getElbowPos(float angShoulder) { return pt; } -Point ArmSubsystem::getWristPos(float shoulderAng, float elbowAng) { - Point elbowPos = getElbowPos(shoulderAng); +Point ArmSubsystem::calcWristPos(float shoulderAng, float elbowAng) { + Point elbowPos = calcElbowPos(shoulderAng); Point pt( k_forearmLenInches * std::cos(shoulderAng + elbowAng), k_forearmLenInches * std::sin(shoulderAng + elbowAng), @@ -25,7 +25,7 @@ Point ArmSubsystem::getWristPos(float shoulderAng, float elbowAng) { return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, 0.0); } -ArmPose ArmSubsystem::getIKJointPoses(Point pt) { +ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { float targetLen = std::sqrt((pt.x * pt.x + pt.y * pt.y)); // Line from shoulder to target float targetToXAxisAng = std::atan2(pt.y, pt.x); From b1d6c12a0ae406067908ce53433a4e065ca7c38f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 18:51:54 -0500 Subject: [PATCH 005/100] Add .0 to arm consts to denote float type hEehOO Add tHe .0 - Emmie Jones --- src/main/include/Constants.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index faa1c94..88b7474 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -2,5 +2,5 @@ #include "Interfaces.h" // ARM PARAMS: -const float k_forearmLenInches = 24; // Update to correct value -const float k_bicepLenInches = 24;// Update to correct value \ No newline at end of file +const float k_forearmLenInches = 24.0; // Update to correct value +const float k_bicepLenInches = 24.0;// Update to correct value \ No newline at end of file From f4badca30f292c8d8dcaa885953ff44ec5266bdb Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 19:07:04 -0500 Subject: [PATCH 006/100] Add empty functions for reading arm angles "Where are your arms?" - Us, 2023 - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 12 ++++++++++++ src/main/include/subsystems/arm/arm.h | 20 ++++++++++++++++---- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index ca8133d..2f4c63b 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -50,3 +50,15 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { return ArmPose(bicepToXAxisAng, bicepToForearmAng); } + +float ArmSubsystem::getShoulderAngle() { + return 0; // TODO when we get reading components available +} + +float ArmSubsystem::getElbowAngle() { + return 0; // TODO when we get reading components available +} + +float ArmSubsystem::getTurretAngle() { + return 0; // TODO when we get reading components available +} \ No newline at end of file diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 8e5eb82..f3397fd 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -2,19 +2,31 @@ #include "armPose.h" #include "point.h" +#include "boundary.h" #include #include class ArmSubsystem : public frc2::SubsystemBase { - public: +public: - Point getElbowPos(float angShoulder); + // Calulating Functions: + Point calcElbowPos(float angShoulder); - Point getWristPos(float shoulderAng, float elbowAng); + Point calcWristPos(float shoulderAng, float elbowAng); - ArmPose getIKJointPoses(Point pt); + ArmPose calcIKJointPoses(Point pt); + + bool checkBoundary(Boundary boundary, Point point); + + + // Reading Functions: + float getShoulderAngle(); + + float getElbowAngle(); + + float getTurretAngle(); private: From ca8bf506b0826beaf8fd3925583752b635ee309e Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 21:31:28 -0500 Subject: [PATCH 007/100] Add functions for getting measured angles on arm "Remember when you tried to kill me twice?" - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 19 ++++++++++++++----- src/main/include/Constants.h | 5 ++++- src/main/include/subsystems/arm/arm.h | 20 +++++++++++++++++--- 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 2f4c63b..72b9fe2 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -51,14 +51,23 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { } -float ArmSubsystem::getShoulderAngle() { - return 0; // TODO when we get reading components available +units::turn_t ArmSubsystem::getShoulderAngle() { + return mShoulderAngleSensor.Get(); } float ArmSubsystem::getElbowAngle() { - return 0; // TODO when we get reading components available + return mElbowAngleSensor.Get(); } float ArmSubsystem::getTurretAngle() { - return 0; // TODO when we get reading components available -} \ No newline at end of file + return mTurretAngleSensor.Get(); +} + +float ArmSubsystem::getWristRollAngle() { + return mWristAngleSensor.Get(); +} + +// // NOT IMPLEMENTED IN HARDWARE +// float ArmSubsystem::getWristPitchAngle() { +// return 0; // TODO when we get reading components available +// } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 88b7474..9249f65 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -3,4 +3,7 @@ // ARM PARAMS: const float k_forearmLenInches = 24.0; // Update to correct value -const float k_bicepLenInches = 24.0;// Update to correct value \ No newline at end of file +const float k_bicepLenInches = 24.0;// Update to correct value + +const int k_shoulderMinDeg = 0; +const int k_shoulderMaxDeg = 270; \ No newline at end of file diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index f3397fd..29e7d04 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -7,6 +7,8 @@ #include #include +#include +#include class ArmSubsystem : public frc2::SubsystemBase { public: @@ -22,12 +24,24 @@ class ArmSubsystem : public frc2::SubsystemBase { // Reading Functions: - float getShoulderAngle(); + float getTurretAngle(); + + units::turn_t getShoulderAngle(); float getElbowAngle(); - float getTurretAngle(); + float getWristRollAngle(); + + // float getWristPitchAngle(); // NOT IMPLEMENTED IN HARDWARE private: -}; \ No newline at end of file + frc::AnalogPotentiometer mTurretAngleSensor {1, 1.0, 0}; + + frc::DutyCycleEncoder mShoulderAngleSensor{1}; // Using Funky Fresh Encoder + + frc::AnalogPotentiometer mElbowAngleSensor {1, 1.0, 0}; + + frc::AnalogPotentiometer mWristAngleSensor {1, 1.0, 0}; + +}; From 3ee54bd6d1dd92e8ab8dbd090622e6d9efa1c1bd Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 31 Jan 2023 21:46:41 -0500 Subject: [PATCH 008/100] Add Boundary class for arm movement safety yeehaw heeyaw - Emmie Jones --- src/main/include/subsystems/arm/boundary.h | 100 +++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 src/main/include/subsystems/arm/boundary.h diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h new file mode 100644 index 0000000..a796d2d --- /dev/null +++ b/src/main/include/subsystems/arm/boundary.h @@ -0,0 +1,100 @@ +#include "arm.h" + +#include + +class Boundary { + public: + bool isInside(Point pt); + bool isOutside(Point pt); +}; + +class BoxBoundary : public Boundary { +public: + BoxBoundary( + float xLo, + float xHi, + float yLo, + float yHi, + float zLo, + float zHi) + : xLo(xLo), xHi(xHi), yLo(yLo), yHi(yHi), zLo(zLo), zHi(zHi) { } + + bool isInside(Point pt) { + return (xLo < pt.x && pt.x < xHi) + && (yLo < pt.y && pt.y < yHi) + && (zLo < pt.z && pt.z < zHi); + } + + bool isOutside(Point pt) { + return !isInside(pt); + } + +private: + float xLo; + float xHi; + float yLo; + float yHi; + float zLo; + float zHi; +}; + +class SphereBoundary : public Boundary { +public: + SphereBoundary(Point center, float radius) : center(center), radius(radius) { } + + bool isInside(Point pt) { + Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); + + float dist = + (offset.x * offset.x) + + (offset.y * offset.y) + + (offset.z * offset.z); + + return (dist < radius * radius); + } + + bool isOutside(Point pt) { + return !isInside(pt); + } + +private: + Point center; + float radius; +}; + +class ComposeBoundary : public Boundary { +public: + ComposeBoundary(std::vector> boundaries) + : boundaries(boundaries) { } + + bool isInsideBounds(Point pt) { + for (auto& bound : boundaries) { + if (bound->isOutside(pt)) { + return false; + } + } + + return true; + } + + bool isOutsideBounds(Point pt) { + return !isInsideBounds(pt); + } + +private: + std::vector> boundaries; +}; + +class NotBoundary : public Boundary { +public: + bool isInsideBounds(Point pt) { + return mBound->isOutside(pt); + } + + bool isOutsideBounds(Point pt) { + return !isInsideBounds(pt); + } + +private: + std::unique_ptr mBound; +}; \ No newline at end of file From c89d6b11c72bfa705b9e7a674d93973ce67388d9 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 7 Feb 2023 17:37:04 -0500 Subject: [PATCH 009/100] Add empty set__Angle arm functions (and add diagram) "I was chosen for the constitutional convention!" - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 18 +++++++++++++++++- src/main/include/subsystems/arm/arm.h | 18 +++++++++++++++++- 2 files changed, 34 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 72b9fe2..a64e5de 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -67,7 +67,23 @@ float ArmSubsystem::getWristRollAngle() { return mWristAngleSensor.Get(); } -// // NOT IMPLEMENTED IN HARDWARE +void ArmSubsystem::setTurretAngle(float angle) { + //NOT YET IMPLEMENTED +} + +void ArmSubsystem::setShoulderAngle(float angle) { + //NOT YET IMPLEMENTED +} + +void ArmSubsystem::setElbowAngle(float angle) { + //NOT YET IMPLEMENTED +} + +void ArmSubsystem::setWristRollAngle(float angle) { + //you get the gist +} + +// NOT IMPLEMENTED IN HARDWARE // float ArmSubsystem::getWristPitchAngle() { // return 0; // TODO when we get reading components available // } diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 29e7d04..d899850 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -34,14 +34,30 @@ class ArmSubsystem : public frc2::SubsystemBase { // float getWristPitchAngle(); // NOT IMPLEMENTED IN HARDWARE + void setTurretAngle(float angle); + void setShoulderAngle(float angle); + void setElbowAngle(float angle); + void setWristRollAngle(float angle); + private: + // Arm Diagram: + // 1: Turret, 2: Shoulder, 3: Elbow, 4: Wrist + // + // (3)---(4){ + // / + // [2] + // (1) + // [=====1720=====] + // O O + // **Not to scale + frc::AnalogPotentiometer mTurretAngleSensor {1, 1.0, 0}; frc::DutyCycleEncoder mShoulderAngleSensor{1}; // Using Funky Fresh Encoder frc::AnalogPotentiometer mElbowAngleSensor {1, 1.0, 0}; - frc::AnalogPotentiometer mWristAngleSensor {1, 1.0, 0}; + frc::AnalogPotentiometer mWristRollAngleSensor {1, 1.0, 0}; }; From 037c0330b24367d434092dd8ce7f21455506ac28 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 7 Feb 2023 17:40:11 -0500 Subject: [PATCH 010/100] Rename getWristAngle() to getWristRollAngle() for consistency "AAAAAAAAAVVVVVVVVVEEEEEEEEE MARRRIIIIAAAAA" - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index a64e5de..0e7eee5 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -64,7 +64,7 @@ float ArmSubsystem::getTurretAngle() { } float ArmSubsystem::getWristRollAngle() { - return mWristAngleSensor.Get(); + return mWristRollAngleSensor.Get(); } void ArmSubsystem::setTurretAngle(float angle) { From e26986e6b3e09408a36bfe4a18cb92b2b1f2f6df Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 7 Feb 2023 17:44:43 -0500 Subject: [PATCH 011/100] Add forgotten Pragma Once to arm custom classes "AAAAAAAA" - Emmie Jones --- src/main/include/subsystems/arm/arm.h | 1 - src/main/include/subsystems/arm/armPose.h | 1 + src/main/include/subsystems/arm/boundary.h | 4 +++- src/main/include/subsystems/arm/point.h | 2 ++ 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index d899850..0a5bb60 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -40,7 +40,6 @@ class ArmSubsystem : public frc2::SubsystemBase { void setWristRollAngle(float angle); private: - // Arm Diagram: // 1: Turret, 2: Shoulder, 3: Elbow, 4: Wrist // diff --git a/src/main/include/subsystems/arm/armPose.h b/src/main/include/subsystems/arm/armPose.h index 4af1184..17c5f10 100644 --- a/src/main/include/subsystems/arm/armPose.h +++ b/src/main/include/subsystems/arm/armPose.h @@ -1,3 +1,4 @@ +#pragma once class ArmPose { public: diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index a796d2d..078bbdb 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -1,3 +1,5 @@ +#pragma once + #include "arm.h" #include @@ -97,4 +99,4 @@ class NotBoundary : public Boundary { private: std::unique_ptr mBound; -}; \ No newline at end of file +}; diff --git a/src/main/include/subsystems/arm/point.h b/src/main/include/subsystems/arm/point.h index 9c7cfba..d0f6130 100644 --- a/src/main/include/subsystems/arm/point.h +++ b/src/main/include/subsystems/arm/point.h @@ -1,3 +1,5 @@ +#pragma once + class Point { public: Point(float x, float y, float z) : x(x), y(y), z(z) {} From 083ce40031315fcd0ebe1b2c33ff9f6d6c5f6cd5 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 7 Feb 2023 20:27:00 -0500 Subject: [PATCH 012/100] Factor in Turret in calcIKJointPoses() "" - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 6 +++++- src/main/include/subsystems/arm/armPose.h | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 0e7eee5..2ae4ff0 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -47,10 +47,14 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { + targetLen * targetLen) / (2.0 * k_forearmLenInches * targetLen)); - return ArmPose(bicepToXAxisAng, bicepToForearmAng); + float turretToXZAng = std::atan2(pt.z, pt.x); + + return ArmPose(turretToXZAng, bicepToXAxisAng, bicepToForearmAng); } +// Getting and setting arm angles: + units::turn_t ArmSubsystem::getShoulderAngle() { return mShoulderAngleSensor.Get(); } diff --git a/src/main/include/subsystems/arm/armPose.h b/src/main/include/subsystems/arm/armPose.h index 17c5f10..c0c7496 100644 --- a/src/main/include/subsystems/arm/armPose.h +++ b/src/main/include/subsystems/arm/armPose.h @@ -2,8 +2,9 @@ class ArmPose { public: - ArmPose(float shoulderAng, float elbowAng) : shoulderAng(shoulderAng), elbowAng(elbowAng) {} + ArmPose(float turretAng, float shoulderAng, float elbowAng) : turretAng(turretAng), shoulderAng(shoulderAng), elbowAng(elbowAng) {} + float turretAng; float shoulderAng; float elbowAng; }; From 0a8c1c49508d9f0ce629718a236d15c3b3e09599 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Feb 2023 11:12:28 -0500 Subject: [PATCH 013/100] Update inverse kinematics for 3 dimensions Initial IK only focused on shoulder and elbow joints on a single plane. Add support for turret angle. - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 26 +++++++++++++++----------- src/main/include/subsystems/arm/arm.h | 4 ++-- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 2ae4ff0..09a4dec 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -4,30 +4,34 @@ #include #include -Point ArmSubsystem::calcElbowPos(float angShoulder) { +Point ArmSubsystem::calcElbowPos(float turretAng, float shoulderAng) { Point pt( - k_bicepLenInches * std::cos(angShoulder), - k_bicepLenInches * std::sin(angShoulder), - 0.0 + k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), + k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), + k_bicepLenInches * std::sin(shoulderAng), ); return pt; } -Point ArmSubsystem::calcWristPos(float shoulderAng, float elbowAng) { +Point ArmSubsystem::calcWristPos( + float turretAng, + float shoulderAng, + float elbowAng +) { Point elbowPos = calcElbowPos(shoulderAng); Point pt( - k_forearmLenInches * std::cos(shoulderAng + elbowAng), - k_forearmLenInches * std::sin(shoulderAng + elbowAng), - 0.0 + k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), + k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), + k_forearmLenInches * std::sin(shoulderAng + elbowAng) ); - return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, 0.0); + return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, elbowPos.z + pt.z); } ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { - float targetLen = std::sqrt((pt.x * pt.x + pt.y * pt.y)); // Line from shoulder to target - float targetToXAxisAng = std::atan2(pt.y, pt.x); + float targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target + float targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); float targetToBicepAng = std::acos( (k_forearmLenInches * k_forearmLenInches diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 0a5bb60..b217248 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -14,9 +14,9 @@ class ArmSubsystem : public frc2::SubsystemBase { public: // Calulating Functions: - Point calcElbowPos(float angShoulder); + Point calcElbowPos(float turretAng, float angShoulder); - Point calcWristPos(float shoulderAng, float elbowAng); + Point calcWristPos(float turretAng, float shoulderAng, float elbowAng); ArmPose calcIKJointPoses(Point pt); From 23efaf18f449ce76a351ac5d9471e886243f4ce8 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 15:49:02 -0500 Subject: [PATCH 014/100] Fix boundary code Use correct method names. add override keyword for compiler checking. add virtual for base class methods. yeehaw - Emmie Jones --- src/main/include/subsystems/arm/boundary.h | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 078bbdb..4166cab 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -6,8 +6,8 @@ class Boundary { public: - bool isInside(Point pt); - bool isOutside(Point pt); + virtual bool isInside(Point pt); + virtual bool isOutside(Point pt); }; class BoxBoundary : public Boundary { @@ -21,13 +21,13 @@ class BoxBoundary : public Boundary { float zHi) : xLo(xLo), xHi(xHi), yLo(yLo), yHi(yHi), zLo(zLo), zHi(zHi) { } - bool isInside(Point pt) { + bool isInside(Point pt) override { return (xLo < pt.x && pt.x < xHi) && (yLo < pt.y && pt.y < yHi) && (zLo < pt.z && pt.z < zHi); } - bool isOutside(Point pt) { + bool isOutside(Point pt) override { return !isInside(pt); } @@ -44,7 +44,7 @@ class SphereBoundary : public Boundary { public: SphereBoundary(Point center, float radius) : center(center), radius(radius) { } - bool isInside(Point pt) { + bool isInside(Point pt) override { Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); float dist = @@ -55,7 +55,7 @@ class SphereBoundary : public Boundary { return (dist < radius * radius); } - bool isOutside(Point pt) { + bool isOutside(Point pt) override { return !isInside(pt); } @@ -69,7 +69,7 @@ class ComposeBoundary : public Boundary { ComposeBoundary(std::vector> boundaries) : boundaries(boundaries) { } - bool isInsideBounds(Point pt) { + bool isInside(Point pt) override { for (auto& bound : boundaries) { if (bound->isOutside(pt)) { return false; @@ -79,8 +79,8 @@ class ComposeBoundary : public Boundary { return true; } - bool isOutsideBounds(Point pt) { - return !isInsideBounds(pt); + bool isOutside(Point pt) override { + return !isInside(pt); } private: @@ -89,12 +89,12 @@ class ComposeBoundary : public Boundary { class NotBoundary : public Boundary { public: - bool isInsideBounds(Point pt) { + bool isInside(Point pt) override { return mBound->isOutside(pt); } - bool isOutsideBounds(Point pt) { - return !isInsideBounds(pt); + bool isOutside(Point pt) override { + return !isInside(pt); } private: From 830a75b7f35d9d348b8387da26e3d569bcea7af7 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 15:51:26 -0500 Subject: [PATCH 015/100] Add constructor for NotBoundary Yeeyee - Emmie Jones --- src/main/include/subsystems/arm/boundary.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 4166cab..77a33fe 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -89,6 +89,9 @@ class ComposeBoundary : public Boundary { class NotBoundary : public Boundary { public: + + NotBoundary(std::unique_ptr boundary) : mBound(std::move(boundary)) {} + bool isInside(Point pt) override { return mBound->isOutside(pt); } From 950b6f39fd15fed5911ba5b34c68ef9a075039fe Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 16:33:21 -0500 Subject: [PATCH 016/100] Switched from floats to doubles (Because Justin said so but doesn't know why) "Double double double" - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 34 +++++++++++++-------------- src/main/include/subsystems/arm/arm.h | 20 ++++++++-------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 09a4dec..a116f26 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -4,7 +4,7 @@ #include #include -Point ArmSubsystem::calcElbowPos(float turretAng, float shoulderAng) { +Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { Point pt( k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), @@ -15,9 +15,9 @@ Point ArmSubsystem::calcElbowPos(float turretAng, float shoulderAng) { } Point ArmSubsystem::calcWristPos( - float turretAng, - float shoulderAng, - float elbowAng + double turretAng, + double shoulderAng, + double elbowAng ) { Point elbowPos = calcElbowPos(shoulderAng); Point pt( @@ -30,10 +30,10 @@ Point ArmSubsystem::calcWristPos( } ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { - float targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target - float targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); + double targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target + double targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); - float targetToBicepAng = std::acos( + double targetToBicepAng = std::acos( (k_forearmLenInches * k_forearmLenInches - k_bicepLenInches * k_bicepLenInches + targetLen * targetLen) @@ -41,9 +41,9 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { ); // Solve IK: - float bicepToXAxisAng = targetToBicepAng + targetToXAxisAng; + double bicepToXAxisAng = targetToBicepAng + targetToXAxisAng; - float bicepToForearmAng = + double bicepToForearmAng = (std::numbers::pi / 2.0) - std::acos( (k_forearmLenInches * k_forearmLenInches @@ -51,7 +51,7 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { + targetLen * targetLen) / (2.0 * k_forearmLenInches * targetLen)); - float turretToXZAng = std::atan2(pt.z, pt.x); + double turretToXZAng = std::atan2(pt.z, pt.x); return ArmPose(turretToXZAng, bicepToXAxisAng, bicepToForearmAng); @@ -63,31 +63,31 @@ units::turn_t ArmSubsystem::getShoulderAngle() { return mShoulderAngleSensor.Get(); } -float ArmSubsystem::getElbowAngle() { +double ArmSubsystem::getElbowAngle() { return mElbowAngleSensor.Get(); } -float ArmSubsystem::getTurretAngle() { +double ArmSubsystem::getTurretAngle() { return mTurretAngleSensor.Get(); } -float ArmSubsystem::getWristRollAngle() { +double ArmSubsystem::getWristRollAngle() { return mWristRollAngleSensor.Get(); } -void ArmSubsystem::setTurretAngle(float angle) { +void ArmSubsystem::setTurretAngle(double angle) { //NOT YET IMPLEMENTED } -void ArmSubsystem::setShoulderAngle(float angle) { +void ArmSubsystem::setShoulderAngle(double angle) { //NOT YET IMPLEMENTED } -void ArmSubsystem::setElbowAngle(float angle) { +void ArmSubsystem::setElbowAngle(double angle) { //NOT YET IMPLEMENTED } -void ArmSubsystem::setWristRollAngle(float angle) { +void ArmSubsystem::setWristRollAngle(double angle) { //you get the gist } diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index b217248..8bf1f96 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -14,9 +14,9 @@ class ArmSubsystem : public frc2::SubsystemBase { public: // Calulating Functions: - Point calcElbowPos(float turretAng, float angShoulder); + Point calcElbowPos(double turretAng, double angShoulder); - Point calcWristPos(float turretAng, float shoulderAng, float elbowAng); + Point calcWristPos(double turretAng, double shoulderAng, double elbowAng); ArmPose calcIKJointPoses(Point pt); @@ -24,20 +24,20 @@ class ArmSubsystem : public frc2::SubsystemBase { // Reading Functions: - float getTurretAngle(); + double getTurretAngle(); units::turn_t getShoulderAngle(); - float getElbowAngle(); + double getElbowAngle(); - float getWristRollAngle(); + double getWristRollAngle(); - // float getWristPitchAngle(); // NOT IMPLEMENTED IN HARDWARE + // double getWristPitchAngle(); // NOT IMPLEMENTED IN HARDWARE - void setTurretAngle(float angle); - void setShoulderAngle(float angle); - void setElbowAngle(float angle); - void setWristRollAngle(float angle); + void setTurretAngle(double angle); + void setShoulderAngle(double angle); + void setElbowAngle(double angle); + void setWristRollAngle(double angle); private: // Arm Diagram: From 77b2175b40f0164a33b4813aee651667b6069ffc Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 16:34:52 -0500 Subject: [PATCH 017/100] Fix constants prefix yee - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index a116f26..1a36b8f 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -6,9 +6,9 @@ Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { Point pt( - k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), - k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), - k_bicepLenInches * std::sin(shoulderAng), + Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), + Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), + Constants::Arm::k_bicepLenInches * std::sin(shoulderAng), ); return pt; @@ -21,9 +21,9 @@ Point ArmSubsystem::calcWristPos( ) { Point elbowPos = calcElbowPos(shoulderAng); Point pt( - k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), - k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), - k_forearmLenInches * std::sin(shoulderAng + elbowAng) + Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), + Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), + Constants::Arm::k_forearmLenInches * std::sin(shoulderAng + elbowAng) ); return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, elbowPos.z + pt.z); @@ -34,10 +34,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { double targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); double targetToBicepAng = std::acos( - (k_forearmLenInches * k_forearmLenInches - - k_bicepLenInches * k_bicepLenInches + (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches + - Constants::Arm::k_bicepLenInches * Constants::Arm::k_bicepLenInches + targetLen * targetLen) - / (2.0 * k_forearmLenInches * targetLen) + / (2.0 * Constants::Arm::k_forearmLenInches * targetLen) ); // Solve IK: @@ -46,10 +46,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { double bicepToForearmAng = (std::numbers::pi / 2.0) - std::acos( - (k_forearmLenInches * k_forearmLenInches - - k_bicepLenInches * k_bicepLenInches + (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches + - Constants::Arm::k_bicepLenInches * Constants::Arm::k_bicepLenInches + targetLen * targetLen) - / (2.0 * k_forearmLenInches * targetLen)); + / (2.0 * Constants::Arm::k_forearmLenInches * targetLen)); double turretToXZAng = std::atan2(pt.z, pt.x); From 2d5d196c5ae0284678f2de39a61cf593590f5a9d Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 16:35:30 -0500 Subject: [PATCH 018/100] Fix arguments for calcElbowPos() weehaw - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 1a36b8f..5e8f96a 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -19,7 +19,7 @@ Point ArmSubsystem::calcWristPos( double shoulderAng, double elbowAng ) { - Point elbowPos = calcElbowPos(shoulderAng); + Point elbowPos = calcElbowPos(turretAng, shoulderAng); Point pt( Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), From 458d3afee066ff40c1df0a2ddc3ca1ad54851707 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 18 Feb 2023 16:38:23 -0500 Subject: [PATCH 019/100] Switch m prefix to m_ We Are Toast - Emmie Jones --- src/main/cpp/subsystems/arm/arm.cpp | 8 ++++---- src/main/include/subsystems/arm/arm.h | 8 ++++---- src/main/include/subsystems/arm/boundary.h | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 5e8f96a..8d658aa 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -60,19 +60,19 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { // Getting and setting arm angles: units::turn_t ArmSubsystem::getShoulderAngle() { - return mShoulderAngleSensor.Get(); + return m_ShoulderAngleSensor.Get(); } double ArmSubsystem::getElbowAngle() { - return mElbowAngleSensor.Get(); + return m_ElbowAngleSensor.Get(); } double ArmSubsystem::getTurretAngle() { - return mTurretAngleSensor.Get(); + return m_TurretAngleSensor.Get(); } double ArmSubsystem::getWristRollAngle() { - return mWristRollAngleSensor.Get(); + return m_WristRollAngleSensor.Get(); } void ArmSubsystem::setTurretAngle(double angle) { diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 8bf1f96..77d4ed5 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -51,12 +51,12 @@ class ArmSubsystem : public frc2::SubsystemBase { // O O // **Not to scale - frc::AnalogPotentiometer mTurretAngleSensor {1, 1.0, 0}; + frc::AnalogPotentiometer m_TurretAngleSensor {1, 1.0, 0}; - frc::DutyCycleEncoder mShoulderAngleSensor{1}; // Using Funky Fresh Encoder + frc::DutyCycleEncoder m_ShoulderAngleSensor{1}; // Using Funky Fresh Encoder - frc::AnalogPotentiometer mElbowAngleSensor {1, 1.0, 0}; + frc::AnalogPotentiometer m_ElbowAngleSensor {1, 1.0, 0}; - frc::AnalogPotentiometer mWristRollAngleSensor {1, 1.0, 0}; + frc::AnalogPotentiometer m_WristRollAngleSensor {1, 1.0, 0}; }; diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 77a33fe..8010a1a 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -90,10 +90,10 @@ class ComposeBoundary : public Boundary { class NotBoundary : public Boundary { public: - NotBoundary(std::unique_ptr boundary) : mBound(std::move(boundary)) {} + NotBoundary(std::unique_ptr boundary) : m_Bound(std::move(boundary)) {} bool isInside(Point pt) override { - return mBound->isOutside(pt); + return m_Bound->isOutside(pt); } bool isOutside(Point pt) override { @@ -101,5 +101,5 @@ class NotBoundary : public Boundary { } private: - std::unique_ptr mBound; + std::unique_ptr m_Bound; }; From a726c2862bd40cda12305b70528738266fcfbca0 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 4 Mar 2023 15:32:34 -0500 Subject: [PATCH 020/100] Mass dump of arm subsystem updates - Add MoveToIntakeCommand. - Add boundary classes. - Add no-go zones. - Define no-go zones. - Add safety points. - Add reach points. - Emmie Jones --- src/main/cpp/commands/arm/MoveToIntake.cpp | 43 +++++ src/main/cpp/subsystems/arm/arm.cpp | 181 +++++++++++++++++-- src/main/cpp/util/math.cpp | 5 + src/main/include/Constants.h | 8 +- src/main/include/Interfaces.h | 23 +++ src/main/include/commands/arm/MoveToIntake.h | 28 +++ src/main/include/subsystems/arm/arm.h | 98 ++++++++-- src/main/include/subsystems/arm/boundary.h | 140 ++++++++++---- src/main/include/subsystems/arm/motionPath.h | 59 ++++++ src/main/include/util/math.h | 3 + src/main/include/util/point.h | 21 +++ src/main/include/util/vector.h | 19 ++ src/test/cpp/boundaryTest.cpp | 36 ++++ 13 files changed, 595 insertions(+), 69 deletions(-) create mode 100644 src/main/cpp/commands/arm/MoveToIntake.cpp create mode 100644 src/main/cpp/util/math.cpp create mode 100644 src/main/include/commands/arm/MoveToIntake.h create mode 100644 src/main/include/subsystems/arm/motionPath.h create mode 100644 src/main/include/util/math.h create mode 100644 src/main/include/util/vector.h create mode 100644 src/test/cpp/boundaryTest.cpp diff --git a/src/main/cpp/commands/arm/MoveToIntake.cpp b/src/main/cpp/commands/arm/MoveToIntake.cpp new file mode 100644 index 0000000..b88b86f --- /dev/null +++ b/src/main/cpp/commands/arm/MoveToIntake.cpp @@ -0,0 +1,43 @@ +#include "commands/arm/MoveToIntake.h" +#include "util/point.h" +#include "subsystems/arm/motionPath.h" +#include "subsystems/arm/arm.h" +#include "subsystems/arm/armPose.h" + +MoveToIntakeCommand::MoveToIntakeCommand(ArmSubsystem * arm, Point currentPoint) { + m_arm = arm; + m_currentPoint = currentPoint; + + AddRequirements(m_arm); +} + +void MoveToIntakeCommand::Initialize() { + m_finalTarget = m_arm->m_pointIntake; + Point currentPoint = m_arm->getWristPoint(); + + m_path.emplace(std::move(m_arm->getPathTo(currentPoint, m_finalTarget))); + m_target = m_path->getNextPoint(); +} + +void MoveToIntakeCommand::Execute() { + if(m_finalTarget.isNear(m_target) && m_arm->isNearPoint(m_target)) { + return; + } else if (m_arm->isNearPoint(m_target)) { + m_target = m_path->getNextPoint(); + } + + if (!m_arm->isPointSafe(m_target)) { + return; + } + + ArmPose calculatedIKPose = m_arm->calcIKJointPoses(m_target); +} + +void MoveToIntakeCommand::End(bool isInterrupted) { + //heeyaw +} + +bool MoveToIntakeCommand::IsFinished() { + return !m_arm->isPointSafe(m_target) + || (m_finalTarget.isNear(m_target) && m_arm->isNearPoint(m_target)); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 8d658aa..84987a6 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -1,15 +1,68 @@ #include "Mandatory.h" #include "subsystems/arm/arm.h" +#include "util/math.h" #include #include +#include + +const double k_ChassisXSize = 0.775; // meters +const double k_ChassisYSize = 0.826; // meters +const double k_ChassisZSize = 0.229; // meters + +const double k_PickupXSize = 0.076; // meters +const double k_PickupYSize = 0.340; // meters +const double k_PickupZSize = 1.000; // meters + +ArmSubsystem::ArmSubsystem() {} + +void ArmSubsystem::Periodic() { + frc::SmartDashboard::PutNumber("Turret Angle", getTurretAngle()); + frc::SmartDashboard::PutNumber("Shoulder Angle", getShoulderAngle()); + frc::SmartDashboard::PutNumber("Elbow Angle", getElbowAngle()); + frc::SmartDashboard::PutNumber("Wrist Roll Angle", getWristRollAngle()); + frc::SmartDashboard::PutNumber("Grip Angle", getGrip()); +} + +void ArmSubsystem::initialiseBoundary() { + // Only create boundaries for no-go zones. + // Otherwise, the code gets really complicated later + // to check for collisions. + + std::shared_ptr chassisNoGoBounds = std::make_shared(std::vector>{ + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0), + (k_PickupYSize / 2.0) , (k_ChassisYSize / 2.0), + 0.0 , (k_ChassisZSize) + ), + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0) - k_PickupXSize, + -(k_PickupYSize / 2.0) , (k_PickupYSize / 2.0), + 0.0 , (k_ChassisZSize) + ), + std::make_shared( + -(k_ChassisXSize / 2.0), (k_ChassisXSize / 2.0), + -(k_ChassisYSize / 2.0), -(k_PickupYSize / 2.0), + 0.0 , (k_ChassisZSize) + ) + }); + + //std::unique_ptr<> turretNoGoZone = std::make_unique<> + + std::shared_ptr defNoGoZone = std::make_shared(std::vector{ + std::move(chassisNoGoBounds) + }); + + m_noGoZone = std::move(defNoGoZone); +} + Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { - Point pt( + Point pt{ Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), Constants::Arm::k_bicepLenInches * std::sin(shoulderAng), - ); + }; return pt; } @@ -29,12 +82,12 @@ Point ArmSubsystem::calcWristPos( return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, elbowPos.z + pt.z); } -ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { +ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target double targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); double targetToBicepAng = std::acos( - (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches + (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches - Constants::Arm::k_bicepLenInches * Constants::Arm::k_bicepLenInches + targetLen * targetLen) / (2.0 * Constants::Arm::k_forearmLenInches * targetLen) @@ -57,38 +110,136 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point pt) { } -// Getting and setting arm angles: +bool ArmSubsystem::isPointSafe(Point const & point) { + if (nullptr == m_noGoZone) { + return false; + } else { + return m_noGoZone->isOutside(point); + } +} -units::turn_t ArmSubsystem::getShoulderAngle() { - return m_ShoulderAngleSensor.Get(); +bool ArmSubsystem::isNearPoint(Point const & point) { + Point currentPosition = calcWristPos( + getTurretAngle(), + getShoulderAngle(), + getElbowAngle() + ); + + return point.isNear(currentPosition); } -double ArmSubsystem::getElbowAngle() { - return m_ElbowAngleSensor.Get(); +MotionPath ArmSubsystem::getPathTo(Point const & current, Point const & target) { + // FIXME: compute proper path sequence for arm to follow. + return MotionPath({ target }); +} + +// ============ Point Grabbers: ============ //| + +Point const & ArmSubsystem::getIntakePoint() { // | Intake + return ArmSubsystem::m_pointIntake; +} +Point const & ArmSubsystem::getHomePoint() { // | Home + return ArmSubsystem::m_pointHome; +} +Point const & ArmSubsystem::getHybridPoint() { // | Hybrid + return ArmSubsystem::m_pointHybrid; } +Point const & ArmSubsystem::getLowPolePoint() { // | Low Pole + return ArmSubsystem::m_pointLowPole; +} +Point const & ArmSubsystem::getHighPolePoint() {// | High Pole + return ArmSubsystem::m_pointHighPole; +} +Point const & ArmSubsystem::getLowShelfPoint() {// | Low Shelf + return ArmSubsystem::m_pointLowShelf; +} +Point const & ArmSubsystem::getHighShelfPoint() {// | High Shelf + return ArmSubsystem::m_pointHighShelf; +} + +// Getting and setting arm angles: double ArmSubsystem::getTurretAngle() { - return m_TurretAngleSensor.Get(); + return m_turretAngleSensor.Get(); +} + +double ArmSubsystem::getShoulderAngle() { + return m_shoulderAngleSensor.Get().value() * M_2_PI; +} + +double ArmSubsystem::getElbowAngle() { + return m_elbowAngleSensor.Get(); } double ArmSubsystem::getWristRollAngle() { - return m_WristRollAngleSensor.Get(); + return m_wristRollAngleSensor.Get(); +} + +double ArmSubsystem::getGrip() { + return m_gripSensor.Get(); +} + +Point ArmSubsystem::getWristPoint() { + return calcWristPos( + getTurretAngle(), + getShoulderAngle(), + getElbowAngle() + ); +} + +Point const & ArmSubsystem::getSafetyPoint(Point pt) { + if (isNearZero(pt.x)) { + return ArmSubsystem::m_safetyPointCenter; + } else if (0.0 > pt.x) { + return ArmSubsystem::m_safetyPointGrid; + } else { + return ArmSubsystem::m_safetyPointIntake; + } } void ArmSubsystem::setTurretAngle(double angle) { - //NOT YET IMPLEMENTED + double da = angle - getTurretAngle(); + if (isNearZero(da)) { + m_turretMotor.Set(0.0); + } else { + m_turretMotor.Set(std::copysign(0.05, da)); + } } void ArmSubsystem::setShoulderAngle(double angle) { - //NOT YET IMPLEMENTED + double da = angle - getShoulderAngle(); + if (isNearZero(da)) { + m_lowJointMotor.Set(0.0); + } else { + m_lowJointMotor.Set(std::copysign(0.05, da)); + } } void ArmSubsystem::setElbowAngle(double angle) { - //NOT YET IMPLEMENTED + double da = angle - getElbowAngle(); + if (isNearZero(da)) { + m_midJointMotor.Set(0.0); + } else { + m_midJointMotor.Set(std::copysign(0.05, da)); + } } void ArmSubsystem::setWristRollAngle(double angle) { - //you get the gist + double da = angle - getWristRollAngle(); + if (isNearZero(da)) { + m_gripperRotateMotor.Set(0.0); + } else { + m_gripperRotateMotor.Set(std::copysign(0.05, da)); + } +} + +void ArmSubsystem::setGrip(double grip) { + double dx = grip - getGrip(); + if (isNearZero(dx)) { + m_gripperGraspMotor.Set(0.0); + } else { + m_gripperGraspMotor.Set(std::copysign(0.05, dx)); + } } // NOT IMPLEMENTED IN HARDWARE diff --git a/src/main/cpp/util/math.cpp b/src/main/cpp/util/math.cpp new file mode 100644 index 0000000..c5c96c1 --- /dev/null +++ b/src/main/cpp/util/math.cpp @@ -0,0 +1,5 @@ +#include "util/math.h" + +bool isNearZero(double val) { + return -0.001 < val && val < 0.001; +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f48f89a..5e4c2fd 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -5,11 +5,11 @@ namespace Constants { const int k_NumberOfSwerveModules = 4; namespace Arm { - const float k_forearmLenInches = 24.0; // Update to correct value - const float k_bicepLenInches = 24.0;// Update to correct value + const double k_forearmLenInches = 24.0; // Update to correct value + const double k_bicepLenInches = 24.0;// Update to correct value - const int k_shoulderMinDeg = 0; - const int k_shoulderMaxDeg = 270; + const double k_shoulderMinDeg = 0; + const double k_shoulderMaxDeg = 270; }; }; diff --git a/src/main/include/Interfaces.h b/src/main/include/Interfaces.h index 7dfaf86..3072c7f 100644 --- a/src/main/include/Interfaces.h +++ b/src/main/include/Interfaces.h @@ -82,6 +82,12 @@ namespace Interfaces const int k_DIO8 = 8; const int k_DIO9 = 9; + // AIO + const int k_AIO0 = 0; + const int k_AIO1 = 1; + const int k_AIO2 = 2; + const int k_AIO3 = 3; + // Solenoids const int k_AIR0 = 0; const int k_AIR1 = 1; @@ -89,4 +95,21 @@ namespace Interfaces const int k_AIR7 = 7; } + namespace Arm { + + // Arm subsystem + const int k_TurretMotor = k_CAN10; + const int k_LowJointMotor = k_CAN11; + const int k_MidJointMotor = k_CAN12; + const int k_GripperRotateMotor = k_CAN13; + const int k_GripperGraspMotor = k_CAN14; + + const int k_TurretSensor = k_AIO0; // FIX VALS LATER + const int k_ElbowSensor = k_AIO1; + const int k_WristRollSensor = k_AIO2; + const int k_GripSensor = k_AIO3; + + const int k_ShoulderSensor = k_DIO0; + } + } diff --git a/src/main/include/commands/arm/MoveToIntake.h b/src/main/include/commands/arm/MoveToIntake.h new file mode 100644 index 0000000..8d40282 --- /dev/null +++ b/src/main/include/commands/arm/MoveToIntake.h @@ -0,0 +1,28 @@ +#pragma once + +#include "subsystems/arm/arm.h" +#include "subsystems/arm/motionPath.h" + +#include + +#include +#include + + +class MoveToIntakeCommand : public frc2::CommandHelper { +public: + MoveToIntakeCommand(ArmSubsystem * arm, Point finalTarget); + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; + +private: + ArmSubsystem * m_arm = nullptr; + Point m_finalTarget; + Point m_target; + std::optional m_path; + + Point m_currentPoint; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 77d4ed5..2330fb0 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -1,43 +1,67 @@ #pragma once -#include "armPose.h" -#include "point.h" -#include "boundary.h" +#include "Mandatory.h" +#include "subsystems/arm/armPose.h" +#include "subsystems/arm/boundary.h" +#include "subsystems/arm/motionPath.h" +#include "util/point.h" #include +#include -#include #include #include +#include + +#include + +using namespace Interfaces::Arm; class ArmSubsystem : public frc2::SubsystemBase { public: + ArmSubsystem(); + + void Periodic() override; + + void initialiseBoundary(); // Calulating Functions: Point calcElbowPos(double turretAng, double angShoulder); Point calcWristPos(double turretAng, double shoulderAng, double elbowAng); - ArmPose calcIKJointPoses(Point pt); + ArmPose calcIKJointPoses(Point const & pt); - bool checkBoundary(Boundary boundary, Point point); + bool isPointSafe(Point const & point); + bool isNearPoint(Point const & point); + MotionPath getPathTo(Point const & current, Point const & target); // Reading Functions: double getTurretAngle(); - - units::turn_t getShoulderAngle(); - + double getShoulderAngle(); double getElbowAngle(); - double getWristRollAngle(); + double getGrip(); + + // Getting Points: + Point getWristPoint(); - // double getWristPitchAngle(); // NOT IMPLEMENTED IN HARDWARE + Point const & getSafetyPoint(Point pt); + + Point const & getIntakePoint(); + Point const & getHomePoint(); + Point const & getHybridPoint(); + Point const & getLowPolePoint(); + Point const & getHighPolePoint(); + Point const & getLowShelfPoint(); + Point const & getHighShelfPoint(); void setTurretAngle(double angle); void setShoulderAngle(double angle); void setElbowAngle(double angle); void setWristRollAngle(double angle); + void setGrip(double grip); // hehe grippy grabby hand private: // Arm Diagram: @@ -50,13 +74,49 @@ class ArmSubsystem : public frc2::SubsystemBase { // [=====1720=====] // O O // **Not to scale + // Shoulder = Low Joint, Elbow = Mid Joint, Wrist = Gripper Rotate/Roll + + rev::CANSparkMax m_turretMotor { + k_TurretMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_lowJointMotor { + k_LowJointMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_midJointMotor { + k_MidJointMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_gripperRotateMotor { + k_GripperRotateMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + rev::CANSparkMax m_gripperGraspMotor { + k_GripperGraspMotor, + rev::CANSparkMaxLowLevel::MotorType::kBrushless + }; + + frc::AnalogPotentiometer m_turretAngleSensor {k_TurretSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_elbowAngleSensor {k_ElbowSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_wristRollAngleSensor {k_WristRollSensor, 1.0, 0.0}; + frc::AnalogPotentiometer m_gripSensor {k_GripSensor, 1.0, 0.0}; + frc::DutyCycleEncoder m_shoulderAngleSensor{k_ShoulderSensor}; // Using Funky Fresh Encoder + + std::shared_ptr m_noGoZone = nullptr; + + // Safety Points + Point m_safetyPointGrid{-0.2540, 0.0, 0.9144}; + Point m_safetyPointCenter{0.0, 0.254, 0.8635}; + Point m_safetyPointIntake{0.254, 0.0, -0.8128}; - frc::AnalogPotentiometer m_TurretAngleSensor {1, 1.0, 0}; - - frc::DutyCycleEncoder m_ShoulderAngleSensor{1}; // Using Funky Fresh Encoder - - frc::AnalogPotentiometer m_ElbowAngleSensor {1, 1.0, 0}; - - frc::AnalogPotentiometer m_WristRollAngleSensor {1, 1.0, 0}; - +public: + // Other Points + Point m_pointIntake{0.4064, 0, 0.1524}; + Point m_pointHome{0.0, 0.0, 0.0}; // ! ! Unknown ! ! + Point m_pointHybrid{-0.5715, 0.0, 0.2286}; + Point m_pointLowPole{-0.9589, 0.0, 1.0668}; + Point m_pointHighPole{-1.3907, 0.0, 1.2700}; + Point m_pointLowShelf{-0.9398, 0.0, 0.7620}; + Point m_pointHighShelf{-1.3335, 0.0, 1.0668}; }; diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 8010a1a..7b17dec 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -1,53 +1,131 @@ #pragma once -#include "arm.h" +#include "util/point.h" +#include "util/vector.h" +#include #include class Boundary { public: - virtual bool isInside(Point pt); - virtual bool isOutside(Point pt); + virtual bool isInside(Point const & pt) { return true; } + virtual bool isOutside(Point const & pt) { return true; } + }; class BoxBoundary : public Boundary { public: BoxBoundary( - float xLo, - float xHi, - float yLo, - float yHi, - float zLo, - float zHi) + double xLo, + double xHi, + double yLo, + double yHi, + double zLo, + double zHi) : xLo(xLo), xHi(xHi), yLo(yLo), yHi(yHi), zLo(zLo), zHi(zHi) { } - bool isInside(Point pt) override { + bool isInside(Point const & pt) override { return (xLo < pt.x && pt.x < xHi) && (yLo < pt.y && pt.y < yHi) && (zLo < pt.z && pt.z < zHi); } - bool isOutside(Point pt) override { + bool isOutside(Point const & pt) override { return !isInside(pt); } private: - float xLo; - float xHi; - float yLo; - float yHi; - float zLo; - float zHi; + double xLo; + double xHi; + double yLo; + double yHi; + double zLo; + double zHi; +}; + +// Polygon points must wind counter-clockwise. +class ConvexPolygonBoundary : public Boundary { +public: + ConvexPolygonBoundary(std::vector points, double zLo, double zHi) + : points(points), zLo(zLo), zHi(zHi) { + // Visit all points except the last point. + // Compute vector to following point. + for (unsigned int i = 0; i < points.size() - 1; i++) { + Vector v = points[i + 1] - points[i]; + vectors.push_back(Vector{-v.y, v.x, v.z}); + } + + // Visit the last point. + // Compute vector back to first point. + vectors.push_back(points[0] - points[points.size() - 1]); + } + + bool isInside(Point const & pt) override { + std::vector targets; + + for (unsigned int i = 0; i < points.size(); i++) { + targets.push_back(pt - points[i]); + } + + for (unsigned int i = 0; i < vectors.size(); i++) { + if (vectors[i].dot(targets[i]) < 0.0) { + return false; + } + } + + return true; + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + std::vector points; + std::vector vectors; + double zLo; + double zHi; +}; + +class CylinderBoundary : public Boundary { +public: + CylinderBoundary(Point center, double radius, double zLo, double zHi) + : center(center), radius(radius), zLo(zLo), zHi(zHi) {} + + bool isInside(Point const & pt) override { + // Check XY plane + Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); + double dist = + (offset.x * offset.x) + + (offset.y * offset.y) + + (offset.z * offset.z); + + if (dist < radius * radius) { + return zLo < pt.z && pt.z < zHi; + } else { + return true; + } + } + + bool isOutside(Point const & pt) override { + return !isInside(pt); + } + +private: + Point center; + double radius; + double zLo; + double zHi; }; class SphereBoundary : public Boundary { public: - SphereBoundary(Point center, float radius) : center(center), radius(radius) { } + SphereBoundary(Point center, double radius) : center(center), radius(radius) { } - bool isInside(Point pt) override { + bool isInside(Point const & pt) override { Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); - float dist = + double dist = (offset.x * offset.x) + (offset.y * offset.y) + (offset.z * offset.z); @@ -55,36 +133,36 @@ class SphereBoundary : public Boundary { return (dist < radius * radius); } - bool isOutside(Point pt) override { + bool isOutside(Point const & pt) override { return !isInside(pt); } private: Point center; - float radius; + double radius; }; class ComposeBoundary : public Boundary { public: - ComposeBoundary(std::vector> boundaries) + ComposeBoundary(std::vector> && boundaries) : boundaries(boundaries) { } - bool isInside(Point pt) override { + bool isInside(Point const & pt) override { for (auto& bound : boundaries) { - if (bound->isOutside(pt)) { - return false; + if (bound->isInside(pt)) { + return true; } } - return true; + return false; } - bool isOutside(Point pt) override { + bool isOutside(Point const & pt) override { return !isInside(pt); } private: - std::vector> boundaries; + std::vector> boundaries; }; class NotBoundary : public Boundary { @@ -92,11 +170,11 @@ class NotBoundary : public Boundary { NotBoundary(std::unique_ptr boundary) : m_Bound(std::move(boundary)) {} - bool isInside(Point pt) override { + bool isInside(Point const & pt) override { return m_Bound->isOutside(pt); } - bool isOutside(Point pt) override { + bool isOutside(Point const & pt) override { return !isInside(pt); } diff --git a/src/main/include/subsystems/arm/motionPath.h b/src/main/include/subsystems/arm/motionPath.h new file mode 100644 index 0000000..ac0f964 --- /dev/null +++ b/src/main/include/subsystems/arm/motionPath.h @@ -0,0 +1,59 @@ +#pragma once + +#include "util/point.h" +#include "subsystems/arm/arm.h" + +class MotionPath { +public: + MotionPath(std::vector path) : m_path(path) { + m_finalTarget = m_path[m_path.size() - 1]; + } + + Point interpolate(Point const & pt0, Point const & pt1); + + Point getNextPoint(); + + bool isDone(); + + // ArmSubsystem::isNearPoint(m_target) && m_path.size() == 1; + +private: + std::vector m_path; + + Point m_finalTarget; + Point m_target; +}; + +// command: +// MotionPath path; +// Point finalTarget; +// Point target = path.getNextPoint(); +// ArmSubsystem arm; +// loop { +// if (finalTarget.isNearPoint(target) && arm.isNearPoint(target)) { +// break; +// } else if (arm.isNearPoint(target)) { +// target = path.getNextPoint(); +// } + +// use ik to get pose +// apply to subsystem + +// } + + +// command: +// MotionPath path; +// Point finalTarget; +// ArmSubsystem arm; + +// loop { +// if (path.isDone()) { +// break; +// } else { +// point = path.getNextPoint(); + +// usee ik to get pose +// apply to subsystem. +// } +// } \ No newline at end of file diff --git a/src/main/include/util/math.h b/src/main/include/util/math.h new file mode 100644 index 0000000..dd2eacf --- /dev/null +++ b/src/main/include/util/math.h @@ -0,0 +1,3 @@ +#pragma once + +bool isNearZero(double val); \ No newline at end of file diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index e1f2fc8..3daf487 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -1,5 +1,11 @@ #pragma once +#include "util/vector.h" + +#include + +static const double NEAR_ZERO_METERS = 0.005; + class Point { public: Point(float x, float y, float z) : x(x), y(y), z(z) {} @@ -12,4 +18,19 @@ class Point { float x; float y; float z; + + + Vector operator- (Point const & rhs) const { + return Vector( + x - rhs.x, + y - rhs.y, + z - rhs.z + ); + } + + bool isNear(Point const & rhs) const { + return std::abs(x - rhs.x) < NEAR_ZERO_METERS + && std::abs(y - rhs.y) < NEAR_ZERO_METERS + && std::abs(z - rhs.z) < NEAR_ZERO_METERS; + } }; diff --git a/src/main/include/util/vector.h b/src/main/include/util/vector.h new file mode 100644 index 0000000..be14f23 --- /dev/null +++ b/src/main/include/util/vector.h @@ -0,0 +1,19 @@ +#pragma once + +class Vector { +public: + Vector(float x, float y, float z) : x(x), y(y), z(z) {} + Vector() { // Heehoo Default Constructor + x = 0; + y = 0; + z = 0; + } + + float dot(Vector & rhs) { + return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); + } + + float x; + float y; + float z; +}; \ No newline at end of file diff --git a/src/test/cpp/boundaryTest.cpp b/src/test/cpp/boundaryTest.cpp new file mode 100644 index 0000000..de509ce --- /dev/null +++ b/src/test/cpp/boundaryTest.cpp @@ -0,0 +1,36 @@ +#include + +#include "util/point.h" +#include "subsystems/arm/boundary.h" + +TEST(ConvexPolygonBoundaryTest, GivenPointInsideTriangle_IsInsideReturnsTrue) { + ConvexPolygonBoundary boundary { + { + Point{0.0, 1.0, 0.0}, + Point{-1.0, -1.0, 0.0}, + Point{1.0, -1.0, 0.0} + }, + 0.0, 1.0 + }; + + Point inTrianglePoint {0.0, 0.0, 0.5}; + + ASSERT_TRUE(boundary.isInside(inTrianglePoint)); + ASSERT_FALSE(boundary.isOutside(inTrianglePoint)); +} + +TEST(ConvexPolygonBoundaryTest, GivenPointOutsideTriangle_IsInsideReturnsFalse) { + ConvexPolygonBoundary boundary { + { + Point{0.0, 1.0, 0.0}, + Point{-1.0, -1.0, 0.0}, + Point{1.0, -1.0, 0.0} + }, + 0.0, 1.0 + }; + + Point inTrianglePoint {0.0, 2.0, 0.5}; + + ASSERT_FALSE(boundary.isInside(inTrianglePoint)); + ASSERT_TRUE(boundary.isOutside(inTrianglePoint)); +} \ No newline at end of file From e032a908dfed54cbca52315404e2e7b98dddcd1c Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 4 Mar 2023 16:36:33 -0500 Subject: [PATCH 021/100] Convert Point to use double instead of float Because Justin said so. - Emmie Jones --- src/main/include/util/point.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index 3daf487..4ec149a 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -8,16 +8,16 @@ static const double NEAR_ZERO_METERS = 0.005; class Point { public: - Point(float x, float y, float z) : x(x), y(y), z(z) {} + Point(double x, double y, double z) : x(x), y(y), z(z) {} Point() { // default constructor x = 0; y = 0; z = 0; } // default constructor - float x; - float y; - float z; + double x; + double y; + double z; Vector operator- (Point const & rhs) const { From 06d03b5b547a5510dd3370291ff4dd978106a665 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 6 Mar 2023 20:47:57 -0500 Subject: [PATCH 022/100] Fix headers - Justin C and Emmie Jones --- src/main/include/subsystems/arm/motionPath.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/include/subsystems/arm/motionPath.h b/src/main/include/subsystems/arm/motionPath.h index ac0f964..235254b 100644 --- a/src/main/include/subsystems/arm/motionPath.h +++ b/src/main/include/subsystems/arm/motionPath.h @@ -1,7 +1,8 @@ #pragma once #include "util/point.h" -#include "subsystems/arm/arm.h" + +#include class MotionPath { public: From 08bbd47a5396fcf3595f5217643955f57c2eb663 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 6 Mar 2023 20:48:13 -0500 Subject: [PATCH 023/100] Add incomplete implementation of motion path. - Justin C and Emmie Jones --- src/main/cpp/subsystems/arm/motionPath.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/main/cpp/subsystems/arm/motionPath.cpp diff --git a/src/main/cpp/subsystems/arm/motionPath.cpp b/src/main/cpp/subsystems/arm/motionPath.cpp new file mode 100644 index 0000000..5a8dff3 --- /dev/null +++ b/src/main/cpp/subsystems/arm/motionPath.cpp @@ -0,0 +1,14 @@ +#include "subsystems/arm/motionPath.h" + +Point MotionPath::getNextPoint() { + // FIXME: return the next step in the path. + return m_finalTarget; +} + +Point MotionPath::interpolate(Point const & p0, Point const & p1) { + return Point(p0); +} + +bool MotionPath::isDone() { + return m_path.size() == 1 && m_finalTarget.isNear(m_path[0]); +} \ No newline at end of file From 346aeeaa58550a49c93e99254826c6d8cf0bc4b4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 12:56:21 -0500 Subject: [PATCH 024/100] Add config limits and setpoints for arm subsystem - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 68 ++++++++++++++++++++++++++- src/main/deploy/config.toml | 33 +++++++++++++ src/main/include/subsystems/arm/arm.h | 62 +++++++++++++++++++++++- 3 files changed, 161 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 84987a6..01b0d89 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -4,6 +4,7 @@ #include #include +#include #include @@ -15,7 +16,9 @@ const double k_PickupXSize = 0.076; // meters const double k_PickupYSize = 0.340; // meters const double k_PickupZSize = 1.000; // meters -ArmSubsystem::ArmSubsystem() {} +ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { + loadConfig(toml); +} void ArmSubsystem::Periodic() { frc::SmartDashboard::PutNumber("Turret Angle", getTurretAngle()); @@ -246,3 +249,66 @@ void ArmSubsystem::setGrip(double grip) { // float ArmSubsystem::getWristPitchAngle() { // return 0; // TODO when we get reading components available // } + +/// LOCAL FREE +static double requireTomlDouble ( + std::shared_ptr toml, + std::string const & name +) { + auto val = toml->get_qualified_as(name); + + if (val) { + return *val; + } else { + std::stringstream ss; + ss << "Missing config option: arm." << name; + throw ss; + } +} + +/// PRIVATE +void ArmSubsystem::loadConfig(std::shared_ptr toml) { + // Load zero offsets. + + config.turret.zeroOffset = requireTomlDouble(toml, "turret.zeroOffsetRadians"); + config.shoulder.zeroOffset = requireTomlDouble(toml, "shoulder.zeroOffsetRadians"); + config.elbow.zeroOffset = requireTomlDouble(toml, "elbow.zeroOffsetRadians"); + config.wrist.zeroOffset = requireTomlDouble(toml, "wrist.zeroOffsetRadians"); + config.grip.zeroOffset = requireTomlDouble(toml, "grip.zeroOffsetMeters"); + + // Load conversion factors. + + config.turret.sensorToRadians = requireTomlDouble(toml, "turret.sensorToRadians"); + // No conversion factor for shoulder since sensor units are units::turns_t. + config.elbow.sensorToRadians = requireTomlDouble(toml, "elbow.sensorToRadians"); + config.wrist.sensorToRadians = requireTomlDouble(toml, "wrist.sensorToRadians"); + config.grip.sensorToMeters = requireTomlDouble(toml, "grip.sensorToMeters"); + + // Load raw limits and convert to radians or meters. + + config.turret.limit.lo = requireTomlDouble(toml, "turret.limit.lo") + * config.turret.sensorToRadians; + config.turret.limit.hi = requireTomlDouble(toml, "turret.limit.hi") + * config.turret.sensorToRadians; + + config.shoulder.limit.lo = requireTomlDouble(toml, "shoulder.limit.lo"); + config.shoulder.limit.hi = requireTomlDouble(toml, "shoulder.limit.hi"); + + config.elbow.limit.lo = requireTomlDouble(toml, "elbow.limit.lo") + * config.elbow.sensorToRadians; + config.elbow.limit.hi = requireTomlDouble(toml, "elbow.limit.hi") + * config.elbow.sensorToRadians; + + config.wrist.limit.lo = requireTomlDouble(toml, "wrist.limit.lo") + * config.wrist.sensorToRadians; + config.wrist.limit.hi = requireTomlDouble(toml, "wrist.limit.hi") + * config.wrist.sensorToRadians; + + config.grip.limit.lo = requireTomlDouble(toml, "grip.limit.lo") + * config.grip.sensorToMeters; + config.grip.limit.hi = requireTomlDouble(toml, "grip.limit.hi") + * config.grip.sensorToMeters; + + config.grip.setpoint.open = requireTomlDouble(toml, "grip.setpoint.open"); + config.grip.setpoint.close = requireTomlDouble(toml, "grip.setpoint.close"); +} diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index e69de29..d45ae0c 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -0,0 +1,33 @@ + +[arm] + # Limits are in raw sensors units + turret.limit.lo = 0 + turret.limit.hi = 0 + turret.zeroOffsetRadians = 0 + turret.sensorToRadians = 0 + + # Limits are in raw sensors units + shoulder.limit.lo = 0 + shoulder.limit.hi = 0 + shoulder.zeroOffsetRadians = 0 + + # Limits are in raw sensors units + elbow.limit.lo = 0 + elbow.limit.hi = 0 + elbow.zeroOffsetRadians = 0 + elbow.sensorToRadians = 0 + + # Limits are in raw sensors units + wrist.limit.lo = 0 + wrist.limit.hi = 0 + wrist.zeroOffsetRadians = 0 + wrist.sensorToRadians = 0 + + # Limits are in raw sensors units + grip.limit.lo = 0 + grip.limit.hi = 0 + # Grip closed is considered 0 meters. + grip.zeroOffsetMeters = 0 + grip.sensorToMeters = 0 + grip.setpoint.open = 0 + grip.setpoint.close = 0 diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 2330fb0..dd9c2c2 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -6,6 +6,8 @@ #include "subsystems/arm/motionPath.h" #include "util/point.h" +#include "external/cpptoml.h" + #include #include @@ -19,7 +21,7 @@ using namespace Interfaces::Arm; class ArmSubsystem : public frc2::SubsystemBase { public: - ArmSubsystem(); + ArmSubsystem(std::shared_ptr toml); void Periodic() override; @@ -64,6 +66,8 @@ class ArmSubsystem : public frc2::SubsystemBase { void setGrip(double grip); // hehe grippy grabby hand private: + void loadConfig(std::shared_ptr toml); + // Arm Diagram: // 1: Turret, 2: Shoulder, 3: Elbow, 4: Wrist // @@ -110,6 +114,62 @@ class ArmSubsystem : public frc2::SubsystemBase { Point m_safetyPointCenter{0.0, 0.254, 0.8635}; Point m_safetyPointIntake{0.254, 0.0, -0.8128}; + struct { + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } turret; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + } shoulder; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } elbow; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToRadians; + } wrist; + + struct { + struct { + double lo; + double hi; + } limit; + + double zeroOffset; + double sensorToMeters; + + struct { + double open; + double close; + } setpoint; + } grip; + } config; + public: // Other Points Point m_pointIntake{0.4064, 0, 0.1524}; From f7980870b0e7f8096ca5c85630b3716e20182a79 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 13:18:32 -0500 Subject: [PATCH 025/100] Apply conversion factors and zero offsets when reading sensors. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 01b0d89..841ce8a 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -163,23 +163,23 @@ Point const & ArmSubsystem::getHighShelfPoint() {// | High Shelf // Getting and setting arm angles: double ArmSubsystem::getTurretAngle() { - return m_turretAngleSensor.Get(); + return m_turretAngleSensor.Get() * config.turret.sensorToRadians + config.turret.zeroOffset; } double ArmSubsystem::getShoulderAngle() { - return m_shoulderAngleSensor.Get().value() * M_2_PI; + return m_shoulderAngleSensor.Get().value() * M_2_PI + config.shoulder.zeroOffset; } double ArmSubsystem::getElbowAngle() { - return m_elbowAngleSensor.Get(); + return m_elbowAngleSensor.Get() * config.elbow.sensorToRadians + config.elbow.zeroOffset; } double ArmSubsystem::getWristRollAngle() { - return m_wristRollAngleSensor.Get(); + return m_wristRollAngleSensor.Get() * config.wrist.sensorToRadians + config.wrist.zeroOffset; } double ArmSubsystem::getGrip() { - return m_gripSensor.Get(); + return m_gripSensor.Get() * config.grip.sensorToMeters + config.grip.zeroOffset; } Point ArmSubsystem::getWristPoint() { From 162a9460999c52d6aee54376d6db630c573be1d4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 13:19:00 -0500 Subject: [PATCH 026/100] Clamp values when adjust angles and distances. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 841ce8a..2ce98f8 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -201,6 +201,8 @@ Point const & ArmSubsystem::getSafetyPoint(Point pt) { } void ArmSubsystem::setTurretAngle(double angle) { + angle = std::clamp(angle, config.turret.limit.lo, config.turret.limit.hi); + double da = angle - getTurretAngle(); if (isNearZero(da)) { m_turretMotor.Set(0.0); @@ -210,6 +212,8 @@ void ArmSubsystem::setTurretAngle(double angle) { } void ArmSubsystem::setShoulderAngle(double angle) { + angle = std::clamp(angle, config.shoulder.limit.lo, config.shoulder.limit.hi); + double da = angle - getShoulderAngle(); if (isNearZero(da)) { m_lowJointMotor.Set(0.0); @@ -219,6 +223,8 @@ void ArmSubsystem::setShoulderAngle(double angle) { } void ArmSubsystem::setElbowAngle(double angle) { + angle = std::clamp(angle, config.elbow.limit.lo, config.elbow.limit.hi); + double da = angle - getElbowAngle(); if (isNearZero(da)) { m_midJointMotor.Set(0.0); @@ -228,6 +234,8 @@ void ArmSubsystem::setElbowAngle(double angle) { } void ArmSubsystem::setWristRollAngle(double angle) { + angle = std::clamp(angle, config.wrist.limit.lo, config.wrist.limit.hi); + double da = angle - getWristRollAngle(); if (isNearZero(da)) { m_gripperRotateMotor.Set(0.0); @@ -237,6 +245,8 @@ void ArmSubsystem::setWristRollAngle(double angle) { } void ArmSubsystem::setGrip(double grip) { + grip = std::clamp(grip, config.grip.limit.lo, config.grip.limit.hi); + double dx = grip - getGrip(); if (isNearZero(dx)) { m_gripperGraspMotor.Set(0.0); From 0340e64e273c4f2340d0104d7a128be947644323 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 13:26:08 -0500 Subject: [PATCH 027/100] Rename vars: Ang -> Angle - Justin C --- src/main/include/subsystems/arm/armPose.h | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/include/subsystems/arm/armPose.h b/src/main/include/subsystems/arm/armPose.h index c0c7496..a0d74e0 100644 --- a/src/main/include/subsystems/arm/armPose.h +++ b/src/main/include/subsystems/arm/armPose.h @@ -2,9 +2,12 @@ class ArmPose { public: - ArmPose(float turretAng, float shoulderAng, float elbowAng) : turretAng(turretAng), shoulderAng(shoulderAng), elbowAng(elbowAng) {} + ArmPose(float turretAngle, float shoulderAngle, float elbowAngle) + : turretAngle(turretAngle), + shoulderAngle(shoulderAngle), + elbowAngle(elbowAngle) {} - float turretAng; - float shoulderAng; - float elbowAng; + float turretAngle; + float shoulderAngle; + float elbowAngle; }; From 543ec576715c559ea42a4d1d789de4434f9782fc Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 13:38:10 -0500 Subject: [PATCH 028/100] arm: Add method moveToPoint() - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 14 ++++++++++++++ src/main/include/subsystems/arm/arm.h | 9 +++++++++ 2 files changed, 23 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 2ce98f8..1eb4c38 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -255,6 +255,20 @@ void ArmSubsystem::setGrip(double grip) { } } +bool ArmSubsystem::moveToPoint(Point const & target) { + if (!isPointSafe(target)) { + return false; + } + + ArmPose pose = calcIKJointPoses(target); + + setTurretAngle(pose.turretAngle); + setShoulderAngle(pose.shoulderAngle); + setElbowAngle(pose.elbowAngle); + + return true; +} + // NOT IMPLEMENTED IN HARDWARE // float ArmSubsystem::getWristPitchAngle() { // return 0; // TODO when we get reading components available diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index dd9c2c2..0f1a7ed 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -65,6 +65,15 @@ class ArmSubsystem : public frc2::SubsystemBase { void setWristRollAngle(double angle); void setGrip(double grip); // hehe grippy grabby hand + /** + * Given a target position, compute necessary joint angles, and move joints + * of arm toward target. Will NOT stop on its own. Must call this method + * repeatedly to stop on target. + * + * @return false if point is within the no-go zone, true if point is safe. + */ + bool moveToPoint(Point const & target); + private: void loadConfig(std::shared_ptr toml); From a08751abd5fc2c5a6f3d2a68af98c9e26906e0fe Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 13:39:17 -0500 Subject: [PATCH 029/100] arm: Rename getWristPoint() to getGripPoint() Likewise, rename calcWristPos() to calcGripPos(). The new name accurately reflects which point we expect from the calculation. - Justin C --- src/main/cpp/commands/arm/MoveToIntake.cpp | 4 ++-- src/main/cpp/subsystems/arm/arm.cpp | 12 ++++-------- src/main/include/subsystems/arm/arm.h | 4 ++-- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/cpp/commands/arm/MoveToIntake.cpp b/src/main/cpp/commands/arm/MoveToIntake.cpp index b88b86f..eec571f 100644 --- a/src/main/cpp/commands/arm/MoveToIntake.cpp +++ b/src/main/cpp/commands/arm/MoveToIntake.cpp @@ -13,7 +13,7 @@ MoveToIntakeCommand::MoveToIntakeCommand(ArmSubsystem * arm, Point currentPoint) void MoveToIntakeCommand::Initialize() { m_finalTarget = m_arm->m_pointIntake; - Point currentPoint = m_arm->getWristPoint(); + Point currentPoint = m_arm->getGripPoint(); m_path.emplace(std::move(m_arm->getPathTo(currentPoint, m_finalTarget))); m_target = m_path->getNextPoint(); @@ -40,4 +40,4 @@ void MoveToIntakeCommand::End(bool isInterrupted) { bool MoveToIntakeCommand::IsFinished() { return !m_arm->isPointSafe(m_target) || (m_finalTarget.isNear(m_target) && m_arm->isNearPoint(m_target)); -} \ No newline at end of file +} diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 1eb4c38..1c3cd3e 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -70,7 +70,7 @@ Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { return pt; } -Point ArmSubsystem::calcWristPos( +Point ArmSubsystem::calcGripPos( double turretAng, double shoulderAng, double elbowAng @@ -122,11 +122,7 @@ bool ArmSubsystem::isPointSafe(Point const & point) { } bool ArmSubsystem::isNearPoint(Point const & point) { - Point currentPosition = calcWristPos( - getTurretAngle(), - getShoulderAngle(), - getElbowAngle() - ); + Point currentPosition = getGripPoint(); return point.isNear(currentPosition); } @@ -182,8 +178,8 @@ double ArmSubsystem::getGrip() { return m_gripSensor.Get() * config.grip.sensorToMeters + config.grip.zeroOffset; } -Point ArmSubsystem::getWristPoint() { - return calcWristPos( +Point ArmSubsystem::getGripPoint() { + return calcGripPos( getTurretAngle(), getShoulderAngle(), getElbowAngle() diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 0f1a7ed..f19df31 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -30,7 +30,7 @@ class ArmSubsystem : public frc2::SubsystemBase { // Calulating Functions: Point calcElbowPos(double turretAng, double angShoulder); - Point calcWristPos(double turretAng, double shoulderAng, double elbowAng); + Point calcGripPos(double turretAng, double shoulderAng, double elbowAng); ArmPose calcIKJointPoses(Point const & pt); @@ -47,7 +47,7 @@ class ArmSubsystem : public frc2::SubsystemBase { double getGrip(); // Getting Points: - Point getWristPoint(); + Point getGripPoint(); Point const & getSafetyPoint(Point pt); From 8fe373aed0a6af690fdd0ef7ad05e913da982a65 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 17:54:35 -0500 Subject: [PATCH 030/100] Update constants for arm subsystem Set the correct lengths for arm segments. Remove unused arm constants. Convert lengths from inches to meters. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 24 ++++++++++++------------ src/main/include/Constants.h | 7 ++----- 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 1c3cd3e..30df5d7 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -62,9 +62,9 @@ void ArmSubsystem::initialiseBoundary() { Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { Point pt{ - Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::cos(turretAng), - Constants::Arm::k_bicepLenInches * std::cos(shoulderAng) * std::sin(turretAng), - Constants::Arm::k_bicepLenInches * std::sin(shoulderAng), + Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::cos(turretAng), + Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::sin(turretAng), + Constants::Arm::k_bicepLenMeters * std::sin(shoulderAng), }; return pt; @@ -77,9 +77,9 @@ Point ArmSubsystem::calcGripPos( ) { Point elbowPos = calcElbowPos(turretAng, shoulderAng); Point pt( - Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), - Constants::Arm::k_forearmLenInches * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), - Constants::Arm::k_forearmLenInches * std::sin(shoulderAng + elbowAng) + Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), + Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), + Constants::Arm::k_forearmLenMeters * std::sin(shoulderAng + elbowAng) ); return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, elbowPos.z + pt.z); @@ -90,10 +90,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); double targetToBicepAng = std::acos( - (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches - - Constants::Arm::k_bicepLenInches * Constants::Arm::k_bicepLenInches + (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + targetLen * targetLen) - / (2.0 * Constants::Arm::k_forearmLenInches * targetLen) + / (2.0 * Constants::Arm::k_forearmLenMeters * targetLen) ); // Solve IK: @@ -102,10 +102,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double bicepToForearmAng = (std::numbers::pi / 2.0) - std::acos( - (Constants::Arm::k_forearmLenInches * Constants::Arm::k_forearmLenInches - - Constants::Arm::k_bicepLenInches * Constants::Arm::k_bicepLenInches + (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + targetLen * targetLen) - / (2.0 * Constants::Arm::k_forearmLenInches * targetLen)); + / (2.0 * Constants::Arm::k_forearmLenMeters * targetLen)); double turretToXZAng = std::atan2(pt.z, pt.x); diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index d010774..f245d32 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -10,10 +10,7 @@ namespace Constants { const double k_maxSpinSpeed = 1.00; namespace Arm { - const double k_forearmLenInches = 24.0; // Update to correct value - const double k_bicepLenInches = 24.0;// Update to correct value - - const double k_shoulderMinDeg = 0; - const double k_shoulderMaxDeg = 270; + const double k_forearmLenMeters = 32.0 /*inch*/ * 0.0254; + const double k_bicepLenMeters = 36.0 /*inch*/ * 0.0254; }; }; From 92f59ef48ff270e9417e43f105f266525a18de20 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 18:07:18 -0500 Subject: [PATCH 031/100] arm: Add comment specifying geometric conventions. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 30df5d7..425a835 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -29,6 +29,15 @@ void ArmSubsystem::Periodic() { } void ArmSubsystem::initialiseBoundary() { + // Geometric Conventions: + // x-axis 0 :: center of arm turret. + // y-axis 0 :: center of arm turret. + // z-axis 0 :: floor. + // + // x+ :: robot right side (with pickup gap in bumpers). + // y+ :: robot front (furthest side from turret). + // z+ :: up. + // Only create boundaries for no-go zones. // Otherwise, the code gets really complicated later // to check for collisions. From 51a1475ccb0ffed92a8c5dc24c3d96b0375a08b8 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 18:17:56 -0500 Subject: [PATCH 032/100] arm: Fix CylinderBoundary check for point isInside Ignore z component of center point. That 3d point is only used to specify the 2d center on the x-y plane.0 - Justin C --- src/main/include/subsystems/arm/boundary.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 7b17dec..542c996 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -94,11 +94,10 @@ class CylinderBoundary : public Boundary { bool isInside(Point const & pt) override { // Check XY plane - Point offset((center.x - pt.x), (center.y - pt.y), (center.z - pt.z)); + Point offset((center.x - pt.x), (center.y - pt.y), 0.0); double dist = (offset.x * offset.x) - + (offset.y * offset.y) - + (offset.z * offset.z); + + (offset.y * offset.y); if (dist < radius * radius) { return zLo < pt.z && pt.z < zHi; From 4f0dee77738028d9d7617fc59e7c6c71c0130474 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 18:27:37 -0500 Subject: [PATCH 033/100] arm: Update no go zones - Add no go zone around turret (cyclinder). - Add no go zone beyond 95% of arm's reach (inverse sphere). - Add no go zone 1 inch above floor (box). - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 425a835..bbd356e 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -60,10 +60,32 @@ void ArmSubsystem::initialiseBoundary() { ) }); - //std::unique_ptr<> turretNoGoZone = std::make_unique<> + std::shared_ptr cannotReachNoGoBounds = std::make_shared( + std::make_unique( + Point { 0.0, 0.0, k_ChassisZSize }, /* center of turret */ + (Constants::Arm::k_forearmLenMeters + + Constants::Arm::k_bicepLenMeters + ) * 0.95 /* 95% of max length of arm */ + ) + ); + + std::shared_ptr turretNoGoZone = std::make_shared( + Point { 0.0, 0.0, 0.0 }, + 0.08, /* radius meters */ + 0.0, 1.0 + ); + + std::shared_ptr floorNoGoZone = std::make_shared( + -5.0, 5.0, + -5.0, 5.0, + 0.0254, -5.0 /* keep gripper 1 inch from floor */ + ); std::shared_ptr defNoGoZone = std::make_shared(std::vector{ - std::move(chassisNoGoBounds) + std::move(cannotReachNoGoBounds), + std::move(chassisNoGoBounds), + std::move(turretNoGoZone), + std::move(floorNoGoZone), }); m_noGoZone = std::move(defNoGoZone); From 72fed2879d3bcb5ba73f98638b81475fb5018e58 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 19:22:32 -0500 Subject: [PATCH 034/100] arm: Add arm subsystem to robot - Justin C --- src/main/cpp/Robot.cpp | 1 + src/main/include/Robot.h | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4caf50b..da89e0e 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -33,6 +33,7 @@ void Robot::RobotInit() { //Subsystems c_drivetrain = new Drivetrain(false); c_odometry = new Odometry(c_drivetrain); + c_arm = new ArmSubsystem(c_toml->get_table("arm")); //Commands c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 6311381..ab718fb 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -13,6 +13,7 @@ #include +#include "subsystems/arm/arm.h" #include "subsystems/drivetrain/drivetrain.h" #include "subsystems/drivetrain/odometry.h" #include "commands/drivetrain/driveTeleopCommand.h" @@ -47,6 +48,7 @@ class Robot : public frc::TimedRobot { //Subsystems Drivetrain* c_drivetrain = nullptr; Odometry* c_odometry = nullptr; + ArmSubsystem* c_arm = nullptr; //Commands DriveTeleopCommand* c_driveTeleopCommand = nullptr; From 056068fb70102d048be5712752dfc9b2b81cdf88 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 19:25:18 -0500 Subject: [PATCH 035/100] Add Point + Vector -> Point overloaded operator - Justin C --- src/main/include/util/point.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index 4ec149a..e9e2586 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -28,6 +28,14 @@ class Point { ); } + Point operator+ (Vector const & rhs) const { + return Point { + x + rhs.x, + y + rhs.y, + z + rhs.z + }; + } + bool isNear(Point const & rhs) const { return std::abs(x - rhs.x) < NEAR_ZERO_METERS && std::abs(y - rhs.y) < NEAR_ZERO_METERS From 4d104f3543981d6ab54d27c3651a428b05167210 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 19:26:07 -0500 Subject: [PATCH 036/100] test: Add tests for box and sphere boundaries - Justin C --- src/test/cpp/boundaryTest.cpp | 52 ++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/src/test/cpp/boundaryTest.cpp b/src/test/cpp/boundaryTest.cpp index de509ce..a658d9b 100644 --- a/src/test/cpp/boundaryTest.cpp +++ b/src/test/cpp/boundaryTest.cpp @@ -3,6 +3,32 @@ #include "util/point.h" #include "subsystems/arm/boundary.h" +TEST(BoxBoundaryTest, GivenPointInsideBox_IsInsideReturnsTrue) { + BoxBoundary boundary { + -1.0, 1.0, + -1.0, 1.0, + -1.0, 1.0 + }; + + Point insidePoint {0.0, 0.0, 0.0}; + + ASSERT_TRUE(boundary.isInside(insidePoint)); + ASSERT_FALSE(boundary.isOutside(insidePoint)); +} + +TEST(BoxBoundaryTest, GivenPointOutsideBox_IsInsideReturnsFalse) { + BoxBoundary boundary { + -1.0, 1.0, + -1.0, 1.0, + -1.0, 1.0 + }; + + Point outsidePoint {2.0, 0.0, 0.0}; + + ASSERT_FALSE(boundary.isInside(outsidePoint)); + ASSERT_TRUE(boundary.isOutside(outsidePoint)); +} + TEST(ConvexPolygonBoundaryTest, GivenPointInsideTriangle_IsInsideReturnsTrue) { ConvexPolygonBoundary boundary { { @@ -33,4 +59,28 @@ TEST(ConvexPolygonBoundaryTest, GivenPointOutsideTriangle_IsInsideReturnsFalse) ASSERT_FALSE(boundary.isInside(inTrianglePoint)); ASSERT_TRUE(boundary.isOutside(inTrianglePoint)); -} \ No newline at end of file +} + +TEST(SphereBoundaryTest, GivenPointInsideSphere_IsInsideReturnsTrue) { + SphereBoundary boundary { + Point{0.0, 0.0, 0.0}, + 1.0 + }; + + Point insidePoint {0.0, 0.0, 0.5}; + + ASSERT_TRUE(boundary.isInside(insidePoint)); + ASSERT_FALSE(boundary.isOutside(insidePoint)); +} + +TEST(SphereBoundaryTest, GivenPointOutsideSphere_IsInsideReturnsFalse) { + SphereBoundary boundary { + Point{0.0, 0.0, 0.0}, + 1.0 + }; + + Point outsidePoint {2.0, 0.0, 0.0}; + + ASSERT_FALSE(boundary.isInside(outsidePoint)); + ASSERT_TRUE(boundary.isOutside(outsidePoint)); +} From 38df1eebb5353857c8aa7ba1a92d7e43c6b572b3 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 20:05:14 -0500 Subject: [PATCH 037/100] arm: Set config values - calibration Note: wrist and gripper disassembled and need recalibration. - Justin C --- src/main/deploy/config.toml | 38 ++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index d45ae0c..68483b5 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -1,33 +1,33 @@ [arm] # Limits are in raw sensors units - turret.limit.lo = 0 - turret.limit.hi = 0 - turret.zeroOffsetRadians = 0 - turret.sensorToRadians = 0 + turret.limit.lo = 0.34 + turret.limit.hi = 0.59 + turret.zeroOffsetRadians = -6.377 + turret.sensorToRadians = 13.426 # Limits are in raw sensors units - shoulder.limit.lo = 0 - shoulder.limit.hi = 0 - shoulder.zeroOffsetRadians = 0 + shoulder.limit.lo = 0.071 + shoulder.limit.hi = 0.646 + shoulder.zeroOffsetRadians = 0.131 # Limits are in raw sensors units - elbow.limit.lo = 0 - elbow.limit.hi = 0 - elbow.zeroOffsetRadians = 0 - elbow.sensorToRadians = 0 + elbow.limit.lo = 0.283 + elbow.limit.hi = 0.414 + elbow.zeroOffsetRadians = -7.018 + elbow.sensorToRadians = 24.54 # Limits are in raw sensors units - wrist.limit.lo = 0 - wrist.limit.hi = 0 - wrist.zeroOffsetRadians = 0 - wrist.sensorToRadians = 0 + wrist.limit.lo = 0.650 + wrist.limit.hi = 0.697 + wrist.zeroOffsetRadians = -43.446 + wrist.sensorToRadians = 66.84 # Limits are in raw sensors units - grip.limit.lo = 0 - grip.limit.hi = 0 + grip.limit.lo = 0.511 + grip.limit.hi = 0.522 # Grip closed is considered 0 meters. - grip.zeroOffsetMeters = 0 - grip.sensorToMeters = 0 + grip.zeroOffsetMeters = 12.338 + grip.sensorToMeters = -23.636 grip.setpoint.open = 0 grip.setpoint.close = 0 From f80676fc3f2b85a309c2fb7655a2bd05a062fdda Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 20:39:24 -0500 Subject: [PATCH 038/100] arm: Report arm angles on dashboard in degrees Also fix grip value. It's distance, not rotation. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 40 +++++++++++++++++++++++++---- 1 file changed, 35 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index bbd356e..894fc04 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -16,16 +17,45 @@ const double k_PickupXSize = 0.076; // meters const double k_PickupYSize = 0.340; // meters const double k_PickupZSize = 1.000; // meters +#define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) + ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { loadConfig(toml); } void ArmSubsystem::Periodic() { - frc::SmartDashboard::PutNumber("Turret Angle", getTurretAngle()); - frc::SmartDashboard::PutNumber("Shoulder Angle", getShoulderAngle()); - frc::SmartDashboard::PutNumber("Elbow Angle", getElbowAngle()); - frc::SmartDashboard::PutNumber("Wrist Roll Angle", getWristRollAngle()); - frc::SmartDashboard::PutNumber("Grip Angle", getGrip()); + frc::SmartDashboard::PutNumber("Turret Angle (deg)", RAD_2_DEG(getTurretAngle())); + frc::SmartDashboard::PutNumber("Shoulder Angle (deg)", RAD_2_DEG(getShoulderAngle())); + frc::SmartDashboard::PutNumber("Elbow Angle (deg)", RAD_2_DEG(getElbowAngle())); + frc::SmartDashboard::PutNumber("Wrist Roll Angle (deg)", RAD_2_DEG(getWristRollAngle())); + frc::SmartDashboard::PutNumber("Grip Distance (meters)", getGrip()); + + // Raw values + frc::SmartDashboard::PutNumber("Raw: Turret Angle (V)", m_turretAngleSensor.Get()); + frc::SmartDashboard::PutNumber("Raw: Shoulder Angle (deg)", m_shoulderAngleSensor.Get().value() * 360.0); + frc::SmartDashboard::PutNumber("Raw: Elbow Angle (V)", m_elbowAngleSensor.Get()); + frc::SmartDashboard::PutNumber("Raw: Wrist Roll Angle (V)", m_wristRollAngleSensor.Get()); + frc::SmartDashboard::PutNumber("Raw: Grip Distance (V)", m_gripSensor.Get()); + + // Report calculated gripper position. + + Point gripPos = getGripPoint(); + std::stringstream gripPosStr; + + gripPosStr << std::fixed << std::setprecision(4) + << "(" << gripPos.x + << ", " << gripPos.y + << ", " << gripPos.z + << ")"; + frc::SmartDashboard::PutString("Grip Pos (m)", gripPosStr.str()); + + // Report calculated arm pose angles. What the robot thinks the angles + // should be to reach gripper position. + + ArmPose pose = calcIKJointPoses(gripPos); + frc::SmartDashboard::PutNumber("IK Turret Angle (deg)", RAD_2_DEG(pose.turretAngle)); + frc::SmartDashboard::PutNumber("IK Shoulder Angle (deg)", RAD_2_DEG(pose.shoulderAngle)); + frc::SmartDashboard::PutNumber("IK Elbow Angle (deg)", RAD_2_DEG(pose.elbowAngle)); } void ArmSubsystem::initialiseBoundary() { From e94e38cf8080acd9ff1dfabc1fa8ba3b2c7405f4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 8 Mar 2023 23:03:42 -0500 Subject: [PATCH 039/100] Fix: M_2_PI != 2 * M_PI Whoops. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 894fc04..615447f 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -224,7 +224,7 @@ double ArmSubsystem::getTurretAngle() { } double ArmSubsystem::getShoulderAngle() { - return m_shoulderAngleSensor.Get().value() * M_2_PI + config.shoulder.zeroOffset; + return m_shoulderAngleSensor.Get().value() * 2.0 * M_PI + config.shoulder.zeroOffset; } double ArmSubsystem::getElbowAngle() { From a9d5d162d1a158e4bdd380753749a68f13a1f7e3 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 13:26:28 -0500 Subject: [PATCH 040/100] arm: typo in forward kinematics Angle conventions require elbowPos - pt instead of elbowPos + pt. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 615447f..b41353e 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -143,7 +143,7 @@ Point ArmSubsystem::calcGripPos( Constants::Arm::k_forearmLenMeters * std::sin(shoulderAng + elbowAng) ); - return Point(elbowPos.x + pt.x, elbowPos.y + pt.y, elbowPos.z + pt.z); + return Point(elbowPos.x - pt.x, elbowPos.y - pt.y, elbowPos.z - pt.z); } ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { From c4f72cd271e4e66df79cffce5344d3969af2dc54 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 13:48:48 -0500 Subject: [PATCH 041/100] arm: Document and fix turrent angle convention - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 4 ++-- src/main/include/subsystems/arm/arm.h | 10 ++++++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index b41353e..0e52cbd 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -123,8 +123,8 @@ void ArmSubsystem::initialiseBoundary() { Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { Point pt{ - Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::cos(turretAng), Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::sin(turretAng), + Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::cos(turretAng), Constants::Arm::k_bicepLenMeters * std::sin(shoulderAng), }; @@ -138,8 +138,8 @@ Point ArmSubsystem::calcGripPos( ) { Point elbowPos = calcElbowPos(turretAng, shoulderAng); Point pt( - Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::sin(turretAng), + Constants::Arm::k_forearmLenMeters * std::cos(shoulderAng + elbowAng) * std::cos(turretAng), Constants::Arm::k_forearmLenMeters * std::sin(shoulderAng + elbowAng) ); diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index f19df31..bbb7003 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -88,6 +88,16 @@ class ArmSubsystem : public frc2::SubsystemBase { // O O // **Not to scale // Shoulder = Low Joint, Elbow = Mid Joint, Wrist = Gripper Rotate/Roll + // + // Turret angle conventions/reference + // 0 deg + // ┌──────────┐ + // │ │ + // │ │ + // │ │ + // -90 deg │ o │ 90 deg + // │ │ + // └──────────┘ rev::CANSparkMax m_turretMotor { k_TurretMotor, From 02af6d2c6d1b8d0375d339260724d906b5371617 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:16:40 -0500 Subject: [PATCH 042/100] util: replace float with double in Vector - Justin C --- src/main/include/util/vector.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/include/util/vector.h b/src/main/include/util/vector.h index be14f23..dad378c 100644 --- a/src/main/include/util/vector.h +++ b/src/main/include/util/vector.h @@ -2,18 +2,18 @@ class Vector { public: - Vector(float x, float y, float z) : x(x), y(y), z(z) {} + Vector(double x, double y, double z) : x(x), y(y), z(z) {} Vector() { // Heehoo Default Constructor x = 0; y = 0; z = 0; } - float dot(Vector & rhs) { + double dot(Vector & rhs) { return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); } - float x; - float y; - float z; -}; \ No newline at end of file + double x; + double y; + double z; +}; From d3e2f38293f607d2bdf324b9ac5cec49894e2bca Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:17:58 -0500 Subject: [PATCH 043/100] arm: Remove duplicate Point class point.h was moved under util. - Justin C --- src/main/include/subsystems/arm/point.h | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 src/main/include/subsystems/arm/point.h diff --git a/src/main/include/subsystems/arm/point.h b/src/main/include/subsystems/arm/point.h deleted file mode 100644 index d0f6130..0000000 --- a/src/main/include/subsystems/arm/point.h +++ /dev/null @@ -1,10 +0,0 @@ -#pragma once - -class Point { - public: - Point(float x, float y, float z) : x(x), y(y), z(z) {} - - float x; - float y; - float z; -}; \ No newline at end of file From 2a24e85061c9b5050e43c81820ac979f6c5d6f50 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:33:10 -0500 Subject: [PATCH 044/100] arm: Reapply desmos ik solver - Rename some vars consistent with desmos ik solver. - Rename output vars turrent, shoulder, elbow. - Add comments. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 0e52cbd..738be46 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -147,10 +147,14 @@ Point ArmSubsystem::calcGripPos( } ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { + // See arm.h for diagram of robot arm angle reference conventions. + // desmos 2d IK solver demo: https://www.desmos.com/calculator/p3uouu2un2 + double targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target - double targetToXAxisAng = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); - double targetToBicepAng = std::acos( + double c3 = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); + + double c1 = std::acos( (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + targetLen * targetLen) @@ -158,9 +162,9 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { ); // Solve IK: - double bicepToXAxisAng = targetToBicepAng + targetToXAxisAng; + double shoulderAngle = c1 + c3; - double bicepToForearmAng = + double elbowAngle = (std::numbers::pi / 2.0) - std::acos( (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters @@ -168,9 +172,9 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { + targetLen * targetLen) / (2.0 * Constants::Arm::k_forearmLenMeters * targetLen)); - double turretToXZAng = std::atan2(pt.z, pt.x); + double turretAngle = std::atan2(-pt.x, pt.y); - return ArmPose(turretToXZAng, bicepToXAxisAng, bicepToForearmAng); + return ArmPose(turretAngle, shoulderAngle, elbowAngle); } From a01cc69fd842c8a3c10bd94604ebabc1f596c590 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:36:51 -0500 Subject: [PATCH 045/100] arm: Reapply desmos ik solver: pt 2 Add max length constraint. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 20 ++++++++++++++------ src/main/cpp/util/vector.cpp | 9 +++++++++ src/main/include/util/point.h | 8 ++++++++ src/main/include/util/vector.h | 21 +++++++++++++++++++++ 4 files changed, 52 insertions(+), 6 deletions(-) create mode 100644 src/main/cpp/util/vector.cpp diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 738be46..29d68f1 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -152,13 +152,21 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target - double c3 = atan2(pt.z, std::sqrt(pt.x * pt.x + pt.y * pt.y)); + // Constrain target point. + double constrainLen = std::min( + Constants::Arm::k_forearmLenMeters + Constants::Arm::k_bicepLenMeters * 0.95, /* Safety margin. Limit length to 95% of max. */ + targetLen + ); + + Point cp = Vector(pt.x, pt.y, pt.z).unit() * constrainLen; + + double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); double c1 = std::acos( (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters - + targetLen * targetLen) - / (2.0 * Constants::Arm::k_forearmLenMeters * targetLen) + + constrainLen * constrainLen) + / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen) ); // Solve IK: @@ -169,10 +177,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { - std::acos( (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters - + targetLen * targetLen) - / (2.0 * Constants::Arm::k_forearmLenMeters * targetLen)); + + constrainLen * constrainLen) + / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen)); - double turretAngle = std::atan2(-pt.x, pt.y); + double turretAngle = std::atan2(-cp.x, cp.y); return ArmPose(turretAngle, shoulderAngle, elbowAngle); diff --git a/src/main/cpp/util/vector.cpp b/src/main/cpp/util/vector.cpp new file mode 100644 index 0000000..87adf63 --- /dev/null +++ b/src/main/cpp/util/vector.cpp @@ -0,0 +1,9 @@ +#include "util/vector.h" + +Vector operator*(Vector const & a, double b) { + return Vector(a.x * b, a.y * b, a.z * b); +} + +Vector operator*(double a, Vector const & b) { + return Vector(a * b.x, a * b.y, a * b.z); +} diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index e9e2586..e445bef 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -36,6 +36,14 @@ class Point { }; } + Point operator- (Vector const & rhs) const { + return Point { + x - rhs.x, + y - rhs.y, + z - rhs.z + }; + } + bool isNear(Point const & rhs) const { return std::abs(x - rhs.x) < NEAR_ZERO_METERS && std::abs(y - rhs.y) < NEAR_ZERO_METERS diff --git a/src/main/include/util/vector.h b/src/main/include/util/vector.h index dad378c..95c262c 100644 --- a/src/main/include/util/vector.h +++ b/src/main/include/util/vector.h @@ -1,5 +1,9 @@ #pragma once +#include "util/math.h" + +#include + class Vector { public: Vector(double x, double y, double z) : x(x), y(y), z(z) {} @@ -13,7 +17,24 @@ class Vector { return (x * rhs.x) + (y * rhs.y) + (z * rhs.z); } + double len() { + return std::sqrt(x * x + y * y + z * z); + } + + Vector unit() { + double m = len(); + + if (isNearZero(m)) { + return Vector(1.0, 0.0, 0.0); + } else { + return Vector(x / m, y / m, z / m); + } + } + double x; double y; double z; }; + +Vector operator* (Vector const &, double); +Vector operator* (double, Vector const &); From 90e34d8abf537628ed0a7859ba74862d404cd3f0 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:40:35 -0500 Subject: [PATCH 046/100] arm: Reapply desmos ik solver: pt 3 Fix elbow angle calculation. Most parts are there, but those are out of order. Missing others. Appears to be failed copy of shoulder angle calculation. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 29d68f1..56ff4bb 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -173,12 +173,16 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double shoulderAngle = c1 + c3; double elbowAngle = - (std::numbers::pi / 2.0) + std::numbers::pi - std::acos( - (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters - - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters - + constrainLen * constrainLen) - / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen)); + ( + Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + - Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + + constrainLen * constrainLen + ) + / (2.0 * Constants::Arm::k_bicepLenMeters * constrainLen) + ) + - c1; double turretAngle = std::atan2(-cp.x, cp.y); From e86e93f3a46cef0c8aac6e0b2de41b5c77a2b415 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 14:51:51 -0500 Subject: [PATCH 047/100] arm: adjust target z from floor to turret in ik solver - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 56ff4bb..65535bf 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -158,7 +158,7 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { targetLen ); - Point cp = Vector(pt.x, pt.y, pt.z).unit() * constrainLen; + Point cp = Point(0.0, 0.0, k_ChassisZSize) - Vector(pt.x, pt.y, pt.z).unit() * constrainLen; double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); From dd4dc4edea62f8f8a79cc7ccf87ed0d0d029b5e9 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 15:59:55 -0500 Subject: [PATCH 048/100] Refactor operator overloads in point.h and vector.h Trying to avoid cyclic dependencies with header files. - Justin C --- src/main/cpp/commands/arm/MoveToIntake.cpp | 2 +- .../cpp/subsystems/drivetrain/drivetrain.cpp | 3 -- src/main/cpp/util/geom.cpp | 33 +++++++++++++++++++ src/main/cpp/util/vector.cpp | 7 ---- src/main/include/commands/arm/MoveToIntake.h | 4 +-- src/main/include/subsystems/arm/arm.h | 2 +- src/main/include/subsystems/arm/boundary.h | 3 +- src/main/include/subsystems/arm/motionPath.h | 4 +-- .../subsystems/drivetrain/drivetrain.h | 2 +- .../include/subsystems/drivetrain/odometry.h | 2 +- src/main/include/util/geom.h | 12 +++++++ src/main/include/util/point.h | 25 -------------- src/main/include/util/vector.h | 3 -- 13 files changed, 54 insertions(+), 48 deletions(-) create mode 100644 src/main/cpp/util/geom.cpp create mode 100644 src/main/include/util/geom.h diff --git a/src/main/cpp/commands/arm/MoveToIntake.cpp b/src/main/cpp/commands/arm/MoveToIntake.cpp index eec571f..e79a924 100644 --- a/src/main/cpp/commands/arm/MoveToIntake.cpp +++ b/src/main/cpp/commands/arm/MoveToIntake.cpp @@ -1,5 +1,5 @@ #include "commands/arm/MoveToIntake.h" -#include "util/point.h" + #include "subsystems/arm/motionPath.h" #include "subsystems/arm/arm.h" #include "subsystems/arm/armPose.h" diff --git a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp index 3f3e0c0..f08f079 100644 --- a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp +++ b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp @@ -4,9 +4,6 @@ #define _USE_MATH_DEFINES #include -#include "util/point.h" -#include "util/polar.h" - #include "subsystems/drivetrain/swerveWheel.h" #include "subsystems/drivetrain/swerveWheelTypes.h" diff --git a/src/main/cpp/util/geom.cpp b/src/main/cpp/util/geom.cpp new file mode 100644 index 0000000..4bdf75c --- /dev/null +++ b/src/main/cpp/util/geom.cpp @@ -0,0 +1,33 @@ +#include "util/geom.h" + +Vector operator- (Point const & lhs, Point const & rhs) { + return Vector( + lhs.x - rhs.x, + lhs.y - rhs.y, + lhs.z - rhs.z + ); +} + +Point operator+ (Point const & lhs, Vector const & rhs) { + return Point { + lhs.x + rhs.x, + lhs.y + rhs.y, + lhs.z + rhs.z + }; +} + +Point operator- (Point const & lhs, Vector const & rhs) { + return Point { + lhs.x - rhs.x, + lhs.y - rhs.y, + lhs.z - rhs.z + }; +} + +Vector operator*(Vector const & a, double b) { + return Vector(a.x * b, a.y * b, a.z * b); +} + +Vector operator*(double a, Vector const & b) { + return Vector(a * b.x, a * b.y, a * b.z); +} diff --git a/src/main/cpp/util/vector.cpp b/src/main/cpp/util/vector.cpp index 87adf63..900c425 100644 --- a/src/main/cpp/util/vector.cpp +++ b/src/main/cpp/util/vector.cpp @@ -1,9 +1,2 @@ #include "util/vector.h" -Vector operator*(Vector const & a, double b) { - return Vector(a.x * b, a.y * b, a.z * b); -} - -Vector operator*(double a, Vector const & b) { - return Vector(a * b.x, a * b.y, a * b.z); -} diff --git a/src/main/include/commands/arm/MoveToIntake.h b/src/main/include/commands/arm/MoveToIntake.h index 8d40282..14bcde3 100644 --- a/src/main/include/commands/arm/MoveToIntake.h +++ b/src/main/include/commands/arm/MoveToIntake.h @@ -3,7 +3,7 @@ #include "subsystems/arm/arm.h" #include "subsystems/arm/motionPath.h" -#include +#include #include #include @@ -25,4 +25,4 @@ class MoveToIntakeCommand : public frc2::CommandHelper m_path; Point m_currentPoint; -}; \ No newline at end of file +}; diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index bbb7003..45d6366 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -4,7 +4,7 @@ #include "subsystems/arm/armPose.h" #include "subsystems/arm/boundary.h" #include "subsystems/arm/motionPath.h" -#include "util/point.h" +#include "util/geom.h" #include "external/cpptoml.h" diff --git a/src/main/include/subsystems/arm/boundary.h b/src/main/include/subsystems/arm/boundary.h index 542c996..0cf5b8b 100644 --- a/src/main/include/subsystems/arm/boundary.h +++ b/src/main/include/subsystems/arm/boundary.h @@ -1,7 +1,6 @@ #pragma once -#include "util/point.h" -#include "util/vector.h" +#include "util/geom.h" #include #include diff --git a/src/main/include/subsystems/arm/motionPath.h b/src/main/include/subsystems/arm/motionPath.h index 235254b..c93ad28 100644 --- a/src/main/include/subsystems/arm/motionPath.h +++ b/src/main/include/subsystems/arm/motionPath.h @@ -1,6 +1,6 @@ #pragma once -#include "util/point.h" +#include "util/geom.h" #include @@ -57,4 +57,4 @@ class MotionPath { // usee ik to get pose // apply to subsystem. // } -// } \ No newline at end of file +// } diff --git a/src/main/include/subsystems/drivetrain/drivetrain.h b/src/main/include/subsystems/drivetrain/drivetrain.h index 8ab5bb6..b832730 100644 --- a/src/main/include/subsystems/drivetrain/drivetrain.h +++ b/src/main/include/subsystems/drivetrain/drivetrain.h @@ -4,7 +4,7 @@ #include #include "subsystems/drivetrain/swerveWheel.h" -#include "util/point.h" +#include "util/geom.h" #include "util/polar.h" #include diff --git a/src/main/include/subsystems/drivetrain/odometry.h b/src/main/include/subsystems/drivetrain/odometry.h index 73db818..ac67d9c 100644 --- a/src/main/include/subsystems/drivetrain/odometry.h +++ b/src/main/include/subsystems/drivetrain/odometry.h @@ -3,7 +3,7 @@ #include "Mandatory.h" #include -#include "util/point.h" +#include "util/geom.h" #include "subsystems/drivetrain/drivetrain.h" class Odometry : frc2::SubsystemBase { diff --git a/src/main/include/util/geom.h b/src/main/include/util/geom.h new file mode 100644 index 0000000..569ca00 --- /dev/null +++ b/src/main/include/util/geom.h @@ -0,0 +1,12 @@ +#pragma once + +#include "util/point.h" +#include "util/vector.h" + +Vector operator- (Point const & lhs, Point const & rhs); + +Point operator+ (Point const & lhs, Vector const & rhs); +Point operator- (Point const & lhs, Vector const & rhs); + +Vector operator* (Vector const &, double); +Vector operator* (double, Vector const &); diff --git a/src/main/include/util/point.h b/src/main/include/util/point.h index e445bef..bbe321a 100644 --- a/src/main/include/util/point.h +++ b/src/main/include/util/point.h @@ -19,31 +19,6 @@ class Point { double y; double z; - - Vector operator- (Point const & rhs) const { - return Vector( - x - rhs.x, - y - rhs.y, - z - rhs.z - ); - } - - Point operator+ (Vector const & rhs) const { - return Point { - x + rhs.x, - y + rhs.y, - z + rhs.z - }; - } - - Point operator- (Vector const & rhs) const { - return Point { - x - rhs.x, - y - rhs.y, - z - rhs.z - }; - } - bool isNear(Point const & rhs) const { return std::abs(x - rhs.x) < NEAR_ZERO_METERS && std::abs(y - rhs.y) < NEAR_ZERO_METERS diff --git a/src/main/include/util/vector.h b/src/main/include/util/vector.h index 95c262c..cab48b1 100644 --- a/src/main/include/util/vector.h +++ b/src/main/include/util/vector.h @@ -35,6 +35,3 @@ class Vector { double y; double z; }; - -Vector operator* (Vector const &, double); -Vector operator* (double, Vector const &); From c679bed2588492aa28ef116ee2f3d7f1afa41889 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 16:07:03 -0500 Subject: [PATCH 049/100] arm: ik solver: move target pt from robot origin to turret origin Vice versa for forward kinematic solver. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 65535bf..1f52b57 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -13,6 +13,8 @@ const double k_ChassisXSize = 0.775; // meters const double k_ChassisYSize = 0.826; // meters const double k_ChassisZSize = 0.229; // meters +const double k_TurretZOffset = 0.210; // meters + const double k_PickupXSize = 0.076; // meters const double k_PickupYSize = 0.340; // meters const double k_PickupZSize = 1.000; // meters @@ -122,11 +124,12 @@ void ArmSubsystem::initialiseBoundary() { } Point ArmSubsystem::calcElbowPos(double turretAng, double shoulderAng) { - Point pt{ + // Origin z is floor level. Add turret z offset to calculated point. + Point pt = Point( Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::sin(turretAng), Constants::Arm::k_bicepLenMeters * std::cos(shoulderAng) * std::cos(turretAng), - Constants::Arm::k_bicepLenMeters * std::sin(shoulderAng), - }; + Constants::Arm::k_bicepLenMeters * std::sin(shoulderAng) + ) + Vector(0.0, 0.0, k_TurretZOffset); return pt; } @@ -150,7 +153,15 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { // See arm.h for diagram of robot arm angle reference conventions. // desmos 2d IK solver demo: https://www.desmos.com/calculator/p3uouu2un2 - double targetLen = std::sqrt((std::pow(pt.x, 2) + std::pow(pt.y, 2) + std::pow(pt.z, 2))); // Line from shoulder to target + // IK solver assumes base of turret is origin. Adjust target point from + // robot origin to turret origin. + Point turretPt = pt - Vector(0.0, 0.0, k_TurretZOffset); + + double targetLen = std::sqrt( + std::pow(turretPt.x, 2) + + std::pow(turretPt.y, 2) + + std::pow(turretPt.z, 2) + ); // Line from shoulder to target // Constrain target point. double constrainLen = std::min( @@ -158,7 +169,7 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { targetLen ); - Point cp = Point(0.0, 0.0, k_ChassisZSize) - Vector(pt.x, pt.y, pt.z).unit() * constrainLen; + Point cp = Point(0.0, 0.0, 0.0) - Vector(turretPt.x, turretPt.y, turretPt.z).unit() * constrainLen; double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); From 5a6eb32a8203c7e46e2ca50ef3a7adaa2ba2504f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 16:08:03 -0500 Subject: [PATCH 050/100] arm: ik solver: fix turret angle again... maybe - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 1f52b57..18f814d 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -195,7 +195,7 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { ) - c1; - double turretAngle = std::atan2(-cp.x, cp.y); + double turretAngle = std::atan2(cp.x, cp.y); return ArmPose(turretAngle, shoulderAngle, elbowAngle); From ec66791176199d0599dd42a59a0ba7aaf3afb2d0 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 16:25:16 -0500 Subject: [PATCH 051/100] arm: ik solver: invert sign of constrained vector Should not be Point - Vector, but rather Point + Vector. After extracting the turret origin translation, which was also wrong, the constrained vector was left negative. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 18f814d..dcdbfc9 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -169,7 +169,7 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { targetLen ); - Point cp = Point(0.0, 0.0, 0.0) - Vector(turretPt.x, turretPt.y, turretPt.z).unit() * constrainLen; + Point cp = Point(0.0, 0.0, 0.0) + Vector(turretPt.x, turretPt.y, turretPt.z).unit() * constrainLen; double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); From 16d3b8a6e3747d914231b6dca66ce2c332cfec3d Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 16:51:40 -0500 Subject: [PATCH 052/100] arm: Add PID to shoulder motor to hold position Needs tuning. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 29 +++++++++++++++++++++++++++ src/main/deploy/config.toml | 5 +++++ src/main/include/subsystems/arm/arm.h | 3 +++ 3 files changed, 37 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index dcdbfc9..01a7145 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -9,6 +9,10 @@ #include +// PROTOTYPES +static double requireTomlDouble (std::shared_ptr toml, std::string const & name); + +// CONSTANTS const double k_ChassisXSize = 0.775; // meters const double k_ChassisYSize = 0.826; // meters const double k_ChassisZSize = 0.229; // meters @@ -19,13 +23,33 @@ const double k_PickupXSize = 0.076; // meters const double k_PickupYSize = 0.340; // meters const double k_PickupZSize = 1.000; // meters +// MACROS #define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { loadConfig(toml); + + double kp, ki, kd, tolerance; + + kp = requireTomlDouble(toml, "shoulder.pid.p"); + ki = requireTomlDouble(toml, "shoulder.pid.i"); + kd = requireTomlDouble(toml, "shoulder.pid.d"); + tolerance = requireTomlDouble(toml, "shoulder.pid.tolerance"); + + m_shoulderPid = new frc2::PIDController(kp, ki, kd); + m_shoulderPid->SetTolerance(tolerance); + m_shoulderPid->SetIntegratorRange(-0.01, 0.01); + + m_shoulderPid->SetSetpoint(getShoulderAngle()); } void ArmSubsystem::Periodic() { + // Move shoulder or hold position. + if (nullptr != m_shoulderPid) { + double output = m_shoulderPid->Calculate(getShoulderAngle()); + m_turretMotor.Set(output); + } + frc::SmartDashboard::PutNumber("Turret Angle (deg)", RAD_2_DEG(getTurretAngle())); frc::SmartDashboard::PutNumber("Shoulder Angle (deg)", RAD_2_DEG(getShoulderAngle())); frc::SmartDashboard::PutNumber("Elbow Angle (deg)", RAD_2_DEG(getElbowAngle())); @@ -298,12 +322,17 @@ void ArmSubsystem::setTurretAngle(double angle) { void ArmSubsystem::setShoulderAngle(double angle) { angle = std::clamp(angle, config.shoulder.limit.lo, config.shoulder.limit.hi); + /* double da = angle - getShoulderAngle(); if (isNearZero(da)) { m_lowJointMotor.Set(0.0); } else { m_lowJointMotor.Set(std::copysign(0.05, da)); } + */ + + // Use PID to hold arm in place once target angle is reached. + m_shoulderPid->SetSetpoint(angle); } void ArmSubsystem::setElbowAngle(double angle) { diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index 68483b5..7dc1673 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -11,6 +11,11 @@ shoulder.limit.hi = 0.646 shoulder.zeroOffsetRadians = 0.131 + shoulder.pid.p = 0.001 + shoulder.pid.i = 0.001 + shoulder.pid.d = 0.000 + shoulder.pid.tolerance = 0.05 + # Limits are in raw sensors units elbow.limit.lo = 0.283 elbow.limit.hi = 0.414 diff --git a/src/main/include/subsystems/arm/arm.h b/src/main/include/subsystems/arm/arm.h index 45d6366..09e6801 100644 --- a/src/main/include/subsystems/arm/arm.h +++ b/src/main/include/subsystems/arm/arm.h @@ -13,6 +13,7 @@ #include #include +#include #include #include @@ -126,6 +127,8 @@ class ArmSubsystem : public frc2::SubsystemBase { frc::AnalogPotentiometer m_gripSensor {k_GripSensor, 1.0, 0.0}; frc::DutyCycleEncoder m_shoulderAngleSensor{k_ShoulderSensor}; // Using Funky Fresh Encoder + frc2::PIDController * m_shoulderPid = nullptr; + std::shared_ptr m_noGoZone = nullptr; // Safety Points From 716d71009aa1bdcbe3045c0edc58249ddf5096ca Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 17:36:46 -0500 Subject: [PATCH 053/100] Print exception cause when failing to parse config.toml - Justin C --- src/main/cpp/Robot.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index da89e0e..4eb5836 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -22,7 +22,10 @@ void Robot::RobotInit() { try{ c_toml = cpptoml::parse_file(frc::filesystem::GetDeployDirectory()+"/config.toml"); } catch (cpptoml::parse_exception & ex){ - std::cerr << "Unable to open file: config.toml" << std::endl; + std::cerr << "Unable to open file: config.toml" + << std::endl + << ex.what() + << std::endl; exit(1); } From 3a24a588711ac9350cfcaa8735174ee02d15ec98 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 17:48:57 -0500 Subject: [PATCH 054/100] Move RAD_2_DEG to util/math for others to use. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 3 --- src/main/include/util/math.h | 5 ++++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 01a7145..cf4fd8f 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -23,9 +23,6 @@ const double k_PickupXSize = 0.076; // meters const double k_PickupYSize = 0.340; // meters const double k_PickupZSize = 1.000; // meters -// MACROS -#define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) - ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { loadConfig(toml); diff --git a/src/main/include/util/math.h b/src/main/include/util/math.h index dd2eacf..4de90a4 100644 --- a/src/main/include/util/math.h +++ b/src/main/include/util/math.h @@ -1,3 +1,6 @@ #pragma once -bool isNearZero(double val); \ No newline at end of file +// MACROS +#define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) + +bool isNearZero(double val); From b7c5e13e17df3c49c04ae8c6b1bc40fa79b14689 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Thu, 9 Mar 2023 19:04:51 -0500 Subject: [PATCH 055/100] arm: ik solver: Fix reversed variables. Bicep is connected to shoulder. Forearm is connected to elbow. Dunno how those were flipped in my head. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index cf4fd8f..f671218 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -195,10 +195,10 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { double c3 = atan2(cp.z, std::sqrt(cp.x * cp.x + cp.y * cp.y)); double c1 = std::acos( - (Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters - - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + (Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + - Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + constrainLen * constrainLen) - / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen) + / (2.0 * Constants::Arm::k_bicepLenMeters * constrainLen) ); // Solve IK: @@ -208,11 +208,11 @@ ArmPose ArmSubsystem::calcIKJointPoses(Point const & pt) { std::numbers::pi - std::acos( ( - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters - - Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + Constants::Arm::k_forearmLenMeters * Constants::Arm::k_forearmLenMeters + - Constants::Arm::k_bicepLenMeters * Constants::Arm::k_bicepLenMeters + constrainLen * constrainLen ) - / (2.0 * Constants::Arm::k_bicepLenMeters * constrainLen) + / (2.0 * Constants::Arm::k_forearmLenMeters * constrainLen) ) - c1; From ae5adde9ec18ee4e7e86fcb462f63a8a181c9b51 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:11:36 -0500 Subject: [PATCH 056/100] arm: Fix typo: control shoulder motor with shoulder pid... ...not the turret motor. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index f671218..17bfa8c 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -44,7 +44,7 @@ void ArmSubsystem::Periodic() { // Move shoulder or hold position. if (nullptr != m_shoulderPid) { double output = m_shoulderPid->Calculate(getShoulderAngle()); - m_turretMotor.Set(output); + m_lowJointMotor.Set(output); } frc::SmartDashboard::PutNumber("Turret Angle (deg)", RAD_2_DEG(getTurretAngle())); From 662ce2796f1f52ee4c89ff49f415d92497e9ea31 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:12:35 -0500 Subject: [PATCH 057/100] arm: Reverse shoulder motor direction STATUS: Tested on compbot. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 17bfa8c..2e988b2 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -44,6 +44,8 @@ void ArmSubsystem::Periodic() { // Move shoulder or hold position. if (nullptr != m_shoulderPid) { double output = m_shoulderPid->Calculate(getShoulderAngle()); + // Reverse motor direction. + output = -output; m_lowJointMotor.Set(output); } From 14341b33b076a72a4883507ad1a7785f5474fee9 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:14:27 -0500 Subject: [PATCH 058/100] arm: Fix boundary limits for floor no-go zone. Specify lo before hi, not hi before lo. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 2e988b2..6d9819a 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -133,7 +133,7 @@ void ArmSubsystem::initialiseBoundary() { std::shared_ptr floorNoGoZone = std::make_shared( -5.0, 5.0, -5.0, 5.0, - 0.0254, -5.0 /* keep gripper 1 inch from floor */ + -5.0, 0.0254 /* keep gripper 1 inch from floor */ ); std::shared_ptr defNoGoZone = std::make_shared(std::vector{ From 2cfbca947525267230637c43805f89833235f209 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:15:11 -0500 Subject: [PATCH 059/100] arm: Adjust near zero tolerance for turret and elbow motors Motor jitters back and forth around setpoint. Enlarge tolerance to stop it. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 4 ++-- src/main/cpp/util/math.cpp | 4 ++-- src/main/include/util/math.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 6d9819a..23b2669 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -311,7 +311,7 @@ void ArmSubsystem::setTurretAngle(double angle) { angle = std::clamp(angle, config.turret.limit.lo, config.turret.limit.hi); double da = angle - getTurretAngle(); - if (isNearZero(da)) { + if (isNearZero(da, 0.02)) { m_turretMotor.Set(0.0); } else { m_turretMotor.Set(std::copysign(0.05, da)); @@ -338,7 +338,7 @@ void ArmSubsystem::setElbowAngle(double angle) { angle = std::clamp(angle, config.elbow.limit.lo, config.elbow.limit.hi); double da = angle - getElbowAngle(); - if (isNearZero(da)) { + if (isNearZero(da, 0.02)) { m_midJointMotor.Set(0.0); } else { m_midJointMotor.Set(std::copysign(0.05, da)); diff --git a/src/main/cpp/util/math.cpp b/src/main/cpp/util/math.cpp index c5c96c1..9a4c2a9 100644 --- a/src/main/cpp/util/math.cpp +++ b/src/main/cpp/util/math.cpp @@ -1,5 +1,5 @@ #include "util/math.h" -bool isNearZero(double val) { - return -0.001 < val && val < 0.001; +bool isNearZero(double val, double tolerance) { + return -tolerance < val && val < tolerance; } \ No newline at end of file diff --git a/src/main/include/util/math.h b/src/main/include/util/math.h index 4de90a4..265c445 100644 --- a/src/main/include/util/math.h +++ b/src/main/include/util/math.h @@ -3,4 +3,4 @@ // MACROS #define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) -bool isNearZero(double val); +bool isNearZero(double val, double tolerance = 0.001); From 18642d0fdf0f4c5d38785ab538c79e9ee71c388d Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:27:48 -0500 Subject: [PATCH 060/100] arm: Replace bang-bang motor control with clamped linear velocity control - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 23b2669..7850047 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -314,7 +314,7 @@ void ArmSubsystem::setTurretAngle(double angle) { if (isNearZero(da, 0.02)) { m_turretMotor.Set(0.0); } else { - m_turretMotor.Set(std::copysign(0.05, da)); + m_turretMotor.Set(std::clamp(da, -0.1, 0.1)); } } @@ -341,7 +341,7 @@ void ArmSubsystem::setElbowAngle(double angle) { if (isNearZero(da, 0.02)) { m_midJointMotor.Set(0.0); } else { - m_midJointMotor.Set(std::copysign(0.05, da)); + m_midJointMotor.Set(std::clamp(da, -0.1, 0.1)); } } From 5c5bf952e61273010abec5572448cbb04540aad4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:30:41 -0500 Subject: [PATCH 061/100] arm: Convert limits from raw sensor value to degrees - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 36 +++++++++++------------------ src/main/deploy/config.toml | 20 ++++++++-------- src/main/include/util/math.h | 1 + 3 files changed, 25 insertions(+), 32 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 7850047..58a331a 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -422,28 +422,20 @@ void ArmSubsystem::loadConfig(std::shared_ptr toml) { // Load raw limits and convert to radians or meters. - config.turret.limit.lo = requireTomlDouble(toml, "turret.limit.lo") - * config.turret.sensorToRadians; - config.turret.limit.hi = requireTomlDouble(toml, "turret.limit.hi") - * config.turret.sensorToRadians; - - config.shoulder.limit.lo = requireTomlDouble(toml, "shoulder.limit.lo"); - config.shoulder.limit.hi = requireTomlDouble(toml, "shoulder.limit.hi"); - - config.elbow.limit.lo = requireTomlDouble(toml, "elbow.limit.lo") - * config.elbow.sensorToRadians; - config.elbow.limit.hi = requireTomlDouble(toml, "elbow.limit.hi") - * config.elbow.sensorToRadians; - - config.wrist.limit.lo = requireTomlDouble(toml, "wrist.limit.lo") - * config.wrist.sensorToRadians; - config.wrist.limit.hi = requireTomlDouble(toml, "wrist.limit.hi") - * config.wrist.sensorToRadians; - - config.grip.limit.lo = requireTomlDouble(toml, "grip.limit.lo") - * config.grip.sensorToMeters; - config.grip.limit.hi = requireTomlDouble(toml, "grip.limit.hi") - * config.grip.sensorToMeters; + config.turret.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "turret.limit.lo")); + config.turret.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "turret.limit.hi")); + + config.shoulder.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "shoulder.limit.lo")); + config.shoulder.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "shoulder.limit.hi")); + + config.elbow.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "elbow.limit.lo")); + config.elbow.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "elbow.limit.hi")); + + config.wrist.limit.lo = DEG_2_RAD(requireTomlDouble(toml, "wrist.limit.lo")); + config.wrist.limit.hi = DEG_2_RAD(requireTomlDouble(toml, "wrist.limit.hi")); + + config.grip.limit.lo = requireTomlDouble(toml, "grip.limit.lo"); + config.grip.limit.hi = requireTomlDouble(toml, "grip.limit.hi"); config.grip.setpoint.open = requireTomlDouble(toml, "grip.setpoint.open"); config.grip.setpoint.close = requireTomlDouble(toml, "grip.setpoint.close"); diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index 7dc1673..3bcdc3f 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -1,14 +1,14 @@ [arm] # Limits are in raw sensors units - turret.limit.lo = 0.34 - turret.limit.hi = 0.59 + turret.limit.lo = -85.0 + turret.limit.hi = 90.0 turret.zeroOffsetRadians = -6.377 turret.sensorToRadians = 13.426 - # Limits are in raw sensors units - shoulder.limit.lo = 0.071 - shoulder.limit.hi = 0.646 + # Limits are in degrees + shoulder.limit.lo = 45.0 + shoulder.limit.hi = 96.0 shoulder.zeroOffsetRadians = 0.131 shoulder.pid.p = 0.001 @@ -16,19 +16,19 @@ shoulder.pid.d = 0.000 shoulder.pid.tolerance = 0.05 - # Limits are in raw sensors units - elbow.limit.lo = 0.283 - elbow.limit.hi = 0.414 + # Limits are in degrees + elbow.limit.lo = 15.0 + elbow.limit.hi = 180.0 elbow.zeroOffsetRadians = -7.018 elbow.sensorToRadians = 24.54 - # Limits are in raw sensors units + # Limits are in degrees wrist.limit.lo = 0.650 wrist.limit.hi = 0.697 wrist.zeroOffsetRadians = -43.446 wrist.sensorToRadians = 66.84 - # Limits are in raw sensors units + # Limits are in meters grip.limit.lo = 0.511 grip.limit.hi = 0.522 # Grip closed is considered 0 meters. diff --git a/src/main/include/util/math.h b/src/main/include/util/math.h index 265c445..533bfc9 100644 --- a/src/main/include/util/math.h +++ b/src/main/include/util/math.h @@ -2,5 +2,6 @@ // MACROS #define RAD_2_DEG(rad) ((rad) * 180.0 / M_PI) +#define DEG_2_RAD(deg) ((deg) * M_PI / 180.0) bool isNearZero(double val, double tolerance = 0.001); From bae168b633a84a2f16756f593eb491f0281e6ee5 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:40:06 -0500 Subject: [PATCH 062/100] arm: Calibrate shoulder pid and tolerance Calibration interrupted due to critical failure at shoulder joint while trying to reach home position. Further calibration strongly recommended. - Justin C --- src/main/deploy/config.toml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index 3bcdc3f..c8a5d04 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -11,10 +11,10 @@ shoulder.limit.hi = 96.0 shoulder.zeroOffsetRadians = 0.131 - shoulder.pid.p = 0.001 - shoulder.pid.i = 0.001 + shoulder.pid.p = 0.35 + shoulder.pid.i = 0.002 shoulder.pid.d = 0.000 - shoulder.pid.tolerance = 0.05 + shoulder.pid.tolerance = 0.10 # Limits are in degrees elbow.limit.lo = 15.0 From 44c7c1c7c2e2d45086d85531db7060c368ca53e1 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Fri, 10 Mar 2023 20:56:19 -0500 Subject: [PATCH 063/100] add basic auto --- build.gradle | 2 +- src/main/cpp/Robot.cpp | 36 +++++++++++++++++++++++++++++++++++- src/main/include/Robot.h | 15 +++++++++++++++ 3 files changed, 51 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 1380276..0fac042 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2023.3.2" + id "edu.wpi.first.GradleRIO" version "2023.4.2" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4caf50b..01d1122 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,6 +18,12 @@ #include "external/cpptoml.h" +#ifdef COMPETITION_MODE +#include +#include +#include +#endif + void Robot::RobotInit() { try{ c_toml = cpptoml::parse_file(frc::filesystem::GetDeployDirectory()+"/config.toml"); @@ -36,6 +42,26 @@ void Robot::RobotInit() { //Commands c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); + + //temp auto + #ifdef COMPETITION_MODE + timerOne = new frc::Timer(); + timerTwo = new frc::Timer(); + outOfSafeZone = new frc2::FunctionalCommand{ + [&](){timerOne->Reset();}, + [&](){c_drivetrain->setMotion(0,0.5,0);}, + [&](bool interrupted){c_drivetrain->setMotion(0,0,0);}, + [&](){return timerOne->AdvanceIfElapsed(3.0_s);}, + {c_drivetrain} + }; + ontoPlatform = new frc2::FunctionalCommand{ + [&](){timerTwo->Reset();}, + [&](){c_drivetrain->setMotion(0,-0.5,0);}, + [&](bool interrupted){c_drivetrain->setMotion(0,0,0);}, + [&](){return timerTwo->AdvanceIfElapsed(2.0_s);}, + {c_drivetrain} + }; + #endif } /** @@ -64,10 +90,18 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { + outOfSafeZone->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } -void Robot::AutonomousPeriodic() {} +void Robot::AutonomousPeriodic() { + if(!outOfSafeZone->IsScheduled()){ // check to see if driving out of the save zone is still running (don't want to drive in opposite directions at the same time) + if(!ontoPlatform->IsScheduled()){ // check to see if we have started the movement onto the platform (should not start it multiple times) + ontoPlatform->Schedule(); + } + } + c_drivetrain->Periodic();// update drivetrain no matter what +} void Robot::TeleopInit() { // TODO: Make sure autonomous command is canceled first. diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 6311381..22aa82a 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -19,6 +19,13 @@ #include "external/cpptoml.h" +//temporary auto +#ifdef COMPETITION_MODE +#include +#include +#include +#endif + class Robot : public frc::TimedRobot { public: void RobotInit() override; @@ -50,4 +57,12 @@ class Robot : public frc::TimedRobot { //Commands DriveTeleopCommand* c_driveTeleopCommand = nullptr; + + //temporary auto + #ifdef COMPETITION_MODE + frc::Timer* timerOne; + frc::Timer* timerTwo; + frc2::FunctionalCommand* outOfSafeZone; + frc2::FunctionalCommand* ontoPlatform; + #endif }; From 576021cc0aff6c8837f64e2c954075c75050d534 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Fri, 10 Mar 2023 21:36:04 -0500 Subject: [PATCH 064/100] change compile mode to competition --- src/main/include/RobotCompileModes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/include/RobotCompileModes.h b/src/main/include/RobotCompileModes.h index f3640a9..951e213 100644 --- a/src/main/include/RobotCompileModes.h +++ b/src/main/include/RobotCompileModes.h @@ -1,8 +1,8 @@ #pragma once -//#define COMPETITION_MODE // only competition related dashboard output +#define COMPETITION_MODE // only competition related dashboard output //#define TESTING_MODE // no console output, but has dashboard output -#define DEBUG_MODE // enables console output and dashboard output +//#define DEBUG_MODE // enables console output and dashboard output //just to guard against multiple things being enabled at the same time From 03bf1cf658e14bbb201798988993dc975ced1be3 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 00:40:36 -0500 Subject: [PATCH 065/100] arm: Add arm teleop command - Justin C --- .../cpp/commands/arm/ArmTeleopCommand.cpp | 126 ++++++++++++++++++ .../include/commands/arm/ArmTeleopCommand.h | 25 ++++ 2 files changed, 151 insertions(+) create mode 100644 src/main/cpp/commands/arm/ArmTeleopCommand.cpp create mode 100644 src/main/include/commands/arm/ArmTeleopCommand.h diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp new file mode 100644 index 0000000..af53c72 --- /dev/null +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -0,0 +1,126 @@ +#include "commands/arm/ArmTeleopCommand.h" +#include "subsystems/arm/arm.h" + +#include + +#include + +#define JOYSTICK_DEADZONE 0.1 +#define DEADZONE(input) ((std::abs(input) < JOYSTICK_DEADZONE) ? 0.0 : input) + +const double k_maxPointSpeed = 0.005; +const double k_maxPointRotSpeed = 2.0 * M_PI / 10.0; // radians per second in 20ms. +const double k_maxWristRotSpeed = 2.0 * M_PI / 2.0; +const double k_maxGripSpeed = 0.1; + +ArmTeleopCommand::ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController) { + c_operatorController = operatorController; + c_arm = arm; + + AddRequirements(c_arm); +} + +void ArmTeleopCommand::Initialize() { + // Update target to current point so arm doesn't move unexpectedly. + m_target = c_arm->getGripPoint(); +} + +void ArmTeleopCommand::Execute() { + { + Vector gripVec(m_target.x, m_target.y, m_target.z); + double gripMag = gripVec.len(); + double gripDir = std::atan2(gripVec.x, gripVec.y); // 0 deg == y axis + + // Rotate turret. Speed of rotation is reduced the further the arm reaches. + double leftX = c_operatorController->GetLeftX(); + // Square input to improve fidelity. + leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX); + leftX = DEADZONE(leftX); + if (0.0 != leftX) { + // (+) leftX should move turret clockwise. + gripDir = gripDir + leftX * k_maxPointRotSpeed / gripMag; + Vector offset( + gripMag * std::cos(gripDir), + gripMag * std::sin(gripDir), + 0.0 + ); + m_target = m_target + offset; + } + + // Extend/retract gripper from/to turret. + double leftY = -c_operatorController->GetLeftY(); /* Invert so + is forward */ + // Square input to improve fidelity. + leftY = std::copysign(std::clamp(leftY * leftY, -1.0, 1.0), leftY); + leftY = DEADZONE(leftY); + if (0.0 != leftY) { + // (+) leftY should move away from turret. + gripMag = gripMag + leftY * k_maxPointSpeed; + Vector offset( + gripMag * std::cos(gripDir), + gripMag * std::sin(gripDir), + 0.0 + ); + m_target = m_target + offset; + } + + // Move gripper up or down. + double rightY = -c_operatorController->GetRightY(); /* Invert so + is up */ + // Square input to improve fidelity. + rightY = std::copysign(std::clamp(rightY * rightY, -1.0, 1.0), rightY); + rightY = DEADZONE(rightY); + if (0.0 != rightY) { + // (+) rightY should move gripper up. + m_target = m_target + Vector(0.0, 0.0, rightY * k_maxPointSpeed); + } + + c_arm->moveToPoint(m_target); + } + + // Rotate wrist clockwise or counterclockwise. + { + double leftTrigger = c_operatorController->GetLeftTriggerAxis(); + double rightTrigger = c_operatorController->GetRightTriggerAxis(); + double trigger = leftTrigger - rightTrigger; + // Square input to improve fidelity. + trigger = std::copysign(std::clamp(trigger * trigger, -1.0, 1.0), trigger); + trigger = DEADZONE(trigger); + + // Use current angle as default so wrist doesn't move wildly upon enable. + double wristTargetAngle = c_arm->getWristRollAngle(); + + if (0.0 != trigger) { + // (+) trigger should rotate wrist counter-clockwise, looking down forearm. + wristTargetAngle += trigger * k_maxWristRotSpeed; + } + + // Move/hold wrist angle. + c_arm->setWristRollAngle(wristTargetAngle); + } + + // Open and close gripper. + { + double bumper = + (c_operatorController->GetRightBumper() ? 1.0 : 0.0) + - (c_operatorController->GetLeftBumper() ? 1.0 : 0.0); + + // Use current position as default so grip doesn't move wildly upon enable. + double gripTargetPos = c_arm->getGrip(); + + if (0.0 != bumper) { + gripTargetPos += bumper * k_maxGripSpeed; + } + + // Move/hold grip. + c_arm->setGrip(gripTargetPos); + } +} + +void ArmTeleopCommand::End(bool interrupted) { + // Update target to current point so arm stops. + // Cannot stop motors because shoulder will backdrive and fall. + m_target = c_arm->getGripPoint(); +} + +bool ArmTeleopCommand::IsFinished() { + return false; // dont end because then we wont be able to drive +} diff --git a/src/main/include/commands/arm/ArmTeleopCommand.h b/src/main/include/commands/arm/ArmTeleopCommand.h new file mode 100644 index 0000000..1011cf4 --- /dev/null +++ b/src/main/include/commands/arm/ArmTeleopCommand.h @@ -0,0 +1,25 @@ +#pragma once + +#include "Mandatory.h" +#include "subsystems/arm/arm.h" +#include "util/geom.h" + +#include +#include + +#include + +class ArmTeleopCommand : public frc2::CommandHelper { +public: + ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController); + + void Initialize() override; + void Execute() override; + void End(bool interrupted) override; + bool IsFinished() override; +private: + ArmSubsystem* c_arm; + frc::XboxController* c_operatorController; + + Point m_target; +}; \ No newline at end of file From 6096d185c2d06a8b790f091819892344ea58a19f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 00:55:45 -0500 Subject: [PATCH 066/100] robot: Start arm teleop command in teleop - Justin C --- src/main/cpp/Robot.cpp | 2 ++ src/main/include/Robot.h | 3 +++ 2 files changed, 5 insertions(+) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 174a879..1a7b507 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -45,6 +45,7 @@ void Robot::RobotInit() { c_arm = new ArmSubsystem(c_toml->get_table("arm")); //Commands + c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); //temp auto @@ -109,6 +110,7 @@ void Robot::AutonomousPeriodic() { void Robot::TeleopInit() { // TODO: Make sure autonomous command is canceled first. + c_armTeleopCommand->Schedule(); c_driveTeleopCommand->Schedule(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 84b18c0..679ce1b 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -16,6 +16,8 @@ #include "subsystems/arm/arm.h" #include "subsystems/drivetrain/drivetrain.h" #include "subsystems/drivetrain/odometry.h" + +#include "commands/arm/ArmTeleopCommand.h" #include "commands/drivetrain/driveTeleopCommand.h" #include "external/cpptoml.h" @@ -58,6 +60,7 @@ class Robot : public frc::TimedRobot { ArmSubsystem* c_arm = nullptr; //Commands + ArmTeleopCommand* c_armTeleopCommand = nullptr; DriveTeleopCommand* c_driveTeleopCommand = nullptr; //temporary auto From 226f6de4e1804ddea8873e22f849bf733d2b8e4b Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 01:32:03 -0500 Subject: [PATCH 067/100] Convert simple auto into single command. - Justin C --- src/main/cpp/Robot.cpp | 40 ++++++++++++++++++---------------------- src/main/include/Robot.h | 7 ++----- 2 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 1a7b507..f4805a7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -50,22 +50,21 @@ void Robot::RobotInit() { //temp auto #ifdef COMPETITION_MODE - timerOne = new frc::Timer(); - timerTwo = new frc::Timer(); - outOfSafeZone = new frc2::FunctionalCommand{ - [&](){timerOne->Reset();}, - [&](){c_drivetrain->setMotion(0,0.5,0);}, - [&](bool interrupted){c_drivetrain->setMotion(0,0,0);}, - [&](){return timerOne->AdvanceIfElapsed(3.0_s);}, + auto outOfSafeZone = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,0.5,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, + { c_drivetrain } + }.ToPtr(); + + auto ontoPlatform = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,-0.5,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, {c_drivetrain} - }; - ontoPlatform = new frc2::FunctionalCommand{ - [&](){timerTwo->Reset();}, - [&](){c_drivetrain->setMotion(0,-0.5,0);}, - [&](bool interrupted){c_drivetrain->setMotion(0,0,0);}, - [&](){return timerTwo->AdvanceIfElapsed(2.0_s);}, - {c_drivetrain} - }; + }.ToPtr(); + + c_simpleAuto = std::move(outOfSafeZone).WithTimeout(3.0_s) + .AndThen(std::move(ontoPlatform).WithTimeout(2.0_s)) + .Unwrap(); #endif } @@ -95,21 +94,18 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { - outOfSafeZone->Schedule(); + c_simpleAuto->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } void Robot::AutonomousPeriodic() { - if(!outOfSafeZone->IsScheduled()){ // check to see if driving out of the save zone is still running (don't want to drive in opposite directions at the same time) - if(!ontoPlatform->IsScheduled()){ // check to see if we have started the movement onto the platform (should not start it multiple times) - ontoPlatform->Schedule(); - } - } c_drivetrain->Periodic();// update drivetrain no matter what } void Robot::TeleopInit() { - // TODO: Make sure autonomous command is canceled first. + // Make sure autonomous command is canceled first. + c_simpleAuto->Cancel(); + c_armTeleopCommand->Schedule(); c_driveTeleopCommand->Schedule(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 679ce1b..e0efbcc 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -24,7 +24,7 @@ //temporary auto #ifdef COMPETITION_MODE -#include +#include #include #include #endif @@ -65,9 +65,6 @@ class Robot : public frc::TimedRobot { //temporary auto #ifdef COMPETITION_MODE - frc::Timer* timerOne; - frc::Timer* timerTwo; - frc2::FunctionalCommand* outOfSafeZone; - frc2::FunctionalCommand* ontoPlatform; + std::unique_ptr c_simpleAuto{nullptr}; #endif }; From 12de51d8c19e752cc0cf52119e0ddc9e04051634 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 08:39:59 -0500 Subject: [PATCH 068/100] dashboard: Disable reports for arm debug values - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 58a331a..2e7019d 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -56,11 +56,13 @@ void ArmSubsystem::Periodic() { frc::SmartDashboard::PutNumber("Grip Distance (meters)", getGrip()); // Raw values + /* DEBUG ARM * frc::SmartDashboard::PutNumber("Raw: Turret Angle (V)", m_turretAngleSensor.Get()); frc::SmartDashboard::PutNumber("Raw: Shoulder Angle (deg)", m_shoulderAngleSensor.Get().value() * 360.0); frc::SmartDashboard::PutNumber("Raw: Elbow Angle (V)", m_elbowAngleSensor.Get()); frc::SmartDashboard::PutNumber("Raw: Wrist Roll Angle (V)", m_wristRollAngleSensor.Get()); frc::SmartDashboard::PutNumber("Raw: Grip Distance (V)", m_gripSensor.Get()); + /* */ // Report calculated gripper position. @@ -77,10 +79,12 @@ void ArmSubsystem::Periodic() { // Report calculated arm pose angles. What the robot thinks the angles // should be to reach gripper position. + /* DEBUG ARM * ArmPose pose = calcIKJointPoses(gripPos); frc::SmartDashboard::PutNumber("IK Turret Angle (deg)", RAD_2_DEG(pose.turretAngle)); frc::SmartDashboard::PutNumber("IK Shoulder Angle (deg)", RAD_2_DEG(pose.shoulderAngle)); frc::SmartDashboard::PutNumber("IK Elbow Angle (deg)", RAD_2_DEG(pose.elbowAngle)); + /* */ } void ArmSubsystem::initialiseBoundary() { From 4be56ec4440001c732fa03a759c069886a0b13d3 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 08:41:07 -0500 Subject: [PATCH 069/100] dashboard: Report arm motor current - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 2e7019d..6813914 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -85,6 +85,12 @@ void ArmSubsystem::Periodic() { frc::SmartDashboard::PutNumber("IK Shoulder Angle (deg)", RAD_2_DEG(pose.shoulderAngle)); frc::SmartDashboard::PutNumber("IK Elbow Angle (deg)", RAD_2_DEG(pose.elbowAngle)); /* */ + + frc::SmartDashboard::PutNumber("Turret Current (A)", m_turretMotor.GetOutputCurrent()); + frc::SmartDashboard::PutNumber("Shoulder Current (A)", m_lowJointMotor.GetOutputCurrent()); + frc::SmartDashboard::PutNumber("Elbow Current (A)", m_midJointMotor.GetOutputCurrent()); + frc::SmartDashboard::PutNumber("Wrist Current (A)", m_gripperRotateMotor.GetOutputCurrent()); + frc::SmartDashboard::PutNumber("Grip Current (A)", m_gripperGraspMotor.GetOutputCurrent()); } void ArmSubsystem::initialiseBoundary() { From 6b708f77580470472abc32fdb6353f82a393194b Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 00:44:48 -0500 Subject: [PATCH 070/100] arm: Limit max speed of shoulder motor Why didn't I think of this before... - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 6813914..ac5f2ce 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -46,6 +46,7 @@ void ArmSubsystem::Periodic() { double output = m_shoulderPid->Calculate(getShoulderAngle()); // Reverse motor direction. output = -output; + output = std::clamp(output, -0.2, 0.2); m_lowJointMotor.Set(output); } From ca005a3f4c49350d0b637bf809ac3dc414fd7efb Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Fri, 10 Mar 2023 03:21:50 -0500 Subject: [PATCH 071/100] arm: Add basic controls for wrist and grip left trigger: rotate wrist counter-clockwise. right trigger: rotate wrist clockwise. left bumper: close grip. right bumper: open grip. Calibrate wrist and grip limits and conversions. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 14 ++++++++++---- src/main/deploy/config.toml | 16 ++++++++-------- 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index ac5f2ce..ab0ba9d 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -360,10 +360,13 @@ void ArmSubsystem::setWristRollAngle(double angle) { angle = std::clamp(angle, config.wrist.limit.lo, config.wrist.limit.hi); double da = angle - getWristRollAngle(); - if (isNearZero(da)) { + if (isNearZero(da, 0.01)) { m_gripperRotateMotor.Set(0.0); } else { - m_gripperRotateMotor.Set(std::copysign(0.05, da)); + m_gripperRotateMotor.Set(std::clamp( + da + std::copysign(0.05, da), + -0.2, 0.2 + )); } } @@ -371,10 +374,13 @@ void ArmSubsystem::setGrip(double grip) { grip = std::clamp(grip, config.grip.limit.lo, config.grip.limit.hi); double dx = grip - getGrip(); - if (isNearZero(dx)) { + if (isNearZero(dx, 0.015 /* meters */)) { m_gripperGraspMotor.Set(0.0); } else { - m_gripperGraspMotor.Set(std::copysign(0.05, dx)); + m_gripperGraspMotor.Set(std::clamp( + dx + std::copysign(0.02, dx), + -0.2, 0.2 + )); } } diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index c8a5d04..5ec68e5 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -23,16 +23,16 @@ elbow.sensorToRadians = 24.54 # Limits are in degrees - wrist.limit.lo = 0.650 - wrist.limit.hi = 0.697 - wrist.zeroOffsetRadians = -43.446 - wrist.sensorToRadians = 66.84 + wrist.limit.lo = 3.0 + wrist.limit.hi = 177.0 + wrist.zeroOffsetRadians = -39.511 + wrist.sensorToRadians = 60.415 # Limits are in meters - grip.limit.lo = 0.511 - grip.limit.hi = 0.522 + grip.limit.lo = 0.01 + grip.limit.hi = 0.260 # Grip closed is considered 0 meters. - grip.zeroOffsetMeters = 12.338 - grip.sensorToMeters = -23.636 + grip.zeroOffsetMeters = -0.704 + grip.sensorToMeters = 1.688 grip.setpoint.open = 0 grip.setpoint.close = 0 From 75ae2ece31ef05ff02fbcd776960745d9f3fecfd Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 14:19:35 -0500 Subject: [PATCH 072/100] Disable simple auto - Justin C --- src/main/cpp/Robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index f4805a7..92ffd18 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -94,12 +94,12 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { - c_simpleAuto->Schedule(); + //c_simpleAuto->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } void Robot::AutonomousPeriodic() { - c_drivetrain->Periodic();// update drivetrain no matter what + // c_drivetrain->Periodic();// update drivetrain no matter what } void Robot::TeleopInit() { From ab33db5f7d11ec3797a0bb438f9c7751300241b5 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 14:20:25 -0500 Subject: [PATCH 073/100] Disable arm teleop command - Justin C --- src/main/cpp/Robot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 92ffd18..92e1312 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -106,7 +106,7 @@ void Robot::TeleopInit() { // Make sure autonomous command is canceled first. c_simpleAuto->Cancel(); - c_armTeleopCommand->Schedule(); + //c_armTeleopCommand->Schedule(); c_driveTeleopCommand->Schedule(); } From b6c94fead2ad801da499a08a2053616fea1e27c6 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 14:34:24 -0500 Subject: [PATCH 074/100] Fix drivetrain rotation Right analog pushed right will turn robot clockwise. - Justin C --- src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp index e4ea2b6..52b3342 100644 --- a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp +++ b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp @@ -36,7 +36,8 @@ void DriveTeleopCommand::Execute() { c_drivetrain->setMotion( -DEADZONE(c_driverController->GetLeftX()) * speedFactor, DEADZONE(c_driverController->GetLeftY()) * speedFactor, - DEADZONE(c_driverController->GetRightX()) * Constants::k_maxSpinSpeed + // Reverse rotation so right is clockwise robot motion. + -DEADZONE(c_driverController->GetRightX()) * Constants::k_maxSpinSpeed ); } From c8e3de6d75efd013dada69a4815ff1f498219bd8 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 15:29:03 -0500 Subject: [PATCH 075/100] Enable field oriented driving... ...because we seem to have problems getting it to initialize on. - Nick Q & Justin C --- src/main/cpp/Robot.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 92e1312..52f9c0a 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -40,7 +40,7 @@ void Robot::RobotInit() { c_operatorController = new frc::XboxController(Interfaces::k_operatorXboxController); //Subsystems - c_drivetrain = new Drivetrain(false); + c_drivetrain = new Drivetrain(true); c_odometry = new Odometry(c_drivetrain); c_arm = new ArmSubsystem(c_toml->get_table("arm")); @@ -108,8 +108,8 @@ void Robot::TeleopInit() { //c_armTeleopCommand->Schedule(); c_driveTeleopCommand->Schedule(); + c_drivetrain->enableFieldCentric(); } - /** * This function is called periodically during operator control. */ From 8bab8785b746da2296f62d58cbffec03e764f9e4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 15:30:11 -0500 Subject: [PATCH 076/100] Report field oriented mode on dashboard - Nick Q --- src/main/cpp/Robot.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 52f9c0a..4263078 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,6 +18,8 @@ #include "external/cpptoml.h" +#include + #ifdef COMPETITION_MODE #include #include @@ -78,6 +80,8 @@ void Robot::RobotInit() { */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); + + frc::SmartDashboard::PutBoolean("Field Centric Enabled",c_drivetrain->getFieldCentric()); } /** From b1ba98724399a5e22525b60937f0d0f7ef50e014 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 17:18:21 -0500 Subject: [PATCH 077/100] Fix arm rotation speed constants Need to multiply by timestep of 20ms. - Justin C --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index af53c72..0b5370e 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -9,8 +9,8 @@ #define DEADZONE(input) ((std::abs(input) < JOYSTICK_DEADZONE) ? 0.0 : input) const double k_maxPointSpeed = 0.005; -const double k_maxPointRotSpeed = 2.0 * M_PI / 10.0; // radians per second in 20ms. -const double k_maxWristRotSpeed = 2.0 * M_PI / 2.0; +const double k_maxPointRotSpeed = 2.0 * M_PI / 10.0 * 0.02; // radians per second in 20ms. +const double k_maxWristRotSpeed = 2.0 * M_PI / 2.0 * 0.02; const double k_maxGripSpeed = 0.1; ArmTeleopCommand::ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController) { From a871339845a775a1a8b20634edb666879c738358 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 17:20:57 -0500 Subject: [PATCH 078/100] Check input deadzone before squaring input - Justin C --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index 0b5370e..3abd07b 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -34,8 +34,8 @@ void ArmTeleopCommand::Execute() { // Rotate turret. Speed of rotation is reduced the further the arm reaches. double leftX = c_operatorController->GetLeftX(); // Square input to improve fidelity. - leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX); leftX = DEADZONE(leftX); + leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX); if (0.0 != leftX) { // (+) leftX should move turret clockwise. gripDir = gripDir + leftX * k_maxPointRotSpeed / gripMag; @@ -50,8 +50,8 @@ void ArmTeleopCommand::Execute() { // Extend/retract gripper from/to turret. double leftY = -c_operatorController->GetLeftY(); /* Invert so + is forward */ // Square input to improve fidelity. - leftY = std::copysign(std::clamp(leftY * leftY, -1.0, 1.0), leftY); leftY = DEADZONE(leftY); + leftY = std::copysign(std::clamp(leftY * leftY, -1.0, 1.0), leftY); if (0.0 != leftY) { // (+) leftY should move away from turret. gripMag = gripMag + leftY * k_maxPointSpeed; @@ -82,8 +82,8 @@ void ArmTeleopCommand::Execute() { double rightTrigger = c_operatorController->GetRightTriggerAxis(); double trigger = leftTrigger - rightTrigger; // Square input to improve fidelity. - trigger = std::copysign(std::clamp(trigger * trigger, -1.0, 1.0), trigger); trigger = DEADZONE(trigger); + trigger = std::copysign(std::clamp(trigger * trigger, -1.0, 1.0), trigger); // Use current angle as default so wrist doesn't move wildly upon enable. double wristTargetAngle = c_arm->getWristRollAngle(); From 08cbbf0f0331ed3dcec3add3ce2ac8850e4ae50c Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 17:22:46 -0500 Subject: [PATCH 079/100] arm: Fix polar calculation for arm motion - Justin C --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index 3abd07b..22c0981 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -40,8 +40,8 @@ void ArmTeleopCommand::Execute() { // (+) leftX should move turret clockwise. gripDir = gripDir + leftX * k_maxPointRotSpeed / gripMag; Vector offset( - gripMag * std::cos(gripDir), gripMag * std::sin(gripDir), + gripMag * std::cos(gripDir), 0.0 ); m_target = m_target + offset; @@ -56,8 +56,8 @@ void ArmTeleopCommand::Execute() { // (+) leftY should move away from turret. gripMag = gripMag + leftY * k_maxPointSpeed; Vector offset( - gripMag * std::cos(gripDir), gripMag * std::sin(gripDir), + gripMag * std::cos(gripDir), 0.0 ); m_target = m_target + offset; From da39782170a06135b660edc968c8fb3157c87de9 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sat, 11 Mar 2023 17:24:00 -0500 Subject: [PATCH 080/100] Fix drive orientation after zero'ing wheel cancoders - Nick Q --- src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp index 52b3342..da71c04 100644 --- a/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp +++ b/src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp @@ -34,10 +34,10 @@ void DriveTeleopCommand::Execute() { // the rotation limit is there in case the driver does not want to spin as fast while driving (specifically limiting the controller input) c_drivetrain->setMotion( - -DEADZONE(c_driverController->GetLeftX()) * speedFactor, - DEADZONE(c_driverController->GetLeftY()) * speedFactor, + DEADZONE(c_driverController->GetLeftX()) * speedFactor, + -DEADZONE(c_driverController->GetLeftY()) * speedFactor, // Reverse rotation so right is clockwise robot motion. - -DEADZONE(c_driverController->GetRightX()) * Constants::k_maxSpinSpeed + DEADZONE(c_driverController->GetRightX()) * Constants::k_maxSpinSpeed ); } From 765555dec72498d9efbf1eb7b9737d1e3f7405b4 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sun, 12 Mar 2023 09:33:16 -0400 Subject: [PATCH 081/100] Take off arm... amputate! - Justin C --- src/main/cpp/Robot.cpp | 5 +++-- src/main/include/Robot.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4263078..2c6518b 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -44,10 +44,10 @@ void Robot::RobotInit() { //Subsystems c_drivetrain = new Drivetrain(true); c_odometry = new Odometry(c_drivetrain); - c_arm = new ArmSubsystem(c_toml->get_table("arm")); + //c_arm = new ArmSubsystem(c_toml->get_table("arm")); //Commands - c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); + //c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); //temp auto @@ -111,6 +111,7 @@ void Robot::TeleopInit() { c_simpleAuto->Cancel(); //c_armTeleopCommand->Schedule(); + //c_armTeleopCommand->resetTarget(); c_driveTeleopCommand->Schedule(); c_drivetrain->enableFieldCentric(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index e0efbcc..0a8342c 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -57,10 +57,10 @@ class Robot : public frc::TimedRobot { //Subsystems Drivetrain* c_drivetrain = nullptr; Odometry* c_odometry = nullptr; - ArmSubsystem* c_arm = nullptr; + //ArmSubsystem* c_arm = nullptr; //Commands - ArmTeleopCommand* c_armTeleopCommand = nullptr; + //ArmTeleopCommand* c_armTeleopCommand = nullptr; DriveTeleopCommand* c_driveTeleopCommand = nullptr; //temporary auto From 7810e0092c78a079a90eee224decefef932a1e90 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sun, 12 Mar 2023 09:36:38 -0400 Subject: [PATCH 082/100] Reformat - Nick Q --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index 22c0981..fad7b6b 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -9,8 +9,8 @@ #define DEADZONE(input) ((std::abs(input) < JOYSTICK_DEADZONE) ? 0.0 : input) const double k_maxPointSpeed = 0.005; -const double k_maxPointRotSpeed = 2.0 * M_PI / 10.0 * 0.02; // radians per second in 20ms. -const double k_maxWristRotSpeed = 2.0 * M_PI / 2.0 * 0.02; +const double k_maxPointRotSpeed = (2.0 * M_PI / 10.0) * 0.02; // radians per second in 20ms. +const double k_maxWristRotSpeed = (2.0 * M_PI / 2.0) * 0.02; const double k_maxGripSpeed = 0.1; ArmTeleopCommand::ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* operatorController) { From b3b1594cdbfdb18d06c3fa7d32af346926fc412f Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sun, 12 Mar 2023 10:22:39 -0400 Subject: [PATCH 083/100] add basic cube auto --- src/main/cpp/Robot.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 2c6518b..f74a1b9 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -52,20 +52,20 @@ void Robot::RobotInit() { //temp auto #ifdef COMPETITION_MODE - auto outOfSafeZone = frc2::StartEndCommand{ + auto forceOffCube = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.5,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } }.ToPtr(); - auto ontoPlatform = frc2::StartEndCommand{ - [&] () { c_drivetrain->setMotion(0,-0.5,0); }, + auto putCubeIntoStation = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,-0.15,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, {c_drivetrain} }.ToPtr(); - c_simpleAuto = std::move(outOfSafeZone).WithTimeout(3.0_s) - .AndThen(std::move(ontoPlatform).WithTimeout(2.0_s)) + c_simpleAuto = std::move(forceOffCube).WithTimeout(0.5_s) + .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); #endif } @@ -98,12 +98,12 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { - //c_simpleAuto->Schedule(); + c_simpleAuto->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } void Robot::AutonomousPeriodic() { - // c_drivetrain->Periodic();// update drivetrain no matter what + c_drivetrain->Periodic();// update drivetrain no matter what } void Robot::TeleopInit() { From e7cf4b6e5643718da1fb85115db695e962ff9f67 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sun, 12 Mar 2023 13:44:35 -0400 Subject: [PATCH 084/100] drive: Increase teleop speed for eliminations rounds - Justin C and Nick Q --- src/main/include/Constants.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f245d32..7c44b7a 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -4,9 +4,9 @@ namespace Constants { const int k_NumberOfSwerveModules = 4; - const double k_minDriveSpeed = 0.15; - const double k_normalDriveSpeed = 0.30; - const double k_maxDriveSpeed = 0.60; + const double k_minDriveSpeed = 0.25; + const double k_normalDriveSpeed = 0.50; + const double k_maxDriveSpeed = 0.70; const double k_maxSpinSpeed = 1.00; namespace Arm { From dea32f19327708a85f1fe1a21fcedf327ebcc082 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Sun, 12 Mar 2023 13:45:39 -0400 Subject: [PATCH 085/100] auto: simple auto scores one cube in hybrid - Nick Q --- src/main/cpp/Robot.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index f74a1b9..fabfa21 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -52,6 +52,11 @@ void Robot::RobotInit() { //temp auto #ifdef COMPETITION_MODE + auto orientWheels = frc2::StartEndCommand{ + [&] () { c_drivetrain->setMotion(0,0.05,0); }, + [&] () { c_drivetrain->setMotion(0,0,0); }, + { c_drivetrain } + }.ToPtr(); auto forceOffCube = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.5,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, @@ -64,7 +69,8 @@ void Robot::RobotInit() { {c_drivetrain} }.ToPtr(); - c_simpleAuto = std::move(forceOffCube).WithTimeout(0.5_s) + c_simpleAuto = std::move(orientWheels).WithTimeout(0.5_s) + .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); #endif From 9aeec2f203b912c63876bf87074fe5737b6f9619 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 13 Mar 2023 21:42:58 -0400 Subject: [PATCH 086/100] drive: Add lock movement to drive X button Turn wheels to form an X and resist shoves or sliding. - Nick Q --- src/main/cpp/Robot.cpp | 4 ++++ .../cpp/subsystems/drivetrain/drivetrain.cpp | 16 ++++++++++++++++ .../include/subsystems/drivetrain/drivetrain.h | 6 ++++++ 3 files changed, 26 insertions(+) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index fabfa21..2fc4be4 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -134,6 +134,10 @@ void Robot::TeleopPeriodic() { if (c_driverController->GetBButtonPressed()) { c_drivetrain->resetNavxHeading(); } + + if(c_driverController->GetXButtonPressed()) { + c_drivetrain->lockMovement(false); + } } /** diff --git a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp index f08f079..17fc3db 100644 --- a/src/main/cpp/subsystems/drivetrain/drivetrain.cpp +++ b/src/main/cpp/subsystems/drivetrain/drivetrain.cpp @@ -81,6 +81,9 @@ void Drivetrain::setupWheels() { } void Drivetrain::calculateWheelAnglesAndSpeeds() { + if(m_forceLockMovement){ + return; + } if ((abs(Drivetrain::m_strife) <= 0.001) && (abs(Drivetrain::m_forwards) <= 0.001)) { if(abs(Drivetrain::m_rotation) <= 0.001){ for (int i = 0; i < Constants::k_NumberOfSwerveModules; i++) { @@ -239,3 +242,16 @@ double Drivetrain::getMovementHeading(int module){ double Drivetrain::getMovementVelocity(int module){ return c_wheels[module]->getVelocity(); } + +void Drivetrain::lockMovement(bool restrictMovement){ + // if set true, skips the calculation of everything, and this stays in its orientation regardless of drive movement + m_forceLockMovement = restrictMovement; + m_strife = 0; // set everything to 0 as a safety + m_forwards = 0; + m_rotation = 0; + for(int i=0;i Date: Mon, 13 Mar 2023 21:44:15 -0400 Subject: [PATCH 087/100] arm: Prevent arm movement by moving to current location - Justin C --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index fad7b6b..9c2b19f 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -23,6 +23,7 @@ ArmTeleopCommand::ArmTeleopCommand(ArmSubsystem* arm, frc::XboxController* opera void ArmTeleopCommand::Initialize() { // Update target to current point so arm doesn't move unexpectedly. m_target = c_arm->getGripPoint(); + c_arm->moveToPoint(m_target); } void ArmTeleopCommand::Execute() { From 749ac4d26f2cc67748035acf1ebf51cbbc44ed38 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 13 Mar 2023 21:47:03 -0400 Subject: [PATCH 088/100] arm: Fix arm teleop controls Arm teleop was moving the target position kilometers away due to exhaustion induced programming errors merging two incompatible concepts into an incorrect implementation. Rewrite to focus on one concept: compute various offsets, then add to current target position. - Nick Q and Justin C --- .../cpp/commands/arm/ArmTeleopCommand.cpp | 38 ++++++++++--------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index 9c2b19f..7f59b36 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -28,9 +28,11 @@ void ArmTeleopCommand::Initialize() { void ArmTeleopCommand::Execute() { { - Vector gripVec(m_target.x, m_target.y, m_target.z); - double gripMag = gripVec.len(); - double gripDir = std::atan2(gripVec.x, gripVec.y); // 0 deg == y axis + Vector offsetLX; + Vector offsetLY; + Vector offsetRY; + double gripMag = std::sqrt(std::pow(m_target.x,2)+std::pow(m_target.y,2)); + double gripDir = std::atan2(m_target.x, m_target.y); // 0 deg == y axis // Rotate turret. Speed of rotation is reduced the further the arm reaches. double leftX = c_operatorController->GetLeftX(); @@ -39,13 +41,13 @@ void ArmTeleopCommand::Execute() { leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX); if (0.0 != leftX) { // (+) leftX should move turret clockwise. - gripDir = gripDir + leftX * k_maxPointRotSpeed / gripMag; - Vector offset( - gripMag * std::sin(gripDir), - gripMag * std::cos(gripDir), - 0.0 + double dir = gripDir + (leftX * k_maxPointRotSpeed) / std::max(gripMag,1.0); + Point point1( + gripMag * std::sin(dir), + gripMag * std::cos(dir), + m_target.z ); - m_target = m_target + offset; + offsetLX = point1 - m_target; } // Extend/retract gripper from/to turret. @@ -55,25 +57,27 @@ void ArmTeleopCommand::Execute() { leftY = std::copysign(std::clamp(leftY * leftY, -1.0, 1.0), leftY); if (0.0 != leftY) { // (+) leftY should move away from turret. - gripMag = gripMag + leftY * k_maxPointSpeed; - Vector offset( - gripMag * std::sin(gripDir), - gripMag * std::cos(gripDir), - 0.0 + double mag = gripMag + (leftY * k_maxPointSpeed); + Point point1( + mag * std::sin(gripDir), + mag * std::cos(gripDir), + m_target.z ); - m_target = m_target + offset; + offsetLY = point1 - m_target; } // Move gripper up or down. double rightY = -c_operatorController->GetRightY(); /* Invert so + is up */ // Square input to improve fidelity. - rightY = std::copysign(std::clamp(rightY * rightY, -1.0, 1.0), rightY); rightY = DEADZONE(rightY); + rightY = std::copysign(std::clamp(rightY * rightY, -1.0, 1.0), rightY); if (0.0 != rightY) { // (+) rightY should move gripper up. - m_target = m_target + Vector(0.0, 0.0, rightY * k_maxPointSpeed); + offsetRY = Vector(0.0, 0.0, rightY * k_maxPointSpeed); } + m_target = m_target + offsetLX + offsetLY + offsetRY; + c_arm->moveToPoint(m_target); } From 4c9a09fcbf9c25b2fdb5cfcbbb2be80f2b2f030c Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 13 Mar 2023 21:49:48 -0400 Subject: [PATCH 089/100] arm: Add reset target method to arm teleop command When arm teleop command is rescheduled, it will continue moving to the previous target. Add method so the command can be reset to hold at the current arm position, not just at initialization. - Nick Q --- src/main/cpp/commands/arm/ArmTeleopCommand.cpp | 4 ++++ src/main/include/commands/arm/ArmTeleopCommand.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp index 7f59b36..fd60e11 100644 --- a/src/main/cpp/commands/arm/ArmTeleopCommand.cpp +++ b/src/main/cpp/commands/arm/ArmTeleopCommand.cpp @@ -129,3 +129,7 @@ void ArmTeleopCommand::End(bool interrupted) { bool ArmTeleopCommand::IsFinished() { return false; // dont end because then we wont be able to drive } + +void ArmTeleopCommand::resetTarget(){ + m_target = c_arm->getGripPoint(); +} \ No newline at end of file diff --git a/src/main/include/commands/arm/ArmTeleopCommand.h b/src/main/include/commands/arm/ArmTeleopCommand.h index 1011cf4..5353c66 100644 --- a/src/main/include/commands/arm/ArmTeleopCommand.h +++ b/src/main/include/commands/arm/ArmTeleopCommand.h @@ -17,6 +17,7 @@ class ArmTeleopCommand : public frc2::CommandHelper Date: Mon, 13 Mar 2023 21:52:24 -0400 Subject: [PATCH 090/100] arm: Set current limits on arm motor Try to avoid further destruction of the arm by reducing the max current limits. - Justin C --- src/main/cpp/subsystems/arm/arm.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index ab0ba9d..5f45813 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -38,6 +38,12 @@ ArmSubsystem::ArmSubsystem(std::shared_ptr toml) { m_shoulderPid->SetIntegratorRange(-0.01, 0.01); m_shoulderPid->SetSetpoint(getShoulderAngle()); + + m_turretMotor.SetSmartCurrentLimit(5.0); + m_lowJointMotor.SetSmartCurrentLimit(5.0); + m_midJointMotor.SetSmartCurrentLimit(5.0); + m_gripperRotateMotor.SetSmartCurrentLimit(5.0); + m_gripperGraspMotor.SetSmartCurrentLimit(5.0); } void ArmSubsystem::Periodic() { From 1b56539b666cf7db544ab85fb56b51886a44054b Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Mon, 13 Mar 2023 21:55:27 -0400 Subject: [PATCH 091/100] arm: Calibrate limits and zeroes - Justin C --- src/main/deploy/config.toml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/config.toml b/src/main/deploy/config.toml index 5ec68e5..96b46c1 100644 --- a/src/main/deploy/config.toml +++ b/src/main/deploy/config.toml @@ -3,12 +3,12 @@ # Limits are in raw sensors units turret.limit.lo = -85.0 turret.limit.hi = 90.0 - turret.zeroOffsetRadians = -6.377 + turret.zeroOffsetRadians = -8.001 turret.sensorToRadians = 13.426 # Limits are in degrees shoulder.limit.lo = 45.0 - shoulder.limit.hi = 96.0 + shoulder.limit.hi = 100.0 shoulder.zeroOffsetRadians = 0.131 shoulder.pid.p = 0.35 @@ -17,7 +17,7 @@ shoulder.pid.tolerance = 0.10 # Limits are in degrees - elbow.limit.lo = 15.0 + elbow.limit.lo = 10.0 elbow.limit.hi = 180.0 elbow.zeroOffsetRadians = -7.018 elbow.sensorToRadians = 24.54 From d1b012c3b425d4c38ba9e4e90ed87d5bc7eb2966 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 14 Mar 2023 19:05:05 -0400 Subject: [PATCH 092/100] remove smartdashboard debugging output -Nick Q --- src/main/cpp/Robot.cpp | 3 --- src/main/cpp/subsystems/arm/arm.cpp | 35 +---------------------------- 2 files changed, 1 insertion(+), 37 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 2fc4be4..deeb507 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,7 +18,6 @@ #include "external/cpptoml.h" -#include #ifdef COMPETITION_MODE #include @@ -86,8 +85,6 @@ void Robot::RobotInit() { */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); - - frc::SmartDashboard::PutBoolean("Field Centric Enabled",c_drivetrain->getFieldCentric()); } /** diff --git a/src/main/cpp/subsystems/arm/arm.cpp b/src/main/cpp/subsystems/arm/arm.cpp index 5f45813..9ac3d50 100644 --- a/src/main/cpp/subsystems/arm/arm.cpp +++ b/src/main/cpp/subsystems/arm/arm.cpp @@ -7,7 +7,6 @@ #include #include -#include // PROTOTYPES static double requireTomlDouble (std::shared_ptr toml, std::string const & name); @@ -56,48 +55,16 @@ void ArmSubsystem::Periodic() { m_lowJointMotor.Set(output); } - frc::SmartDashboard::PutNumber("Turret Angle (deg)", RAD_2_DEG(getTurretAngle())); - frc::SmartDashboard::PutNumber("Shoulder Angle (deg)", RAD_2_DEG(getShoulderAngle())); - frc::SmartDashboard::PutNumber("Elbow Angle (deg)", RAD_2_DEG(getElbowAngle())); - frc::SmartDashboard::PutNumber("Wrist Roll Angle (deg)", RAD_2_DEG(getWristRollAngle())); - frc::SmartDashboard::PutNumber("Grip Distance (meters)", getGrip()); - - // Raw values - /* DEBUG ARM * - frc::SmartDashboard::PutNumber("Raw: Turret Angle (V)", m_turretAngleSensor.Get()); - frc::SmartDashboard::PutNumber("Raw: Shoulder Angle (deg)", m_shoulderAngleSensor.Get().value() * 360.0); - frc::SmartDashboard::PutNumber("Raw: Elbow Angle (V)", m_elbowAngleSensor.Get()); - frc::SmartDashboard::PutNumber("Raw: Wrist Roll Angle (V)", m_wristRollAngleSensor.Get()); - frc::SmartDashboard::PutNumber("Raw: Grip Distance (V)", m_gripSensor.Get()); - /* */ + // Report calculated gripper position. Point gripPos = getGripPoint(); std::stringstream gripPosStr; - gripPosStr << std::fixed << std::setprecision(4) - << "(" << gripPos.x - << ", " << gripPos.y - << ", " << gripPos.z - << ")"; - frc::SmartDashboard::PutString("Grip Pos (m)", gripPosStr.str()); // Report calculated arm pose angles. What the robot thinks the angles // should be to reach gripper position. - - /* DEBUG ARM * - ArmPose pose = calcIKJointPoses(gripPos); - frc::SmartDashboard::PutNumber("IK Turret Angle (deg)", RAD_2_DEG(pose.turretAngle)); - frc::SmartDashboard::PutNumber("IK Shoulder Angle (deg)", RAD_2_DEG(pose.shoulderAngle)); - frc::SmartDashboard::PutNumber("IK Elbow Angle (deg)", RAD_2_DEG(pose.elbowAngle)); - /* */ - - frc::SmartDashboard::PutNumber("Turret Current (A)", m_turretMotor.GetOutputCurrent()); - frc::SmartDashboard::PutNumber("Shoulder Current (A)", m_lowJointMotor.GetOutputCurrent()); - frc::SmartDashboard::PutNumber("Elbow Current (A)", m_midJointMotor.GetOutputCurrent()); - frc::SmartDashboard::PutNumber("Wrist Current (A)", m_gripperRotateMotor.GetOutputCurrent()); - frc::SmartDashboard::PutNumber("Grip Current (A)", m_gripperGraspMotor.GetOutputCurrent()); } void ArmSubsystem::initialiseBoundary() { From f67f4097eae2f61f812ebd020554e56eac1f6721 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 14 Mar 2023 19:17:32 -0400 Subject: [PATCH 093/100] remove competition define headers for the simple auto -Nick Q --- src/main/cpp/Robot.cpp | 7 +------ src/main/include/Robot.h | 6 +----- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index deeb507..2196d33 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,12 +18,9 @@ #include "external/cpptoml.h" - -#ifdef COMPETITION_MODE #include #include #include -#endif void Robot::RobotInit() { try{ @@ -49,9 +46,8 @@ void Robot::RobotInit() { //c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); - //temp auto - #ifdef COMPETITION_MODE auto orientWheels = frc2::StartEndCommand{ + //dump cube and move auto [&] () { c_drivetrain->setMotion(0,0.05,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } @@ -72,7 +68,6 @@ void Robot::RobotInit() { .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); - #endif } /** diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 0a8342c..f7ec78a 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -23,11 +23,9 @@ #include "external/cpptoml.h" //temporary auto -#ifdef COMPETITION_MODE #include #include #include -#endif class Robot : public frc::TimedRobot { public: @@ -63,8 +61,6 @@ class Robot : public frc::TimedRobot { //ArmTeleopCommand* c_armTeleopCommand = nullptr; DriveTeleopCommand* c_driveTeleopCommand = nullptr; - //temporary auto - #ifdef COMPETITION_MODE + //dump cube and put into scoring zone auto std::unique_ptr c_simpleAuto{nullptr}; - #endif }; From 0f4ec5205246e84a3b328445186f9f7fa0d2dff6 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Tue, 14 Mar 2023 19:18:13 -0400 Subject: [PATCH 094/100] change code to align with coding guidelines -Nick Q --- src/main/cpp/Robot.cpp | 13 +++++++------ src/main/include/Robot.h | 2 -- src/main/include/RobotCompileModes.h | 4 ++-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 2196d33..4db36d5 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -19,6 +19,7 @@ #include "external/cpptoml.h" #include +#include #include #include @@ -46,27 +47,27 @@ void Robot::RobotInit() { //c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); - auto orientWheels = frc2::StartEndCommand{ //dump cube and move auto + auto c_orientWheels = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.05,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } }.ToPtr(); - auto forceOffCube = frc2::StartEndCommand{ + auto c_forceOffCube = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.5,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } }.ToPtr(); - auto putCubeIntoStation = frc2::StartEndCommand{ + auto c_putCubeIntoStation = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,-0.15,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, {c_drivetrain} }.ToPtr(); - c_simpleAuto = std::move(orientWheels).WithTimeout(0.5_s) - .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) - .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) + c_simpleAuto = std::move(c_orientWheels).WithTimeout(0.5_s) + .AndThen(std::move(c_forceOffCube).WithTimeout(0.3_s)) + .AndThen(std::move(c_putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index f7ec78a..92e9eb9 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -24,8 +24,6 @@ //temporary auto #include -#include -#include class Robot : public frc::TimedRobot { public: diff --git a/src/main/include/RobotCompileModes.h b/src/main/include/RobotCompileModes.h index 951e213..f3640a9 100644 --- a/src/main/include/RobotCompileModes.h +++ b/src/main/include/RobotCompileModes.h @@ -1,8 +1,8 @@ #pragma once -#define COMPETITION_MODE // only competition related dashboard output +//#define COMPETITION_MODE // only competition related dashboard output //#define TESTING_MODE // no console output, but has dashboard output -//#define DEBUG_MODE // enables console output and dashboard output +#define DEBUG_MODE // enables console output and dashboard output //just to guard against multiple things being enabled at the same time From a335343a75efd6a4c999f3836e5405fa02704403 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Wed, 22 Mar 2023 12:25:29 -0400 Subject: [PATCH 095/100] Revert "change code to align with coding guidelines" This reverts commit 0f4ec5205246e84a3b328445186f9f7fa0d2dff6. combined multiple things into one commit. undoing. --- src/main/cpp/Robot.cpp | 13 ++++++------- src/main/include/Robot.h | 2 ++ src/main/include/RobotCompileModes.h | 4 ++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 4db36d5..2196d33 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -19,7 +19,6 @@ #include "external/cpptoml.h" #include -#include #include #include @@ -47,27 +46,27 @@ void Robot::RobotInit() { //c_armTeleopCommand = new ArmTeleopCommand(c_arm, c_operatorController); c_driveTeleopCommand = new DriveTeleopCommand(c_drivetrain, c_driverController); + auto orientWheels = frc2::StartEndCommand{ //dump cube and move auto - auto c_orientWheels = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.05,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } }.ToPtr(); - auto c_forceOffCube = frc2::StartEndCommand{ + auto forceOffCube = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,0.5,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, { c_drivetrain } }.ToPtr(); - auto c_putCubeIntoStation = frc2::StartEndCommand{ + auto putCubeIntoStation = frc2::StartEndCommand{ [&] () { c_drivetrain->setMotion(0,-0.15,0); }, [&] () { c_drivetrain->setMotion(0,0,0); }, {c_drivetrain} }.ToPtr(); - c_simpleAuto = std::move(c_orientWheels).WithTimeout(0.5_s) - .AndThen(std::move(c_forceOffCube).WithTimeout(0.3_s)) - .AndThen(std::move(c_putCubeIntoStation).WithTimeout(2.0_s)) + c_simpleAuto = std::move(orientWheels).WithTimeout(0.5_s) + .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) + .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); } diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 92e9eb9..f7ec78a 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -24,6 +24,8 @@ //temporary auto #include +#include +#include class Robot : public frc::TimedRobot { public: diff --git a/src/main/include/RobotCompileModes.h b/src/main/include/RobotCompileModes.h index f3640a9..951e213 100644 --- a/src/main/include/RobotCompileModes.h +++ b/src/main/include/RobotCompileModes.h @@ -1,8 +1,8 @@ #pragma once -//#define COMPETITION_MODE // only competition related dashboard output +#define COMPETITION_MODE // only competition related dashboard output //#define TESTING_MODE // no console output, but has dashboard output -#define DEBUG_MODE // enables console output and dashboard output +//#define DEBUG_MODE // enables console output and dashboard output //just to guard against multiple things being enabled at the same time From fe571bf75b5446e143a01c1ace7b71ddc5ce0052 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Wed, 22 Mar 2023 12:27:00 -0400 Subject: [PATCH 096/100] change compile mode to debug --- src/main/include/RobotCompileModes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/include/RobotCompileModes.h b/src/main/include/RobotCompileModes.h index 951e213..f3640a9 100644 --- a/src/main/include/RobotCompileModes.h +++ b/src/main/include/RobotCompileModes.h @@ -1,8 +1,8 @@ #pragma once -#define COMPETITION_MODE // only competition related dashboard output +//#define COMPETITION_MODE // only competition related dashboard output //#define TESTING_MODE // no console output, but has dashboard output -//#define DEBUG_MODE // enables console output and dashboard output +#define DEBUG_MODE // enables console output and dashboard output //just to guard against multiple things being enabled at the same time From d002d5fa88321e2f7106bf992a7d58cf4e944ef8 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Wed, 22 Mar 2023 12:28:12 -0400 Subject: [PATCH 097/100] remove unneeded headers in robot.h --- src/main/include/Robot.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index f7ec78a..92e9eb9 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -24,8 +24,6 @@ //temporary auto #include -#include -#include class Robot : public frc::TimedRobot { public: From e5009ac268838064e7da8bf61b9bbdc53eeabc70 Mon Sep 17 00:00:00 2001 From: nickquednow Date: Wed, 22 Mar 2023 12:31:06 -0400 Subject: [PATCH 098/100] re-add driverstation field centric output did not realize it was used by the driver, so it was removes. undone :) --- src/main/cpp/Robot.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 2196d33..b330def 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,6 +18,8 @@ #include "external/cpptoml.h" +#include + #include #include #include @@ -80,6 +82,8 @@ void Robot::RobotInit() { */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); + + frc::SmartDashboard::PutBoolean("Field Centric Enabled",c_drivetrain->getFieldCentric()); } /** From 2bbe6ebb810638561fabd70f9877156489d194fc Mon Sep 17 00:00:00 2001 From: nickquednow Date: Wed, 22 Mar 2023 12:33:13 -0400 Subject: [PATCH 099/100] rename c_simpleSuto to c_autoDumpCubeAndScore for clarity --- src/main/cpp/Robot.cpp | 6 +++--- src/main/include/Robot.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index b330def..83f1877 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -66,7 +66,7 @@ void Robot::RobotInit() { {c_drivetrain} }.ToPtr(); - c_simpleAuto = std::move(orientWheels).WithTimeout(0.5_s) + c_autoDumpCubeAndScore = std::move(orientWheels).WithTimeout(0.5_s) .AndThen(std::move(forceOffCube).WithTimeout(0.3_s)) .AndThen(std::move(putCubeIntoStation).WithTimeout(2.0_s)) .Unwrap(); @@ -100,7 +100,7 @@ void Robot::DisabledPeriodic() {} * RobotContainer} class. */ void Robot::AutonomousInit() { - c_simpleAuto->Schedule(); + c_autoDumpCubeAndScore->Schedule(); // TODO: Make sure to cancel autonomous command in teleop init. } @@ -110,7 +110,7 @@ void Robot::AutonomousPeriodic() { void Robot::TeleopInit() { // Make sure autonomous command is canceled first. - c_simpleAuto->Cancel(); + c_autoDumpCubeAndScore->Cancel(); //c_armTeleopCommand->Schedule(); //c_armTeleopCommand->resetTarget(); diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 92e9eb9..01e4d6f 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -60,5 +60,5 @@ class Robot : public frc::TimedRobot { DriveTeleopCommand* c_driveTeleopCommand = nullptr; //dump cube and put into scoring zone auto - std::unique_ptr c_simpleAuto{nullptr}; + std::unique_ptr c_autoDumpCubeAndScore{nullptr}; }; From a907c65ec28d65aecaa3132e523867e9567ac249 Mon Sep 17 00:00:00 2001 From: PhyXTGears Date: Wed, 22 Mar 2023 17:21:37 -0400 Subject: [PATCH 100/100] Restore missing header import StartEndCommand used by autonomous. - Justin C --- src/main/include/Robot.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 01e4d6f..4f4b54d 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -24,6 +24,7 @@ //temporary auto #include +#include class Robot : public frc::TimedRobot { public: