ChassiTank 2025/2026

Code developed by Megazord team #7563

Robot Systems Overview

ChassiTank Drive: Basic system of three wheels and two motors in each side.

1. Arcade Drive: This method use two axes of controller, where one control the speed of robot and the other controls de robot direction.

 /**
   * 
   * @param forward
   * @param turn
   */
  public void drive(double forward, double turn) 
  { 
    double leftSpeed = (forward * speed) + (turn * speed);
    double rightSpeed = (forward * speed) - (turn * speed);

    leftMotor.set(leftSpeed);
    rightMotor.set(rightSpeed);
  }

2. Default Command: This method is used every time of teleop mode, so we called it like default.

we also create a deadband to solve drift issue in a controller

  /** The container for the robot. Contains subsystems, OI devices, and commands. */
  public RobotContainer() 
  {
    chooserAuto.setDefaultOption("nenhum", null);

    chooserAuto.addOption("AutoA", m_autoA);
    chooserAuto.addOption("AutoB", m_autoB);

    SmartDashboard.putData("Autonomus", chooserAuto);
    // Configure the trigger bindings
    intakeCmd.addRequirements(inTakeSubsystem);
    configureBindings();
    drive.setDefaultCommand(new RunCommand(()-> drive.drive
                                          (
                                            MathUtil.applyDeadband((m_driverController.getLeftY()), 0.1),
                                            MathUtil.applyDeadband((m_driverController.getRightX()),0.1)), 
                                            drive));
  }

Intake System: Basic intake using a neo motor to get a boll game piece

1. Output Move: Move with a determinated output

/**
   * 
   * @param direction direção do motor
   * @param speed velocidade do motor
   */
  public void runMotorInTake(Boolean direction, double speed)
  {
    if(direction)
    {
      inTakeMotor.set(speed);
    }
    else
    {
      inTakeMotor.set(-speed);
    }
  }

2. Position PID: It´s also include a SparkeMax closedloop to move motor for a determinated position with a good accuracy

  /**
   * 
   * @param position argument to move motor
   */

   public void runToPosition(double position)
   {
     inTakeController.setReference(position,ControlType.kPosition);
   }

NT Speeed choose: Virtual booleans to choose speed level

This code also include a system to disable the last speed, when a new one is choosed.

1. Create Speed booleans:

Method SmartDashboard.putBoolean(string name, boolean default)

    //speed controls
    SmartDashboard.putBoolean("speed/30%", false);
    SmartDashboard.putBoolean("speed/40%", true); // default speed 
    SmartDashboard.putBoolean("speed/50%", false);
    SmartDashboard.putBoolean("speed/60%", false);
    SmartDashboard.putBoolean("speed/70%", false);
    SmartDashboard.putBoolean("speed/80%", false);
    SmartDashboard.putBoolean("speed/90%", false);
    SmartDashboard.putBoolean("speed/100%", false);

2. Get New Speed value:

Compare speed value with smartDashboard and update code based on NT

 if(SmartDashboard.getBoolean("speed/100%", false) && lastSpeed != 1)
    {
      speed = 1;
    } if (SmartDashboard.getBoolean("speed/90%", false) && lastSpeed != 0.9)
    {
      speed = 0.9;
    } if (SmartDashboard.getBoolean("speed/80%", false) && lastSpeed != 0.8)
    {
      speed = 0.8;
    } if (SmartDashboard.getBoolean("speed/70%", false) && lastSpeed != 0.7)
    {
      speed = 0.7;
    } if (SmartDashboard.getBoolean("speed/60%", false) && lastSpeed != 0.6)
    {
      speed = 0.6;
    } if (SmartDashboard.getBoolean("speed/50%", false) && lastSpeed != 0.5)
    {
      speed = 0.5;
    } if (SmartDashboard.getBoolean("speed/40%", false) && lastSpeed != 0.4)
    {
      speed = 0.4;
    } if (SmartDashboard.getBoolean("speed/30%", false) && lastSpeed != 0.3)
    {
      speed = 0.3;
    }  

3. Uncheck Last Speed:

Compare the new speed value with the last, if is diferent, uncheck the last speed on NT and update the last speed to speed

  if(lastSpeed != Math.abs(speed))
    {
      int lastSpeedPercent = (int)(lastSpeed * 100);

      switch (lastSpeedPercent) {
        case 30:
          SmartDashboard.putBoolean("speed/30%", false);
          break;
        case 40:
          SmartDashboard.putBoolean("speed/40%", false);
          break;
        case 50:
          SmartDashboard.putBoolean("speed/50%", false);
          break;
        case 60:
          SmartDashboard.putBoolean("speed/60%", false);
          break;
        case 70:
          SmartDashboard.putBoolean("speed/70%", false);
          break;
        case 80:
          SmartDashboard.putBoolean("speed/80%", false);
          break;
        case 90:
          SmartDashboard.putBoolean("speed/90%", false);
          break;
        case 100:
          SmartDashboard.putBoolean("speed/100%", false);
          break;
        default:
          break;
      }
      for(int i=0; i < 3; i++)
      {
        System.out.println("speed seted to " + speed*100 + "%" );
      }
      lastSpeed = speed;
    }

* Aditional Infos: If is the controls inverted, change the value of boolean controls/isInverted on NT

Note: You can see all wpi documentation in https://docs.wpilib.org/en/stable/index.html