Auto Alignment

In order to make a shot starting from any arbitrary position on the field, we are potentially going to need to do a number of things.  We will need to drive until we are roughly aligned with the front of the tower, then move forward to the correct distance, and finally adjust our angle so that the shot aligns with the target. Rather than attempting to do all of these things at once, we are going to take it step by step.

The first step, we are going to assume that the robot is the correct distance and roughly aligned with the front of the tower, but needs to be rotated to the correct position to make the shot.

Let’s start by creating a new command.  Let’s call it AutoShootCommand.  Go ahead and create the framework for this new command and then compare you solution with the one below.  Remember that since we are going to need to both drive and shoot, this command will require the three subsystem, DriveSubsystem, SpinnerSubsystem, and ShooterSubsystem.  Also, connect this new command to button 3 on the joystick.






You file should now look like:

And your file should look like:

Note that for this command we are using the whenPressed function, rather than the whileHeld function.  This is because we want the command to continue until the shot is complete, not as long as we are holding the button.

We are going to implement this command as a simple state machine.  We start with three states and will define this by creating an enum at the top of our class as follows:

When the command starts, we will check the camera and then turn right or left depending on what we see.  Add the following function that will be used to start this turn:

First we use the vision data to determine the center of the target and then look to see if it left or right of the horizontal target line, starting our turn accordingly.  The turn speed k_turnSpeed constant must, of course be defined:

As does the SetState functon:

Note that in addition to setting the m_state variable, we are logging the new state.  This will help us in the debugging of our state machine.

In the initalize() function we now need call this function to begin the turn:

In the execute() function we need to check to see if our turn is complete:

We have added a switch with case statements for each of the possible state of our state machine.  Note that instead of putting the logic for each state inline in the switch we are calling functions. This is a good practice which you are encouraged to follow.  Declare these two functions as follows:

In either case the turn in complete when the center of the target moves past the horizontal target line.  When this happens, we set the state to Done and our command is complete.

The isFinished() function should return true when the turn is complete (i.e. the state is Done):

Finally, when the end() or interrupted() functions are called, we need to turn the motors off:

Your file should now look like:

Now deploy and run your program and let’s see how this works.  Move your robot so that it is at the correct position and the horizontal green line matches the horizontal red line.  Then rotate the robot so that it is slightly off target to either the left or the right and press button 3. The following shows the target positions before and after the auto turn:


You will notice that it has a tendency to overshoot the target line. This is because there is a bit of a delay between the time the robot sees the target move past the center and the robot stops.  To counter this, we will try and stop the robot at a point before it reaches the center line.  We will define a constant for this distance as follows:

And we modify the TurningLeft and TurningRight functions to stop the turn when the target is within this distance of the horizontal target line:

Now deploy and run the program and try the Auto Shoot Command again:


As you can see, it is much closer.  You can play around with the k_turnLead number to try and make it better, but you will quickly find that the amount it is off varies a fair amount making it impossible to choose a k_turnLead the will work for all turns.  This turn code will get us close, but we will need another function that will allow us to make the final small adjustments required.

Next: Fine tuning the alignment.

Pierre Pierre, who replica handbag 185cm tall, gucci replica handbags the "Diamond King" in his hermes replica handbags . He has been selected as one of the handbag replica most beautiful 50 people in the "People" magazine. It is also known as replica handbags most elegant and quiet in the world. Unforgettable prince.