001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj.motorcontrol; 006 007import edu.wpi.first.util.sendable.Sendable; 008import edu.wpi.first.util.sendable.SendableBuilder; 009import edu.wpi.first.util.sendable.SendableRegistry; 010import edu.wpi.first.wpilibj.MotorSafety; 011import edu.wpi.first.wpilibj.PWM; 012 013/** Common base class for all PWM Motor Controllers. */ 014public abstract class PWMMotorController extends MotorSafety 015 implements MotorController, Sendable, AutoCloseable { 016 private boolean m_isInverted; 017 protected PWM m_pwm; 018 019 /** 020 * Constructor. 021 * 022 * @param name Name to use for SendableRegistry 023 * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are 024 * on the MXP port 025 */ 026 @SuppressWarnings("this-escape") 027 protected PWMMotorController(final String name, final int channel) { 028 m_pwm = new PWM(channel, false); 029 SendableRegistry.addLW(this, name, channel); 030 } 031 032 /** Free the resource associated with the PWM channel and set the value to 0. */ 033 @Override 034 public void close() { 035 SendableRegistry.remove(this); 036 m_pwm.close(); 037 } 038 039 /** 040 * Set the PWM value. 041 * 042 * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the 043 * FPGA. 044 * 045 * @param speed The speed value between -1.0 and 1.0 to set. 046 */ 047 @Override 048 public void set(double speed) { 049 m_pwm.setSpeed(m_isInverted ? -speed : speed); 050 feed(); 051 } 052 053 /** 054 * Get the recently set value of the PWM. This value is affected by the inversion property. If you 055 * want the value that is sent directly to the MotorController, use {@link 056 * edu.wpi.first.wpilibj.PWM#getSpeed()} instead. 057 * 058 * @return The most recently set value for the PWM between -1.0 and 1.0. 059 */ 060 @Override 061 public double get() { 062 return m_pwm.getSpeed() * (m_isInverted ? -1.0 : 1.0); 063 } 064 065 @Override 066 public void setInverted(boolean isInverted) { 067 m_isInverted = isInverted; 068 } 069 070 @Override 071 public boolean getInverted() { 072 return m_isInverted; 073 } 074 075 @Override 076 public void disable() { 077 m_pwm.setDisabled(); 078 } 079 080 @Override 081 public void stopMotor() { 082 // Don't use set(0) as that will feed the watch kitty 083 m_pwm.setSpeed(0); 084 } 085 086 @Override 087 public String getDescription() { 088 return "PWM " + getChannel(); 089 } 090 091 /** 092 * Gets the backing PWM handle. 093 * 094 * @return The pwm handle. 095 */ 096 public int getPwmHandle() { 097 return m_pwm.getHandle(); 098 } 099 100 /** 101 * Gets the PWM channel number. 102 * 103 * @return The channel number. 104 */ 105 public int getChannel() { 106 return m_pwm.getChannel(); 107 } 108 109 /** 110 * Optionally eliminate the deadband from a motor controller. 111 * 112 * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the 113 * deadband in the middle of the range. Otherwise, keep the full range without modifying any 114 * values. 115 */ 116 public void enableDeadbandElimination(boolean eliminateDeadband) { 117 m_pwm.enableDeadbandElimination(eliminateDeadband); 118 } 119 120 @Override 121 public void initSendable(SendableBuilder builder) { 122 builder.setSmartDashboardType("Motor Controller"); 123 builder.setActuator(true); 124 builder.setSafeState(this::disable); 125 builder.addDoubleProperty("Value", this::get, this::set); 126 } 127}