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:
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[i][0];
point.velocity = profile[i][1];
point.timeDurMs = (int) profile[i][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()
{
}
}