CANSparkMax Position PID Java

We are attempting to setup a CANSparkMax under position control, but are unable to get it to seek the encoder setpoint of -20. We based the -20 value on the range we get of our arm from 0 to -50 when reading IntakeRotateEncoder.getPosition(). We were able to get the motors to run, but it would continue to drive in only 1 direction and not switch states after it passed our setpoint. We are using the internal NEO motor encoder, but cannot seem to figure out what we are doing wrong.

We tried to follow the Rev example here.

We were able to get the SparkMaxPIDcontroller to work in velocity mode for our shooter and intake wheels (ShooterPID & IntakePID), but have failed to code one to seek the correct position when switching to position control. We checked the relative encoder set conversion factor because we thought it might have been adjusting either our setpoint or our encoder reading, but it is registering at 1.

Here is our code and the piece we are stuck on is the IntakeRotateLeft & IntakeRotateRight motors. We were attempting to use a Controller2 A button to use the IntakeRotatePID to travel to -20 setpoint. Thanks!

// 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 edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import com.revrobotics.RelativeEncoder;
//import com.revrobotics.;
import com.revrobotics.SparkMaxPIDController;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the functions 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 build.gradle file in the
 * project.
 */
public class Robot extends TimedRobot {
  public static final CANSparkMax Spark1 = new CANSparkMax(11, MotorType.kBrushless);
  public static final CANSparkMax Spark2 = new CANSparkMax(10, MotorType.kBrushless);
  public static final CANSparkMax Spark3 = new CANSparkMax(12, MotorType.kBrushless);
  public static final MotorControllerGroup Left = new MotorControllerGroup(Spark1, Spark2, Spark3);

  public static final CANSparkMax Spark4 = new CANSparkMax(7, MotorType.kBrushless);
  public static final CANSparkMax Spark5 = new CANSparkMax(8, MotorType.kBrushless);
  public static final CANSparkMax Spark6 = new CANSparkMax(9, MotorType.kBrushless);
  public static final MotorControllerGroup Right = new MotorControllerGroup(Spark4, Spark5, Spark6);

  public static final CANSparkMax Shooter = new CANSparkMax(17, MotorType.kBrushless);
  private RelativeEncoder ShooterEncoder;
  private SparkMaxPIDController ShooterPID;
  public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM;
 

  public static final CANSparkMax Intake = new CANSparkMax(2, MotorType.kBrushless);
  private RelativeEncoder IntakeEncoder;
  private SparkMaxPIDController IntakePID;

  public static final CANSparkMax hangRight = new CANSparkMax(1, MotorType.kBrushless);
  public static final CANSparkMax hangLeft = new CANSparkMax(19, MotorType.kBrushless);

  public static final CANSparkMax IntakeRotateRight = new CANSparkMax(4, MotorType.kBrushless);
  public static final CANSparkMax IntakeRotateLeft = new CANSparkMax(15, MotorType.kBrushless);
  private SparkMaxPIDController IntakeRotatePID;

  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  private final DifferentialDrive m_robotDrive
      = new DifferentialDrive(Left, Right);
  public static final XboxController Controller1 = new XboxController(0);
  public static final XboxController Controller2 = new XboxController(1);
  
