Created
February 19, 2019 09:24
-
-
Save auscompgeek/954de6c332227778c4ba53e2619b03f8 to your computer and use it in GitHub Desktop.
SPARK MAX Python Smart Motion example
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python3 | |
import rev | |
import wpilib | |
class Robot(wpilib.TimedRobot): | |
def robotInit(self): | |
self.motor = rev.CANSparkMax(1, rev.MotorType.kBrushless) | |
self.motor.restoreFactoryDefaults() | |
self.pid_controller = self.motor.getPIDController() | |
self.encoder = self.motor.getEncoder() | |
self.kP = 5e-5 | |
self.kI = 1e-6 | |
self.kD = 0 | |
self.kIz = 0 | |
self.kFF = 0.000156 | |
self.kMaxOutput = 1 | |
self.kMinOutput = -1 | |
self.max_rpm = 5700 | |
# Smart Motion coefficients | |
self.max_vel = 2000 # rpm | |
self.max_acc = 1500 | |
self.min_vel = 0 | |
self.allowed_err = 0 | |
self.pid_controller.setP(self.kP) | |
self.pid_controller.setI(self.kI) | |
self.pid_controller.setD(self.kD) | |
self.pid_controller.setIZone(self.kIz) | |
self.pid_controller.setFF(self.kFF) | |
self.pid_controller.setOutputRange(self.kMinOutput, self.kMaxOutput) | |
smart_motion_slot = 0 | |
self.pid_controller.setSmartMotionMaxVelocity(self.max_vel, smart_motion_slot) | |
self.pid_controller.setSmartMotionMinOutputVelocity(self.min_vel, smart_motion_slot) | |
self.pid_controller.setSmartMotionMaxAccel(self.max_acc, smart_motion_slot) | |
self.pid_controller.setSmartMotionAllowedClosedLoopError(self.allowed_err, smart_motion_slot) | |
wpilib.SmartDashboard.putNumber("P Gain", self.kP) | |
wpilib.SmartDashboard.putNumber("I Gain", self.kI) | |
wpilib.SmartDashboard.putNumber("D Gain", self.kD) | |
wpilib.SmartDashboard.putNumber("I Zone", self.kIz) | |
wpilib.SmartDashboard.putNumber("Feed Forward", self.kFF) | |
wpilib.SmartDashboard.putNumber("Max Output", self.kMaxOutput) | |
wpilib.SmartDashboard.putNumber("Min Output", self.kMinOutput) | |
wpilib.SmartDashboard.putNumber("Max Velocity", self.max_vel) | |
wpilib.SmartDashboard.putNumber("Min Velocity", self.min_vel) | |
wpilib.SmartDashboard.putNumber("Max Acceleration", self.max_acc) | |
wpilib.SmartDashboard.putNumber("Allowed Closed Loop Error", self.allowed_err) | |
wpilib.SmartDashboard.putNumber("Set Position", 0) | |
wpilib.SmartDashboard.putNumber("Set Velocity", 0) | |
wpilib.SmartDashboard.putBoolean("Mode", True) | |
def teleopPeriodic(self): | |
p = wpilib.SmartDashboard.getNumber("P Gain", 0) | |
i = wpilib.SmartDashboard.getNumber("I Gain", 0) | |
d = wpilib.SmartDashboard.getNumber("D Gain", 0) | |
iz = wpilib.SmartDashboard.getNumber("I Zone", 0) | |
ff = wpilib.SmartDashboard.getNumber("Feed Forward", 0) | |
max_out = wpilib.SmartDashboard.getNumber("Max Output", 0) | |
min_out = wpilib.SmartDashboard.getNumber("Min Output", 0) | |
maxV = wpilib.SmartDashboard.getNumber("Max Velocity", 0) | |
minV = wpilib.SmartDashboard.getNumber("Min Velocity", 0) | |
maxA = wpilib.SmartDashboard.getNumber("Max Acceleration", 0) | |
allE = wpilib.SmartDashboard.getNumber("Allowed Closed Loop Error", 0) | |
if p != self.kP: | |
self.pid_controller.setP(p) | |
self.kP = p | |
if i != self.kI: | |
self.pid_controller.setI(i) | |
self.kI = i | |
if d != self.kD: | |
self.pid_controller.setD(d) | |
self.kD = d | |
if iz != self.kIz: | |
self.pid_controller.setIZone(iz) | |
self.kIz = iz | |
if ff != self.kFF: | |
self.pid_controller.setFF(ff) | |
self.kFF = ff | |
if max_out != self.kMaxOutput or min_out != self.kMinOutput: | |
self.pid_controller.setOutputRange(min_out, max_out) | |
self.kMinOutput = min_out | |
self.kMaxOutput = max_out | |
if maxV != self.max_vel: | |
self.pid_controller.setSmartMotionMaxVelocity(maxV, 0) | |
self.max_vel = maxV | |
if minV != self.min_vel: | |
self.pid_controller.setSmartMotionMinOutputVelocity(minV, 0) | |
self.min_vel = minV | |
if maxA != self.max_acc: | |
self.pid_controller.setSmartMotionMaxAccel(maxA, 0) | |
self.max_acc = maxA | |
if allE != self.allowed_err: | |
self.pid_controller.setSmartMotionAllowedClosedLoopError(allE, 0) | |
self.allowed_err = allE | |
mode = wpilib.SmartDashboard.getBoolean("Mode", False) | |
if mode: | |
setpoint = wpilib.SmartDashboard.getNumber("Set Velocity", 0) | |
self.pid_controller.setReference(setpoint, rev.ControlType.kVelocity) | |
pv = self.encoder.getVelocity() | |
else: | |
setpoint = wpilib.SmartDashboard.getNumber("Set Position", 0) | |
self.pid_controller.setReference(setpoint, rev.ControlType.kSmartMotion) | |
pv = self.encoder.getPosition() | |
wpilib.SmartDashboard.putNumber("SetPoint", setpoint) | |
wpilib.SmartDashboard.putNumber("Process Variable", pv) | |
wpilib.SmartDashboard.putNumber("Output", self.motor.getAppliedOutput()) | |
if __name__ == "__main__": | |
wpilib.run(Robot) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment