Next we are going to create a command called TurnCommand which will allow us to turn the robot in place by a specified angle. You should know enough now to create the framework for this command. We will want to specify the speed to be used in the turn as well as the angle so create the constructor for your class so that it takes two double parameters, the first to specify the speed and the second to specify the angle. After you have created the new command, compare you code to the code below.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class TurnCommand extends Command { private final DriveSubsystem m_subsystem; private double m_speed; private double m_angle; /** * Creates a new TurnCommand. * * @param subsystem The subsystem used by this command. */ public TurnCommand(DriveSubsystem subsystem, double speed, double angle) { Logger.log("TurnCommand", 3, "TurnCommand()"); m_subsystem = subsystem; m_speed = speed; m_angle = angle; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { Logger.log("TurnCommand", 2, "initialize()"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Logger.log("TurnCommand", -1, "execute()"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { Logger.log("TurnCommand", 2, String.format("end(%b)", interrupted)); } // Returns true when the command should end. @Override public boolean isFinished() { Logger.log("TurnCommand", -1, "isFinished()"); return false; } } |
Did you create some member variables (e.g. m_speed and m_angle) to store the speed and angle that are passed in on the constructor?
Now we are going to use the encoders to measure the turn. Once again, we will want to use copies of the encoders like we did in our DriveForDistanceCommand. Add code to declare and initialize the left and right encoder:
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 |
public class TurnCommand extends Command { private final DriveSubsystem m_subsystem; private double m_speed; private double m_angle; private Encoder m_leftEncoder; private Encoder m_rightEncoder; /** * Creates a new TurnCommand. * * @param subsystem The subsystem used by this command. */ public TurnCommand(DriveSubsystem subsystem, double speed, double angle) { Logger.log("TurnCommand", 3, "TurnCommand()"); m_subsystem = subsystem; m_speed = speed; m_angle = angle; m_leftEncoder = m_subsystem.getLeftEncoder(); m_rightEncoder = m_subsystem.getRightEncoder(); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } |
Now in the initialize() function we also want reset the encoders and turn the motors on.
To make the robot turn, we need to set the speed to the left and right motors opposite, and we need to set the signs so that it turns in the correct direction. Let’s say that a positive angle should make the robot turn to the right (clockwise) and a negative angle should make the robot turn to the left (anticlockwise). See if you can add code to your initialize() function to accomplish this and then check your result with the one below.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
public void initialize() { Logger.log("TurnCommand", 2, "initialize()"); m_leftEncoder.reset(); m_rightEncoder.reset(); if (m_angle < 0) { m_subsystem.setSpeed(-m_speed, m_speed); // Turn left } else { m_subsystem.setSpeed(m_speed, -m_speed); // Turn right } } |
Now we need to add some code to the isFinished() function to return true when the robot has turned by the requested angle. Add the following line to the isFinished() function:
|
1 |
int delta = m_leftEncoder.get() - m_rightEncoder.get(); |
Note that delta will be positive if the robot is turning right (i.e. the left wheel is moving forward and the right wheel is moving backward), and negative if the robot is turning left. The magnitude of delta will also be a measure of how far the robot is turned. So lets return true when the magnitude of delta exceeds the magnitude of the angle.
|
1 2 3 4 5 6 7 |
public boolean isFinished() { Logger.log("TurnCommand", -1, "isFinished()"); int delta = m_leftEncoder.get() - m_rightEncoder.get(); return (Math.abs(delta) >= Math.abs(m_angle)); } |
Note that we are using the absolute value of both delta and m_angle in the comparison since they can both be negative.
Remember to turn the motors back off in the end() function. Your TurnCommand.java file should now look like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.Encoder; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class TurnCommand extends Command { private final DriveSubsystem m_subsystem; private double m_speed; private double m_angle; private Encoder m_leftEncoder; private Encoder m_rightEncoder; /** * Creates a new TurnCommand. * * @param subsystem The subsystem used by this command. */ public TurnCommand(DriveSubsystem subsystem, double speed, double angle) { Logger.log("TurnCommand", 3, "TurnCommand()"); m_subsystem = subsystem; m_speed = speed; m_angle = angle; m_leftEncoder = m_subsystem.getLeftEncoder(); m_rightEncoder = m_subsystem.getRightEncoder(); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } // Called when the command is initially scheduled. @Override public void initialize() { Logger.log("TurnCommand", 2, "initialize()"); m_leftEncoder.reset(); m_rightEncoder.reset(); if (m_angle < 0) { m_subsystem.setSpeed(-m_speed, m_speed); // Turn left } else { m_subsystem.setSpeed(m_speed, -m_speed); // Turn right } } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Logger.log("TurnCommand", -1, "execute()"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { Logger.log("TurnCommand", 2, String.format("end(%b)", interrupted)); m_subsystem.setPower(0, 0); } // Returns true when the command should end. @Override public boolean isFinished() { Logger.log("TurnCommand", -1, "isFinished()"); int delta = m_leftEncoder.get() - m_rightEncoder.get(); return (Math.abs(delta) >= Math.abs(m_angle)); } } |
For now we will pass in an angle in the encoder units and see how far the robot turns (later we will find the conversion factor so we can enter our angle in degrees). Configure the RobotContainer class so that this command will be executed with 0.3 speed for and angle of 1500 when we press button 5 on the joystick. When you have made that change, compare it to the code below.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 |
/*----------------------------------------------------------------------------*/ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.commands.DriveForTimeCommand; import frc.robot.commands.TestMotorSpeedCommand; import frc.robot.commands.TurnCommand; import frc.robot.commands.ArcadeDriveCommand; import frc.robot.commands.CalibrateSpeedCommand; import frc.robot.commands.DriveForDistanceCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ExampleSubsystem; /** * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the * scheduler calls). Instead, the structure of the robot (including subsystems, * commands, and button mappings) should be declared here. */ public class RobotContainer { // The robot's subsystems and commands are defined here... @SuppressWarnings("unused") private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CommandJoystick m_joystick = new CommandJoystick(0); private final ArcadeDriveCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick)); // Configure the button bindings configureButtonBindings(); } /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { m_joystick.button(1).onTrue(new DriveForTimeCommand(m_driveSubsystem, 0.50, 3.0)); m_joystick.button(2).onTrue(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 30)); m_joystick.button(3).onTrue(new TestMotorSpeedCommand(m_driveSubsystem)); m_joystick.button(4).whileTrue(new CalibrateSpeedCommand(m_driveSubsystem)); m_joystick.button(5).onTrue(new TurnCommand(m_driveSubsystem, 0.3, 1500)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous return m_autoCommand; } } |
Now deploy and run your program and see how far the robot turns. Try and find a number to replace the 1500 value that will make the robot turn exactly 360 degrees.
.
.
.
.
In my case I found that approximately 1850 (4700 for the blue motors) encoder units will make the robot turn 360 degrees. This means that if the input angle is in degrees, we need to multiply it by (1850.0/360) or (4700.0/360) for the blue motors to get the number of encoder units that we need. NOTE that the value for your robot may be different, but the math is the same! Implementing this in the code we get:
|
1 2 3 4 5 6 7 8 9 10 11 12 |
public TurnCommand(DriveSubsystem subsystem, double speed, double angle) { Logger.log("TurnCommand", 3, "TurnCommand()"); m_subsystem = subsystem; m_speed = speed; m_angle = angle * k_ticksPerDegree; m_leftEncoder = m_subsystem.getLeftEncoder(); m_rightEncoder = m_subsystem.getRightEncoder(); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } |
Where I have defined k_ticksPerDegree as:
|
1 2 |
private final double k_ticksPerDegree = 1850.0 / 360; // private final doubl ek_ticksPerDegree = 4700.0 / 360; // Blue motors |
Note that I have used 1850.0 rather than just 1800 because I need to force it to use floating point when evaluating the expressiong.
Now change the line in the RobotContainer to turn the robot by 180 degrees:
|
1 |
m_button5.whenPressed(new TurnCommand(m_driveSubsystem, 0.3, 180)); |
Before moving on, verify that if you specify a negative angle the robot will turn the other way:
|
1 |
m_button5.whenPressed(new TurnCommand(m_driveSubsystem, 0.3, -180)); |
You may find that the robot does not turn exactly the desired 180 degrees. This is because the accuracy of the turns using this method is poor because of wheel slippage and other issues. If the robot had a gyroscope, we could use that for more accurate turns, but since it does not, we will need to live with the inaccuracies.