First Time Doing Robot Characterization

Our team is trying to get robot characterization going and we think we’re almost there but when we deployed the code generated by the python Robot Characterization tool I see the following on the driver station, complaining about the Pigeon IMU. By the way we’re using 4 TalonSRX’s and two of those Talons have quad encoders attached, one on each side (left front and right front), along with a Pigeon IMU as our gyro:

I double checked everything with the Phoenix Tuner, and the Pigeon IMU checks out and we were able to do a self-tests on it:

Still, we were able to connect with the Robot Characterization tool and get data back, but not sure if it’s valid data (since it’s our first time trying this out):

When we do analysis of our data I see the following pop up:

Anyone have any inputs? We would appreciate it. Thank you.

You might have already checked these things, but…

  1. Has the firmware been upgraded on the Pigeon? If you’re using 2020 Phoenix libraries, you also have to be using 2020 firmware.

  2. Is the CAN ID set in Phoenix Tuner the same as the int passed into the PigeonImu constructor?

  1. Yes, we had update the IMU firmware through Phoenix Tuner, we downloaded the latest firmware from CTRE site. We are also using the 2020 Phoenix library (also I think the library config that comes with the Robot Characterization tool is default to 2020 version, in the latest version).

  2. Yes, we checked the CAN ID for the IMU; the Pigeon IMU is actually connected via a TalonSRX, but the CAN communication checks out. We use the same ID value, in our case 42, in setting up our Robot Characterization settings.

Thank you.

Can’t help with the gyro issue, but your data look just fine, except it is not clear what units they are in.

Okay thanks. And if you’re referring to the what unit, I think you’re referencing wheel diameter? In that case it’s in inches.

Are you attaching your Pigeon IMU to a motor controller or are you putting it directly on the CAN bus? Maybe frc-characterization is expecting whatever you’re not doing.

That would indicate that your top speed is on the order of 60 inches per second, or 5 feet per second. That seems awfully slow (unless this is the low gear of a two-speed drive). Be sure you haven’t specified your post-encoder gearing incorrectly.

We are attaching into the motor controller, the TalonSRX in this case. We could try it directly onto the CAN bus and see if it’ll make a difference.

That sounds about right. We have a gear ratio of 1 at the moment for testing. When we were testing it we were trying to go slow just to make sure everything is fine… where do you specify the post-encoder gearing? Sorry, still new at this.

For the Talon project, you wrap it into your encoderEPR setting.

I’d check the code generated by frc-characterization first and see what it expects. It might default to CAN mode. You can fix the code manually and redeploy before running the characterization again.

I don’t understand @Oblarg comment, but given that he’s the expert, I bet it’s important.

Sorry not sure what you mean by wrap it into the encoderEPR setting.

As described in the comments, encoderEPR should be encoder edges per revolution of the wheels. That means it must take into account any gearing between the encoder and the wheel shafts.

We have used Pigeon IMU connected to the CAN Bus directly but it should work just fine when hooked up to a Talon. Could you share the project generated by the Characterization Tool and people here might be actually examine the code better :slight_smile: I would also look into what @Oblarg mentioned as he is a pro at this.

Also a tip: the tool will invert the gyro automatically for testing. Make sure you don’t forget to invert it when you type out your own auto. That was a small mistake that resulted in hours of code examination :smiley:

Okay, the way we understood it when we first read that comment was that since we’re using a 1:1 gear ratio at the moment and we’re using TalonSRX, we’d put down 4096 as the setting. (4096 tick was specified in the CTRE documentation).

If there’s a 1:1 gear ratio between the encoder and the wheel shafts, that’s correct. Where is your encoder mounted?

Oh, thank you for the the invert gyro info, i’m sure that’ll save us a lot of headache later on. And here is the generated robot.java file we’re deploying to the robot:

/**
 * This is a very simple robot program that can be used to send telemetry to
 * the data_logger script to characterize your drivetrain. If you wish to use
 * your actual robot code, you only need to implement the simple logic in the
 * autonomousPeriodic function and change the NetworkTables update rate
 */

package dc;

import java.util.function.Supplier;

import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;

import com.ctre.phoenix.sensors.PigeonIMU;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SPI;

