FRC Ball Indexing

A complete overview of the ball sorting/indexing system I designed. Starting with an intro to the sensors and finishing with the code, this article is a full deep dive into how I made the ball sorting/indexing system for the 2022 FRC robot.

Introduction

While this small project might not be spectacularly interesting to most, I am quite fond of this small mix of hardware and software working together to automate and perform a task. The picture above depicts our 2021-2022 First Robotics Competition (FRC) robot. In the middle of the robot are two uprights that contain our ball indexing subsystem. The design goal of this robot was to be able to intake balls on either side of the robot and store them within the indexing subsystem until they were ready to be shot out into the low goal. At worlds, the indexing subsystem was slightly modified to be able to score in the high goal. These changes will be discussed in depth later.

After initial testing of the physical design, it became rather apparent that the system would have to be automated to function correctly. Strict timing combined with added stress on the driver was not something our team wanted, and as the person driving at the time, neither did I. Therefore, the decision to automate the system was made.

The Sensors

The first step in automating the system was finding suitable sensors for the application. Ideally, whatever sensors we chose would be contactless to reduce the risk getting knocked off. Searching around within what the team already had, I began playing around with some red-light photoelectric switches.

Upon initial testing (with red balls), these switches appeared to work great, although they were not without their problems. The sensors struggled to detect blue balls. When they did, they usually stopped in different places than the red balls. This created the problem of having to adjust the sensors depending on what color we were playing for. Another major problem with these sensors is although they did have a digital output, the “LOW” signal was too high for the RoboRio’s (main microcontroller aboard the robot) threshold resulting in a constant “HIGH” read. This was fixed by tying a 10.2kΩ resistor between the signal and ground to drop the voltage down to a readable level.

We ran these sensors at the Victoria Regional with minimal issues but decided to keep exploring other options to minimize faults. Eventually, we settled on using the REV Robotics V2 Color Sensors.

We already had one on the robot for color sorting and their IR proximity sensor made detecting the balls a lot more consistent, regardless of color.

These sensors were not without their issues either though. Because of their fixed I2C addresses, there would be address conflicts when putting them on the same bus. While the RoboRio does come with two I2C buses, one of the buses caused frequent system lockups preventing it from being used. Therefore, a Raspberry Pi was used to add a second I2C bus to the system. The Raspberry Pi used Network Tables to communicate with the RoboRio.

After extensive testing both in preparation for and during the world championship tournament, the REV V2 Color sensors held up extremely well and proved to be a reliable option for detecting balls in the indexing system. It would be my recommendation to use these sensors in the future when trying to detect objects of varying color. Otherwise, the red light photoelectric sensors are much easier to use with the most complicated part being to not forget the resistor tying signal into ground.

The Software

The software for this project can easily be split into two parts: The software running on the Pi, and the algorithm itself. The python script running on the Raspberry Pi was not written by me and is used by a lot of other teams for the same purpose. The only thing that I changed was to expose the IR proximity reading of the sensor, as well as to restructure some of the Network Tables directories.

sensor1 = ColorSensorV3(1)
rawColorEntry1 = ntinst.getEntry("/rpi/rawcolor1")
proxEntry1 = ntinst.getEntry("/rpi/proximity1")
smartDashboardProximEntry = ntinst.getEntry("/SmartDashboard/Bot Proxim")
colorEntry1 = ntinst.getEntry("/rpi/color1")

# loop forever
while True:
    # read sensor and send to NT
    rawcolor = sensor1.getRawColor()
    color = [sensor1.getColor().red, sensor1.getColor().green, sensor1.getColor().blue]
    prox = sensor1.getProximity()
    rawColorEntry1.setDoubleArray(
        [rawcolor.red, rawcolor.green, rawcolor.blue, rawcolor.ir])
    proxEntry1.setDouble(prox)
    smartDashboardProximEntry.setDouble(prox)
    colorEntry1.setDoubleArray(color)
            </code></pre>

While there was a lot of trial and error that went into building this algorithm, I will simply examine the final state of it for the purpose of this writeup.

The Indexing system uses a command-based robot structure. This is essentially the FRC equivalent of a scheduler. The subsystem is assigned a default task. This default task will run until it is interrupted by another task, then resume when the interrupt task is finished. This indexing subsystem has 6 commands. Below is a flowchart that demonstrates the tasking works. When each task reaches its finished condition, the subsystem returns to its default task: Index.

The indexing task itself is a finite state machine that uses a variable called “codex” to track the state of the system. Depending on the state of the system, the top, bottom, and feed rollers are set to move at certain intake speeds or stop. Below is the code that controls these conditions.

if ( codex == 2 ){
subsystem->setTop(0);
subsystem->setBottom(0);
subsystem->setFeed(0);
}
else if ( codex == 1 ){
subsystem->setTop(ControlMode::Velocity, 0);
subsystem->setBottom(iPow);
subsystem->setFeed(1);
}
else if ( codex == 0 ){
subsystem->setTopVel(vPow);
subsystem->setBottom(iPow);
subsystem->setFeed(1);
}
else{
printf("Codex OverFlow");
codex = 0;
}  

While the above code controls the state of the motors using the codex as an input, the code below controls the state of the codex depending on its current state, and if any of the sensors have been triggered. Also within this code is some color sorting logic to prevent indexing balls of the wrong color.

if(teamColor.GetSelected() != IndexSubsystem::TeamColors::null){
    // if BallColor == TeamColor && codex == 0 -> Index ball
    if (teamColor.GetSelected() == subsystem->getTopBallColor() && codex == 0){
        codex = 1;
    }

    // If a ball has been registered by the Proximity Sensor, But the Color is undeterminable, set top indexer drive power to zero
    // Fixes Balls getting caught too high in the top indexer
    if(subsystem->getTopIR() && subsystem->getTopBallColor() == IndexSubsystem::TeamColors::null){
        vPow = 0;
    }
    else{
        vPow = 6000;
    }
}
else{
if ( subsystem->getTopIR() && codex == 0){ codex = 1; };
}

if ( subsystem->getBotIR() && codex == 1 ){ codex = 2; };

Once the driver is ready to score the balls indexed by the system, they can press a trigger button that will interrupt the “Index” task and schedule a different task depending on which button was pressed.

// Shoot Balls Stored within the index when Right Bumper is Pressed
frc2::JoystickButton(&master, frc::XboxController::Button::kRightBumper)
.WhenPressed(new IndexCommands::ShootLow(&subsystem_index));

// Shoot High Goal -> Speeds up top indexer and shoots once it reaches the correct Velocity
frc2::JoystickButton(&master, frc::XboxController::Button::kLeftBumper)
.WhenPressed(new IndexCommands::ShootHigh(&subsystem_index)
);

// Master A -> Runs Auton Indexing sequence for high goal shooting
frc2::JoystickButton(&master, frc::XboxController::Button::kA)
.ToggleWhenPressed(IndexCommands::Index(&subsystem_index)); 

The driver can also choose to stop all motor movement or engage manual control of the indexing system.

/**
* @brief Index Toggle Command
* Note: Shoot Commands will override this command,
* Upon Exiting a shoot command, the Indexing command will resume
* To continue manual operation, the partner controller will have to re-enable the override
* 
* Allow the partner controller to toggle between indexing modes
* 
* Default Mode: Automatic Indexing though Photoelectric Sensors
*               IndexCommands/Index
* 
* Override Mode: Manual Indexing through the Partner Controller
*                IndexCommands/Manual
*/
frc2::JoystickButton(&partner, frc::XboxController::Button::kLeftBumper)
    .ToggleWhenPressed(IndexCommands::Manual(&subsystem_index));

The system features two different shooting tasks that can be run. The first is the “Low Shot” task that we made for the Victoria Regional tournament and simply spins all the indexing motors upwards for a given period.

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "commands/IndexCommands/ShootLow.hpp"
#include "commands/IndexCommands/Index.hpp"
#include <frc/smartdashboard/SmartDashboard.h>
#include "RobotContainer.h"
#include <frc/Timer.h>
#include <tools/Tools.hh>

using namespace IndexCommands;

ShootLow::ShootLow(IndexSubsystem* indexSubsystem) : subsystem{indexSubsystem}, isFinished{false}, startTime{frcTools::Time::Millis()}{
    // Use addRequirements() here to declare subsystem dependencies.
    AddRequirements(indexSubsystem);
    SetName("Low Shot");
}

// Called when the command is initially scheduled.
void ShootLow::Initialize() {

    frc::SmartDashboard::PutString("Indexing", "Low Shot");

    startTime = frcTools::Time::Millis();
    isFinished = false;

    subsystem->setFeed(0);
    //subsystem->setTopVel(TopIndexConverter(fxMaxRPM*0.5));
    subsystem->setTop(0.55);
    subsystem->setBottom(1);
    IndexCommands::codex = 0;

    compressor.Disable();
}

// Called repeatedly when this Command is scheduled to run
void ShootLow::Execute() {
    if(frcTools::Time::Millis() > startTime + 1500){
    isFinished = true;
    }
}

// Called once the command ends or is interrupted.
void ShootLow::End(bool interrupted) {
    frc::SmartDashboard::PutString("Indexing", "Disabled");
    IndexCommands::codex = 0;
    compressor.EnableDigital();
}

// Returns true when the command should end.
bool ShootLow::IsFinished() {
    return isFinished;
}

The “Shoot High” task functions similarly to the “Shoot Low” task but also includes a delay before firing the balls to allow the flywheel to spin up to speed before firing.

/**
* @file ShootHigh.hpp
* @author Jacob Chisholm
* @brief Index High Shot Command
* @date 2022-03-21
* 
* @copyright Copyright (c) 2022
* 
*/

#include "commands/IndexCommands/ShootHigh.hpp"
#include "Constants.h"
#include "tools/Tools.hh"
#include "commands/IndexCommands/Index.hpp"
#include "frc/smartdashboard/SmartDashboard.h"

IndexCommands::ShootHigh::ShootHigh(IndexSubsystem* sys_index) : index{sys_index}, isFinished{false}, reachedRPM{false} {
    AddRequirements(index);
    SetName("High Shot");
}

// Called when the command is initially scheduled.
void IndexCommands::ShootHigh::Initialize() {

    frc::SmartDashboard::PutString("Indexing", "High Shot");

    index->setFeed(0);
    index->setBottom(0);
    index->setTopVel(c_TalonUPR(6300));
    //index->setTop(1);

    startTime = frcTools::Time::Millis();
    spinUpTime = frcTools::Time::Millis();
    isFinished = false;
    reachedRPM = false;
    IndexCommands::codex = 0;
}

// Called repeatedly when this Command is scheduled to run
void IndexCommands::ShootHigh::Execute() {
    
    if(index->getTopVelocity() > 17000 && reachedRPM == false){
    index->setBottom(1);
    reachedRPM = true;
    spinUpTime = frcTools::Time::Millis();
    }

    if(reachedRPM && (spinUpTime + 666 < frcTools::Time::Millis())){
    index->setFeed(1);
    }

    if(frcTools::Time::Millis() > startTime + 3000){
    isFinished = true;
    }
}

// Called once the command ends or is interrupted.
void IndexCommands::ShootHigh::End(bool interrupted) {
    frc::SmartDashboard::PutString("Indexing", "Disabled");
}

// Returns true when the command should end.
bool IndexCommands::ShootHigh::IsFinished() {
    return isFinished;
}  

Again, an if statement is used as a delay to avoid system lockups.

All code for the Indexing system can be found here, on the 6364 GitHub Repository.