Removed unneeded pointers, wrote non-working test for the Arm. Need t…

…o figure out simulation controllers

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.2.1"
+    id "edu.wpi.first.GradleRIO" version "2023.4.2"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)

src/main/cpp/Arm.cpp

@@ -5,10 +5,10 @@
 Arm::Arm(shared_ptr<XboxController>& controller) : armController(controller) {
   //All encoders assume initial position is 0.
   //Therefore, arm must be all the way down and fully retracted at start.
-  armJointEncoder->Reset();
-  armGrabberEncoder->Reset();
-  armExtensionEncoder->Reset();
-  armGrabberEncoder->SetDistancePerRotation(4.0);
+  armJointEncoder.Reset();
+  armGrabberEncoder.Reset();
+  armExtensionEncoder.Reset();
+  armGrabberEncoder.SetDistancePerRotation(4.0);
 
   SetJointAndGrabberLimits(JointPositions::POS1); //Default joint & grabber limits
   SetExtensionLimits(ExtensionPositions::EXT_L);  //Default extension limits
@@ -50,9 +50,9 @@ void Arm::SetExtensionLimits(ExtensionPositions pos) {
 
 void Arm::ArmUpdate() {
  /* --=[ UPDATE VARIABLES]=-- */
-  armGrabberEncoderDistance = armGrabberEncoder->GetAbsolutePosition();
-  armJointEncoderDistance = armJointEncoder->GetDistance();
-  armExtensionEncoderDistance = armExtensionEncoder->GetDistance();
+  armGrabberEncoderDistance = armGrabberEncoder.GetAbsolutePosition();
+  armJointEncoderDistance = armJointEncoder.GetDistance();
+  armExtensionEncoderDistance = armExtensionEncoder.GetDistance();
  /* --=[ END ]=-- */
   if ( armController->GetLeftStickButtonPressed() ) {
     if ( MODE == Mode::DEBUG ) MODE = Mode::NORMAL;
@@ -84,7 +84,7 @@ void Arm::HandleExtensionInput() {
 }
 void Arm::MoveArmExtension() {
   MoveWithinLimits ( //deref, pass by value. @see: MoveWithinLimits()
-    *armExtension,
+    &armExtension,
     armExtensionEncoderDistance,
     Speeds::EXTEND_SPEED,
     Speeds::RETRACT_SPEED,
@@ -95,13 +95,13 @@ void Arm::MoveArmExtension() {
 
 void Arm::HandleGrabberPneumatics() {
   if ( armController->GetAButtonPressed() ) {
-    armGrabberPiston->Toggle();
+    armGrabberPiston.Toggle();
   }
 }
 
 void Arm::MoveGrabber() {
   MoveWithinLimits (
-    *armGrabberJoint,
+    &armGrabberJoint,
     armGrabberEncoderDistance,
     Speeds::GRABBER_UPWARDS_SPEED,
     Speeds::GRABBER_DOWNWARDS_SPEED,
@@ -142,7 +142,7 @@ void Arm::HandleJointInput() {
 
 void Arm::MoveArmJoint() {
   MoveWithinLimits (
-    *armJoint, 
+    &armJoint, 
     armJointEncoderDistance,
     Speeds::JOINT_UPWARDS_SPEED,
     Speeds::JOINT_DOWNWARDS_SPEED,
@@ -152,18 +152,18 @@ void Arm::MoveArmJoint() {
 }
 
 //recieve motor by reference, was passed by value
-void Arm::MoveWithinLimits( WPI_TalonSRX &motor, int distance,
+void Arm::MoveWithinLimits( WPI_TalonSRX *motor, int distance,
   double speedf, double speedb,
   int limitUpper, int limitLower )
 {
   if ( limitLower < distance && distance < limitUpper ) {
-    motor.Set(0.0);
+    motor->Set(0.0);
   }
   else if ( limitLower > distance ) {
-    motor.Set(speedf);
+    motor->Set(speedf);
   }
   else if ( limitUpper < distance ) {
-    motor.Set(speedb);
+    motor->Set(speedb);
   }
 }
 
@@ -194,12 +194,12 @@ void Arm::DebugArmJoint() {
     SmartDashboard::PutNumber("Joint: ",armJointEncoderDistance);
     SmartDashboard::PutNumber("Grabber: ",armGrabberEncoderDistance);
     SmartDashboard::PutNumber("Extension: ",armExtensionEncoderDistance);
-    std::cout << armGrabberEncoder->GetAbsolutePosition() << std::endl;
+    std::cout << armGrabberEncoder.GetAbsolutePosition() << std::endl;
     DebugTimer.Reset();
     DebugTimer.Start();
   }
  /* --=[ ARM JOINT ]=-- */
-  armGrabberJoint->Set(armController->GetLeftY());
+  armGrabberJoint.Set(armController->GetLeftY());
   // === LOOP CONDITIONS ===
   if (armController->GetRightBumperPressed()) armMovingUp = true;
   if (armController->GetRightBumperReleased()) armMovingUp = false;
@@ -210,17 +210,17 @@ void Arm::DebugArmJoint() {
   //===== IF "LOOPS" =====
   //make sure that if both buttons are pressed, arm doesn't try to move both ways
   if (armMovingUp && !armMovingDown) {
-    armJoint->Set(-0.5);
+    armJoint.Set(-0.5);
   }
   else if (!armMovingDown) { //arm isn't currently trying to move back
-    armJoint->Set(0.0);
+    armJoint.Set(0.0);
   }
 
   if (armMovingDown && !armMovingUp) {
-    armJoint->Set(0.5);
+    armJoint.Set(0.5);
   }
   else if (!armMovingUp) {
-    armJoint->Set(0.0);
+    armJoint.Set(0.0);
   }
   //===== END IF "LOOPS" =====
  /* --=[ END ]=-- */
@@ -239,18 +239,18 @@ void Arm::DebugArmExtension() {
 
 void Arm::DebugArmExtend() {
   if(armIsExtending && !armIsRetracting) {
-    armExtension->Set(-0.5);
+    armExtension.Set(-0.5);
   }
   else if (!armIsRetracting) {
-    armExtension->Set(0.0);
+    armExtension.Set(0.0);
   }
 }
 
 void Arm::DebugArmRetract() {
   if(armIsRetracting && !armIsExtending) {
-    armExtension->Set(0.5);
+    armExtension.Set(0.5);
   }
   else if (!armIsExtending) {
-    armExtension->Set(0.0);
+    armExtension.Set(0.0);
   }
 }

src/main/cpp/Drive.cpp

@@ -1,11 +1,12 @@
 #include "Drive.h"
+#include <iostream>
 
 Drive::Drive(shared_ptr<XboxController>& controller) : m_controller(controller) {
   m_timer.Start();
 }
 
 void Drive::TuxDrive() {
-  m_drive->ArcadeDrive(m_controller->GetLeftY(),-m_controller->GetRightX() * 0.6);
+  m_drive.ArcadeDrive(m_controller->GetLeftY(),-m_controller->GetRightX() * 0.6);
 }
 
 void Drive::Autonomous() {

src/main/cpp/Robot.cpp

@@ -40,15 +40,15 @@ void Robot::AutonomousInit() {
 
 void Robot::AutonomousPeriodic() {
   if (m_autoSelected == kAutoNameCustom) {
-    m_drive->Autonomous();
+    m_drive.Autonomous();
   }
 }
 
 void Robot::TeleopInit() {}
 
 void Robot::TeleopPeriodic() {
-  m_drive->TuxDrive();
-  m_arm->ArmUpdate();
+  m_drive.TuxDrive();
+  m_arm.ArmUpdate();
 }
 
 void Robot::DisabledInit() {}

src/main/include/Arm.h

@@ -22,7 +22,7 @@ using namespace std;
 using namespace Constants;
 using namespace Constants::Limits;
 
-enum Mode {
+enum class Mode {
   DEBUG,
   NORMAL
 };
@@ -33,7 +33,7 @@ class Arm {
   void SetJointAndGrabberLimits(JointPositions pos);
   void SetExtensionLimits(ExtensionPositions pos);
   void MoveWithinLimits (
-    WPI_TalonSRX &motor,
+    WPI_TalonSRX* motor,
     int distance,
     double speedf,
     double speedb,
@@ -57,18 +57,23 @@ class Arm {
   void DebugArmExtension();
   void DebugArmExtend();
   void DebugArmRetract();
+
+  // Unit Testing
+  WPI_TalonSRX *GetExtensionMotor() {
+    return &armExtension;
+  }
  private:
   shared_ptr<XboxController> armController;
-  unique_ptr<WPI_TalonSRX> armJoint = make_unique<WPI_TalonSRX>(MotorControllers::ARM_JOINT);
-  unique_ptr<WPI_TalonSRX> armGrabberJoint = make_unique<WPI_TalonSRX>(MotorControllers::ARM_GRABBER_JOINT);
-  unique_ptr<WPI_TalonSRX> armExtension = make_unique<WPI_TalonSRX>(MotorControllers::ARM_EXTENSION);
-  unique_ptr<Encoder> armJointEncoder = make_unique<Encoder>(Encoders::JOINT_ENCODER_ACHANNEL, Encoders::JOINT_ENCODER_BCHANNEL);
-  unique_ptr<Encoder> armExtensionEncoder = make_unique<Encoder>(Encoders::EXTEND_ENCODER_ACHANNEL, Encoders::EXTEND_ENCODER_BCHANNEL);
-  unique_ptr<AnalogEncoder> armGrabberEncoder = make_unique<AnalogEncoder>(Encoders::GRABBER_ENCODER);
-  unique_ptr<Solenoid> armGrabberPiston = make_unique<Solenoid>(PneumaticsModuleType::CTREPCM, Solenoids::ARM_SOLENOID);
-  unique_ptr<Compressor> compressor = make_unique<Compressor>(COMPRESSOR, PneumaticsModuleType::CTREPCM);
+  WPI_TalonSRX armJoint{MotorControllers::ARM_JOINT};
+  WPI_TalonSRX armGrabberJoint{MotorControllers::ARM_GRABBER_JOINT};
+  WPI_TalonSRX armExtension{MotorControllers::ARM_EXTENSION};
+  Encoder armJointEncoder{Encoders::JOINT_ENCODER_ACHANNEL, Encoders::JOINT_ENCODER_BCHANNEL};
+  Encoder armExtensionEncoder{Encoders::EXTEND_ENCODER_ACHANNEL, Encoders::EXTEND_ENCODER_BCHANNEL};
+  AnalogEncoder armGrabberEncoder{Encoders::GRABBER_ENCODER};
+  Solenoid armGrabberPiston{PneumaticsModuleType::CTREPCM, Solenoids::ARM_SOLENOID};
+  Compressor compressor{COMPRESSOR, PneumaticsModuleType::CTREPCM};
 
-  int MODE = Mode::DEBUG;
+  Mode MODE = Mode::DEBUG;
 
   int UPPER_JOINT_LIMIT;
   int LOWER_JOINT_LIMIT;

src/main/include/Constants.h

@@ -56,12 +56,12 @@ enum Solenoids {
 namespace Speeds {
 /* === ARM === */
 units::second_t const BBUTTON_CHECK_INTERVAL = 1_s; 
-double const JOINT_UPWARDS_SPEED = -0.3;
-double const JOINT_DOWNWARDS_SPEED = 0.3;
-double const EXTEND_SPEED = 0.2;
-double const RETRACT_SPEED = -0.2;
-double const GRABBER_UPWARDS_SPEED = -0.2;
-double const GRABBER_DOWNWARDS_SPEED = 0.2;
+double const JOINT_UPWARDS_SPEED = -0.5;
+double const JOINT_DOWNWARDS_SPEED = 0.5;
+double const EXTEND_SPEED = 0.5;
+double const RETRACT_SPEED = -0.5;
+double const GRABBER_UPWARDS_SPEED = -0.5;
+double const GRABBER_DOWNWARDS_SPEED = 0.5;
 /* === DRIVETRAIN === */
 double const AUTO_SPEED = 0.5;
 } //namespace Speeds

src/main/include/Drive.h

@@ -3,37 +3,44 @@
 #include "Constants.h"
 
 #include <memory>
-
+#include <stdexcept>
+#include <frc/SPI.h>
+#include <frc/SerialPort.h>
+#include "AHRS.h"
 #include "ctre/Phoenix.h"
+
 #include <frc/Timer.h>
 #include <units/time.h>
 #include <frc/XboxController.h>
 #include <frc/motorcontrol/Spark.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/motorcontrol/MotorControllerGroup.h>
-//#include <frc/SpeedControllerGroup.h>
+
 using namespace frc;
 using namespace std;
 
 class Drive {
  public:
-
+ 
   Drive(shared_ptr<XboxController>& controller); //Constructor
   void TuxDrive(); //Actual driving
   void Autonomous();
 
  private:
-  
+
   shared_ptr<XboxController> m_controller;
-  unique_ptr<WPI_TalonSRX> m_frontLeft = make_unique<WPI_TalonSRX>(Constants::MotorControllers::FRONT_LEFT);
-  unique_ptr<WPI_TalonSRX> m_backLeft = make_unique<WPI_TalonSRX>(Constants::MotorControllers::BACK_LEFT);
-  unique_ptr<MotorControllerGroup> m_left = make_unique<MotorControllerGroup>(*m_frontLeft, *m_backLeft);
+  WPI_TalonSRX m_frontLeft{Constants::MotorControllers::FRONT_LEFT};
+  WPI_TalonSRX m_backLeft{Constants::MotorControllers::BACK_LEFT};
+  MotorControllerGroup m_left{m_frontLeft, m_backLeft};
+
+  WPI_TalonSRX m_frontRight{Constants::MotorControllers::FRONT_RIGHT};
+  WPI_TalonSRX m_backRight{Constants::MotorControllers::BACK_RIGHT};
+  MotorControllerGroup m_right{m_frontRight, m_backRight};
 
-  unique_ptr<WPI_TalonSRX> m_frontRight = make_unique<WPI_TalonSRX>(Constants::MotorControllers::FRONT_RIGHT);
-  unique_ptr<WPI_TalonSRX> m_backRight = make_unique<WPI_TalonSRX>(Constants::MotorControllers::BACK_RIGHT);
-  unique_ptr<MotorControllerGroup> m_right = make_unique<MotorControllerGroup>(*m_frontRight, *m_backRight);
+  DifferentialDrive m_drive{m_left,m_right};
 
-  unique_ptr<DifferentialDrive> m_drive = make_unique<DifferentialDrive>(*m_left,*m_right);
+
+  //AHRS m_gyro{SPI::Port::kMXP};
 
   Timer m_timer;
 };

src/main/include/Robot.h

@@ -48,6 +48,6 @@ class Robot : public TimedRobot {
    * Left Joystick Y-Axis = Move robot forwards/backwards
    * Right Joystick X-Axis = Rotate robot left/right
    */
-  unique_ptr<Arm> m_arm = make_unique<Arm>(m_controller);
-  unique_ptr<Drive> m_drive = make_unique<Drive>(m_controller);
+  Arm m_arm{m_controller};
+  Drive m_drive{m_controller};
 };

src/test/cpp/ArmTest.cpp

@@ -22,6 +22,16 @@ TEST_F(ArmTest, ArmPistonDefaultFalse) {
   EXPECT_EQ(m_solenoid.GetOutput(),false);
 }
 
+// TEST_F(ArmTest, ArmExtensionMovesManually) {
+//   WPI_TalonSRX* extensionSim = m_arm.GetExtensionMotor();
+//   EXPECT_EQ(extensionSim->GetMotorOutputPercent(),0.0);
+//   m_simController.SetYButton(true);
+//   m_arm.DebugArmExtension();
+//   EXPECT_EQ(extensionSim->GetMotorOutputPercent(),0.5);
+//   m_simController.SetYButton(false);
+//   m_arm.DebugArmExtension();
+//   EXPECT_EQ(extensionSim->GetMotorOutputPercent(),0.0);
+// }
 /**
  * @note: This test will fail because testing OOP code is a pain. 
  */

vendordeps/NavX.json

@@ -0,0 +1,39 @@
+{
+    "fileName": "NavX.json",
+    "name": "KauaiLabs_navX_FRC",
+    "version": "2023.0.3",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2023/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-java",
+            "version": "2023.0.3"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-cpp",
+            "version": "2023.0.3",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "linuxarm32",
+                "linuxarm64",
+                "linux86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}