Pid encoding max spark brushless

Technical Technical Discussion
, , , , ,
1

I’m a bit new to coding and been trying to figure out the PID/ encoder for the past hour on my test board. What I want to happen is a smooth motor running and for me to press one button then automatically it would run 200 or so rotations…

This is what it looks like rn:

My Code is:

package frc.robot;

import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.revrobotics.CANEncoder;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.ControlType;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends TimedRobot {

private DifferentialDrive m_myRobot;

// private SpeedControllerGroup m_leftDrive;

// private SpeedControllerGroup m_rightDrive;

private static final int leftDeviceID = 1;

private static final int rightDeviceID = 2;

private static final int leftDeviceID2 = 4;

private static final int rightDeviceID2 = 5;

private static final int liftDeviceID = 3;

private static final int intakeliftDeviceID = 6;


private CANSparkMax m_leftMotor;
private CANSparkMax m_rightMotor;
private CANSparkMax m_liftMotor;
private CANSparkMax m_leftMotor2;
private CANSparkMax m_rightMotor2;
private CANSparkMax m_intakeliftMotor;
private CANEncoder m_liftEncoder;
private Spark intake;
private XboxController gamePad;
private XboxController gamePad2;
private CANPIDController m_pidController;
public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput;
private DoubleSolenoid SmolBoi;
private DoubleSolenoid BigBoi;

@Override

public void robotInit() {
m_leftMotor = new CANSparkMax(leftDeviceID, MotorType.kBrushless);

  m_rightMotor = new CANSparkMax(rightDeviceID, MotorType.kBrushless);

  m_leftMotor2 = new CANSparkMax(leftDeviceID2, MotorType.kBrushless);

  m_rightMotor2 = new CANSparkMax(rightDeviceID2, MotorType.kBrushless);

 
  m_liftMotor =new CANSparkMax(liftDeviceID, MotorType.kBrushless);

  m_liftEncoder = m_liftMotor.getEncoder(); 

  m_intakeliftMotor= new CANSparkMax(intakeliftDeviceID, MotorType.kBrushless);

  SpeedControllerGroup m_left = new SpeedControllerGroup(m_leftMotor, m_rightMotor);
  SpeedControllerGroup m_right = new SpeedControllerGroup(m_leftMotor2, m_rightMotor2);

  m_myRobot = new DifferentialDrive(m_left, m_right);

  intake = new Spark(0);

  SmolBoi = new DoubleSolenoid(0, 2);

  BigBoi = new DoubleSolenoid(1, 3);

  gamePad = new XboxController(0);
  gamePad2 = new XboxController(1);

  m_pidController = m_liftMotor.getPIDController();

// PID coefficients
kP = 0.09;
kI = 0;
kD = 0.03;
kIz = 0;
kFF = 0;
kMaxOutput = 1;
kMinOutput = -1;

// set PID coefficients
m_pidController.setP(kP);
m_pidController.setI(kI);
m_pidController.setD(kD);
m_pidController.setIZone(kIz);
m_pidController.setFF(kFF);
m_pidController.setOutputRange(kMinOutput, kMaxOutput);

// display PID coefficients on SmartDashboard
SmartDashboard.putNumber("P Gain", kP);
SmartDashboard.putNumber("I Gain", kI);
SmartDashboard.putNumber("D Gain", kD);
SmartDashboard.putNumber("I Zone", kIz);
SmartDashboard.putNumber("Feed Forward", kFF);
SmartDashboard.putNumber("Max Output", kMaxOutput);
SmartDashboard.putNumber("Min Output", kMinOutput);
SmartDashboard.putNumber("Set Rotations", 0);

// camera
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
NetworkTableEntry tx = table.getEntry("tx");
NetworkTableEntry ty = table.getEntry("ty");
NetworkTableEntry ta = table.getEntry("ta");

// post to smart dashboard periodically
SmartDashboard.putNumber("LimelightX", 0);
SmartDashboard.putNumber("LimelightY", 0);
SmartDashboard.putNumber("LimelightArea", 0);

// read values periodically
double x = tx.getDouble(0.0);
double y = ty.getDouble(0.0);
double area = ta.getDouble(0.0);

}

@Override
public void teleopPeriodic() {
SmartDashboard.updateValues();

// encoders
SmartDashboard.putNumber("Encoder Position", m_liftEncoder.getPosition());
SmartDashboard.putNumber("Encoder Velocity", m_liftEncoder.getVelocity());
m_myRobot.setSafetyEnabled(false);

// Tank drive controlled with the left and right joysticks of the gamepad
m_myRobot.tankDrive(-gamePad.getRawAxis(3), -gamePad.getRawAxis(1));

// Intake
if (gamePad.getRawButton(8)) {
m_intakeliftMotor.set(1);
}
else if (gamePad.getRawButton(6)) {
m_intakeliftMotor.set(-1);
}
else {
m_intakeliftMotor.set(.25);
}

// Elevator Lift

if (gamePad2.getRawButton(6)) {
  m_liftMotor.set(0.5);
} 
else if (gamePad2.getRawButton(5)) {
  m_liftMotor.set(-0.5);
}
else{
  m_liftMotor.set(0);
}
//Heigh Button low
if (gamePad2.getRawButton(1)) {
  m_pidController.setReference(200, ControlType.kPosition);
}
else {
  m_liftMotor.set(0);
}

 // Height Button med
 if (gamePad2.getRawButton(3)) {
  m_pidController.setReference(300, ControlType.kPosition);  
}
else {
  m_liftMotor.set(0);
}

// Height Button high  
if (gamePad2.getRawButton(4))    {
  m_pidController.setReference(400, ControlType.kPosition);  
}
else {
  m_liftMotor.set(0);
}

//intake’s Lift
if (gamePad.getRawButton(9)) {
intake.set(-0.5);
} else if (gamePad.getRawButton(10)) {
intake.set(0.5);
} else {
intake.set(0);
}

// Breaking System According to the Binder
if (gamePad.getRawButton(1)) {
SmolBoi.set(DoubleSolenoid.Value.kForward);
} 
else if (gamePad.getRawButton(4)) {
  SmolBoi.set(DoubleSolenoid.Value.kReverse);
}

//big boi breaking system

if (gamePad.getRawButton(2)) {
  BigBoi.set(DoubleSolenoid.Value.kForward);
  } 
  else if (gamePad.getRawButton(3)) {
    BigBoi.set(DoubleSolenoid.Value.kReverse);
  }

// read PID coefficients from SmartDashboard
double p = SmartDashboard.getNumber("P Gain", 0);
double i = SmartDashboard.getNumber("I Gain", 0);
double d = SmartDashboard.getNumber("D Gain", 0);
double iz = SmartDashboard.getNumber("I Zone", 0);
double ff = SmartDashboard.getNumber("Feed Forward", 0);
double max = SmartDashboard.getNumber("Max Output", 0);
double min = SmartDashboard.getNumber("Min Output", 0);
double rotations = SmartDashboard.getNumber("Set Rotations", 0);

// controller
if((p!=kP)) {
  m_pidController.setP(p);
  kP = p;
}
if((i!=kI)) {
  m_pidController.setI(i);
  kI = i;
}
if((d!=kD)) {
  m_pidController.setD(d);
  kD = d;
}
if((iz!=kIz)) {
  m_pidController.setIZone(iz);
  kIz = iz;
}
if((ff!=kFF)) {
  m_pidController.setFF(ff);
  kFF = ff;
}
if((max!=kMaxOutput)||(min!=kMinOutput)) {
  m_pidController.setOutputRange(min, max);
  kMinOutput = min;
  kMaxOutput = max;
}
SmartDashboard.putNumber("SetPoint",rotations);SmartDashboard.putNumber("ProcessVariable",m_liftEncoder.getPosition());

}
}

