From c9824f9fe87c03b58059fda56078e339dfaddddc Mon Sep 17 00:00:00 2001 From: William Date: Tue, 12 Apr 2022 12:52:02 -0400 Subject: [PATCH 1/8] Added poc for Ball2b --- src/main/cpp/commands/autonomous/Ball2b.cpp | 46 +++++++++++++++++++ src/main/include/commands/autonomous/Ball2b.h | 23 ++++++++++ 2 files changed, 69 insertions(+) create mode 100644 src/main/cpp/commands/autonomous/Ball2b.cpp create mode 100644 src/main/include/commands/autonomous/Ball2b.h diff --git a/src/main/cpp/commands/autonomous/Ball2b.cpp b/src/main/cpp/commands/autonomous/Ball2b.cpp new file mode 100644 index 0000000..6089aa4 --- /dev/null +++ b/src/main/cpp/commands/autonomous/Ball2b.cpp @@ -0,0 +1,46 @@ +#include "commands/autonomous/Ball2.h" + +#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" +#include "commands/common/GrabBall.h" +#include "commands/common/ScoreBall.h" + + +Ball2::Ball2( + Arm& arm, + Drivetrain& drivetrain, + Intake& intake): + m_arm(&arm), + m_drivetrain(&drivetrain), + m_intake(&intake){ + + SetName("Ball2"); + AddCommands( + // Remove ball + IntakeMove(intake, -1, 0.3), + // Taxi out + TankMoveGyro(drivetrain, -0.4, 0.6), + // Break tape + ArmLower(arm), + // Turn around to face the ball + TankTurn(drivetrain, 165), + // Get the ball + GrabBall(arm, drivetrain, intake), + // Turn back around to face hub + TankTurn(drivetrain, -165), + TankStop(drivetrain, 0.2), + // Move towards hub + TankMoveGyro(drivetrain, 1.8, 0.6), + // Score ball + ScoreBall(arm, drivetrain, intake) + ); + +} diff --git a/src/main/include/commands/autonomous/Ball2b.h b/src/main/include/commands/autonomous/Ball2b.h new file mode 100644 index 0000000..dc3bcbd --- /dev/null +++ b/src/main/include/commands/autonomous/Ball2b.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +#include "subsystems/Arm.h" +#include "subsystems/Drivetrain.h" +#include "subsystems/Intake.h" + + +class Ball2: public frc2::CommandHelper { + + public: + + Ball2(Arm& arm, Drivetrain& drivetrain, Intake& intake); + + private: + + Arm* m_arm; + Drivetrain* m_drivetrain; + Intake* m_intake; + +}; From de9608fc16d9bb0d7b98fe3b2da322a5fa4dd04d Mon Sep 17 00:00:00 2001 From: William Date: Wed, 13 Apr 2022 10:11:24 -0400 Subject: [PATCH 2/8] Fixed typos --- src/main/cpp/RobotContainer.cpp | 3 +++ src/main/cpp/commands/TankMoveGyro.cpp | 3 ++- src/main/cpp/commands/TankTurnPID.cpp | 4 ++-- src/main/cpp/commands/autonomous/Ball2b.cpp | 24 ++++++------------- src/main/include/RobotContainer.h | 2 ++ src/main/include/commands/autonomous/Ball2b.h | 4 ++-- src/main/include/subsystems/Drivetrain.h | 3 ++- 7 files changed, 20 insertions(+), 23 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index a4be547..9e2d825 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -15,11 +15,14 @@ // Testing #include "commands/TankTurn.h" +#include "commands/TankTurnTime.h" +#include "commands/TankTurnPID.h" RobotContainer::RobotContainer() { m_chooser.SetDefaultOption("Only Taxi", &m_only_taxi); m_chooser.AddOption("2 Ball", &m_ball2); + m_chooser.AddOption("2 Ball b", &m_ball2b); m_chooser.AddOption("3 Ball", &m_ball3); frc::SmartDashboard::PutData(&m_chooser); diff --git a/src/main/cpp/commands/TankMoveGyro.cpp b/src/main/cpp/commands/TankMoveGyro.cpp index bc60896..18811a7 100644 --- a/src/main/cpp/commands/TankMoveGyro.cpp +++ b/src/main/cpp/commands/TankMoveGyro.cpp @@ -25,6 +25,7 @@ void TankMoveGyro::Initialize() { m_drivetrain->m_leftLeadEncoder.SetPosition(0); m_drivetrain->m_rightLeadEncoder.SetPosition(0); m_drivetrain->m_navX.Reset(); + m_drivetrain->m_pid_move.SetSetpoint(0); distanceCounter = 0; m_drivetrain->Drive(0, 0); } @@ -33,7 +34,7 @@ void TankMoveGyro::Initialize() { // 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()); + double correction = m_drivetrain->m_pid_move.Calculate(m_drivetrain->m_navX.GetAngle()); if (m_distance >= 0 && distanceCounter < m_distance){ m_drivetrain->Drive(m_speed-correction, m_speed+correction); } diff --git a/src/main/cpp/commands/TankTurnPID.cpp b/src/main/cpp/commands/TankTurnPID.cpp index c774142..37e2bc8 100644 --- a/src/main/cpp/commands/TankTurnPID.cpp +++ b/src/main/cpp/commands/TankTurnPID.cpp @@ -16,7 +16,7 @@ TankTurnPID::TankTurnPID(Drivetrain& drivetrain, double angle): void TankTurnPID::Initialize() { m_drivetrain->m_navX.Reset();//SetAngleAdjustment(0); //m_drivetrain->m_turnPid.Reset(); - m_drivetrain->m_turnPid.SetSetpoint(m_angle); + m_drivetrain->m_pid_turn.SetSetpoint(m_angle); m_drivetrain->Drive(0, 0); } @@ -29,7 +29,7 @@ void TankTurnPID::Execute() { } else if(m_angle < 0 && angle > 90){ angle -= 360; } - double correction = m_drivetrain->m_turnPid.Calculate(angle); + double correction = m_drivetrain->m_pid_turn.Calculate(angle); if(correction > 0.4){ correction = 0.4; } else if(correction < -0.4){ diff --git a/src/main/cpp/commands/autonomous/Ball2b.cpp b/src/main/cpp/commands/autonomous/Ball2b.cpp index 6089aa4..ea96a85 100644 --- a/src/main/cpp/commands/autonomous/Ball2b.cpp +++ b/src/main/cpp/commands/autonomous/Ball2b.cpp @@ -1,4 +1,4 @@ -#include "commands/autonomous/Ball2.h" +#include "commands/autonomous/Ball2b.h" #include "commands/ArmMove.h" #include "commands/TankMove.h" @@ -14,7 +14,7 @@ #include "commands/common/ScoreBall.h" -Ball2::Ball2( +Ball2b::Ball2b( Arm& arm, Drivetrain& drivetrain, Intake& intake): @@ -22,23 +22,13 @@ Ball2::Ball2( m_drivetrain(&drivetrain), m_intake(&intake){ - SetName("Ball2"); + SetName("Ball2b"); AddCommands( - // Remove ball - IntakeMove(intake, -1, 0.3), - // Taxi out - TankMoveGyro(drivetrain, -0.4, 0.6), - // Break tape - ArmLower(arm), - // Turn around to face the ball - TankTurn(drivetrain, 165), - // Get the ball GrabBall(arm, drivetrain, intake), - // Turn back around to face hub - TankTurn(drivetrain, -165), - TankStop(drivetrain, 0.2), - // Move towards hub - TankMoveGyro(drivetrain, 1.8, 0.6), + TankMoveGyro(drivetrain, -0.9, 0.5), + TankStop(drivetrain, 0.3), + TankTurn(drivetrain, 158), + TankMoveGyro(drivetrain, 0.25, 0.45), // Score ball ScoreBall(arm, drivetrain, intake) ); diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 26f2de3..e6d5762 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -10,6 +10,7 @@ #include #include "commands/autonomous/Ball2.h" +#include "commands/autonomous/Ball2b.h" #include "commands/autonomous/Ball3.h" #include "commands/autonomous/OnlyTaxi.h" @@ -40,6 +41,7 @@ class RobotContainer { Arm m_arm; Ball2 m_ball2{m_arm, m_drivetrain, m_intake}; + Ball2 m_ball2b{m_arm, m_drivetrain, m_intake}; Ball3 m_ball3{m_arm, m_drivetrain, m_intake}; OnlyTaxi m_only_taxi{m_drivetrain, m_intake}; diff --git a/src/main/include/commands/autonomous/Ball2b.h b/src/main/include/commands/autonomous/Ball2b.h index dc3bcbd..9da10f0 100644 --- a/src/main/include/commands/autonomous/Ball2b.h +++ b/src/main/include/commands/autonomous/Ball2b.h @@ -8,11 +8,11 @@ #include "subsystems/Intake.h" -class Ball2: public frc2::CommandHelper { +class Ball2b: public frc2::CommandHelper { public: - Ball2(Arm& arm, Drivetrain& drivetrain, Intake& intake); + Ball2b(Arm& arm, Drivetrain& drivetrain, Intake& intake); private: diff --git a/src/main/include/subsystems/Drivetrain.h b/src/main/include/subsystems/Drivetrain.h index d21a9d3..a861e6c 100644 --- a/src/main/include/subsystems/Drivetrain.h +++ b/src/main/include/subsystems/Drivetrain.h @@ -47,6 +47,7 @@ class Drivetrain: public frc2::SubsystemBase { AHRS m_navX{frc::SPI::Port::kMXP}; rev::SparkMaxRelativeEncoder m_leftLeadEncoder = m_leftLeadMotor.GetEncoder(); rev::SparkMaxRelativeEncoder m_rightLeadEncoder = m_rightLeadMotor.GetEncoder(); - frc2::PIDController m_pid{0.005, 0, 0}; // PID Controller + frc2::PIDController m_pid_move{0.005, 0, 0}; // PID Controller + frc2::PIDController m_pid_turn{0.005, 0, 0}; }; From 586064ea9510e21f9d8a1e1ddc84a59cc8fa170a Mon Sep 17 00:00:00 2001 From: William Date: Thu, 14 Apr 2022 22:34:13 -0400 Subject: [PATCH 3/8] added brake/coast --- src/main/cpp/Robot.cpp | 6 ++++-- src/main/cpp/RobotContainer.cpp | 20 +++++++++++++------- src/main/cpp/commands/TankTurnPID.cpp | 4 ++-- src/main/cpp/commands/TankTurnTime.cpp | 4 ++-- src/main/cpp/commands/autonomous/Ball2.cpp | 9 ++++----- src/main/cpp/commands/autonomous/Ball2b.cpp | 4 ++-- src/main/cpp/commands/common/GrabBall.cpp | 8 ++++---- src/main/cpp/commands/common/ScoreBall.cpp | 4 +--- src/main/cpp/subsystems/Drivetrain.cpp | 11 +++++++++-- src/main/include/RobotContainer.h | 3 ++- src/main/include/subsystems/Drivetrain.h | 1 + 11 files changed, 44 insertions(+), 30 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index e8ccc60..b7ea060 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -12,8 +12,8 @@ #include void Robot::RobotInit() { - frc::CameraServer::GetInstance()->StartAutomaticCapture(0); - frc::CameraServer::GetInstance()->StartAutomaticCapture(1); + //frc::CameraServer::GetInstance()->StartAutomaticCapture(0); + //frc::CameraServer::GetInstance()->StartAutomaticCapture(1); frc::RobotController::SetBrownoutVoltage(6_V); } @@ -45,6 +45,7 @@ void Robot::DisabledPeriodic() {} */ void Robot::AutonomousInit() { + //m_container.m_drivetrain.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake); m_autonomousCommand = m_container.GetAutonomousCommand(); @@ -57,6 +58,7 @@ void Robot::AutonomousInit() { void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { + //m_container.m_drivetrain.SetIdleMode(rev::CANSparkMax::IdleMode::kCoast); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 9e2d825..3cb8568 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -17,6 +17,10 @@ #include "commands/TankTurn.h" #include "commands/TankTurnTime.h" #include "commands/TankTurnPID.h" +#include "commands/common/GrabBall.h" +#include "commands/common/ScoreBall.h" +#include "commands/common/ArmLower.h" +#include "commands/TankMoveGyro.h" RobotContainer::RobotContainer() { @@ -173,24 +177,26 @@ void RobotContainer::ConfigureButtonBindings() { IntakeDrive(m_intake, -1) ); - // Tank Turn Tuning frc2::JoystickButton(&m_stick2, 5).WhenPressed( - TankTurn(m_drivetrain, -45) + TankTurn(m_drivetrain, 180) ); frc2::JoystickButton(&m_stick2, 3).WhenPressed( - TankTurn(m_drivetrain, -135) + TankTurn(m_drivetrain, -180) ); frc2::JoystickButton(&m_stick2, 6).WhenPressed( - TankTurn(m_drivetrain, 45) + TankTurnPID(m_drivetrain, 180) ); frc2::JoystickButton(&m_stick2, 4).WhenPressed( - TankTurn(m_drivetrain, 135) + TankTurnPID(m_drivetrain, -180) + ); + frc2::JoystickButton(&m_stick3, 4).WhenPressed( + TankMoveGyro(m_drivetrain, 1, 0.5) ); frc2::JoystickButton(&m_stick3, 5).WhenPressed( - TankTurn(m_drivetrain, -180) + TankTurnTime(m_drivetrain, 180, 1.5) ); frc2::JoystickButton(&m_stick3, 6).WhenPressed( - TankTurn(m_drivetrain, 180) + TankTurnTime(m_drivetrain, -180, 1.5) ); } diff --git a/src/main/cpp/commands/TankTurnPID.cpp b/src/main/cpp/commands/TankTurnPID.cpp index 37e2bc8..dcd6b11 100644 --- a/src/main/cpp/commands/TankTurnPID.cpp +++ b/src/main/cpp/commands/TankTurnPID.cpp @@ -36,10 +36,10 @@ void TankTurnPID::Execute() { correction = -0.4; } if (m_angle >= 0){ - m_drivetrain->Drive(-0.4-correction, 0.4+correction); + m_drivetrain->Drive(-0.3-correction, 0.3+correction); } else { - m_drivetrain->Drive(0.4-correction, -0.4+correction); + m_drivetrain->Drive(0.3-correction, -0.3+correction); }; } diff --git a/src/main/cpp/commands/TankTurnTime.cpp b/src/main/cpp/commands/TankTurnTime.cpp index 62b683b..42626f6 100644 --- a/src/main/cpp/commands/TankTurnTime.cpp +++ b/src/main/cpp/commands/TankTurnTime.cpp @@ -32,10 +32,10 @@ void TankTurnTime::Execute() { } int time = (period_target - periods)/period_target; if (m_angle <= angle){ - m_drivetrain->Drive(-0.4-0.3*time, 0.4+0.3*time); + m_drivetrain->Drive(-0.3-0.3*time, 0.3+0.3*time); } else{ - m_drivetrain->Drive(0.4-0.3*time, -0.4+0.3*time); + m_drivetrain->Drive(0.3-0.3*time, -0.3+0.3*time); } periods++; } diff --git a/src/main/cpp/commands/autonomous/Ball2.cpp b/src/main/cpp/commands/autonomous/Ball2.cpp index 6089aa4..4ea5460 100644 --- a/src/main/cpp/commands/autonomous/Ball2.cpp +++ b/src/main/cpp/commands/autonomous/Ball2.cpp @@ -27,18 +27,17 @@ Ball2::Ball2( // Remove ball IntakeMove(intake, -1, 0.3), // Taxi out - TankMoveGyro(drivetrain, -0.4, 0.6), + TankMoveGyro(drivetrain, -1.4, 0.6), // Break tape ArmLower(arm), // Turn around to face the ball - TankTurn(drivetrain, 165), + TankTurn(drivetrain, 180), // Get the ball GrabBall(arm, drivetrain, intake), // Turn back around to face hub - TankTurn(drivetrain, -165), - TankStop(drivetrain, 0.2), + TankTurn(drivetrain, -180), // Move towards hub - TankMoveGyro(drivetrain, 1.8, 0.6), + TankMoveGyro(drivetrain, 1.9, 0.6), // Score ball ScoreBall(arm, drivetrain, intake) ); diff --git a/src/main/cpp/commands/autonomous/Ball2b.cpp b/src/main/cpp/commands/autonomous/Ball2b.cpp index ea96a85..623066b 100644 --- a/src/main/cpp/commands/autonomous/Ball2b.cpp +++ b/src/main/cpp/commands/autonomous/Ball2b.cpp @@ -24,9 +24,9 @@ Ball2b::Ball2b( SetName("Ball2b"); AddCommands( + TankMoveGyro(drivetrain, 0.6, 0.5), GrabBall(arm, drivetrain, intake), - TankMoveGyro(drivetrain, -0.9, 0.5), - TankStop(drivetrain, 0.3), + TankMoveGyro(drivetrain, -2.35, 0.5), TankTurn(drivetrain, 158), TankMoveGyro(drivetrain, 0.25, 0.45), // Score ball diff --git a/src/main/cpp/commands/common/GrabBall.cpp b/src/main/cpp/commands/common/GrabBall.cpp index ad5f3fe..3b4ee05 100644 --- a/src/main/cpp/commands/common/GrabBall.cpp +++ b/src/main/cpp/commands/common/GrabBall.cpp @@ -18,12 +18,12 @@ GrabBall::GrabBall( SetName("GrabBall"); AddCommands( - ArmMove(arm, -0.15, 0.6), + ArmMove(arm, -0.23, 0.6), // Move forward while intake in frc2::ParallelCommandGroup( - ArmMove(arm, -0.1, 1), - TankRawMove(drivetrain, 0.5 * 1.1, 0.5 * 1, 1), - IntakeMove(intake, 0.5, 1)) + ArmMove(arm, -0.1, 1.3), + TankMoveGyro(drivetrain, 0.5, 0.5), + IntakeMove(intake, 0.5, 1.3)) ); } diff --git a/src/main/cpp/commands/common/ScoreBall.cpp b/src/main/cpp/commands/common/ScoreBall.cpp index 83c514e..edb8ced 100644 --- a/src/main/cpp/commands/common/ScoreBall.cpp +++ b/src/main/cpp/commands/common/ScoreBall.cpp @@ -21,9 +21,8 @@ ScoreBall::ScoreBall( SetName("ScoreBall"); AddCommands( - TankStop(drivetrain, 0.1), // Raise arm - ArmMove(arm, 0.4, 1.5), + ArmMove(arm, 0.3, 0.7), // Move forward while keeping arm up frc2::ParallelRaceGroup( TankMove(drivetrain, 0.3, 0.4), @@ -33,7 +32,6 @@ ScoreBall::ScoreBall( IntakeMove(intake, -1, 0.3), ArmMove(arm, 0.1, 0.3)), // Move back - TankStop(drivetrain, 0.4), TankMove(drivetrain, -2, 0.7), ArmLower(arm) ); diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp index 2367619..62277ca 100644 --- a/src/main/cpp/subsystems/Drivetrain.cpp +++ b/src/main/cpp/subsystems/Drivetrain.cpp @@ -14,8 +14,8 @@ Drivetrain::Drivetrain() { m_rightFollowMotor.Follow(m_rightLeadMotor); //Conversion factor from rotations to meters - m_leftLeadEncoder.SetPositionConversionFactor(0.0442); - m_rightLeadEncoder.SetPositionConversionFactor(0.0442); + m_leftLeadEncoder.SetPositionConversionFactor(0.0443); + m_rightLeadEncoder.SetPositionConversionFactor(0.0443); SetName("Drivetrain"); @@ -48,6 +48,13 @@ void Drivetrain::Log() { frc::SmartDashboard::PutNumber("Gyro", m_navX.GetYaw()); } +void Drivetrain::SetIdleMode(rev::CANSparkMax::IdleMode mode){ + m_leftLeadMotor.SetIdleMode(mode); + m_rightLeadMotor.SetIdleMode(mode); + m_leftFollowMotor.SetIdleMode(mode); + m_rightFollowMotor.SetIdleMode(mode); +} + void Drivetrain::Periodic() { // Implementation of subsystem periodic method goes here. Log(); diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index e6d5762..3361675 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -30,13 +30,14 @@ class RobotContainer { public: RobotContainer(); frc2::Command* GetAutonomousCommand(); + Drivetrain m_drivetrain; private: frc::Joystick m_stick1{0}; frc::Joystick m_stick2{1}; frc::Joystick m_stick3{2}; - Drivetrain m_drivetrain; + Intake m_intake; Arm m_arm; diff --git a/src/main/include/subsystems/Drivetrain.h b/src/main/include/subsystems/Drivetrain.h index a861e6c..9253a76 100644 --- a/src/main/include/subsystems/Drivetrain.h +++ b/src/main/include/subsystems/Drivetrain.h @@ -30,6 +30,7 @@ class Drivetrain: public frc2::SubsystemBase { void Drive(double left, double right); void Log(); void Periodic() override; + void SetIdleMode(rev::CANSparkMax::IdleMode mode); private: From 2101ba30283ce2f9f0d15637a705ca58ab8ea476 Mon Sep 17 00:00:00 2001 From: Steven Wu <93849557+wusteven815@users.noreply.github.com> Date: Fri, 15 Apr 2022 12:41:01 -0400 Subject: [PATCH 4/8] remove turn test bindings --- src/main/cpp/RobotContainer.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 3cb8568..2bfb5eb 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -15,8 +15,6 @@ // Testing #include "commands/TankTurn.h" -#include "commands/TankTurnTime.h" -#include "commands/TankTurnPID.h" #include "commands/common/GrabBall.h" #include "commands/common/ScoreBall.h" #include "commands/common/ArmLower.h" @@ -183,21 +181,6 @@ void RobotContainer::ConfigureButtonBindings() { frc2::JoystickButton(&m_stick2, 3).WhenPressed( TankTurn(m_drivetrain, -180) ); - frc2::JoystickButton(&m_stick2, 6).WhenPressed( - TankTurnPID(m_drivetrain, 180) - ); - frc2::JoystickButton(&m_stick2, 4).WhenPressed( - TankTurnPID(m_drivetrain, -180) - ); - frc2::JoystickButton(&m_stick3, 4).WhenPressed( - TankMoveGyro(m_drivetrain, 1, 0.5) - ); - frc2::JoystickButton(&m_stick3, 5).WhenPressed( - TankTurnTime(m_drivetrain, 180, 1.5) - ); - frc2::JoystickButton(&m_stick3, 6).WhenPressed( - TankTurnTime(m_drivetrain, -180, 1.5) - ); } From e31c3baf79a1b714d1394f8bdfdc81b58183a470 Mon Sep 17 00:00:00 2001 From: Steven Wu <93849557+wusteven815@users.noreply.github.com> Date: Fri, 15 Apr 2022 12:43:36 -0400 Subject: [PATCH 5/8] fix wreorder --- src/main/cpp/commands/TankMoveGyro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cpp/commands/TankMoveGyro.cpp b/src/main/cpp/commands/TankMoveGyro.cpp index 18811a7..79e3870 100644 --- a/src/main/cpp/commands/TankMoveGyro.cpp +++ b/src/main/cpp/commands/TankMoveGyro.cpp @@ -12,8 +12,8 @@ conversion factor around 2.3 TankMoveGyro::TankMoveGyro(Drivetrain& drivetrain, double distance, double speed): - m_distance(distance), m_drivetrain(&drivetrain), + m_distance(distance), m_speed(speed) { SetName("TankMove"); From abef64f1231edc11032c1bcec462a6f5a231a348 Mon Sep 17 00:00:00 2001 From: Steven Wu <93849557+wusteven815@users.noreply.github.com> Date: Fri, 15 Apr 2022 13:54:34 -0400 Subject: [PATCH 6/8] remove all test bindings --- src/main/cpp/RobotContainer.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 2bfb5eb..3911399 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -175,13 +175,6 @@ void RobotContainer::ConfigureButtonBindings() { IntakeDrive(m_intake, -1) ); - frc2::JoystickButton(&m_stick2, 5).WhenPressed( - TankTurn(m_drivetrain, 180) - ); - frc2::JoystickButton(&m_stick2, 3).WhenPressed( - TankTurn(m_drivetrain, -180) - ); - } frc2::Command* RobotContainer::GetAutonomousCommand() { From cdd317dfcc12adfec973b50e6cad3586ad29ee55 Mon Sep 17 00:00:00 2001 From: William Date: Fri, 15 Apr 2022 14:16:13 -0400 Subject: [PATCH 7/8] cmasll changes --- src/main/cpp/commands/autonomous/Ball2.cpp | 6 +++--- src/main/cpp/commands/autonomous/OnlyTaxi.cpp | 2 +- src/main/cpp/commands/common/GrabBall.cpp | 4 ++-- src/main/cpp/commands/common/ScoreBall.cpp | 4 ++-- src/main/include/subsystems/Drivetrain.h | 2 +- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/cpp/commands/autonomous/Ball2.cpp b/src/main/cpp/commands/autonomous/Ball2.cpp index 4ea5460..ebf8057 100644 --- a/src/main/cpp/commands/autonomous/Ball2.cpp +++ b/src/main/cpp/commands/autonomous/Ball2.cpp @@ -27,7 +27,7 @@ Ball2::Ball2( // Remove ball IntakeMove(intake, -1, 0.3), // Taxi out - TankMoveGyro(drivetrain, -1.4, 0.6), + TankMoveGyro(drivetrain, -1.4, 0.63), // Break tape ArmLower(arm), // Turn around to face the ball @@ -35,9 +35,9 @@ Ball2::Ball2( // Get the ball GrabBall(arm, drivetrain, intake), // Turn back around to face hub - TankTurn(drivetrain, -180), + TankTurn(drivetrain, 180), // Move towards hub - TankMoveGyro(drivetrain, 1.9, 0.6), + TankMoveGyro(drivetrain, 1.9, 0.63), // Score ball ScoreBall(arm, drivetrain, intake) ); diff --git a/src/main/cpp/commands/autonomous/OnlyTaxi.cpp b/src/main/cpp/commands/autonomous/OnlyTaxi.cpp index e0a1986..88bfd5e 100644 --- a/src/main/cpp/commands/autonomous/OnlyTaxi.cpp +++ b/src/main/cpp/commands/autonomous/OnlyTaxi.cpp @@ -15,7 +15,7 @@ OnlyTaxi::OnlyTaxi( // Remove ball IntakeMove(intake, -1, 0.3), // Taxi out - TankMove(drivetrain, -2, 0.5) + TankMove(drivetrain, -3, 0.5) ); } diff --git a/src/main/cpp/commands/common/GrabBall.cpp b/src/main/cpp/commands/common/GrabBall.cpp index 3b4ee05..cebf078 100644 --- a/src/main/cpp/commands/common/GrabBall.cpp +++ b/src/main/cpp/commands/common/GrabBall.cpp @@ -22,8 +22,8 @@ GrabBall::GrabBall( // Move forward while intake in frc2::ParallelCommandGroup( ArmMove(arm, -0.1, 1.3), - TankMoveGyro(drivetrain, 0.5, 0.5), - IntakeMove(intake, 0.5, 1.3)) + TankMoveGyro(drivetrain, 0.5, 0.7), + IntakeMove(intake, 0.8, 1.3)) ); } diff --git a/src/main/cpp/commands/common/ScoreBall.cpp b/src/main/cpp/commands/common/ScoreBall.cpp index edb8ced..221f5f0 100644 --- a/src/main/cpp/commands/common/ScoreBall.cpp +++ b/src/main/cpp/commands/common/ScoreBall.cpp @@ -25,14 +25,14 @@ ScoreBall::ScoreBall( ArmMove(arm, 0.3, 0.7), // Move forward while keeping arm up frc2::ParallelRaceGroup( - TankMove(drivetrain, 0.3, 0.4), + TankMove(drivetrain, 0.4, 0.4), ArmMove(arm, 0.1, 5)), // Dump ball while keeping arm up frc2::ParallelCommandGroup( IntakeMove(intake, -1, 0.3), ArmMove(arm, 0.1, 0.3)), // Move back - TankMove(drivetrain, -2, 0.7), + TankMove(drivetrain, -3, 0.65), ArmLower(arm) ); diff --git a/src/main/include/subsystems/Drivetrain.h b/src/main/include/subsystems/Drivetrain.h index 9253a76..04c6ad7 100644 --- a/src/main/include/subsystems/Drivetrain.h +++ b/src/main/include/subsystems/Drivetrain.h @@ -49,6 +49,6 @@ class Drivetrain: public frc2::SubsystemBase { rev::SparkMaxRelativeEncoder m_leftLeadEncoder = m_leftLeadMotor.GetEncoder(); rev::SparkMaxRelativeEncoder m_rightLeadEncoder = m_rightLeadMotor.GetEncoder(); frc2::PIDController m_pid_move{0.005, 0, 0}; // PID Controller - frc2::PIDController m_pid_turn{0.005, 0, 0}; + frc2::PIDController m_pid_turn{0.001, 0, 0}; }; From 7eddd2052c3d4a3d586a2fc3ca00406d5ddfc9af Mon Sep 17 00:00:00 2001 From: William Date: Fri, 15 Apr 2022 16:18:11 -0400 Subject: [PATCH 8/8] changed ball 2 move gyro --- src/main/cpp/RobotContainer.cpp | 49 +--------------------- src/main/cpp/commands/autonomous/Ball2.cpp | 2 +- 2 files changed, 2 insertions(+), 49 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 3911399..755e2dc 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -95,54 +95,7 @@ RobotContainer::RobotContainer() { ) ); -/* Me reading the above code: -&&&&&&&&&&&&&##BGPPPGBB#BBGBGJ???J5BB#BBB###BBPYY5Y5B#&&&&GY5PGGGYJYGGPG&&&&&&#BJ:.:?B##BGGGGPY!JGPP -&&###BBBGG##&&##BGGG##BGGBBBG?7???5#&#BBGG###BPYYYP#&##&&&BY5GGGPYJ5GBPB&&#BGPP57. .7GBBGPPPPPY!JPPG -&&#BGPPPGB####&&######BBBBBBG?7?7?5#&#GB#BG##G5GGG&&#GP!5&BYPGBGPYYGBP?7!~~!~77?77!~~^~~~!????7~!??? -#&&&#BGB#&&&&#&&&##B########G?!7775##BB##GB##GB&&&&&#!. J@BYPBBGPY5J~^!JP#&@@@@@@@@@@#B5?^:~J5J!?5PP -##&&###&&&&&&&&#BGGB#######BP?!77?5BBGGGBB#GG#&#&&&&B: ?&BYGBBG5!.^5&@@@@@@@@@@@@@@@@@@@@B7.~J7YBBB -#BBBBPG#######BGPGB###BBBGGGBB#BB##########B&&&#&&&&#J. ^B&GGB#5::P@@@@@@@BY7!!!~^!?YB@@@@@@#! ^YB#B -G###GG#&&###BGGGPG#BBBB###&&&&&BB&&&&&&##&&&&&&&&&&&&#J.~##PB#P.~&@@@@@@G~:?5G#B?. .::~G@@@@@@J ?### -!.!?JJ5#&&&&&&&######&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&B#PJ#G5B#^.&@@@@@@5 7#@&&&#Y:.:75! P@@@@@@^.B## -#~ :~?P#&&&&#&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&#&&&&&&&&#BGG5JBB::!??J5P5 7&&&##:.^7GP J@@@@@@? GBB -&G: 7YYP#&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&###&#BBGP##B5Y55YJ7~!#&&&&#^.^?J.~#@@@@@@~.PBB -&&P: ^5BBB#&&&&&&&&&&&&&&&&&&&&&&&&&&##&&&&&&&&&&&&&##&&BPG#&BGB&@@&#GB&&&#BBBGY^.:.:J&@@@@@@J !GGG -#&&G! .P#BB###&&##&&&&&&&&&&&&&&#B&&&&&##&&&&&&&&&&&&&B#&&#G#&BGB&&#BG5GBBGPYJJJ!. :J#@@@@@@#! ^7??? -##&&#J.Y&#BBBGB&##&&&&&#B#&&&&&&&G#&&&###&&&&&#&&##&&&#B#&&#BBBGB&&&&#GB&&#BBBP!.!5&@@@@@@#?.~J!?YYY -&&#BB#PYG###BBG#B#&&&&&&B#&&&&&&&GB&###&##&&&&&#B#B#&&&#B&###GGG#&&&&#GB&&&#B7:7B@@@@@@@B7:~YGY!?555 -&&&#PG##PJYGB##BG#&&&&&###&&&&&&&B5##B#&&#&&&&&&GBGGB###B##B##PG&&&&&#GB&&&G::G@@@@@@&5~:75GBBY!JPGG -&&&#G5P#&#PJ7YGBGGB########&&&&###5PGGG&&&&&&&&&GB#BB#&&&G##B#BB#&&#&#GB&&#^:#@@@@@B7:!YB#GPBGY!JPPP -&&&#GPG#&&&&#GGBGGG####&&&GB&####BGGGGPB#&&&&&&#GBB#BB&&&GP##B#BB&#&&#GB&@Y 5@@@@@G :P&&#BGPGGY!JPPP -&&&#PGBGBB#&&&&&B#B####&&&BP#&&&&GGB#BGPG###&&&BGBB#GG#&##GG&BBGG###&#PB&&? &@@@@@J !B&&&&#BBGY!JPPP -&&GBB#BBB#&&&######&&&&&PPB&&&PPBBGPPGB#####G###57!?YJ5PPGGGBPB###BPG#&J.777777^ 7B@@@&&&&#Y!JPGG -&&GGG&##BBBBBBB&G#&&&&&&BGBBGBY7!^::^YB##BBBB###P7!^~7Y?PBGGBBGB#BGPPBBPJ????!. .!5GGGGGGGPJ!?5PP -&&&#PPGB&BBB#&##BG&5B&&&&&&#GBP!Y5?7!!J7J####BB#BB##5Y5YJ#PPB#BG#BBBGG55GG7 !777!!!^ 7JJJJ??77!^!777 -&&&#PGGB&BBB#&&#&B&55&&&&&&&BGB#B#5Y5P5YG############BGPB#PPBG#BBGG&&#GG#&? &@@@@@@J PBBBBBGGPJ!JPPP -&&&#GGGB###B##&&&##GPB#&&&&&&GG#&##BGGBB##############BBBBG5BB#BGGG###GG#&? &@@@@@@J P#BBGGGBGY!JGGG -&&&#BGGB5#####&BBPB##&&&&&BGB##BBBBBBB#################BPG#&GBGGB&#GB#&? B#####&? 5GGGPPGBBY!YB## -&&&##BGBP#&####&PJGP5P###&@&&&GG###################&#######P5BB##BBB&#GB##Y~!!!!~:..:5BBBBBGGGY!Y#&& -&&&#&#GGB#&#BB#GJY5P55G##&@&&&#P#############GPPPP5PB######P5GGBGGBG##GB####BBBGY^.:7PBBB###&#Y!JB## -&&#&##GGBB&#BBBYY55PP55###&&&#&B###########&B5PPGGP5G#####G55GBBBBGG&#GB&&#GGGGPY^.:!5GBBBB###Y7JB#& -####&#PBBB&#BBPY5P55PG5B&#&&&BB&B#######################BGPP5PB##GPGGGPPBBPYJJJJ7:.:~????JJJJJ7~7Y55 -######5BBB&##GY55PP5PGGPB&&&&#P#G5B###&&##############BPPGP555#BBGBGBBGB##BGGGBGY:.:!YPPPPPP55?~7Y5Y -PPPGGPYBBG&&&P555PPP5PPPPB&&&&GPB?JY55PGBB#####&####BP55GPP5557GG##B#&&###&##5^.:!5###&&&!Y&&& -YYYJJY?YYG&&G5555PPPYYPP5P#&&BYJYYYYYYYYY555PPP555PPPGP555P&&GPGB##B#&&&###^.:!P##&&&&!Y#&& -YYJJ?J??JG&BP555Y5PPY?5PPYP&&&&PPGJYYYYYYYYYYYYJ55Y5PP5PG555PP&#BBBB&#B#&&&&##&&5^.:7G&&&&&&&#Y~Y#&& -JJJJ?J5GBB#5GP5P5Y55YYJPP5JP&&&BYGJ?YYYYYYYYYY77PP55PP5PP55YY5&#G#GG&#B#&&&###&BJ:.^?P&&&&&&&#Y~Y#&& -YYYYY5BB#BBPBG55PY5B5?7?Y5YJP&&B55? :~7JYYYYJ^ .55Y5P5YJ?!~^.!&#G##B&#B#&&&GYYJ!:..:^7YPBB####Y~J##& -JJJYG####GBGBBP55G#&5 .:^~75&B7:!. .755Y: :..:~~^. 7&YP##GBBGGBBBJ^:......:^~7?Y5YYJ7~7Y5P -7?YG&&&&&BB#GBG5P#&&&Y 7#7 . !Y~ JJ^B#BBBPPPBBGP5Y!^~!~:.:?5PGBBGGP5PP55 -J5B&&&&&&#P#BGGBB#&&&&Y. ~! . : 7 7##B#&BBBBBBG5J77JYY?!!7??5BBGGBB##&& -#&&&&&&&&&BPBBB##&&&&&@5. . ^JPY?~. .GBB#&&&###GY?7777??YYYJJJYYPPPGBBB#&& -&&&&&&&&&&BG##########&&P. .~JPGBBBG5J?!^. Y#GB##&#BGGYY7~!!!!!77????JJJJYYYY5PGB -########&&##GB#&&&&#####&G: :5#BBBBBBBBB##G: ~&#B####BGB#55Y???7777777???77!!!!!!!!! -############PB#BB##&&&&&&@B^ ^5#&#########7 !#&&@&BBB####GPBG55PGGPP55YYJJJ??777!!!! -############PB###BBB##&&&&&#! ?B#######&P 7&@&BP5G######BPBB?:!5BBBBBBBBBBGGGPPP55Y -############P#######BBBB##&@&Y. ~G###BB#&? J#G5YJ5B########GP5^ ~PBBBBBBBBBBBBBBBBBB -############G#####B###BGGPPPGBP?!^:. :YPJ~:~?. .:~JPJ!7YG##########BY!.^:^5BBBBBBBBBBBBBBBBBB -######BBBBBBP#####BBB###BGGP?7?5PPP5YJ7~: .:!?JYYYY5J?YB#B#BB######Y .7GBBBBBBBBBBBBBBBBBB -BBBBGGGGBB#BG######BGGGBB###PJJYY55J?JJYYY?!:^7J?7JYJ??JY555GGYP#BBGB######G: .JBBBBBBBBBBBBBBBBBBB -PPPGGGGGBGJ!Y#######BG5P###BGYGBGP555YJ7777!!J????7!?Y5GBBB&&&G5###BPG#####B?.~JBBBBBBBBBBBBBBBBBBBB +/* no u */ m_arm.SetDefaultCommand( diff --git a/src/main/cpp/commands/autonomous/Ball2.cpp b/src/main/cpp/commands/autonomous/Ball2.cpp index ebf8057..90fb048 100644 --- a/src/main/cpp/commands/autonomous/Ball2.cpp +++ b/src/main/cpp/commands/autonomous/Ball2.cpp @@ -37,7 +37,7 @@ Ball2::Ball2( // Turn back around to face hub TankTurn(drivetrain, 180), // Move towards hub - TankMoveGyro(drivetrain, 1.9, 0.63), + TankMove(drivetrain, 1.9, 0.63), // Score ball ScoreBall(arm, drivetrain, intake) );