remove forward, fix taxi, increase auto intake power (#4)

remove forward, fix taxi, increase auto intake power

src/main/cpp/RobotContainer.cpp

@@ -12,6 +12,7 @@
 #include "commands/TankDrive.h"
 #include "commands/IntakeIn.h"
 #include "commands/IntakeOut.h"
+#include "commands/IntakeYeet.h"
 #include "commands/ArmUp.h"
 #include "commands/ArmDown.h"
 #include "commands/ArmDrive.h"
@@ -112,6 +113,9 @@ void RobotContainer::ConfigureButtonBindings() {
     frc2::JoystickButton(&m_stick3,3).WhenHeld(
         IntakeOut(m_intake)
     );
+    frc2::JoystickButton(&m_stick3,1).WhenHeld(
+        IntakeYeet(m_intake)
+    );
     frc2::JoystickButton(&m_stick3,6).WhenHeld(
         ArmUp(m_arm)
     );

src/main/cpp/commands/AutoDriveForward.cpp

@@ -0,0 +1,44 @@
+
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/AutoDriveForward.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Robot.h"
+
+AutoDriveForward::AutoDriveForward(
+        Drivetrain& drivetrain):
+            m_drivetrain(&drivetrain) {
+
+    SetName("MoveBack");
+    AddRequirements({m_drivetrain});
+}
+
+// Called just before this Command runs the first time
+void AutoDriveForward::Initialize() {
+    duration_counter = 0;
+    // Get everything in a safe starting state.
+    m_drivetrain->Drive(0, 0);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void AutoDriveForward::Execute() {
+    if (duration_counter < duration){
+        m_drivetrain->Drive(-0.5, 0.5);
+        duration_counter++;
+    }
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool AutoDriveForward::IsFinished() {
+    return (duration_counter >= duration);
+}
+
+// Called once after isFinished returns true
+void AutoDriveForward::End(bool) {
+    m_drivetrain->Drive(0, 0);
+}
+

src/main/cpp/commands/DriveBack.cpp → src/main/cpp/commands/AutoDriveTaxi.cpp

@@ -3,13 +3,13 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "commands/DriveBack.h"
+#include "commands/AutoDriveTaxi.h"
 
 #include <frc/controller/PIDController.h>
 
 #include "Robot.h"
 
-DriveBack::DriveBack(
+AutoDriveTaxi::AutoDriveTaxi(
         Drivetrain& drivetrain):
             m_drivetrain(&drivetrain) {
 
@@ -18,27 +18,28 @@ DriveBack::DriveBack(
 }
 
 // Called just before this Command runs the first time
-void DriveBack::Initialize() {
+void AutoDriveTaxi::Initialize() {
     duration_counter = 0;
     // Get everything in a safe starting state.
     m_drivetrain->Drive(0, 0);
 }
 
+
 // Called repeatedly when this Command is scheduled to run
-void DriveBack::Execute() {
-    if (duration_counter < 150){
-        m_drivetrain->Drive(0.5, 0.5);
+void AutoDriveTaxi::Execute() {
+    if (duration_counter < duration){
+        m_drivetrain->Drive(0.5, -0.5);
         duration_counter++;
     }
 }
 
 // Make this return true when this Command no longer needs to run execute()
-bool DriveBack::IsFinished() {
-    return false;
+bool AutoDriveTaxi::IsFinished() {
+    return (duration_counter >= duration);
 }
 
 // Called once after isFinished returns true
-void DriveBack::End(bool) {
+void AutoDriveTaxi::End(bool) {
     m_drivetrain->Drive(0, 0);
 }
 

src/main/cpp/commands/AutoIntakeOut.cpp

@@ -0,0 +1,44 @@
+
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/AutoIntakeOut.h"
+
+#include <frc/controller/PIDController.h>
+
+#include "Robot.h"
+
+AutoIntakeOut::AutoIntakeOut(
+        Intake& intake):
+            m_intake(&intake) {
+
+    SetName("MoveBack");
+    AddRequirements({m_intake});
+}
+
+// Called just before this Command runs the first time
+void AutoIntakeOut::Initialize() {
+    duration_counter = 0;
+    // Get everything in a safe starting state.
+    m_intake->Stop();
+}
+
+// Called repeatedly when this Command is scheduled to run
+void AutoIntakeOut::Execute() {
+    if (duration_counter < duration){
+        m_intake->Rotate(1);
+        duration_counter++;
+    }
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool AutoIntakeOut::IsFinished() {
+    return (duration_counter >= duration);;
+}
+
+// Called once after isFinished returns true
+void AutoIntakeOut::End(bool) {
+    m_intake->Stop();
+}
+

src/main/cpp/commands/Autonomous.cpp

@@ -6,13 +6,20 @@
 
 #include <frc2/command/ParallelCommandGroup.h>
 
-#include "commands/DriveBack.h"
+#include "commands/AutoDriveForward.h"
+#include "commands/AutoDriveTaxi.h"
+#include "commands/AutoIntakeOut.h"
 
 Autonomous::Autonomous(
-        Drivetrain& drivetrain):
-            m_drivetrain(&drivetrain){
+        Drivetrain& drivetrain, Intake& intake):
+            m_drivetrain(&drivetrain),
+            m_intake(&intake){
 
     SetName("Autonomous");
-    AddCommands(DriveBack(drivetrain));
+    AddCommands(
+        //AutoDriveForward(drivetrain),
+        AutoIntakeOut(intake),
+        AutoDriveTaxi(drivetrain)
+    );
 
 }

src/main/cpp/commands/IntakeYeet.cpp

@@ -0,0 +1,21 @@
+#include "commands/IntakeYeet.h"
+
+#include "Robot.h"
+#include "RobotContainer.h"
+
+IntakeYeet::IntakeYeet(Intake& intake) : m_intake(&intake){
+    SetName("IntakeYeet");
+    AddRequirements({m_intake});
+}
+
+void IntakeYeet::Initialize(){
+    m_intake->Stop();
+}
+
+void IntakeYeet::Execute() {
+    m_intake->Rotate(speed);
+}
+
+void IntakeYeet::End(bool){
+    m_intake->Stop();
+}

src/main/include/RobotContainer.h

@@ -35,7 +35,7 @@ class RobotContainer {
         Intake m_intake;
         Arm m_arm;
 
-        Autonomous m_autonomousCommand{m_drivetrain};
+        Autonomous m_autonomousCommand{m_drivetrain, m_intake};
 
         double speed = 0;
 

src/main/include/commands/DriveBack.h → src/main/include/commands/AutoDriveForward.h

@@ -15,11 +15,11 @@
  * enabled while this command is running. The input is the averaged
  * values of the left and right encoders.
  */
-class DriveBack: public frc2::CommandHelper<frc2::CommandBase, DriveBack> {
+class AutoDriveForward: public frc2::CommandHelper<frc2::CommandBase, AutoDriveForward> {
 
     public:
 
-        DriveBack(Drivetrain& drivetrain);
+        AutoDriveForward(Drivetrain& drivetrain);
         void Initialize() override;
         void Execute() override;
         bool IsFinished() override;
@@ -28,5 +28,6 @@ class DriveBack: public frc2::CommandHelper<frc2::CommandBase, DriveBack> {
     private:
 
         Drivetrain* m_drivetrain;
+        int duration = 40;
         int duration_counter;
 };

src/main/include/commands/AutoDriveTaxi.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Drivetrain.h"
+
+/**
+ * Drive the given distance straight (negative values go backwards).
+ * Uses a local PID controller to run a simple PID loop that is only
+ * enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
+ */
+class AutoDriveTaxi: public frc2::CommandHelper<frc2::CommandBase, AutoDriveTaxi> {
+
+    public:
+
+        AutoDriveTaxi(Drivetrain& drivetrain);
+        void Initialize() override;
+        void Execute() override;
+        bool IsFinished() override;
+        void End(bool interrupted) override;
+
+    private:
+
+        Drivetrain* m_drivetrain;
+        int duration = 150;
+        int duration_counter;
+};

src/main/include/commands/AutoIntakeOut.h

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Intake.h"
+
+/**
+ * Drive the given distance straight (negative values go backwards).
+ * Uses a local PID controller to run a simple PID loop that is only
+ * enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
+ */
+class AutoIntakeOut: public frc2::CommandHelper<frc2::CommandBase, AutoIntakeOut> {
+
+    public:
+
+        AutoIntakeOut(Intake& intake);
+        void Initialize() override;
+        void Execute() override;
+        bool IsFinished() override;
+        void End(bool interrupted) override;
+
+    private:
+
+        Intake* m_intake;
+        int duration = 25;
+        int duration_counter;
+};

src/main/include/commands/Autonomous.h

@@ -8,6 +8,7 @@
 #include <frc2/command/SequentialCommandGroup.h>
 
 #include "subsystems/Drivetrain.h"
+#include "subsystems/Intake.h"
 
 /**
  * The main autonomous command to pickup and deliver the soda to the box.
@@ -17,10 +18,11 @@ class Autonomous: public frc2::CommandHelper<frc2::SequentialCommandGroup, Auton
 
     public:
 
-        Autonomous(Drivetrain& drivetrain);
+        Autonomous(Drivetrain& drivetrain, Intake& intake);
 
     private:
 
         Drivetrain* m_drivetrain;
+        Intake* m_intake;
 
 };

src/main/include/commands/IntakeIn.h

@@ -14,5 +14,5 @@ class IntakeIn : public frc2::CommandHelper<frc2::CommandBase, IntakeIn> {
 
     private:
         Intake* m_intake;
-        double speed = 0.25;
+        double speed = 0.4;
 };

src/main/include/commands/IntakeOut.h

@@ -14,5 +14,5 @@ class IntakeOut : public frc2::CommandHelper<frc2::CommandBase, IntakeOut> {
 
     private:
         Intake* m_intake;
-        double speed = 0.25;
+        double speed = 0.4;
 };

src/main/include/commands/IntakeYeet.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Intake.h"
+
+class IntakeYeet : public frc2::CommandHelper<frc2::CommandBase, IntakeYeet> {
+    public:
+        explicit IntakeYeet(Intake& intake);
+        void Initialize() override;
+        void Execute() override;
+        void End(bool interrupted) override;
+
+    private:
+        Intake* m_intake;
+        double speed = 1;
+};