Autonomous Doesn't work

Technical Programming
1

my team is programming in java, and our autonomous doesnt work, and our robot is still in this period

package frc.robot;

import java.util.List;
//import java.util.List;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants.AutoConstants;
import frc.robot.Constants.DriveConstants;
import frc.robot.Constants.OIConstants;
import frc.robot.commands.MoveArmCmd;
import frc.robot.commands.SwerveJoystickCmd;
import frc.robot.commands.inshooterCmd;
import frc.robot.commands.inshooterCmdAuto;
import frc.robot.subsystems.SwerveSubsystem;
import frc.robot.subsystems.inshooterSubsystem;
import frc.robot.subsystems.ArmSubsystem;
//import edu.wpi.first.wpilibj.XboxController;


public class RobotContainer {



        //coloque seus subsystems aqui
    private final ArmSubsystem ArmSubsystem = new ArmSubsystem();
    private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
    private final inshooterSubsystem inshooterSubsystem = new inshooterSubsystem();
    
    private final Joystick clawJoystick = new Joystick(OIConstants.kClawControllerPort);
    private final Joystick driverJoytick = new Joystick(OIConstants.kDriverControllerPort);
        //static XboxController driverJoystick = new XboxController(OIConstants.kDriverControllerPort);
    private final double veloIntake = 1; 
    public RobotContainer() {



        swerveSubsystem.setDefaultCommand(new SwerveJoystickCmd(//essa linha de código seta o command do swerve para sempre atuar
                swerveSubsystem,
                () -> -driverJoytick.getRawAxis(OIConstants.kDriverYAxis),// driverJoystick.getLeftY(),
                () -> driverJoytick.getRawAxis(OIConstants.kDriverXAxis),//driverJoystick.getLeftX(),
                () -> driverJoytick.getRawAxis(OIConstants.kDriverRotAxis),//driverJoystick.getRightX(),
                () -> !driverJoytick.getRawButton(OIConstants.kDriverFieldOrientedButtonIdx), /* driverJoystick.getAButton()*/
                () -> !driverJoytick.getRawButton(OIConstants.kDriverMirarButtonIdx)));

        inshooterSubsystem.setDefaultCommand(new inshooterCmd(inshooterSubsystem, 
                 veloIntake,
                () ->clawJoystick.getRawButton(2),
                () ->clawJoystick.getRawButton(4),
                () ->clawJoystick.getRawButton(5)));
                
        configureButtonBindings();
    }

    private void configureButtonBindings() {
        new JoystickButton(driverJoytick, 2).onTrue(swerveSubsystem.runOnce(swerveSubsystem::zeroHeading));
        new JoystickButton(clawJoystick, 1 ).onTrue(new MoveArmCmd(ArmSubsystem ,-21.5));
        new JoystickButton(clawJoystick, 3).onTrue(new MoveArmCmd(ArmSubsystem , 4));
        new JoystickButton(clawJoystick, 6).onTrue(new MoveArmCmd(ArmSubsystem , -19.8));
    }
       
    

