fixed lift and auto tuning

Valor6800 · Apr 16, 2022 · f601b36 · f601b36
1 parent 8535de8
commit f601b36
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 19 deletions.
diff --git a/Competition/src/main/cpp/Robot.cpp b/Competition/src/main/cpp/Robot.cpp
@@ -37,11 +37,11 @@ void Robot::DisabledInit() {
     m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::DISABLED;
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::DISABLED;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::DISABLED;
-    //m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested
+    m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested
 
     m_container.m_shooter.resetState();
     m_container.m_drivetrain.resetState();
-    // m_container.m_lift.resetState();
+    m_container.m_lift.resetState();
     m_container.m_turretTracker.resetState();
     //m_container.m_feeder.resetState(); //just added, not tested
 
@@ -70,7 +70,7 @@ void Robot::AutonomousInit() {
     m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::AUTO;
     m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::AUTO;
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO;
-    // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO;
+    m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO;
 
     m_container.m_drivetrain.pullSwerveModuleZeroReference();
@@ -95,7 +95,7 @@ void Robot::TeleopInit() {
     m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK;
-    // m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP;
+    m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; 
 
 }

diff --git a/Competition/src/main/cpp/RobotContainer.cpp b/Competition/src/main/cpp/RobotContainer.cpp
@@ -19,7 +19,7 @@ void RobotContainer::ConfigureButtonBindings() {
     m_feeder.setControllers(&m_GamepadOperator, &m_GamepadDriver);
     m_drivetrain.setController(&m_GamepadDriver);
     m_shooter.setControllers(&m_GamepadOperator, &m_GamepadDriver);
-    // m_lift.setController(&m_GamepadOperator);
+    m_lift.setController(&m_GamepadOperator);
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {

diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp
@@ -78,8 +78,8 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
     frc::Pose2d hangarSpotRed = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg));
     frc::Pose2d hangarSpotBlue = frc::Pose2d(units::meter_t(4), units::meter_t(7.5), frc::Rotation2d(170_deg));
 
-    frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg));
-    frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(6.4), frc::Rotation2d(-43_deg));
+    frc::Pose2d trenchSpotRed = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg));
+    frc::Pose2d trenchSpotBlue = frc::Pose2d(units::meter_t(4.5), units::meter_t(5.4), frc::Rotation2d(-43_deg));
 
     frc::Pose2d speedyRed = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0
     frc::Pose2d speedyBlue = frc::Pose2d(3.35_m, 3.2_m, frc::Rotation2d(-60_deg)); //originally 0
@@ -301,12 +301,12 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
         reverseConfig);
 
     auto moveTrenchRed = frc::TrajectoryGenerator::GenerateTrajectory(
-        hangarSpotRed,
+        tasRed,
         {},
         trenchSpotRed,
         reverseConfig);
     auto moveTrenchBlue = frc::TrajectoryGenerator::GenerateTrajectory(
-        hangarSpotBlue,
+        tasBlue,
         {},
         trenchSpotBlue,
         reverseConfig);
@@ -1006,7 +1006,6 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder
     frc2::WaitCommand((units::second_t).5),
     cmd_intakeOne,
     cmd_move_moveTasRed,
-    cmd_move_moveHangarRed,
     cmd_move_moveTrenchRed,
     cmd_intakeReverse,
     frc2::WaitCommand((units::second_t)1),

diff --git a/Competition/src/main/cpp/subsystems/TurretTracker.cpp b/Competition/src/main/cpp/subsystems/TurretTracker.cpp
@@ -82,11 +82,11 @@ void TurretTracker::assignOutputs() {
         state.target -= 360;
     }
 
-    if (state.target < -16) {
-        state.target = -16;
+    if (state.target < -12) {
+        state.target = -12;
     }
-    else if (state.target > 196) {
-        state.target = 196;
+    else if (state.target > 192) {
+        state.target = 192;
     }
 
     shooter->assignTurret(state.target);

diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h
@@ -214,8 +214,8 @@ namespace ShooterConstants{
     constexpr static double homePositionMid = 90;
     constexpr static double homePositionLeft = 180;
     constexpr static double homePositionRight = 0;
-    constexpr static double turretLimitLeft = 180 + 16;
-    constexpr static double turretLimitRight = 0 - 16;
+    constexpr static double turretLimitLeft = 180 + 12;
+    constexpr static double turretLimitRight = 0 - 12;
 
     constexpr static double turretRotateLiftThreshold = 20000; // lowered from 64500
     constexpr static double hubX = 0;
@@ -259,7 +259,7 @@ namespace LiftConstants{
 
     constexpr static int MAIN_FIRST_POSITION = 62000;
     constexpr static int MAIN_SECOND_POSITION = 78500;
-    constexpr static int MAIN_THIRD_POSITION = 103000;
+    constexpr static int MAIN_THIRD_POSITION = 98000;
     constexpr static int MAIN_DOWN_POSITION = 125;
     constexpr static int MAIN_BOTTOM_POSITION = 0;
     constexpr static int MAIN_SLOW_UP_POSITION = 5500;
@@ -270,7 +270,7 @@ namespace LiftConstants{
     constexpr static double rotateForwardLimit = 40;
     constexpr static double rotateReverseLimit = 0;
 
-    constexpr static double extendForwardLimit = 103000;
+    constexpr static double extendForwardLimit = 98000;
     constexpr static double extendReverseLimit = 125;
 
     constexpr static double pivotGearRatio = 1 / 95.67;

diff --git a/Competition/src/main/include/RobotContainer.h b/Competition/src/main/include/RobotContainer.h
@@ -33,7 +33,7 @@ class RobotContainer {
         Drivetrain m_drivetrain;
         Shooter m_shooter;
         Feeder m_feeder;
-        // Lift m_lift;
+        Lift m_lift;
         TurretTracker m_turretTracker;
 
     private: