I think I'm going to run these parts in the execute method to see if there is a part that is taking too long
Code:
private double t_loopstart, t_loopend, t_getCX, t_getSpin, t_setSetpoint;
Code:
// Called repeatedly when this Command is scheduled to run
protected void execute() {
t_loopstart = timeSinceInitialized(); // seconds since startup
double cX = Robot.vision.my_Get_Xcenter();
t_getCX = timeSinceInitialized(); // seconds since startup
double spin = Robot.vision.my_SpinToTarget_Lookup(cX);
t_getSpin = timeSinceInitialized(); // seconds since startup
m_setpoint = Robot.driveTrain.my_Get_Gyro() + (spin*-1);
t_setSetpoint = timeSinceInitialized(); // seconds since startup
//getPIDController().setSetpointRelative(spin*-1);
getPIDController().setSetpoint(m_setpoint);
t_loopend = timeSinceInitialized(); // seconds since startup
SmartDashboard.putNumber("t_loopstart", t_loopstart);
SmartDashboard.putNumber("t_getCX", t_getCX);
SmartDashboard.putNumber("t_getSpin", t_getSpin);
SmartDashboard.putNumber("t_setSetpoint", t_setSetpoint);
SmartDashboard.putNumber("t_loopend", t_loopend);
}
Then plot on line graphs