    public Command getAutonomousCommand() {
       
        inshooterCmdAuto sugar = new inshooterCmdAuto(inshooterSubsystem, veloIntake, true, false);
        inshooterCmdAuto esquentarShooter = new inshooterCmdAuto(inshooterSubsystem, veloIntake, false, true);
        inshooterCmdAuto lancar = new inshooterCmdAuto(inshooterSubsystem, veloIntake, true, true);
        inshooterCmdAuto parar = new inshooterCmdAuto (inshooterSubsystem, veloIntake, false, false);
        // 1. Create trajectory settings
        TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
                AutoConstants.kMaxSpeedMetersPerSecond,
                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
                        .setKinematics(DriveConstants.kDriveKinematics);

        // 2. Generate trajectory
        Trajectory trajectory1 = TrajectoryGenerator.generateTrajectory(
                new Pose2d(0, 0, new Rotation2d(0)),//ponto inicial
                List.of(
                        new Translation2d(1, 0)),//pontos intermediarios
                new Pose2d(2, 0, Rotation2d.fromDegrees(180)),//ponto final
                trajectoryConfig);

        Trajectory trajectory2 = TrajectoryGenerator.generateTrajectory(
                new Pose2d(0, 0, new Rotation2d(0)),//ponto inicial
                List.of(
                        new Translation2d(3, 0),//pontos intermediarios
                        new Translation2d(1.5, 1.5)),
                new Pose2d(0, 0, Rotation2d.fromDegrees(180)),//ponto final
                trajectoryConfig);

        // 3. Define PID controllers for tracking trajectory
        PIDController xController = new PIDController(AutoConstants.kPXController, 0, 0);
        PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0);
        ProfiledPIDController thetaController = new ProfiledPIDController(
                AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
        thetaController.enableContinuousInput(-Math.PI, Math.PI);

        // 4. Construct command to follow trajectory
        SwerveControllerCommand swerveControllerCommand1 = new SwerveControllerCommand(
                trajectory1,
                swerveSubsystem::getPose,
                DriveConstants.kDriveKinematics,
                xController,
                yController,
                thetaController,
                swerveSubsystem::setModuleStates,
                swerveSubsystem);

        SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand(
                trajectory2,
                swerveSubsystem::getPose,
                DriveConstants.kDriveKinematics,
                xController,
                yController,
                thetaController,
                swerveSubsystem::setModuleStates,
                swerveSubsystem);

        // 5. Add some init and wrap-up, and return everything
        return  new SequentialCommandGroup(
                new InstantCommand(() -> swerveSubsystem.resetOdometry(trajectory1.getInitialPose())),
                swerveControllerCommand1,
                new InstantCommand(() -> swerveSubsystem.stopModules())
                //new ParallelCommandGroup(new MoveArmCmd(ArmSubsystem,-21.5),
                  //      new SequentialCommandGroup(esquentarShooter,
                    //                    new WaitCommand(2),
                      //                  lancar,
                        //                new WaitCommand(2),
                          //              parar))
                );/*
                
                esquentarShooter,
                new WaitCommand(2),
                lancar,
                parar,
                sugar,
                swerveControllerCommand1,
                parar,
                swerveControllerCommand2,
                new inshooterCmdAuto(inshooterSubsystem, veloIntake, false, true),
                new WaitCommand(3),
                new InstantCommand(() -> swerveSubsystem.stopModules()));*/
    }
}

it is a simple route, but it doesnt work. i dont have any idea why tho

2

This community is always willing to help, but you need to give more information if you want help.

Start with some basics:

  1. What are you intending your robot to do in auto?
  2. What exactly isn’t working? (Is the robot driving the wrong way? Not moving at all? Is the code not even compiling? Are there error messages?)
  3. What have you already tried to fix it?
2 Likes
3

i’m sorry. this trajectory of auto is intended to only make the robot go foward 2 meters. the rest is commented. the thing is, that in the autonomous period, the robot simply doesn’t work, and the robot does nothing. the strange thing is that it doesnt show any errors. I had a older code that have worked, but when i have used the exactly same logic, the robot did’nt work.

4

One useful thing to debug are print commands.

You say it does nothing at all, are you certain the commands getAutonomousCommand() returns are being run in auton?

Maybe try adding some commands to your sequence which print out messages.
Something like

return new SequentialCommandGroup(
             new PrintCommand("Starting auton"),
             new InstantCommand(() -> swerveSubsystem.resetOdometry(trajectory1.getInitialPose())),
             new PrintCommand("Odometry Reset"),
             swerveControllerCommand1,
             new PrintCommand("Trajectory Finished Executing"),
             new InstantCommand(() -> swerveSubsystem.stopModules()),
             new PrintCommand("Modules Stopped, Auton Finished")

(dont forget to import PrintCommand)

This style of debugging can help diagnose problems like “auton does nothing.”

5

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.