Robot.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkBase.ControlType;
import com.revrobotics.CANSparkBase.PersistMode;
import com.revrobotics.CANSparkBase.ResetMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.sparkconfig.SparkFlexConfig;
import com.revrobotics.sparkconfig.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.sparkconfig.MAXMotionConfig.MAXMotionPositionMode;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The methods in this class are called automatically corresponding to each
 * mode, as described in
 * the TimedRobot documentation. If you change the name of this class or the
 * package after creating
 * this project, you must also update the Main.java file in the project.
 */
public class Robot extends TimedRobot {
  CANSparkFlex m_flex = new CANSparkFlex(1, MotorType.kBrushless);

  public Robot() {
    SparkFlexConfig config = new SparkFlexConfig();
    config
      .closedLoopConfig.pidf(0.0001, 0, 0, 0.00015)
                       .feedbackSensor(FeedbackSensor.kMainEncoder)
      .maxMotionConfig.maxAcceleration(500)
                      .maxVelocity(500)
                      .allowedClosedLoopError(1)
                      .positionMode(MAXMotionPositionMode.kMAXMotionTrapezoidal);
    config.signalsConfig.primaryEncoderPositionPeriodMs(2);
    m_flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
  }

  @Override
  public void robotPeriodic() {
    SmartDashboard.putNumber("position", m_flex.getEncoder().getPosition());
    SmartDashboard.putNumber("velocity", m_flex.getEncoder().getVelocity());
  }

  @Override
  public void autonomousInit() {
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopInit() {
    m_flex.getClosedLoopController().setReference(500, ControlType.kMAXMotionVelocityControl);
  }

  @Override
  public void teleopPeriodic() {
  }

  @Override
  public void disabledInit() {
    m_flex.getEncoder().setPosition(0);
  }

  @Override
  public void disabledPeriodic() {
  }

  @Override
  public void testInit() {
  }

  @Override
  public void testPeriodic() {
  }

  @Override
  public void simulationInit() {
  }

  @Override
  public void simulationPeriodic() {
  }
}