diff --git a/Competition/src/main/cpp/ValorAuto.cpp b/Competition/src/main/cpp/ValorAuto.cpp index d736a98..30e2273 100644 --- a/Competition/src/main/cpp/ValorAuto.cpp +++ b/Competition/src/main/cpp/ValorAuto.cpp @@ -380,6 +380,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder frc2::SequentialCommandGroup *shoot5Blue = new frc2::SequentialCommandGroup(); shoot5Blue->AddCommands (cmd_set_odometry, + cmd_turretDisable, cmd_shooterPrime, cmd_intakeOne, cmd_move_moveBugsBlue, @@ -414,6 +415,7 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder (cmd_set_odometry, cmd_shooterPrime, cmd_intakeOne, + cmd_turretDisable, cmd_move_moveBugsRed, cmd_move_movePreDaffyRed, cmd_turretTrack, @@ -442,11 +444,28 @@ ValorAuto::ValorAuto(Drivetrain *_drivetrain, Shooter *_shooter, Feeder *_feeder ); + frc2::SequentialCommandGroup *shooterTest = new frc2::SequentialCommandGroup(); + shooterTest->AddCommands + (cmd_set_odometry, + cmd_shooterPrime, + cmd_intakeOne, + cmd_turretTrack, + frc2::WaitCommand((units::second_t)3), + cmd_intakeShoot, + frc2::WaitCommand((units::second_t)1.0), + cmd_intakeOne + ); + + + //right now shoot 5 auto is shoot 3, need to make a red and blue auto m_chooser.AddOption("RED 3 ball auto", shoot3Red); m_chooser.AddOption("RED 5 ball auto", shoot5Red); m_chooser.AddOption("BLUE 3 ball auto", shoot3Blue); m_chooser.AddOption("BLUE 5 ball auto", shoot5Blue); + + m_chooser.AddOption("SHOOTER TEST", shooterTest); + frc::SmartDashboard::PutData(&m_chooser); diff --git a/Competition/src/main/include/Constants.h b/Competition/src/main/include/Constants.h index 2075167..ee42e6c 100644 --- a/Competition/src/main/include/Constants.h +++ b/Competition/src/main/include/Constants.h @@ -58,15 +58,15 @@ namespace DriveConstants { constexpr static double TURN_KP = 3.8 * M_PI / 180.0; constexpr static double LIMELIGHT_KP = .02; - constexpr static double KPX = .75; //.2 + constexpr static double KPX = .5; //.75 constexpr static double KIX = 0.0; //0 constexpr static double KDX = 0.0; //.1 - constexpr static double KPY = .75; //.2 + constexpr static double KPY = .5; //.75 constexpr static double KIY = 0.0; //0 constexpr static double KDY = 0.0; //.1 - constexpr static double KPT = 2.5; //.2 + constexpr static double KPT = 2; //2.5 constexpr static double KIT = 0.0; //0 constexpr static double KDT = 0.0; //.1 } @@ -83,7 +83,7 @@ namespace SwerveConstants { constexpr static double DRIVE_GEAR_RATIO = 1.0 / 5.12; // 1/8.14 constexpr static double AZIMUTH_GEAR_RATIO = (15.0 / 32.0) * (10.0 / 60.0); // 0.078125 - constexpr static double WHEEL_DIAMETER_M = 0.1016; + constexpr static double WHEEL_DIAMETER_M = .091; // original number with blue tread 0.1016; constexpr static double WHEEL_CIRCUMFERENCE_M = WHEEL_DIAMETER_M * M_PI; constexpr static double MOTOR_FREE_SPEED = 6380.0; @@ -140,6 +140,7 @@ namespace ShooterConstants{ constexpr static double flywheelAutoValue = 0.47; constexpr static double flywheelDefaultValue = 0.345; + constexpr static double turretKP = 1e-5; constexpr static double turretKI = 0; constexpr static double turretKD = 0;