tune auto and add arm decrease to grab ball

roboticsmgci · Apr 10, 2022 · e079646 · e079646
1 parent 220e62f
commit e079646
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 9 deletions.
diff --git a/src/main/cpp/commands/autonomous/Ball2.cpp b/src/main/cpp/commands/autonomous/Ball2.cpp
@@ -27,13 +27,13 @@ Ball2::Ball2(
         // Remove ball
         IntakeMove(intake, -1, 0.3),
         // Taxi out
-        TankMoveGyro(drivetrain, -1.3, 0.6),
+        TankMoveGyro(drivetrain, -0.4, 0.6),
         // Break tape
         ArmLower(arm),
         // Turn around to face the ball
         TankTurn(drivetrain, 165),
         // Get the ball
-        GrabBall(drivetrain, intake),
+        GrabBall(arm, drivetrain, intake),
         // Turn back around to face hub
         TankTurn(drivetrain, -165),
         TankStop(drivetrain, 0.2),

diff --git a/src/main/cpp/commands/autonomous/Ball3.cpp b/src/main/cpp/commands/autonomous/Ball3.cpp
@@ -31,13 +31,13 @@ Ball3::Ball3(
         // Move to ball 1
         TankMove(drivetrain, 1.10, 0.4),
         // Get ball 1
-        GrabBall(drivetrain, intake),
+        GrabBall(arm, drivetrain, intake),
         // Turn to face ball 2
         TankTurn(drivetrain, -115),
         // Move to ball 2
         TankMove(drivetrain, 1.4, 0.4),
         // Get ball 2
-        GrabBall(drivetrain, intake),
+        GrabBall(arm, drivetrain, intake),
         // Turn to hub
         TankTurn(drivetrain, -100),
         // Move to hub

diff --git a/src/main/cpp/commands/common/ArmLower.cpp b/src/main/cpp/commands/common/ArmLower.cpp
@@ -8,7 +8,7 @@ ArmLower::ArmLower(Arm& arm): m_arm(&arm){
     SetName("ArmLower");
     AddCommands(
         // Break tape and lower
-        ArmMove(arm, -0.3, 0.5),
+        ArmMove(arm, -0.3, 0.4),
         // Stop, let it drop by gravity
         ArmMove(arm, 0, 0.2)
     );

diff --git a/src/main/cpp/commands/common/GrabBall.cpp b/src/main/cpp/commands/common/GrabBall.cpp
@@ -2,22 +2,28 @@
 
 #include <frc2/command/ParallelCommandGroup.h>
 
+#include "commands/ArmMove.h"
 #include "commands/IntakeMove.h"
-#include "commands/TankMove.h"
+#include "commands/TankMoveGyro.h"
+#include "commands/TankRawMove.h"
 
 
 GrabBall::GrabBall(
+        Arm& arm,
         Drivetrain& drivetrain, 
         Intake& intake):
+            m_arm(&arm),
             m_drivetrain(&drivetrain),
             m_intake(&intake){
 
     SetName("GrabBall");
     AddCommands(
+        ArmMove(arm, -0.15, 0.6),
         // Move forward while intake in
         frc2::ParallelCommandGroup(
-            TankMove(drivetrain, 0.5, 0.6),
-            IntakeMove(intake, 0.5, 0.8))
+            ArmMove(arm, -0.1, 1),
+            TankRawMove(drivetrain, 0.5 * 1.1, 0.5 * 1, 1),
+            IntakeMove(intake, 0.5, 1))
     );
 
 }
diff --git a/src/main/include/commands/common/GrabBall.h b/src/main/include/commands/common/GrabBall.h
@@ -3,6 +3,7 @@
 #include <frc2/command/CommandHelper.h>
 #include <frc2/command/SequentialCommandGroup.h>
 
+#include "subsystems/Arm.h"
 #include "subsystems/Drivetrain.h"
 #include "subsystems/Intake.h"
 
@@ -11,10 +12,11 @@ class GrabBall: public frc2::CommandHelper<frc2::SequentialCommandGroup, GrabBal
 
     public:
 
-        GrabBall(Drivetrain& drivetrain, Intake& intake);
+        GrabBall(Arm& arm, Drivetrain& drivetrain, Intake& intake);
 
     private:
 
+        Arm* m_arm;
         Drivetrain* m_drivetrain;
         Intake* m_intake;