  private final DoubleSolenoid shift = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,0,1);
  private final DoubleSolenoid a = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,2,3);
  private final DoubleSolenoid greenEggs = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,4,5);
  private final DoubleSolenoid andHam = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,6,7);
  private ADIS16470_IMU gyro = new ADIS16470_IMU();
  private RelativeEncoder m_encoder;
  private RelativeEncoder IntakeRotateEncoder;
  private RelativeEncoder HangEncoder;
  private final Timer m_timer = new Timer();


  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  @Override
  public void robotInit() {
    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
    m_chooser.addOption("My Auto", kCustomAuto);
    SmartDashboard.putData("Auto choices", m_chooser);
    m_encoder = Spark1.getEncoder();
    ShooterEncoder = Shooter.getEncoder();
    IntakeEncoder = Intake.getEncoder();
    IntakeRotateEncoder = IntakeRotateLeft.getEncoder();
    HangEncoder = hangLeft.getEncoder();
    //test
    ShooterPID = Shooter.getPIDController();
    IntakePID = Intake.getPIDController();
    IntakeRotatePID = IntakeRotateLeft.getPIDController();

    IntakeRotateRight.follow(IntakeRotateLeft, true);

     // PID coefficients
     kP = 5e-3; //-4 was pretty good
     kI = 0.0; // 0
     kD = 1.0; //1 was pretty good
     kIz = 0; 
     kFF = 0.000015; 
     kMaxOutput = 1; 
     kMinOutput = -1;
     maxRPM = 5700;
 
     // set PID coefficients
     ShooterPID.setP(0.00021);//.001
     ShooterPID.setI(0);//
     ShooterPID.setD(0);//
     ShooterPID.setIZone(0);
     ShooterPID.setFF(0.00017);//.00015
     ShooterPID.setOutputRange(-1, 0);
     IntakePID.setP(0.00021);
     IntakePID.setI(0);
     IntakePID.setD(0);
     IntakePID.setIZone(0);
     IntakePID.setFF(0.00017);
     IntakePID.setOutputRange(-1, 0);

    IntakeRotatePID.setP(0.001);
    IntakeRotatePID.setI(0);
    IntakeRotatePID.setD(0);
    IntakeRotatePID.setIZone(0);
    IntakeRotatePID.setFF(0.0001);
    IntakeRotatePID.setOutputRange(-1, 1);
  }

  /**
   * This function is called every robot packet, no matter the mode. Use this for items like
   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
   *
   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
   * SmartDashboard integrated updating.
   */
  @Override
  public void robotPeriodic() {

    System.out.println("Gyro" + gyro.getAngle());
    System.out.println(" Intake Rotate Encoder" + IntakeRotateEncoder.getPosition());
    System.out.println("Encoder" + m_encoder.getPosition());
    System.out.println("HangEncoder" + HangEncoder.getPosition());
    System.out.println("ShooterEncoder" + ShooterEncoder.getVelocity());
    SmartDashboard.putNumber("Gyro", gyro.getAngle());
    SmartDashboard.putNumber("Intake Rotate Encoder", IntakeRotateEncoder.getPosition());
    SmartDashboard.putNumber("Encoder", m_encoder.getPosition());
    SmartDashboard.putNumber("HangEncoder", HangEncoder.getPosition());
    SmartDashboard.putNumber("ShooterEncoder", ShooterEncoder.getVelocity());
    SmartDashboard.putNumber("PositionConversionFactor", IntakeRotateLeft.getEncoder().getPosition());
  }

  /**
   * This autonomous (along with the chooser code above) shows how to select between different
   * autonomous modes using the dashboard. The sendable chooser code works with the Java
   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
   * uncomment the getString line to get the auto name from the text box below the Gyro
   *
   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
   * below with additional strings. If using the SendableChooser make sure to add them to the
   * chooser code above as well.
   */
  @Override
  public void autonomousInit() {
    m_autoSelected = m_chooser.getSelected();
    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
    System.out.println("Auto selected: " + m_autoSelected);
    m_timer.reset();
    m_timer.start();
    gyro.reset();
  m_encoder.setPosition(0);
  }

  /** This function is called periodically during autonomous. */
  @Override
  public void autonomousPeriodic() {
    switch (m_autoSelected) {
      case kCustomAuto:
        // Put custom auto code here  
     
        break;
      case kDefaultAuto:
      default:
        // Put default auto code here
        break;
    }}
  

  /** This function is called once when teleop is enabled. */
  @Override
  public void teleopInit() {}

  /** This function is called periodically during operator control. */
  @Override
  public void teleopPeriodic() {
    m_robotDrive.arcadeDrive(-Controller1.getRawAxis(4), Controller1.getRawAxis(1));

    if (Controller1.getLeftBumper()) {
      shift.set(Value.kReverse);
    } else {
      shift.set(Value.kForward);         
    }


    //high shooter - shooter and intake forward
    //right trigger
    if (Controller2.getRawAxis(3)>.1) {
      double setPoint = -4300;
      ShooterPID.setReference(setPoint, CANSparkMax.ControlType.kVelocity);
      IntakePID.setReference(setPoint, CANSparkMax.ControlType.kVelocity);}
      //Intake.set(-1.0);}
    //low shooter - shooter forward and intake backwards
    //left trigger
    else if (Controller2.getRawAxis(2)>.1) {
      Shooter.set(0);
      Intake.set(1.0);}
    //intake cargo-left trigger
    else if (Controller1.getRawAxis(2)>.1) {
      Shooter.set(0.5);
      Intake.set(-1.0);}
    else{
        Shooter.set(0);
        Intake.set(0);
        double setPoint = 0;
        }

   
  // if (Controller2.getBButton()){
  //     double setPoint = -5700;
  //   ShooterPID.setReference(setPoint, CANSparkMax.ControlType.kVelocity);}
  //   else{
  //     double setPoint = 0;
  //     //ShooterPID.setReference(setPoint, CANSparkMax.ControlType.kVelocity); removing to eliminate slow down jerk
  //     Shooter.set(0);
  //   }
 


      //hanging-left joystick
      if (Math.abs(Controller2.getRawAxis(5))>.2){
        hangLeft.set(Controller2.getRawAxis(5));
        hangRight.set(-Controller2.getRawAxis(5));}
        else{
          hangLeft.set(0);
          hangRight.set(0);
        }
       
        if(Controller2.getAButton()) {
          double pidsetpoint = -20;
        IntakeRotatePID.setReference(pidsetpoint, CANSparkMax.ControlType.kPosition);
      }
        else{IntakeRotateLeft.set(0);}
        /* 
      //Intake Rotate-left joystick with soft limits
      if((IntakeRotateEncoder.getPosition()<-50 && Controller2.getRawAxis(1)>0)|(IntakeRotateEncoder.getPosition()>-5 && Controller2.getRawAxis(1)<0)){
        IntakeRotateLeft.set(0);
        //IntakeRotateRight.set(0);
      }
      else if(Math.abs(Controller2.getRawAxis(1))>.2){
        IntakeRotateLeft.set(-Controller2.getRawAxis(1));
        //IntakeRotateRight.set(Controller2.getRawAxis(1));
      }
      else if(Controller2.getAButton()) {
        IntakeRotateLeft.set(0);
        //IntakeRotateRight.set(0);
      }
      else {
        IntakeRotateLeft.set(0);
        //IntakeRotateRight.set(0);
      }
  */



  }

  /** This function is called once when the robot is disabled. */
  @Override
  public void disabledInit() {}

  /** This function is called periodically when disabled. */
  @Override
  public void disabledPeriodic() {}

  /** This function is called once when test mode is enabled. */
  @Override
  public void testInit() {}

  /** This function is called periodically during test mode. */
  @Override
  public void testPeriodic() {}
}

We are seeing the same problems with kPosition, but we are using c++. It will move an arm from one direction of the setpoint but not the other. We are going to disable limit switches to see if that helps later today. Did you get yours to work yet?

We are having the same problem. Any solutions?

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.