Fixes/princeton

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)

src/main/cpp/Robot.cpp

@@ -18,11 +18,20 @@
 
 #include "external/cpptoml.h"
 
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include <frc2/command/SequentialCommandGroup.h>
+#include <frc2/command/Command.h>
+#include <frc/Timer.h>
+
 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);
   }
 
@@ -31,11 +40,36 @@ 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"));
 
   //Commands
+  //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
+    [&] () { 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); },
+    { c_drivetrain }
+  }.ToPtr();
+
+  auto putCubeIntoStation = frc2::StartEndCommand{
+    [&] () { c_drivetrain->setMotion(0,-0.15,0); },
+    [&] () { c_drivetrain->setMotion(0,0,0); },
+    {c_drivetrain}
+  }.ToPtr();
+
+  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();
 }
 
 /**
@@ -48,6 +82,8 @@ void Robot::RobotInit() {
  */
 void Robot::RobotPeriodic() {
   frc2::CommandScheduler::GetInstance().Run();
+
+  frc::SmartDashboard::PutBoolean("Field Centric Enabled",c_drivetrain->getFieldCentric());
 }
 
 /**
@@ -64,16 +100,23 @@ void Robot::DisabledPeriodic() {}
  * RobotContainer} class.
  */
 void Robot::AutonomousInit() {
+  c_autoDumpCubeAndScore->Schedule();
   // TODO: Make sure to cancel autonomous command in teleop init.
 }
 
-void Robot::AutonomousPeriodic() {}
+void Robot::AutonomousPeriodic() {
+  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_autoDumpCubeAndScore->Cancel();
+
+  //c_armTeleopCommand->Schedule();
+  //c_armTeleopCommand->resetTarget();
   c_driveTeleopCommand->Schedule();
+  c_drivetrain->enableFieldCentric();
 }
-
 /**
  * This function is called periodically during operator control.
  */
@@ -87,6 +130,10 @@ void Robot::TeleopPeriodic() {
   if (c_driverController->GetBButtonPressed()) {
     c_drivetrain->resetNavxHeading();
   }
+
+  if(c_driverController->GetXButtonPressed()) {
+    c_drivetrain->lockMovement(false);
+  }
 }
 
 /**

src/main/cpp/commands/arm/ArmTeleopCommand.cpp

@@ -0,0 +1,135 @@
+#include "commands/arm/ArmTeleopCommand.h"
+#include "subsystems/arm/arm.h"
+
+#include <frc/XboxController.h>
+
+#include <cmath>
+
+#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) * 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) {
+    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();
+    c_arm->moveToPoint(m_target);
+}
+
+void ArmTeleopCommand::Execute() {
+    {
+        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();
+        // Square input to improve fidelity.
+        leftX = DEADZONE(leftX);
+        leftX = std::copysign(std::clamp(leftX * leftX, -1.0, 1.0), leftX);
+        if (0.0 != leftX) {
+            // (+) leftX should move turret clockwise.
+            double dir = gripDir + (leftX * k_maxPointRotSpeed) / std::max(gripMag,1.0);
+            Point point1(
+                gripMag * std::sin(dir),
+                gripMag * std::cos(dir),
+                m_target.z
+            );
+            offsetLX = point1 - m_target;
+        }
+
+        // Extend/retract gripper from/to turret.
+        double leftY = -c_operatorController->GetLeftY(); /* Invert so + is forward */
+        // Square input to improve fidelity.
+        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.
+            double mag = gripMag + (leftY * k_maxPointSpeed);
+            Point point1(
+                mag * std::sin(gripDir),
+                mag * std::cos(gripDir),
+                m_target.z
+            );
+            offsetLY = point1 - m_target;
+        }
+
+        // Move gripper up or down.
+        double rightY = -c_operatorController->GetRightY();    /* Invert so + is up */
+        // Square input to improve fidelity.
+        rightY = DEADZONE(rightY);
+        rightY = std::copysign(std::clamp(rightY * rightY, -1.0, 1.0), rightY);
+        if (0.0 != rightY) {
+            // (+) rightY should move gripper up.
+            offsetRY = Vector(0.0, 0.0, rightY * k_maxPointSpeed);
+        }
+
+        m_target = m_target + offsetLX + offsetLY + offsetRY;
+
+        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 = 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();
+
+        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
+}
+
+void ArmTeleopCommand::resetTarget(){
+    m_target = c_arm->getGripPoint();
+}

src/main/cpp/commands/arm/MoveToIntake.cpp

@@ -0,0 +1,43 @@
+#include "commands/arm/MoveToIntake.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->getGripPoint();
+
+    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));
+}

src/main/cpp/commands/drivetrain/driveTeleopCommand.cpp

@@ -34,8 +34,9 @@ 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
     );
 }