import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends TimedRobot {

  static private double WHEEL_DIAMETER = 6;
  static private double ENCODER_EDGES_PER_REV = 4096;
  static private int PIDIDX = 0;

  Joystick stick;
  DifferentialDrive drive;

  WPI_TalonSRX leftMaster;
  WPI_TalonSRX rightMaster;

  Supplier<Double> leftEncoderPosition;
  Supplier<Double> leftEncoderRate;
  Supplier<Double> rightEncoderPosition;
  Supplier<Double> rightEncoderRate;
  Supplier<Double> gyroAngleRadians;

  NetworkTableEntry autoSpeedEntry =
      NetworkTableInstance.getDefault().getEntry("/robot/autospeed");
  NetworkTableEntry telemetryEntry =
      NetworkTableInstance.getDefault().getEntry("/robot/telemetry");
  NetworkTableEntry rotateEntry =
    NetworkTableInstance.getDefault().getEntry("/robot/rotate");

  double priorAutospeed = 0;
  Number[] numberArray = new Number[10];

  @Override
  public void robotInit() {
    if (!isReal()) SmartDashboard.putData(new SimEnabler());

    stick = new Joystick(0);

    leftMaster = new WPI_TalonSRX(15);
    leftMaster.setInverted(false);
    leftMaster.setSensorPhase(true);
    leftMaster.setNeutralMode(NeutralMode.Brake);

    rightMaster = new WPI_TalonSRX(45);
    rightMaster.setInverted(false);
    rightMaster.setSensorPhase(false);
    rightMaster.setNeutralMode(NeutralMode.Brake);

    WPI_TalonSRX leftSlave0 = new WPI_TalonSRX(6);
    leftSlave0.setInverted(false);
    leftSlave0.follow(leftMaster);
    leftSlave0.setNeutralMode(NeutralMode.Brake);

    WPI_TalonSRX rightSlave0 = new WPI_TalonSRX(12);
    rightSlave0.setInverted(false);
    rightSlave0.follow(rightMaster);
    rightSlave0.setNeutralMode(NeutralMode.Brake);

    //
    // Configure gyro
    //

    // Note that the angle from the NavX and all implementors of wpilib Gyro
    // must be negated because getAngle returns a clockwise positive angle
    // Uncomment for Pigeon
    PigeonIMU pigeon = new PigeonIMU(42);
    gyroAngleRadians = () -> {
      // Allocating a new array every loop is bad but concise
      double[] xyz = new double[3];
      pigeon.getAccumGyro(xyz);
      return Math.toRadians(xyz[2]);
    };

    //
    // Configure drivetrain movement
    //

    drive = new DifferentialDrive(leftMaster, rightMaster);

    drive.setDeadband(0);

    //
    // Configure encoder related functions -- getDistance and getrate should
    // return units and units/s
    //

    double encoderConstant =
        (1 / ENCODER_EDGES_PER_REV) * WHEEL_DIAMETER * Math.PI;

    leftMaster.configSelectedFeedbackSensor(
        FeedbackDevice.QuadEncoder,
        PIDIDX, 10
    );
    leftEncoderPosition = ()
        -> leftMaster.getSelectedSensorPosition(PIDIDX) * encoderConstant;
    leftEncoderRate = ()
        -> leftMaster.getSelectedSensorVelocity(PIDIDX) * encoderConstant *
               10;

    rightMaster.configSelectedFeedbackSensor(
        FeedbackDevice.QuadEncoder,
        PIDIDX, 10
    );
    rightEncoderPosition = ()
        -> rightMaster.getSelectedSensorPosition(PIDIDX) * encoderConstant;
    rightEncoderRate = ()
        -> rightMaster.getSelectedSensorVelocity(PIDIDX) * encoderConstant *
               10;

    // Reset encoders
    leftMaster.setSelectedSensorPosition(0);
    rightMaster.setSelectedSensorPosition(0);

    // Set the update rate instead of using flush because of a ntcore bug
    // -> probably don't want to do this on a robot in competition
    NetworkTableInstance.getDefault().setUpdateRate(0.010);
  }

  @Override
  public void disabledInit() {
    System.out.println("Robot disabled");
    drive.tankDrive(0, 0);
  }

  @Override
  public void disabledPeriodic() {}

  @Override
  public void robotPeriodic() {
    // feedback for users, but not used by the control program
    SmartDashboard.putNumber("l_encoder_pos", leftEncoderPosition.get());
    SmartDashboard.putNumber("l_encoder_rate", leftEncoderRate.get());
    SmartDashboard.putNumber("r_encoder_pos", rightEncoderPosition.get());
    SmartDashboard.putNumber("r_encoder_rate", rightEncoderRate.get());
  }

  @Override
  public void teleopInit() {
    System.out.println("Robot in operator control mode");
  }

  @Override
  public void teleopPeriodic() {
    drive.arcadeDrive(-stick.getY(), stick.getX());
  }

  @Override
  public void autonomousInit() {
    System.out.println("Robot in autonomous mode");
  }

  /**
   * If you wish to just use your own robot program to use with the data logging
   * program, you only need to copy/paste the logic below into your code and
   * ensure it gets called periodically in autonomous mode
   *
   * Additionally, you need to set NetworkTables update rate to 10ms using the
   * setUpdateRate call.
   */
  @Override
  public void autonomousPeriodic() {

    // Retrieve values to send back before telling the motors to do something
    double now = Timer.getFPGATimestamp();

    double leftPosition = leftEncoderPosition.get();
    double leftRate = leftEncoderRate.get();

    double rightPosition = rightEncoderPosition.get();
    double rightRate = rightEncoderRate.get();

    double battery = RobotController.getBatteryVoltage();

    double leftMotorVolts = leftMaster.getMotorOutputVoltage();
    double rightMotorVolts = rightMaster.getMotorOutputVoltage();

    // Retrieve the commanded speed from NetworkTables
    double autospeed = autoSpeedEntry.getDouble(0);
    priorAutospeed = autospeed;

    // command motors to do things
    drive.tankDrive(
      (rotateEntry.getBoolean(false) ? -1 : 1) * autospeed, autospeed,
      false
    );

    // send telemetry data array back to NT
    numberArray[0] = now;
    numberArray[1] = battery;
    numberArray[2] = autospeed;
    numberArray[3] = leftMotorVolts;
    numberArray[4] = rightMotorVolts;
    numberArray[5] = leftPosition;
    numberArray[6] = rightPosition;
    numberArray[7] = leftRate;
    numberArray[8] = rightRate;
    numberArray[9] = gyroAngleRadians.get();

    telemetryEntry.setNumberArray(numberArray);
  }
}

