CTRE Phoenix Framework TalonSRX Velocity Control is Unimplemented

So I have a few encoders on a Mecanum Drive I’m trying to configure for velocity control. Here’s my current custom class I built for it. It’s more of a port of the regular MecanumDrive so that it uses TalonSRX directly and can engage with the velocity control.

package org.usfirst.frc.team135.robot.util;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.drive.RobotDriveBase;
import edu.wpi.first.wpilibj.drive.Vector2d;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
import edu.wpi.first.wpilibj.hal.HAL;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;

public class MecanumDriveVelocity extends RobotDriveBase{

	private static int instances = 0;
	
	private boolean m_reported = false;

    private WPI_TalonSRX] mTalons;
    
    private static final int 
    	FRONT_LEFT = 0,
    	REAR_LEFT = 1,
    	FRONT_RIGHT = 2,
    	REAR_RIGHT = 3;
    
    enum Side
    {
    	LEFT,
    	RIGHT
    }
	
	public MecanumDriveVelocity(WPI_TalonSRX] talons, Side invertedSide)
	{
		mTalons = talons;
		
		for (WPI_TalonSRX talon : mTalons)
		{
			addChild(talon);
		}
		
		
		/* Motors are inverted already by main drive functions.
		if (invertedSide == Side.LEFT)
		{
			talons[FRONT_LEFT].setInverted(true);
			talons[REAR_LEFT].setInverted(true);
		}
		else if(invertedSide == Side.RIGHT)
		{
			talons[FRONT_RIGHT].setInverted(true);
			talons[REAR_RIGHT].setInverted(true);
		}
		*/
		instances++;
		
		setName("MecanumDriveVelocity", instances);
		
	}
	
	@Override
	public void initSendable(SendableBuilder builder) {
		builder.setSmartDashboardType("MecanumDrivevelocity");
		builder.addDoubleProperty("Front Left Motor Speed", mTalons[FRONT_LEFT]::get,
		        mTalons[FRONT_LEFT]::set);
		
		builder.addDoubleProperty("Front Right Motor Speed", mTalons[FRONT_RIGHT]::get,
		        mTalons[FRONT_RIGHT]::set);
		
		builder.addDoubleProperty("Rear Left Motor Speed", mTalons[REAR_LEFT]::get,
		        mTalons[REAR_LEFT]::set);
		
		builder.addDoubleProperty("Rear Right Motor Speed", mTalons[REAR_RIGHT]::get,
		        mTalons[REAR_RIGHT]::set);
		
	}
	
	public void driveCartesian(double x, double y, double zRotation, double fieldOrientation, double maxVel)
	{
	    if (!m_reported) {
	        HAL.report(tResourceType.kResourceType_RobotDrive, 4,
	                   tInstances.kRobotDrive_MecanumCartesian);
	        m_reported = true;
	      }

	      y = limit(y);
	      y = applyDeadband(y, m_deadband);

	      x = limit(x);
	      x = applyDeadband(x, m_deadband);

	      // Compensate for gyro angle.
	      Vector2d input = new Vector2d(y, x);
	      input.rotate(fieldOrientation);

	      double] wheelSpeeds = new double[4];
	      wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
	      wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y + zRotation;
	      wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
	      wheelSpeeds[MotorType.kRearRight.value] = -input.x - input.y + zRotation;

	      normalize(wheelSpeeds);

	      for (int i = 0; i < 4; i++)
	      {
	    	  mTalons*.set(ControlMode.Velocity, wheelSpeeds* * maxVel * m_maxOutput);
	      }

	      m_safetyHelper.feed();
	}
	
	public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double maxVel) {
	    driveCartesian(ySpeed, xSpeed, zRotation, 0.0, maxVel);
	}
	
	public void drivePolar(double magnitude, double angle, double zRotation, double maxVel) 
	{
	    if (!m_reported) 
	    {
	    	HAL.report(tResourceType.kResourceType_RobotDrive, 4, tInstances.kRobotDrive_MecanumPolar);
		      m_reported = true;
		}

		driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
		                   magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0, maxVel);
	}

	@Override
	public void stopMotor() {
		for(WPI_TalonSRX talon : mTalons)
		{
			talon.set(0.0);
		}
	}

	@Override
	public String getDescription() {
	    return "MecanumDriveVelocity";
	}

}

I executed the code and found that response is absent. I later found out that Velocity mode is not implemented in the Phoenix Framework as of the time this was posted.

I find that I can’t post pictures so I’ll just link the relevant page and line numbers within github.

The relevant class is BaseMotorController as I’ve linked. Lines 108 to 139 are of particular interest. Here they implement the mode enumeration you must pass to the TalonSRX.set function. Note that velocity control is blank.

I’m thinking we can work around this with a wpilibj PIDController and some encoders. What do you all think?

I’m not really sure how speed control on a mecanum would even work as I’m new to it. Any advice would be appreciated.

Thanks all.**