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",
}