From 8efe998270b0b7bfeb1309809d8eefdc10d48c78 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 12 Dec 2014 18:01:24 -0500 Subject: [PATCH] Make VictorSP and TalonSRX use correct bounds (give an extra 1us on min and max to ensure saturation). Fixes artf3914 for C++ and Java Change-Id: Ia1a848e011615e4ff50b9f5ef6e1017764aeb904 --- wpilibc/wpilibC++Devices/include/TalonSRX.h | 15 +++- wpilibc/wpilibC++Devices/include/VictorSP.h | 15 +++- wpilibc/wpilibC++Devices/src/TalonSRX.cpp | 72 +++++++++++++++- wpilibc/wpilibC++Devices/src/VictorSP.cpp | 72 +++++++++++++++- .../java/edu/wpi/first/wpilibj/TalonSRX.java | 81 +++++++++++++++++- .../java/edu/wpi/first/wpilibj/VictorSP.java | 83 ++++++++++++++++++- 6 files changed, 324 insertions(+), 14 deletions(-) diff --git a/wpilibc/wpilibC++Devices/include/TalonSRX.h b/wpilibc/wpilibC++Devices/include/TalonSRX.h index 9437ae861f..fe36b45a1f 100644 --- a/wpilibc/wpilibC++Devices/include/TalonSRX.h +++ b/wpilibc/wpilibC++Devices/include/TalonSRX.h @@ -5,14 +5,25 @@ /*----------------------------------------------------------------------------*/ #pragma once -#include "Talon.h" +#include "SafePWM.h" +#include "SpeedController.h" +#include "PIDOutput.h" /** * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control * @see CANTalon for CAN control */ -class TalonSRX: public Talon { +class TalonSRX : public SafePWM, public SpeedController +{ public: explicit TalonSRX(uint32_t channel); virtual ~TalonSRX(); + virtual void Set(float value, uint8_t syncGroup = 0); + virtual float Get(); + virtual void Disable(); + + virtual void PIDWrite(float output); + +private: + void InitTalonSRX(); }; diff --git a/wpilibc/wpilibC++Devices/include/VictorSP.h b/wpilibc/wpilibC++Devices/include/VictorSP.h index 6ae7ca0c0b..e2ba4dc3f7 100644 --- a/wpilibc/wpilibC++Devices/include/VictorSP.h +++ b/wpilibc/wpilibC++Devices/include/VictorSP.h @@ -5,13 +5,24 @@ /*----------------------------------------------------------------------------*/ #pragma once -#include "Talon.h" +#include "SafePWM.h" +#include "SpeedController.h" +#include "PIDOutput.h" /** * Vex Robotics Victor SP Speed Controller */ -class VictorSP: public Talon { +class VictorSP : public SafePWM, public SpeedController +{ public: explicit VictorSP(uint32_t channel); virtual ~VictorSP(); + virtual void Set(float value, uint8_t syncGroup = 0); + virtual float Get(); + virtual void Disable(); + + virtual void PIDWrite(float output); + +private: + void InitVictorSP(); }; diff --git a/wpilibc/wpilibC++Devices/src/TalonSRX.cpp b/wpilibc/wpilibC++Devices/src/TalonSRX.cpp index adda181d1a..40c0a3d2b1 100644 --- a/wpilibc/wpilibC++Devices/src/TalonSRX.cpp +++ b/wpilibc/wpilibC++Devices/src/TalonSRX.cpp @@ -6,10 +6,78 @@ #include "TalonSRX.h" -TalonSRX::TalonSRX(uint32_t channel) : Talon(channel) { +//#include "NetworkCommunication/UsageReporting.h" +#include "LiveWindow/LiveWindow.h" +/** + * Common initialization code called by all constructors. + * + * Note that the TalonSRX uses the following bounds for PWM values. These values should work reasonably well for + * most controllers, but if users experience issues such as asymmetric behavior around + * the deadband or inability to saturate the controller in either direction, calibration is recommended. + * The calibration procedure can be found in the TalonSRX User Manual available from Cross The Road Electronics. + * + */ +void TalonSRX::InitTalonSRX() { + SetBounds(2.001, 1.52, 1.50, 1.48, .999); + SetPeriodMultiplier(kPeriodMultiplier_1X); + SetRaw(m_centerPwm); + SetZeroLatch(); + + HALReport(HALUsageReporting::kResourceType_Talon, GetChannel()); + LiveWindow::GetInstance()->AddActuator("TalonSRX", GetChannel(), this); } -TalonSRX::~TalonSRX() { +/** + * @param channel The PWM channel that the TalonSRX is attached to. + */ +TalonSRX::TalonSRX(uint32_t channel) : SafePWM(channel) +{ + InitTalonSRX(); } +TalonSRX::~TalonSRX() +{ +} + +/** + * Set the PWM value. + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed value between -1.0 and 1.0 to set. + * @param syncGroup Unused interface. + */ +void TalonSRX::Set(float speed, uint8_t syncGroup) +{ + SetSpeed(speed); +} + +/** + * Get the recently set value of the PWM. + * + * @return The most recently set value for the PWM between -1.0 and 1.0. + */ +float TalonSRX::Get() +{ + return GetSpeed(); +} + +/** + * Common interface for disabling a motor. + */ +void TalonSRX::Disable() +{ + SetRaw(kPwmDisabled); +} + +/** + * Write out the PID value as seen in the PIDOutput base object. + * + * @param output Write out the PWM value as was found in the PIDController + */ +void TalonSRX::PIDWrite(float output) +{ + Set(output); +} diff --git a/wpilibc/wpilibC++Devices/src/VictorSP.cpp b/wpilibc/wpilibC++Devices/src/VictorSP.cpp index c286998665..d57ab32e4f 100644 --- a/wpilibc/wpilibC++Devices/src/VictorSP.cpp +++ b/wpilibc/wpilibC++Devices/src/VictorSP.cpp @@ -6,10 +6,78 @@ #include "VictorSP.h" -VictorSP::VictorSP(uint32_t channel) : Talon(channel) { +//#include "NetworkCommunication/UsageReporting.h" +#include "LiveWindow/LiveWindow.h" +/** + * Common initialization code called by all constructors. + * + * Note that the VictorSP uses the following bounds for PWM values. These values should work reasonably well for + * most controllers, but if users experience issues such as asymmetric behavior around + * the deadband or inability to saturate the controller in either direction, calibration is recommended. + * The calibration procedure can be found in the VictorSP User Manual available from Vex. + * + */ +void VictorSP::InitVictorSP() { + SetBounds(2.001, 1.52, 1.50, 1.48, .999); + SetPeriodMultiplier(kPeriodMultiplier_1X); + SetRaw(m_centerPwm); + SetZeroLatch(); + + HALReport(HALUsageReporting::kResourceType_Talon, GetChannel()); + LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this); } -VictorSP::~VictorSP() { +/** + * @param channel The PWM channel that the VictorSP is attached to. + */ +VictorSP::VictorSP(uint32_t channel) : SafePWM(channel) +{ + InitVictorSP(); } +VictorSP::~VictorSP() +{ +} + +/** + * Set the PWM value. + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed value between -1.0 and 1.0 to set. + * @param syncGroup Unused interface. + */ +void VictorSP::Set(float speed, uint8_t syncGroup) +{ + SetSpeed(speed); +} + +/** + * Get the recently set value of the PWM. + * + * @return The most recently set value for the PWM between -1.0 and 1.0. + */ +float VictorSP::Get() +{ + return GetSpeed(); +} + +/** + * Common interface for disabling a motor. + */ +void VictorSP::Disable() +{ + SetRaw(kPwmDisabled); +} + +/** + * Write out the PID value as seen in the PIDOutput base object. + * + * @param output Write out the PWM value as was found in the PIDController + */ +void VictorSP::PIDWrite(float output) +{ + Set(output); +} diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/TalonSRX.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/TalonSRX.java index fb12ddf8f2..de3ec0a898 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/TalonSRX.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/TalonSRX.java @@ -7,17 +7,94 @@ package edu.wpi.first.wpilibj; +import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; +import edu.wpi.first.wpilibj.communication.UsageReporting; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + /** * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control * @see CANTalon CANTalon for CAN control of Talon SRX */ -public class TalonSRX extends Talon { +public class TalonSRX extends SafePWM implements SpeedController { + + /** + * Common initialization code called by all constructors. + * + * Note that the TalonSRX uses the following bounds for PWM values. These values should work reasonably well for + * most controllers, but if users experience issues such as asymmetric behavior around + * the deadband or inability to saturate the controller in either direction, calibration is recommended. + * The calibration procedure can be found in the TalonSRX User Manual available from CTRE. + * + * - 2.037ms = full "forward" + * - 1.539ms = the "high end" of the deadband range + * - 1.513ms = center of the deadband range (off) + * - 1.487ms = the "low end" of the deadband range + * - .989ms = full "reverse" + */ + protected void initTalonSRX() { + setBounds(2.001, 1.52, 1.50, 1.48, 0.999); + setPeriodMultiplier(PeriodMultiplier.k1X); + setRaw(m_centerPwm); + setZeroLatch(); + + LiveWindow.addActuator("TalonSRX", getChannel(), this); + UsageReporting.report(tResourceType.kResourceType_Talon, getChannel()); + } + /** * Constructor. * - * @param channel The PWM channel that the Talon SRX is attached to. + * @param channel The PWM channel that the Talon is attached to. */ public TalonSRX(final int channel) { super(channel); + initTalonSRX(); + } + + /** + * Set the PWM value. + * + * @deprecated For compatibility with CANJaguar + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed to set. Value should be between -1.0 and 1.0. + * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, update immediately. + */ + public void set(double speed, byte syncGroup) { + setSpeed(speed); + Feed(); + } + + /** + * Set the PWM value. + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed value between -1.0 and 1.0 to set. + */ + public void set(double speed) { + setSpeed(speed); + Feed(); + } + + /** + * Get the recently set value of the PWM. + * + * @return The most recently set value for the PWM between -1.0 and 1.0. + */ + public double get() { + return getSpeed(); + } + + /** + * Write out the PID value as seen in the PIDOutput base object. + * + * @param output Write out the PWM value as was found in the PIDController + */ + public void pidWrite(double output) { + set(output); } } diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/VictorSP.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/VictorSP.java index 106a973871..b328d0bbc9 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/VictorSP.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/VictorSP.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2014. All Rights Reserved. */ +/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,11 +7,39 @@ package edu.wpi.first.wpilibj; +import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; +import edu.wpi.first.wpilibj.communication.UsageReporting; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + /** - * Vex Robotics Victor SP Speed Controller + * VEX Robotics Victor SP Speed Controller */ -public class VictorSP extends Talon { - +public class VictorSP extends SafePWM implements SpeedController { + + /** + * Common initialization code called by all constructors. + * + * Note that the VictorSP uses the following bounds for PWM values. These values should work reasonably well for + * most controllers, but if users experience issues such as asymmetric behavior around + * the deadband or inability to saturate the controller in either direction, calibration is recommended. + * The calibration procedure can be found in the VictorSP User Manual available from CTRE. + * + * - 2.037ms = full "forward" + * - 1.539ms = the "high end" of the deadband range + * - 1.513ms = center of the deadband range (off) + * - 1.487ms = the "low end" of the deadband range + * - .989ms = full "reverse" + */ + protected void initVictorSP() { + setBounds(2.001, 1.52, 1.50, 1.48, 0.999); + setPeriodMultiplier(PeriodMultiplier.k1X); + setRaw(m_centerPwm); + setZeroLatch(); + + LiveWindow.addActuator("VictorSP", getChannel(), this); + UsageReporting.report(tResourceType.kResourceType_Talon, getChannel()); + } + /** * Constructor. * @@ -19,6 +47,53 @@ public class VictorSP extends Talon { */ public VictorSP(final int channel) { super(channel); + initVictorSP(); } + /** + * Set the PWM value. + * + * @deprecated For compatibility with CANJaguar + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed to set. Value should be between -1.0 and 1.0. + * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, update immediately. + */ + public void set(double speed, byte syncGroup) { + setSpeed(speed); + Feed(); + } + + /** + * Set the PWM value. + * + * The PWM value is set using a range of -1.0 to 1.0, appropriately + * scaling the value for the FPGA. + * + * @param speed The speed value between -1.0 and 1.0 to set. + */ + public void set(double speed) { + setSpeed(speed); + Feed(); + } + + /** + * Get the recently set value of the PWM. + * + * @return The most recently set value for the PWM between -1.0 and 1.0. + */ + public double get() { + return getSpeed(); + } + + /** + * Write out the PID value as seen in the PIDOutput base object. + * + * @param output Write out the PWM value as was found in the PIDController + */ + public void pidWrite(double output) { + set(output); + } }