Motion Profiling won't work

Hello! So, we are testing motion profiling on a robot, and we are able to feed the points to it, but it doesn’t seem to want to actually utilize these points for anything (as in, it is getting the points, but not moving at all). We’ve made sure that the robot is physically fully functional, so it isn’t a hardware problem. Any help would be appreciated. Thanks :cool:

Something like this is really hard to diagnose without some code. Could you share it?

Have you carefully followed the detailed steps provided in Section 6 of [this document](https://www.ctr-electronics.com/downloads/pdf/Talon SRX Motion Profile Reference Manual.pdf)?

First of all, that entire document has been read in it’s entirety, and we made sure that we added in everything that was specified that we would need. Second of all, here is the code:

package org.usfirst.frc.team839.robot.commands;

import org.usfirst.frc.team839.robot.instrumentation;

import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.command.Command;

public class GoodMotionProfileCommand extends Command
{

	CANTalon _talon = new CANTalon(5);
	CANTalon _talonSlave = new CANTalon(4);
	
	//MotionProfileExample _example = new MotionProfileExample(_talon);
	private CANTalon.MotionProfileStatus _status = new CANTalon.MotionProfileStatus();

	boolean isFinished = false;
	double]] profile;
	public GoodMotionProfileCommand(double]] points)
	{
		System.out.println("GoodMotionProfileCommand constructor");
		profile = points;
	}

	@Override
	protected void initialize()
	{

		System.out.println("GoodMotionProfileCommand initialize");

		_talon.clearMotionProfileTrajectories();
		_talon.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
		_talon.reverseSensor(false);
		
		_talon.changeControlMode(TalonControlMode.MotionProfile);
//		_talonSlave.changeControlMode(TalonControlMode.Follower);
//		_talonSlave.set(5);
		
		_talon.set(CANTalon.SetValueMotionProfile.Disable.value);
		_talon.changeMotionControlFramePeriod(10);
		
		//feed Trajectory Points
		fill(profile,profile.length);
		_talon.set(CANTalon.SetValueMotionProfile.Enable.value);
//		_talon.processMotionProfileBuffer();
	}

	private void fill(double]] profile, int totalCnt) {

		System.out.println("GoodMotionProfileCommand fill");
		/* create an empty point */
		CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint();

		for (int i = 0; i < totalCnt; ++i) {
			/* for each point, fill our structure and pass it to API */
			point.position = profile*[0];
			point.velocity = profile*[1];
			point.timeDurMs = (int) profile*[2];
			point.profileSlotSelect = 0; /* which set of gains would you like to use? */
			point.velocityOnly = true; /* set true to not do any position
										 * servo, just velocity feedforward
										 */
			point.zeroPos = false;
			if (i == 0)
				point.zeroPos = true; /* set this to true on the first point */

			point.isLastPoint = false;
			if ((i + 1) == totalCnt)
				point.isLastPoint = true; /* set this to true on the last point  */

			_talon.pushMotionProfileTrajectory(point);
		}
	}
	

	
	@Override
	protected void execute()
	{
		_talon.getMotionProfileStatus(_status);

		if(_status.hasUnderrun)
		{
			System.out.println("GoodMotionProfileCommand _status.hasUnderrun");

			//load points
			_talon.clearMotionProfileHasUnderrun();;
			_talon.processMotionProfileBuffer();
		}
		
		if (_status.activePointValid && _status.activePoint.isLastPoint)
		{
			System.out.println("GoodMotionProfileCommand  _status.activePoint.isLastPoint");
			_talon.set(CANTalon.SetValueMotionProfile.Hold.value);
			isFinished = true;

		}
		else
		{
			instrumentation.process(_status);
			_talon.set(CANTalon.SetValueMotionProfile.Enable.value);
			_talon.processMotionProfileBuffer();
		}

	}
	@Override
	protected boolean isFinished()
	{
		return isFinished;
	}

	@Override
	protected void end()
	{
		System.out.println("GoodMotionProfileCommand end");
		_talon.set(CANTalon.SetValueMotionProfile.Hold.value);
		_talon.clearMotionProfileTrajectories();
		_talon.set(CANTalon.SetValueMotionProfile.Disable.value);
		_talon.changeControlMode(TalonControlMode.Voltage);
	}

	@Override
	protected void interrupted()
	{
		
	}
}

Did you set the gains?

Screenshot the selftest in the web based config.

What firmware version?

Firmware is up to date, and we added the Feed-forward gains as well as P, I, and D gains. We finally have the motors turning now :slight_smile: However, there is a new problem: we’ve had to ramp up the acceleration value to something that sounds unreasonable.