Merge pull request #9 from Welpyfish/auto-dev

Ball 2

src/main/cpp/RobotContainer.cpp

@@ -13,9 +13,6 @@
 #include "commands/ArmDrive.h"
 #include "commands/IntakeDrive.h"
 
-// Testing temp
-#include "commands/common/ScoreBall.h"
-
 RobotContainer::RobotContainer() {
 
     m_chooser.SetDefaultOption("Only Taxi", &m_only_taxi);
@@ -169,9 +166,6 @@ void RobotContainer::ConfigureButtonBindings() {
     frc2::JoystickButton(&m_stick3,4).WhenHeld(
         IntakeDrive(m_intake, -1)
     );
-    frc2::JoystickButton(&m_stick3, 1).WhenPressed(
-        ScoreBall(m_arm, m_drivetrain, m_intake)
-    );
 
 }
 

src/main/cpp/commands/TankMove.cpp

@@ -7,9 +7,10 @@
 #include "commands/TankMove.h"
 
 
-TankMove::TankMove(Drivetrain& drivetrain, double distance):
+TankMove::TankMove(Drivetrain& drivetrain, double distance, double speed):
     m_drivetrain(&drivetrain),
-    m_distance(distance){
+    m_distance(distance),
+    m_speed(speed){
 
     SetName("TankMove");
     AddRequirements({m_drivetrain});
@@ -28,10 +29,10 @@ void TankMove::Initialize() {
 void TankMove::Execute() {
     distanceCounter = (-m_drivetrain->m_leftLeadEncoder.GetPosition() + m_drivetrain->m_rightLeadEncoder.GetPosition())/2;
     if (m_distance >= 0 && distanceCounter < m_distance){
-        m_drivetrain->Drive(0.4, 0.4);
+        m_drivetrain->Drive(m_speed, m_speed);
     }
     else if(m_distance < 0 && distanceCounter > m_distance){
-        m_drivetrain->Drive(-0.4, -0.4);
+        m_drivetrain->Drive(-m_speed, -m_speed);
     }
 }
 

src/main/cpp/commands/TankMoveGyro.cpp

@@ -0,0 +1,60 @@
+
+// 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.
+
+/*
+test 1: moved about 0.2 speed 212 cm 5 rotations -> 42cm per rotation
+conversion factor around 2.3
+*/
+
+#include "commands/TankMoveGyro.h"
+
+
+TankMoveGyro::TankMoveGyro(Drivetrain& drivetrain, double distance, double speed):
+    m_distance(distance),
+    m_drivetrain(&drivetrain),
+    m_speed(speed) {
+
+    SetName("TankMove");
+    AddRequirements({m_drivetrain});
+}
+
+// Called just before this Command runs the first time
+void TankMoveGyro::Initialize() {
+    m_drivetrain->m_leftLeadEncoder.SetPosition(0);
+    m_drivetrain->m_rightLeadEncoder.SetPosition(0);
+    m_drivetrain->m_navX.Reset();
+    distanceCounter = 0;
+    m_drivetrain->Drive(0, 0);
+}
+
+
+// Called repeatedly when this Command is scheduled to run
+void TankMoveGyro::Execute() {
+    distanceCounter = (-m_drivetrain->m_leftLeadEncoder.GetPosition() + m_drivetrain->m_rightLeadEncoder.GetPosition())/2;
+    double correction = m_drivetrain->m_pid.Calculate(m_drivetrain->m_navX.GetAngle());
+    if (m_distance >= 0 && distanceCounter < m_distance){
+        m_drivetrain->Drive(m_speed-correction, m_speed+correction);
+    }
+    else if(m_distance < 0 && distanceCounter > m_distance){
+        m_drivetrain->Drive(-m_speed-correction, -m_speed+correction);
+    }
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool TankMoveGyro::IsFinished() {
+    distanceCounter = (-m_drivetrain->m_leftLeadEncoder.GetPosition() + m_drivetrain->m_rightLeadEncoder.GetPosition())/2;
+    if (m_distance >= 0){
+        return(distanceCounter >= m_distance);
+    }
+    else{
+        return(distanceCounter <= m_distance);
+    }
+}
+
+// Called once after isFinished returns true
+void TankMoveGyro::End(bool) {
+    m_drivetrain->Drive(0, 0);
+}
+

src/main/cpp/commands/TankStop.cpp

@@ -0,0 +1,39 @@
+#include "commands/TankStop.h"
+
+TankStop::TankStop(
+        Drivetrain& drivetrain, double duration):
+            m_drivetrain(&drivetrain),
+            m_duration(duration){
+
+    SetName("TankStop");
+    AddRequirements({m_drivetrain});
+
+}
+
+void TankStop::Initialize(){
+
+    periods = 0;
+    period_target = m_duration * 50;
+
+    m_drivetrain->Drive(0, 0);
+
+}
+
+void TankStop::Execute(){
+
+    m_drivetrain->Drive(0, 0);
+    periods++;
+
+}
+
+bool TankStop::IsFinished(){
+
+    return (periods >= period_target);
+
+}
+
+void TankStop::End(bool){
+
+    m_drivetrain->Drive(0, 0);
+
+}

src/main/cpp/commands/TankTurn.cpp

@@ -23,10 +23,10 @@ void TankTurn::Initialize() {
 void TankTurn::Execute() {
     double angle = m_drivetrain->m_navX.GetAngle();
     if (m_angle >= 0 && angle < m_angle){
-        m_drivetrain->Drive(0.5, -0.5);
+        m_drivetrain->Drive(-0.5, 0.5);
     }
     else if(m_angle < 0 && angle > m_angle){
-        m_drivetrain->Drive(-0.5, 0.5);
+        m_drivetrain->Drive(0.5, -0.5);
     };
 }
 

src/main/cpp/commands/autonomous/Ball2.cpp

@@ -2,9 +2,11 @@
 
 #include "commands/ArmMove.h"
 #include "commands/TankMove.h"
+#include "commands/TankMoveGyro.h"
 #include "commands/TankTurn.h"
 #include "commands/IntakeMove.h"
 
+#include "commands/TankStop.h"
 #include "commands/TankRawMove.h"
 
 #include "commands/common/ArmLower.h"
@@ -23,21 +25,20 @@ Ball2::Ball2(
     SetName("Ball2");
     AddCommands(
         // Remove ball
-        IntakeMove(intake, 1, 0.2),
+        IntakeMove(intake, -1, 0.3),
         // Taxi out
-        TankMove(drivetrain, -1.4),
+        TankMoveGyro(drivetrain, -1.3, 0.6),
         // Break tape
-        //ArmLower(arm),
+        ArmLower(arm),
         // Turn around to face the ball
-        TankRawMove(drivetrain, 0.5, -0.5, 1),
-        TankRawMove(drivetrain, 0.5, -0.5, 1),
+        TankTurn(drivetrain, 165),
         // Get the ball
         GrabBall(drivetrain, intake),
         // Turn back around to face hub
-        TankTurn(drivetrain, 90),
-        TankTurn(drivetrain, 90),
+        TankTurn(drivetrain, -165),
+        TankStop(drivetrain, 0.2),
         // Move towards hub
-        TankMove(drivetrain, 1.9),
+        TankMoveGyro(drivetrain, 1.8, 0.6),
         // Score ball
         ScoreBall(arm, drivetrain, intake)
     );

src/main/cpp/commands/autonomous/Ball3.cpp

@@ -21,29 +21,29 @@ Ball3::Ball3(
     SetName("Ball3");
     AddCommands(
         // Remove ball
-        IntakeMove(intake, 1, 0.2),
+        IntakeMove(intake, -1, 0.2),
         // Taxi out
-        TankMove(drivetrain, -0.5),
+        TankMove(drivetrain, -0.5, 0.4),
         // Break tape
         //ArmLower(arm),
         // Turn to face ball 1
-        TankTurn(drivetrain, -133),
+        TankTurn(drivetrain, -120),
         // Move to ball 1
-        TankMove(drivetrain, 2),
+        TankMove(drivetrain, 1.10, 0.4),
         // Get ball 1
         GrabBall(drivetrain, intake),
         // Turn to face ball 2
-        TankTurn(drivetrain, -125),
+        TankTurn(drivetrain, -115),
         // Move to ball 2
-        TankMove(drivetrain, 2),
+        TankMove(drivetrain, 1.4, 0.4),
         // Get ball 2
         GrabBall(drivetrain, intake),
         // Turn to hub
-        TankTurn(drivetrain, -112),
+        TankTurn(drivetrain, -100),
         // Move to hub
-        TankMove(drivetrain, 3),
+        TankMove(drivetrain, 2, 0.4),
         // Realign perpendicular to hub
-        TankTurn(drivetrain, -10),
+        //TankTurn(drivetrain, -10),
         // Score ball
         ScoreBall(arm, drivetrain, intake)
     );

src/main/cpp/commands/autonomous/OnlyTaxi.cpp

@@ -13,9 +13,9 @@ OnlyTaxi::OnlyTaxi(
     SetName("OnlyTaxi");
     AddCommands(
         // Remove ball
-        IntakeMove(intake, 1, 0.2),
+        IntakeMove(intake, -1, 0.3),
         // Taxi out
-        TankMove(drivetrain, -2)
+        TankMove(drivetrain, -2, 0.5)
     );
 
 }

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.3),
+        ArmMove(arm, -0.3, 0.5),
         // Stop, let it drop by gravity
         ArmMove(arm, 0, 0.2)
     );

src/main/cpp/commands/common/GrabBall.cpp

@@ -16,8 +16,8 @@ GrabBall::GrabBall(
     AddCommands(
         // Move forward while intake in
         frc2::ParallelCommandGroup(
-            TankMove(drivetrain, 0.5),
-            IntakeMove(intake, -0.6, 1))
+            TankMove(drivetrain, 0.5, 0.6),
+            IntakeMove(intake, 0.5, 0.8))
     );
 
 }

src/main/cpp/commands/common/ScoreBall.cpp

@@ -6,6 +6,7 @@
 #include "commands/ArmMove.h"
 #include "commands/IntakeMove.h"
 #include "commands/TankMove.h"
+#include "commands/TankStop.h"
 
 #include "commands/common/ArmLower.h"
 
@@ -20,18 +21,20 @@ ScoreBall::ScoreBall(
 
     SetName("ScoreBall");
     AddCommands(
+        TankStop(drivetrain, 0.1),
         // Raise arm
-        ArmMove(arm, 0.2, 3),
+        ArmMove(arm, 0.4, 1.5),
         // Move forward while keeping arm up
         frc2::ParallelRaceGroup(
-            TankMove(drivetrain, 0.3),
+            TankMove(drivetrain, 0.3, 0.4),
             ArmMove(arm, 0.1, 5)),
         // Dump ball while keeping arm up
         frc2::ParallelCommandGroup(
-            IntakeMove(intake, 1, 0.2),
-            ArmMove(arm, 0.1, 0.4)),
+            IntakeMove(intake, -1, 0.3),
+            ArmMove(arm, 0.1, 0.3)),
         // Move back
-        TankMove(drivetrain, -2.5),
+        TankStop(drivetrain, 0.4),
+        TankMove(drivetrain, -2, 0.7),
         ArmLower(arm)
     );
 

src/main/include/commands/TankMove.h

@@ -15,7 +15,7 @@ class TankMove: public frc2::CommandHelper<frc2::CommandBase, TankMove> {
 
     public:
 
-        TankMove(Drivetrain& drivetrain, double distance);
+        TankMove(Drivetrain& drivetrain, double distance, double speed);
         void Initialize() override;
         void Execute() override;
         bool IsFinished() override;
@@ -25,5 +25,6 @@ class TankMove: public frc2::CommandHelper<frc2::CommandBase, TankMove> {
 
         Drivetrain* m_drivetrain;
         double m_distance;
+        double m_speed;
         double distanceCounter;
 };

src/main/include/commands/TankMoveGyro.h

@@ -0,0 +1,30 @@
+#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 TankMoveGyro: public frc2::CommandHelper<frc2::CommandBase, TankMoveGyro> {
+
+    public:
+
+        TankMoveGyro(Drivetrain& drivetrain, double distance, double speed);
+        void Initialize() override;
+        void Execute() override;
+        bool IsFinished() override;
+        void End(bool interrupted) override;
+
+    private:
+
+        Drivetrain* m_drivetrain;
+        double m_distance;
+        double m_speed;
+        double distanceCounter;
+};

src/main/include/commands/TankStop.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Drivetrain.h"
+
+
+class TankStop: public frc2::CommandHelper<frc2::CommandBase, TankStop> {
+
+    public:
+
+        TankStop(Drivetrain& drivetrain, double duration);
+
+        void Initialize() override;
+        void Execute() override;
+        bool IsFinished() override;
+        void End(bool interrupted) override;
+
+    private:
+
+        Drivetrain* m_drivetrain;
+        double m_duration;
+
+        int period_target;
+        int periods;
+
+};