2

For tuning the Positiion PID loop, at a minimum I would recommend graphing a few signals while you are running to see exactly what is happening:

  1. The position of the motor
  2. The velocity of the mechanism
  3. The commanded position value
  4. The applied output (motor.getAppliedOutput)
3

Here’s something I haven’t figured out yet: Does creating CANPIDController create a separate thread? May I hit setReference once in an InstantCommand and forget about it? Or do I need to continually feed the PID in the execute() of a command?

4

No, a new thread is not created, however (since API v1.1.9) there is a single thread for all SPARK MAX devices which sends the setpoint command, which means…

…yes, you can set and forget.

1 Like
5

Thanks for the immediate reply!
I was probably too specific in the first question; my basic worry was if I needed to fret about time. And I like writing one line of code to control the motor (with 20 more in language overhead) to take care of the motor.

6

With the API by itself, no, you can let the internal controller deal with that (i.e. the 1kHz control loop), and with the thread the API setReference() calls are quick. The exception is if you are wrapping it in one of the WPILib helper classes that includes a watchdog that must be fed.

7

Hey Will,
restoreFactoryDefaults is eluding us. Are we behind on Java API libs?
TIA,
Tim

8

Oh, here’s another one. We’re fretting about resetting the encoder position, and would like to directly drive a mechanism down to a limit switch, independent of the PID controller (we think that should be disabled). The, when we know what the encoder reading is there, when can control to other positions. Are there CANPIDController enable/disable methods? or are we barking up the wrong tree?

9

The latest is Firmware v1.1.31 and API v1.1.9

Calling Set() or setReference() will turn off the PID controller and use the selected control mode (Set() simply uses Duty Cycle mode)

10

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