FTC Blocks-Move to Position

This post is about For Inspiration and Recognition of Science and Technology (FIRST) Tech Challenge (FTS) Blocks Programming. More information about FTC is available at https://firstinspires.org. Most teams start their FTC programming using OnBot Blocks.

This post is about using a motor with an encoder to move to a set position and hold the position. This approach is most common when moving a manipulator into position to gather Game Pieces or deliver Game Pieces. This can also be used in an Autonomous OpMode to drive the robot to a specific position. (Note that Mecanum Wheels slip by design. Using Run to Position with Mecanum Wheels will have different results every time it is run.)

Several teams developed an arm manipulator for the 2021-2022 Freight Frenzy presented by Raytheon game. This allowed the robot to pick up Freight from the floor and place it at any level of the Alliance Specific Shipping Hub. (Details on the 2021-2022 Freight Frenzy game are available at https://www.youtube.com/watch?v=I6lX12idAf8.) 

This post provides an example of how to program an arm manipulator or Lift Arm using OnBot Blocks. There are many ways to design your robot. There are many ways to program your robot in OnBot Blocks or other languages. This post is provided as one example to get started with. FTC Team Members are strongly encouraged to modify and add to this programming for their own robot. The code has been tested on the goBILDA 2021-2022 Master FTC Kit (https://www.gobilda.com/master-ftc-kit-8mm-rex-shaft-2021-2022-season/). This kit uses a goBILDA Yellow Jacket Series 5203 motor with two stages of 20:100 gears to raise and lower the Lift Arm. The OpMode was designed for use with a single PS4 gamepad.

An image of the program is available at https://drive.google.com/file/d/1RRABcEWfHENKGyPyqJ7NqfiCoVtQEJZD/view. The Blocks File (*.blk) is available at https://drive.google.com/file/d/1r6KXJH3AQqdBavsHRCpk2p_ydWvkrwOE/view. This OpMode was exported to Java (*.java) and is available at https://drive.google.com/file/d/1mhsZ_3dDud-xDVWWWjrcTA8j8jeq1lBo/view.

In addition to the Main program at the bottom there are two functions: SetGlobalParameters and Telemetry_TeleOp. These are standard functions that are part of my coding style. 

The SetGlobalParameters function brings all the parameter variables together at the top of the code. This makes it easier to tune robot behavior during testing. Any parameter can be adjusted in this compact function. Having this function at the top of the code makes it easy to make adjustments between Matches at an FTC Event.

The Telemetry_TeleOp function provides a single location to manage Telemetry displayed on the Driver's Station during operation. The Telemetry is part of the Human-Machine Interface (HMI). In this one function the Telemetry can be tailored to the desires of the Driver. Having this function near the top of the code makes it easy to make adjustments between Matches at an FTC Event.

Telemetry is a great way to tune the robot functions when programming to specific positions. In the initial code, you make a wild guess at what positions the motor needs to achieve. Then during testing you can see the actual values from the robot. These observations allow you to update the global parameters and re-test the code. In addition to preset position commands in the code, provide manual control of the motors as well. This way you can make small adjustments during testing. This will get you to a final set of tuning parameters faster than merely trial and error.

In the sample code, the main routine starts with setting the global parameters. Then the Lift Arm motor is initialized. From testing and Telemetry, we determined that as mounted moving the Lift Arm up used negative velocity and moving the Lift Arm down used positive velocity. Instead of trying to keep track of this movement, the Lift Arm Direction is set to Reverse. Now larger positions are higher than smaller positions.

After the Lift Arm Direction is set, the motor is switched to RUN_TO_POSITION mode. When setting RUN_TO_POSITION, the Target Position value property must be set. We set it to the current position which keeps it in place when the OpMode is activated by pressing the play button. For Freight Frenzy, the Lift Arm doesn't need to be perfectly placed, so a position tolerance of 50 ticks is allowed. These three blocks are a great way to initialize any motor for using position control.

In the case of Freight Frenzy, the Lift Arm with the Freight starts on the ground. To be ready to move as soon as Teleop begins, the first commands after the OpMode is activated is to move the Lift Arm to the Drive position. Then the OpMode moves into the continuous loop monitoring for Driver commands.

Inside the Teleop loop, the series of IF blocks pick up when the Driver presses he buttons on the PS4 gamepad. The PS4 gamepad has Cross, Circle, Triangle, and Square buttons. The Xbox gamepad has A, B, Y, and X buttons respectively. The code is programmed for the following operations:

  • Cross: Lift Arm to the Ground to pick up another Game Piece
  • Circle: Lift Arm to deliver Freight to Shipping Hub Level 1
  • Triangle: Lift Arm to deliver Freight to Alliance Shipping Hub Level 2
  • Square: Lift Arm to deliver Freight to Alliance Shipping Hub Level 3


The use of variables with meaningful names makes this part of the code is easy to read. The actual position values for these set point are available in the SetGlobalParameters function and set during initialization.

As mentioned earlier, when using pre-programmed positions, provide a backup manual control. Lots of things can throw off the settings so these pre-programmed settings don't behave as expected during a Match. The manual backups allows you to continue in the Match. 

The Manual Control of the Lift Arm in this code is achieved by incrementally increasing or decreasing the Target Position of the motor. This means the motor can stay in position control even if you want to give it the appearance of continuous control. During Driver Practice, explain this approach to the manual controls. It is possible for them to get ahead of the arm motion. When this happens, the Driver can release the button and the arm continues to the Target they set. The Telemetry on the Driver Station can show the Driver this affect during practice. 

Once the Lift Arm motor's Target Position has been updated, it needs a velocity command to move the Lift Arm to the new position. The velocity needs a positive value to move up and a negative value to move down. An IF block is used to determine if the Current Position is below the Target Position. If it is, a positive Velocity is applies. Otherwise a negative Velocity is applied.

The motor's encoder is initialized when the robot is powered up. If it is not in the correct position at this time, the preset positions will be off. The D-pad Left button is programmed to reset the encoder on this motor. The Driver can use the manual control to reposition the Lift Arm to ground level, then press this button and the encoder will zero here. This is achieved by switching the motor's mode to Stop and Reset Encoder and then returning it to the Run to Position mode. Since the arm had to be positioned to a non-zero target to start the reset process, the Target Position is reset to the current position after the encoder is reset and before the motor is returned to the Run to Position mode. This prevents it from moving when returned to this mode.

Tuning the Lift Arm operation included tuning the Velocity value to match the available speed of the motor. The no load speed is available from the vendor, but needed to be confirmed when the motor is put under load while lifting and lowering the Lift Arm. The Velocity value changes so quickly during testing that it can be hard to read the value from the Telemetry screen, so an ArmMaxSpeed variable was created. The Arm Speed is determined as the absolute value of the Arm Velocity. This allows speed moving up to be compared with speed moving down. When the current Arm Speed is larger than the stored value in the variable, the variable is updated to the current Arm Speed. This is then displayed on the Telemetry section of the Driver Station.

The Arm Velocity can be programmed to a very large number and when applied to the motor, it will run to maximum power, even if it can't achieve the programmed velocity value. This eliminates the value of using velocity control over direct power commands. Velocity control provides a smooth acceleration of the Lift Arm as well as smooth deceleration as it reaches its target position. The direct power commands can cause the Lift Arm to jerk. This increases the risk of shaking the Freight from the gripper.

Additional FTC related files are available at Jack's FTC Folder (https://drive.google.com/drive/folders/1IPGEh3w7usQq_4Bv16lMXpnHAK1UqMi4).

Comments

Post a Comment

Popular posts from this blog

FRC 4414, High Tide, Pit Writeup

 Check out this Pit Writeup on Chief Delphi: https://www.chiefdelphi.com/t/team-4414-hightide-pit-writeup/411862.  This provides a great example of how to design and improve a pit. I've been interested in road cases for use with pits since I volunteered at my first event and saw that this is how FIRST transports the field from venue to venue. The High Tide team has worked with a road case company and developed a great pit. The pit is your home when you're at an event. There are enough other things going on during an event, so you want your pit to be familiar. I recommend setting up the pit every meeting at your home workspace. You may have more equipment or additional work tables at your home base, but you should keep your tools and parts in your pit during regular meetings. This way everyone knows were to find things when you load in your robot and are setup for an event.
Read more

Museum Robot Exhibit

I was recently at the Indianapolis Children's Museum (https://www.childrensmuseum.org). It is like no other museum. You have to check it out. The good news is you don't need children to enter. I grew up in Indiana and have fond memories of going to the top floor to the Science Exploration exhibit. Unfortunately all the cool physical science experiments which were there when I was a kid have been replaced with an Agriscience exhibit. I'm not taking away from the importance of Agriscience, and it makes sense that a museum in Indiana needs to highlight this critical area. I just wish it could live in harmony with the old science hands on area that I remember. Ah-ha! Why did Dean Kamen choose robots when he wanted to inspire kids to pursue careers in science and technology? The reason was because robots cover everything. There are mechanical parts, electrical parts, pneumatic parts, and software. There are small robots for young kids and big robots for kids and adult mentors. W...
Read more

Driver-centric or Field-centric

The terms "Field-centric" and "Driver-centric" are used interchangeably in FIRST robotics. I think we need to differentiate them. I also think we need to use them properly. This drive system is common in both FTC and FRC since both have teams building holonomic drive systems. In FTC, Mecanum wheels are popular since defensive play is limited by the rules. In FRC, there are some teams with Mecanum wheels, but there are more teams building Swerve drive systems. Both terms are used to refer to a control system where when the Driver moves the control input to the right, the robot moves to the right from the Driver's point of view. Since the Driver is in a fixed position on the field, the robot moves to the right side of the field.  This control system is in contrast with a Robot-centric control system, which is where most teams start because it is easiest to understand. When you move the joystick forward, the robot goes forward, no matter which direction it is facin...
Read more