and this is the python configs we’re using in the characterization tool:

{
    # Class names of motor controllers used.
    # Options:
    # 'WPI_TalonSRX'
    # 'WPI_TalonFX' (for Falcon 500 motors)
    # 'WPI_VictorSPX'
    # Note: The first motor on each side should always be a Talon SRX/FX, as the
    # VictorSPX does not support encoder connections
    "rightControllerTypes": ["WPI_TalonSRX", "WPI_TalonSRX"],
    "leftControllerTypes": ["WPI_TalonSRX", "WPI_TalonSRX"],
    # Note: The first id in the list of ports should be the one with an encoder
    # Ports for the left-side motors
    "leftMotorPorts": [15, 6],
    # Ports for the right-side motors
    "rightMotorPorts": [45, 12],
    # Inversions for the left-side motors
    "leftMotorsInverted": [False, False],
    # Inversions for the right side motors
    "rightMotorsInverted": [False, False],
    # Wheel diameter (in units of your choice - will dictate units of analysis)
    "wheelDiameter": 6,
    # If your robot has only one encoder, set all right encoder fields to `None`
    # Encoder edges-per-revolution (*NOT* cycles per revolution!)
    # This value should be the edges per revolution *of the wheels*, and so
    # should take into account gearing between the encoder and the wheels
    "encoderEPR": 4096,
    # Whether the left encoder is inverted
    "leftEncoderInverted": True,
    # Whether the right encoder is inverted:
    "rightEncoderInverted": False,
    # Your gyro type (one of "NavX", "Pigeon", "ADXRS450", "AnalogGyro", or "None")
    "gyroType": "Pigeon",
    # Whatever you put into the constructor of your gyro
    # Could be:
    # "SPI.Port.kMXP" (MXP SPI port for NavX or ADXRS450),
    # "SerialPort.Port.kMXP" (MXP Serial port for NavX),
    # "I2C.Port.kOnboard" (Onboard I2C port for NavX),
    # "0" (Pigeon CAN ID or AnalogGyro channel),
    # "new WPI_TalonSRX(3)" (Pigeon on a Talon SRX),
    # "leftSlave" (Pigeon on the left slave Talon SRX/FX),
    # "" (NavX using default SPI, ADXRS450 using onboard CS0, or no gyro)
    "gyroPort": "42",
}
1 Like

The encoders are mounted on top of the TalonSRX’s right now, and the SRX’s are on top of a makeshift wooden board on a AndyMark chassis.

As in, to which shafts are the encoders coupled?

I am current not at school with the robot right now but i’ll get back to your question asap.