From 0122086d231bced4be29c8f710d72cd5ad6b3cde Mon Sep 17 00:00:00 2001 From: Patrick Date: Tue, 24 Mar 2015 15:01:17 -0400 Subject: [PATCH] Added tests for motor inversions. This commit squashes all of Patrick's eleven commits into one so that things are a bit more sane. The original commit messages and change ids (for gerrit) can be found below. Testing Motor Inversion Feature (Java tests only so far) Change-Id: I44cd9b5a3fe066e1071316831dde14bff5ec3bd9 Test 2 of java testing for Motor Inverting Change-Id: I96cc0534bb1d28a70d10c582f0b40ea3a2d83cab Added another test to try to track down issue with InvertingMotor jaguar and Talon Change-Id: I9b5292315c93ec0d568d53a6bcdac5b998a6d857 More Testing on the Inverting motors with jaguars and talons. Change-Id: I896210a54903e3c0af68e8c41360c165cf9c3122 Added C++ integration Tests for the motor inversion. Change-Id: I81af5d4aab78d755340d99608b838046bf7ddda1 C++ tests for Motor Inversion now without crashing Change-Id: Ifdecdbfc1aeb18aafb2b4c63709b27636074a274 More testing of inverted motors (now with c++ tests) Talon seems not to be working on test rig Also added a CANJaguartest file in java since was missing Currently porting the CANJaguar tests from c++ to java Change-Id: Ib578d6ee1256ac31ddf20603aa6f24adde08065b Another attempt at adding java tests for can jaguar inversion. Change-Id: I971a886a4e555ada5bd15a814094da2a1eb5c8e1 Minor changes and attempt to rerun tests after yesterday's jenkins crash. Change-Id: I7ed0904d4243499c3246e9c39e5493d0d9c962c5 All motor inversion tests should be working now. Talon on the test rig has been fixed. Change-Id: I20bd6d7486b758ce1ce47ac799150475b3152b6f Updated Inversion tests again. Should work this time. (worked on the test rig prior) Change-Id: Ifdf222d5e5733fe802f29e7d939b72e84972e8da Added tests for motor inversions. This commit squashes all of Patrick's eleven commits into one so that things are a bit more sane. The original commit messages and change ids (for gerrit) can be found below. Testing Motor Inversion Feature (Java tests only so far) Change-Id: I44cd9b5a3fe066e1071316831dde14bff5ec3bd9 Test 2 of java testing for Motor Inverting Change-Id: I96cc0534bb1d28a70d10c582f0b40ea3a2d83cab Added another test to try to track down issue with InvertingMotor jaguar and Talon Change-Id: I9b5292315c93ec0d568d53a6bcdac5b998a6d857 More Testing on the Inverting motors with jaguars and talons. Change-Id: I896210a54903e3c0af68e8c41360c165cf9c3122 Added C++ integration Tests for the motor inversion. Change-Id: I81af5d4aab78d755340d99608b838046bf7ddda1 C++ tests for Motor Inversion now without crashing Change-Id: Ifdecdbfc1aeb18aafb2b4c63709b27636074a274 More testing of inverted motors (now with c++ tests) Talon seems not to be working on test rig Also added a CANJaguartest file in java since was missing Currently porting the CANJaguar tests from c++ to java Change-Id: Ib578d6ee1256ac31ddf20603aa6f24adde08065b Another attempt at adding java tests for can jaguar inversion. Change-Id: I971a886a4e555ada5bd15a814094da2a1eb5c8e1 Minor changes and attempt to rerun tests after yesterday's jenkins crash. Change-Id: I7ed0904d4243499c3246e9c39e5493d0d9c962c5 All motor inversion tests should be working now. Talon on the test rig has been fixed. Change-Id: I20bd6d7486b758ce1ce47ac799150475b3152b6f Updated Inversion tests again. Should work this time. (worked on the test rig prior) Change-Id: Ifdf222d5e5733fe802f29e7d939b72e84972e8da --- wpilibc/wpilibC++Devices/include/CANJaguar.h | 3 + wpilibc/wpilibC++Devices/include/CANTalon.h | 2 + wpilibc/wpilibC++Devices/include/Jaguar.h | 3 +- wpilibc/wpilibC++Devices/include/RobotDrive.h | 2 - .../include/SpeedController.h | 9 +- wpilibc/wpilibC++Devices/include/Talon.h | 3 +- wpilibc/wpilibC++Devices/include/TalonSRX.h | 2 + wpilibc/wpilibC++Devices/include/Victor.h | 2 + wpilibc/wpilibC++Devices/include/VictorSP.h | 2 + wpilibc/wpilibC++Devices/src/CANJaguar.cpp | 17 ++- wpilibc/wpilibC++Devices/src/CANTalon.cpp | 16 ++- wpilibc/wpilibC++Devices/src/Jaguar.cpp | 11 +- wpilibc/wpilibC++Devices/src/RobotDrive.cpp | 55 +++------ wpilibc/wpilibC++Devices/src/Talon.cpp | 11 +- wpilibc/wpilibC++Devices/src/TalonSRX.cpp | 9 +- wpilibc/wpilibC++Devices/src/Victor.cpp | 11 +- wpilibc/wpilibC++Devices/src/VictorSP.cpp | 11 +- .../src/CANJaguarTest.cpp | 63 ++++++++++ .../src/MotorInvertingTest.cpp | 110 ++++++++++++++++++ .../java/edu/wpi/first/wpilibj/CANJaguar.java | 20 +++- .../java/edu/wpi/first/wpilibj/CANTalon.java | 20 +++- .../java/edu/wpi/first/wpilibj/Jaguar.java | 16 ++- .../edu/wpi/first/wpilibj/RobotDrive.java | 46 +++----- .../wpi/first/wpilibj/SpeedController.java | 6 + .../java/edu/wpi/first/wpilibj/Talon.java | 16 ++- .../java/edu/wpi/first/wpilibj/TalonSRX.java | 16 ++- .../java/edu/wpi/first/wpilibj/Victor.java | 16 ++- .../java/edu/wpi/first/wpilibj/VictorSP.java | 16 ++- .../wpi/first/wpilibj/MotorEncoderTest.java | 4 +- .../wpi/first/wpilibj/MotorInvertingTest.java | 110 ++++++++++++++++++ .../wpi/first/wpilibj/WpiLibJTestSuite.java | 1 + .../wpilibj/can/CANJaguarInversionTest.java | 75 ++++++++++++ .../wpi/first/wpilibj/can/CANTestSuite.java | 1 + .../wpilibj/fixtures/MotorEncoderFixture.java | 4 +- 34 files changed, 593 insertions(+), 116 deletions(-) create mode 100644 wpilibc/wpilibC++IntegrationTests/src/MotorInvertingTest.cpp create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java create mode 100644 wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java diff --git a/wpilibc/wpilibC++Devices/include/CANJaguar.h b/wpilibc/wpilibC++Devices/include/CANJaguar.h index 1b741f4849..fcd1cba6d7 100644 --- a/wpilibc/wpilibC++Devices/include/CANJaguar.h +++ b/wpilibc/wpilibC++Devices/include/CANJaguar.h @@ -123,6 +123,8 @@ public: void GetDescription(char *desc) const override; uint8_t GetDeviceID() const; + //SpeedController overrides + virtual void SetInverted(bool isInverted) override; protected: // Control mode helpers void SetSpeedReference(uint8_t reference); @@ -223,4 +225,5 @@ protected: private: void InitCANJaguar(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/CANTalon.h b/wpilibc/wpilibC++Devices/include/CANTalon.h index 520e1b676d..582d2bf728 100644 --- a/wpilibc/wpilibC++Devices/include/CANTalon.h +++ b/wpilibc/wpilibC++Devices/include/CANTalon.h @@ -163,6 +163,7 @@ public: void InitTable(ITable *subTable) override; ITable * GetTable() const override; + virtual void SetInverted(bool isInverted) override; private: // Values for various modes as is sent in the CAN packets for the Talon. enum TalonControlMode { @@ -196,4 +197,5 @@ private: // LiveWindow stuff. ITable *m_table; + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/Jaguar.h b/wpilibc/wpilibC++Devices/include/Jaguar.h index 4cc5cbc6f7..81ec13b0bd 100644 --- a/wpilibc/wpilibC++Devices/include/Jaguar.h +++ b/wpilibc/wpilibC++Devices/include/Jaguar.h @@ -22,7 +22,8 @@ public: virtual void Disable(); virtual void PIDWrite(float output) override; - + virtual void SetInverted(bool isInverted) override; private: void InitJaguar(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/RobotDrive.h b/wpilibc/wpilibC++Devices/include/RobotDrive.h index e0c3393a1f..65302e5552 100644 --- a/wpilibc/wpilibC++Devices/include/RobotDrive.h +++ b/wpilibc/wpilibC++Devices/include/RobotDrive.h @@ -84,8 +84,6 @@ protected: void RotateVector(double &x, double &y, double angle); static const int32_t kMaxNumberOfMotors = 4; - - int32_t m_invertedMotors[kMaxNumberOfMotors]; float m_sensitivity; double m_maxOutput; bool m_deleteSpeedControllers; diff --git a/wpilibc/wpilibC++Devices/include/SpeedController.h b/wpilibc/wpilibC++Devices/include/SpeedController.h index 8b59ab722b..eca4035ea4 100644 --- a/wpilibc/wpilibC++Devices/include/SpeedController.h +++ b/wpilibc/wpilibC++Devices/include/SpeedController.h @@ -17,17 +17,22 @@ public: virtual ~SpeedController() {} /** * Common interface for setting the speed of a speed controller. - * + * * @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. */ virtual void Set(float speed, uint8_t syncGroup = 0) = 0; /** * Common interface for getting the current set speed of a speed controller. - * + * * @return The current set speed. Value is between -1.0 and 1.0. */ virtual float Get() const = 0; + /** + * common interface for inverting direction of a speed controller + * @param isInverted The state of inversion true is inverted + */ + virtual void SetInverted(bool isInverted) = 0; /** * Common interface for disabling a motor. */ diff --git a/wpilibc/wpilibC++Devices/include/Talon.h b/wpilibc/wpilibC++Devices/include/Talon.h index 41857afa43..cee3fb7d1e 100644 --- a/wpilibc/wpilibC++Devices/include/Talon.h +++ b/wpilibc/wpilibC++Devices/include/Talon.h @@ -22,7 +22,8 @@ public: virtual void Disable(); virtual void PIDWrite(float output) override; - + virtual void SetInverted(bool isInverted) override; private: void InitTalon(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/TalonSRX.h b/wpilibc/wpilibC++Devices/include/TalonSRX.h index 94c1d3f0a8..b5a5702185 100644 --- a/wpilibc/wpilibC++Devices/include/TalonSRX.h +++ b/wpilibc/wpilibC++Devices/include/TalonSRX.h @@ -23,7 +23,9 @@ public: virtual void Disable() override; virtual void PIDWrite(float output) override; + virtual void SetInverted(bool isInverted) override; private: void InitTalonSRX(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/Victor.h b/wpilibc/wpilibC++Devices/include/Victor.h index 082ef2d1ce..6016022bf5 100644 --- a/wpilibc/wpilibC++Devices/include/Victor.h +++ b/wpilibc/wpilibC++Devices/include/Victor.h @@ -26,6 +26,8 @@ public: virtual void PIDWrite(float output) override; + virtual void SetInverted(bool isInverted); private: void InitVictor(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/include/VictorSP.h b/wpilibc/wpilibC++Devices/include/VictorSP.h index cae1d7e22c..21851ce933 100644 --- a/wpilibc/wpilibC++Devices/include/VictorSP.h +++ b/wpilibc/wpilibC++Devices/include/VictorSP.h @@ -23,6 +23,8 @@ public: virtual void PIDWrite(float output) override; + virtual void SetInverted(bool isInverted); private: void InitVictorSP(); + bool m_isInverted; }; diff --git a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp index 48fa9b2c08..baddbcad58 100644 --- a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp +++ b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp @@ -204,7 +204,7 @@ void CANJaguar::InitCANJaguar() default: break; } - + m_isInverted = false; HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber, m_controlMode); LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this); } @@ -315,13 +315,13 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) messageID = LM_API_VOLT_T_SET; if (outputValue > 1.0) outputValue = 1.0; if (outputValue < -1.0) outputValue = -1.0; - dataSize = packPercentage(dataBuffer, outputValue); + dataSize = packPercentage(dataBuffer, (m_isInverted ? -outputValue : outputValue)); } break; case kSpeed: { messageID = LM_API_SPD_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); + dataSize = packFXP16_16(dataBuffer, (m_isInverted ? -outputValue : outputValue)); } break; case kPosition: @@ -339,7 +339,7 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) case kVoltage: { messageID = LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); + dataSize = packFXP8_8(dataBuffer, (m_isInverted ? -outputValue : outputValue)); } break; default: @@ -2118,6 +2118,15 @@ void CANJaguar::StopLiveWindowMode() } } +/** +* common interface for inverting direction of a speed controller +* Only works in PercentVbus, speed, and Voltage modes +* @param isInverted The state of inversion true is inverted +*/ +void CANJaguar::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} + std::string CANJaguar::GetSmartDashboardType() const { return "Speed Controller"; diff --git a/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/wpilibc/wpilibC++Devices/src/CANTalon.cpp index 575a262762..3c27dfdab5 100644 --- a/wpilibc/wpilibC++Devices/src/CANTalon.cpp +++ b/wpilibc/wpilibC++Devices/src/CANTalon.cpp @@ -23,6 +23,7 @@ CANTalon::CANTalon(int deviceNumber) { ApplyControlMode(m_controlMode); m_impl->SetProfileSlotSelect(m_profile); + m_isInverted = false; } /** * Constructor for the CANTalon device. @@ -131,7 +132,7 @@ void CANTalon::Set(float value, uint8_t syncGroup) switch(m_controlMode) { case CANSpeedController::kPercentVbus: { - m_impl->Set(value); + m_impl->Set(m_isInverted ? -value : value); status = CTR_OKAY; } break; @@ -143,12 +144,12 @@ void CANTalon::Set(float value, uint8_t syncGroup) case CANSpeedController::kVoltage: { // Voltage is an 8.8 fixed point number. - int volts = int(value * 256); + int volts = int((m_isInverted ? value: -value) * 256); status = m_impl->SetDemand(volts); } break; case CANSpeedController::kSpeed: - status = m_impl->SetDemand(value); + status = m_impl->SetDemand(m_isInverted ? -value : value); break; case CANSpeedController::kPosition: status = m_impl->SetDemand(value); @@ -1291,7 +1292,14 @@ void CANTalon::GetDescription(char *desc) const { sprintf(desc, "CANTalon ID %d", m_deviceNumber); } - +/** +* common interface for inverting direction of a speed controller +* Only works in PercentVbus, speed, and Voltage modes +* @param isInverted The state of inversion true is inverted +*/ +void CANTalon::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Common interface for stopping the motor * Part of the MotorSafety interface diff --git a/wpilibc/wpilibC++Devices/src/Jaguar.cpp b/wpilibc/wpilibC++Devices/src/Jaguar.cpp index 2699e2d875..e6c29c0554 100644 --- a/wpilibc/wpilibC++Devices/src/Jaguar.cpp +++ b/wpilibc/wpilibC++Devices/src/Jaguar.cpp @@ -30,6 +30,7 @@ void Jaguar::InitJaguar() HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel()); LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); + m_isInverted = false; } /** @@ -56,7 +57,7 @@ Jaguar::~Jaguar() */ void Jaguar::Set(float speed, uint8_t syncGroup) { - SetSpeed(speed); + SetSpeed(m_isInverted ? -speed : speed); } /** @@ -76,7 +77,13 @@ void Jaguar::Disable() { SetRaw(kPwmDisabled); } - +/** +* common interface for inverting direction of a speed controller +* @param isInverted The state of inversion true is inverted +*/ +void Jaguar::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Write out the PID value as seen in the PIDOutput base object. * diff --git a/wpilibc/wpilibC++Devices/src/RobotDrive.cpp b/wpilibc/wpilibC++Devices/src/RobotDrive.cpp index f4edf5cd64..31bd8d216b 100644 --- a/wpilibc/wpilibC++Devices/src/RobotDrive.cpp +++ b/wpilibc/wpilibC++Devices/src/RobotDrive.cpp @@ -57,10 +57,6 @@ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) InitRobotDrive(); m_rearLeftMotor = new Talon(leftMotorChannel); m_rearRightMotor = new Talon(rightMotorChannel); - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } SetLeftRightMotorOutputs(0.0, 0.0); m_deleteSpeedControllers = true; } @@ -83,10 +79,6 @@ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, m_rearRightMotor = new Talon(rearRightMotor); m_frontLeftMotor = new Talon(frontLeftMotor); m_frontRightMotor = new Talon(frontRightMotor); - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } SetLeftRightMotorOutputs(0.0, 0.0); m_deleteSpeedControllers = true; } @@ -110,10 +102,6 @@ RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor) } m_rearLeftMotor = leftMotor; m_rearRightMotor = rightMotor; - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } m_deleteSpeedControllers = false; } @@ -122,10 +110,6 @@ RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor) InitRobotDrive(); m_rearLeftMotor = &leftMotor; m_rearRightMotor = &rightMotor; - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } m_deleteSpeedControllers = false; } @@ -150,10 +134,6 @@ RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLef m_rearLeftMotor = rearLeftMotor; m_frontRightMotor = frontRightMotor; m_rearRightMotor = rearRightMotor; - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } m_deleteSpeedControllers = false; } @@ -165,10 +145,6 @@ RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLef m_rearLeftMotor = &rearLeftMotor; m_frontRightMotor = &frontRightMotor; m_rearRightMotor = &rearRightMotor; - for (int32_t i=0; i < kMaxNumberOfMotors; i++) - { - m_invertedMotors[i] = 1; - } m_deleteSpeedControllers = false; } @@ -508,10 +484,10 @@ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float Normalize(wheelSpeeds); - m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup); + m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput, m_syncGroup); + m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput, m_syncGroup); + m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput, m_syncGroup); + m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput, m_syncGroup); if (m_syncGroup != 0) { @@ -558,10 +534,10 @@ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rota Normalize(wheelSpeeds); - m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup); + m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput, m_syncGroup); + m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput, m_syncGroup); + m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput, m_syncGroup); + m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput, m_syncGroup); if (m_syncGroup != 0) { @@ -599,12 +575,12 @@ void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL); if (m_frontLeftMotor != NULL) - m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup); - m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup); + m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput, m_syncGroup); + m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput, m_syncGroup); if (m_frontRightMotor != NULL) - m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup); - m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup); + m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput, m_syncGroup); + m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput, m_syncGroup); if (m_syncGroup != 0) { @@ -679,7 +655,12 @@ void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) wpi_setWPIError(InvalidMotorIndex); return; } - m_invertedMotors[motor] = isInverted ? -1 : 1; + switch(motor){ + case kFrontLeftMotor: m_frontLeftMotor -> SetInverted(isInverted); break; + case kFrontRightMotor: m_frontRightMotor -> SetInverted(isInverted); break; + case kRearLeftMotor: m_rearLeftMotor -> SetInverted(isInverted); break; + case kRearRightMotor: m_rearRightMotor -> SetInverted(isInverted); break; + } } /** diff --git a/wpilibc/wpilibC++Devices/src/Talon.cpp b/wpilibc/wpilibC++Devices/src/Talon.cpp index 84d7a60110..5742741964 100644 --- a/wpilibc/wpilibC++Devices/src/Talon.cpp +++ b/wpilibc/wpilibC++Devices/src/Talon.cpp @@ -31,6 +31,7 @@ void Talon::InitTalon() { HALReport(HALUsageReporting::kResourceType_Talon, GetChannel()); LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this); + m_isInverted = false; } /** @@ -57,7 +58,7 @@ Talon::~Talon() */ void Talon::Set(float speed, uint8_t syncGroup) { - SetSpeed(speed); + SetSpeed(m_isInverted ? -speed : speed); } /** @@ -77,7 +78,13 @@ void Talon::Disable() { SetRaw(kPwmDisabled); } - +/** +* common interface for inverting direction of a speed controller +* @param isInverted The state of inversion true is inverted +*/ +void Talon::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Write out the PID value as seen in the PIDOutput base object. * diff --git a/wpilibc/wpilibC++Devices/src/TalonSRX.cpp b/wpilibc/wpilibC++Devices/src/TalonSRX.cpp index 712215174c..40494b7c83 100644 --- a/wpilibc/wpilibC++Devices/src/TalonSRX.cpp +++ b/wpilibc/wpilibC++Devices/src/TalonSRX.cpp @@ -30,6 +30,7 @@ void TalonSRX::InitTalonSRX() { HALReport(HALUsageReporting::kResourceType_TalonSRX, GetChannel()); LiveWindow::GetInstance()->AddActuator("TalonSRX", GetChannel(), this); + m_isInverted = false; } /** @@ -76,7 +77,13 @@ void TalonSRX::Disable() { SetRaw(kPwmDisabled); } - +/** +* common interface for inverting direction of a speed controller +* @param isInverted The state of inversion true is inverted +*/ +void TalonSRX::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Write out the PID value as seen in the PIDOutput base object. * diff --git a/wpilibc/wpilibC++Devices/src/Victor.cpp b/wpilibc/wpilibC++Devices/src/Victor.cpp index cd435571e6..d59acbed91 100644 --- a/wpilibc/wpilibC++Devices/src/Victor.cpp +++ b/wpilibc/wpilibC++Devices/src/Victor.cpp @@ -33,6 +33,7 @@ void Victor::InitVictor() { LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this); HALReport(HALUsageReporting::kResourceType_Victor, GetChannel()); + m_isInverted = false; } /** @@ -59,7 +60,7 @@ Victor::~Victor() */ void Victor::Set(float speed, uint8_t syncGroup) { - SetSpeed(speed); + SetSpeed(m_isInverted ? -speed: speed); } /** @@ -79,7 +80,13 @@ void Victor::Disable() { SetRaw(kPwmDisabled); } - +/** +* common interface for inverting direction of a speed controller +* @param isInverted The state of inversion true is inverted +*/ +void Victor::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Write out the PID value as seen in the PIDOutput base object. * diff --git a/wpilibc/wpilibC++Devices/src/VictorSP.cpp b/wpilibc/wpilibC++Devices/src/VictorSP.cpp index 701b3d487f..8a12862a80 100644 --- a/wpilibc/wpilibC++Devices/src/VictorSP.cpp +++ b/wpilibc/wpilibC++Devices/src/VictorSP.cpp @@ -31,6 +31,7 @@ void VictorSP::InitVictorSP() { HALReport(HALUsageReporting::kResourceType_VictorSP, GetChannel()); LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this); + m_isInverted = false; } /** @@ -57,7 +58,7 @@ VictorSP::~VictorSP() */ void VictorSP::Set(float speed, uint8_t syncGroup) { - SetSpeed(speed); + SetSpeed(m_isInverted ? -speed: speed); } /** @@ -69,7 +70,13 @@ float VictorSP::Get() const { return GetSpeed(); } - +/** +* common interface for inverting direction of a speed controller +* @param isInverted The state of inversion true is inverted +*/ +void VictorSP::SetInverted(bool isInverted){ +m_isInverted = isInverted; +} /** * Common interface for disabling a motor. */ diff --git a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp index a39b0b9d54..fcb9a586a5 100644 --- a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp +++ b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp @@ -27,6 +27,11 @@ static constexpr double kCurrentTolerance = 0.1; static constexpr double kVoltageTolerance = 0.1; +static constexpr double kMotorVoltage = 5.0; + +static constexpr double kMotorPercent = 0.5; + +static constexpr double kMotorSpeed = 100; class CANJaguarTest : public testing::Test { protected: CANJaguar *m_jaguar; @@ -73,6 +78,39 @@ protected: Wait(totalTime / 50.0); } } + /** + * returns the sign of the given number + */ + int SignNum(double value){ + return -(value<0) + (value>0); + } + void InversionTest(float motorValue, float delayTime = kMotorTime){ + m_jaguar->EnableControl(); + m_jaguar->SetInverted(false); + SetJaguar(delayTime,motorValue); + double initialSpeed = m_jaguar->GetSpeed(); + m_jaguar->Set(0.0); + m_jaguar->SetInverted(true); + SetJaguar(delayTime,motorValue); + double finalSpeed = m_jaguar->GetSpeed(); + //checks that the motor has changed direction + EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed)) + << "CAN Jaguar did not invert direction positive. Initial speed was: " + << initialSpeed << " Final displacement was: " << finalSpeed + << " Sign of initial displacement was: " << SignNum(initialSpeed) + << " Sign of final displacement was: " << SignNum(finalSpeed); + SetJaguar(delayTime,-motorValue); + initialSpeed = m_jaguar->GetSpeed(); + m_jaguar->Set(0.0); + m_jaguar->SetInverted(false); + SetJaguar(delayTime,-motorValue); + finalSpeed = m_jaguar->GetSpeed(); + EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed)) + << "CAN Jaguar did not invert direction negative. Initial displacement " + "was: " << initialSpeed << " Final displacement was: " << finalSpeed + << " Sign of initial displacement was: " << SignNum(initialSpeed) + << " Sign of final displacement was: " << SignNum(finalSpeed); + } }; /** @@ -423,3 +461,28 @@ TEST_F(CANJaguarTest, FakeLimitSwitchReverse) { EXPECT_GT(m_jaguar->GetPosition(), initialPosition) << "CAN Jaguar should have moved forwards while the reverse limit was on"; } +/** +*Tests that inversion works in voltage mode +*/ +TEST_F(CANJaguarTest, InvertingVoltageMode){ +m_jaguar->SetVoltageMode(CANJaguar::QuadEncoder, 360); +m_jaguar->EnableControl(); +InversionTest(kMotorVoltage); +} + +/** +*Tests that inversion works in percentMode +*/ +TEST_F(CANJaguarTest, InvertingPercentMode){ +m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); +m_jaguar->EnableControl(); +InversionTest(kMotorPercent); +} +/** +* Tests that inversion works in SpeedMode +*/ +TEST_F(CANJaguarTest, InvertingSpeedMode){ +m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 0.1f, 0.005f, 0.00f); +m_jaguar->EnableControl(); +InversionTest(kMotorSpeed, kMotorTime); +} diff --git a/wpilibc/wpilibC++IntegrationTests/src/MotorInvertingTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/MotorInvertingTest.cpp new file mode 100644 index 0000000000..ba9bdb92e2 --- /dev/null +++ b/wpilibc/wpilibC++IntegrationTests/src/MotorInvertingTest.cpp @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. 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. */ +/*----------------------------------------------------------------------------*/ + +#include "WPILib.h" +#include "gtest/gtest.h" +#include "TestBench.h" + +enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON }; +static const double motorSpeed = 0.25; +static const double delayTime = 0.5; +std::ostream &operator<<(std::ostream &os, MotorInvertingTestType const &type) { + switch(type) { + case TEST_VICTOR: os << "Victor"; break; + case TEST_JAGUAR: os << "Jaguar"; break; + case TEST_TALON: os << "Talon"; break; + } + + return os; +} +class MotorInvertingTest : public testing::TestWithParam{ +protected: + SpeedController *m_speedController; + Encoder *m_encoder; + virtual void SetUp() { + switch(GetParam()) { + case TEST_VICTOR: + m_speedController = new Victor(TestBench::kVictorChannel); + m_encoder = new Encoder(TestBench::kVictorEncoderChannelA, + TestBench::kVictorEncoderChannelB); + break; + + case TEST_JAGUAR: + m_speedController = new Jaguar(TestBench::kJaguarChannel); + m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA, + TestBench::kJaguarEncoderChannelB); + break; + + case TEST_TALON: + m_speedController = new Talon(TestBench::kTalonChannel); + m_encoder = new Encoder(TestBench::kTalonEncoderChannelA, + TestBench::kTalonEncoderChannelB); + break; + } + + } + virtual void TearDown() { + delete m_speedController; + delete m_encoder; + } + + void Reset() { + m_speedController->SetInverted(false); + m_speedController->Set(0.0f); + m_encoder->Reset(); + } +}; + +TEST_P(MotorInvertingTest, InvertingPositive){ + Reset(); + m_speedController->Set(motorSpeed); + Wait(delayTime); + bool initDirection = m_encoder->GetDirection(); + m_speedController->SetInverted(true); + m_speedController->Set(motorSpeed); + Wait(delayTime); + EXPECT_TRUE(m_encoder->GetDirection()!=initDirection) <<"Inverting with Positive value does not change direction"; + Reset(); +} +TEST_P(MotorInvertingTest, InvertingNegative){ + Reset(); + m_speedController->SetInverted(false); + m_speedController->Set(-motorSpeed); + Wait(delayTime); + bool initDirection = m_encoder->GetDirection(); + m_speedController->SetInverted(true); + m_speedController->Set(-motorSpeed); + Wait(delayTime); + EXPECT_TRUE(m_encoder->GetDirection()!=initDirection) <<"Inverting with Negative value does not change direction"; + Reset(); +} +TEST_P(MotorInvertingTest, InvertingSwitchingPosToNeg){ + Reset(); + m_speedController->SetInverted(false); + m_speedController->Set(motorSpeed); + Wait(delayTime); + bool initDirection = m_encoder->GetDirection(); + m_speedController->SetInverted(true); + m_speedController->Set(-motorSpeed); + Wait(delayTime); + EXPECT_TRUE(m_encoder->GetDirection()==initDirection) <<"Inverting with Switching value does change direction"; + Reset(); +} +TEST_P(MotorInvertingTest, InvertingSwitchingNegToPos){ + Reset(); + m_speedController->SetInverted(false); + m_speedController->Set(-motorSpeed); + Wait(delayTime); + bool initDirection = m_encoder->GetDirection(); + m_speedController->SetInverted(true); + m_speedController->Set(motorSpeed); + Wait(delayTime); + EXPECT_TRUE(m_encoder->GetDirection()==initDirection) <<"Inverting with Switching value does change direction"; + Reset(); +} +INSTANTIATE_TEST_CASE_P(Test, MotorInvertingTest, + testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON)); diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index a378400e64..69b3d43328 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -50,7 +50,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed /** Sets a potentiometer as the position reference only.
Passed as the "tag" when setting the control mode. */ public final static PotentiometerTag kPotentiometer = new PotentiometerTag(); - + private boolean isInverted = false; /** * Mode determines how the Jaguar is controlled, used internally. */ @@ -348,12 +348,12 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed switch(m_controlMode) { case PercentVbus: messageID = CANJNI.LM_API_VOLT_T_SET; - dataSize = packPercentage(data, outputValue); + dataSize = packPercentage(data, isInverted ? -outputValue: outputValue); break; case Speed: messageID = CANJNI.LM_API_SPD_T_SET; - dataSize = packFXP16_16(data, outputValue); + dataSize = packFXP16_16(data, isInverted ? -outputValue: outputValue); break; case Position: @@ -369,7 +369,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed case Voltage: messageID = CANJNI.LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(data, outputValue); + dataSize = packFXP8_8(data, isInverted ? -outputValue: outputValue); break; default: @@ -422,7 +422,17 @@ public class CANJaguar implements MotorSafety, PIDOutput, PIDInterface, CANSpeed disableControl(); } - /** + /** + * Inverts the direction of rotation of the motor + * Only works in percentVbus, Speed, and Voltage mode + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + + /** * Check all unverified params and make sure they're equal to their local * cached versions. If a value isn't available, it gets requested. If a value * doesn't match up, it gets set again. diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java index e2d0ff0745..7b3dc5f342 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.tables.ITableListener; public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface, CANSpeedController, LiveWindowSendable { private MotorSafetyHelper m_safetyHelper; + private boolean isInverted = false; public enum TalonControlMode { PercentVbus(0), Follower(5), Voltage(4), Position(1), Speed(2), Current(3), Disabled(15); @@ -144,18 +145,18 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface m_setPoint = outputValue; switch (m_controlMode) { case PercentVbus: - m_impl.Set(outputValue); + m_impl.Set(isInverted ? -outputValue: outputValue); break; case Follower: m_impl.SetDemand((int)outputValue); break; case Voltage: // Voltage is an 8.8 fixed point number. - int volts = (int)(outputValue * 256); + int volts = (int)((isInverted ? -outputValue: outputValue) * 256); m_impl.SetDemand(volts); break; case Speed: - m_impl.SetDemand((int)outputValue); + m_impl.SetDemand((int)(isInverted ? -outputValue: outputValue)); break; case Position: m_impl.SetDemand((int)outputValue); @@ -167,7 +168,18 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface } } - /** + /** + * Inverts the direction of the motor's rotation + * Only works in PercentVbus mode + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + + /** * Sets the output of the Talon. * * @param outputValue See set(). diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Jaguar.java index 3f6834c627..88f7805dbf 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Jaguar.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Jaguar.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; * @see CANJaguar CANJaguar for CAN control */ public class Jaguar extends SafePWM implements SpeedController { - +private boolean isInverted = false; /** * Common initialization code called by all constructors. */ @@ -63,7 +63,7 @@ public class Jaguar extends SafePWM implements SpeedController { @Deprecated @Override public void set(double speed, byte syncGroup) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } @@ -77,10 +77,20 @@ public class Jaguar extends SafePWM implements SpeedController { */ @Override public void set(double speed) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + /** * Get the recently set value of the PWM. * diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java index 861d899343..36622941f6 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java @@ -60,7 +60,6 @@ public class RobotDrive implements MotorSafety { public static final double kDefaultSensitivity = 0.5; public static final double kDefaultMaxOutput = 1.0; protected static final int kMaxNumberOfMotors = 4; - protected final int m_invertedMotors[] = new int[4]; protected double m_sensitivity; protected double m_maxOutput; protected SpeedController m_frontLeftMotor; @@ -89,9 +88,6 @@ public class RobotDrive implements MotorSafety { m_rearLeftMotor = new Talon(leftMotorChannel); m_frontRightMotor = null; m_rearRightMotor = new Talon(rightMotorChannel); - for (int i = 0; i < kMaxNumberOfMotors; i++) { - m_invertedMotors[i] = 1; - } m_allocatedSpeedControllers = true; setupMotorSafety(); drive(0, 0); @@ -115,9 +111,6 @@ public class RobotDrive implements MotorSafety { m_rearRightMotor = new Talon(rearRightMotor); m_frontLeftMotor = new Talon(frontLeftMotor); m_frontRightMotor = new Talon(frontRightMotor); - for (int i = 0; i < kMaxNumberOfMotors; i++) { - m_invertedMotors[i] = 1; - } m_allocatedSpeedControllers = true; setupMotorSafety(); drive(0, 0); @@ -142,9 +135,6 @@ public class RobotDrive implements MotorSafety { m_rearRightMotor = rightMotor; m_sensitivity = kDefaultSensitivity; m_maxOutput = kDefaultMaxOutput; - for (int i = 0; i < kMaxNumberOfMotors; i++) { - m_invertedMotors[i] = 1; - } m_allocatedSpeedControllers = false; setupMotorSafety(); drive(0, 0); @@ -169,10 +159,6 @@ public class RobotDrive implements MotorSafety { m_frontRightMotor = frontRightMotor; m_rearRightMotor = rearRightMotor; m_sensitivity = kDefaultSensitivity; - m_maxOutput = kDefaultMaxOutput; - for (int i = 0; i < kMaxNumberOfMotors; i++) { - m_invertedMotors[i] = 1; - } m_allocatedSpeedControllers = false; setupMotorSafety(); drive(0, 0); @@ -487,10 +473,10 @@ public class RobotDrive implements MotorSafety { wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation; normalize(wheelSpeeds); - m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); - m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); - m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); - m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); + m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); + m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); + m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); + m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); if (m_syncGroup != 0) { CANJaguar.updateSyncGroup(m_syncGroup); @@ -533,10 +519,10 @@ public class RobotDrive implements MotorSafety { normalize(wheelSpeeds); - m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); - m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); - m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); - m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); + m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); + m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); + m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); + m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); if (this.m_syncGroup != 0) { CANJaguar.updateSyncGroup(m_syncGroup); @@ -573,14 +559,14 @@ public class RobotDrive implements MotorSafety { } if (m_frontLeftMotor != null) { - m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); + m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput, m_syncGroup); } - m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); + m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput, m_syncGroup); if (m_frontRightMotor != null) { - m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); + m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput, m_syncGroup); } - m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); + m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput, m_syncGroup); if (this.m_syncGroup != 0) { CANJaguar.updateSyncGroup(m_syncGroup); @@ -640,9 +626,13 @@ public class RobotDrive implements MotorSafety { * @param isInverted True if the motor should be inverted when operated. */ public void setInvertedMotor(MotorType motor, boolean isInverted) { - m_invertedMotors[motor.value] = isInverted ? -1 : 1; + switch (motor.value){ + case MotorType.kFrontLeft_val: m_frontLeftMotor.setInverted(isInverted); break; + case MotorType.kFrontRight_val: m_frontRightMotor.setInverted(isInverted); break; + case MotorType.kRearLeft_val: m_rearLeftMotor.setInverted(isInverted); break; + case MotorType.kRearRight_val: m_rearRightMotor.setInverted(isInverted); break; + } } - /** * Set the turning sensitivity. * diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SpeedController.java index 21c95e5078..33f8053a81 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SpeedController.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/SpeedController.java @@ -34,6 +34,12 @@ public interface SpeedController extends PIDOutput { */ void set(double speed); + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + void setInverted(boolean isInverted); /** * Disable the speed controller */ diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Talon.java index cf54c0b107..377013612d 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Talon.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Talon.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller */ public class Talon extends SafePWM implements SpeedController { - +private boolean isInverted = false; /** * Common initialization code called by all constructors. * @@ -62,10 +62,11 @@ public class Talon extends SafePWM implements SpeedController { * @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); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** * Set the PWM value. * @@ -75,9 +76,18 @@ public class Talon extends SafePWM implements SpeedController { * @param speed The speed value between -1.0 and 1.0 to set. */ public void set(double speed) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } /** * Get the recently set value of the PWM. 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 f57eaa72a9..f082b75bd8 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 @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; * @see CANTalon CANTalon for CAN control of Talon SRX */ public class TalonSRX extends SafePWM implements SpeedController { - + private boolean isInverted = false; /** * Common initialization code called by all constructors. * @@ -63,7 +63,7 @@ public class TalonSRX extends SafePWM implements SpeedController { * @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); + setSpeed(isInverted ? -speed: speed); Feed(); } @@ -76,10 +76,20 @@ public class TalonSRX extends SafePWM implements SpeedController { * @param speed The speed value between -1.0 and 1.0 to set. */ public void set(double speed) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + /** * Get the recently set value of the PWM. * diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Victor.java index 5e7c78f0ef..f03d0344b8 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Victor.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Victor.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; * class but may need to be calibrated per the Victor 884 user manual. */ public class Victor extends SafePWM implements SpeedController { - +private boolean isInverted = false; /** * Common initialization code called by all constructors. * @@ -67,7 +67,7 @@ public class Victor extends SafePWM implements SpeedController { * @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); + setSpeed(isInverted ? -speed: speed); Feed(); } @@ -80,10 +80,20 @@ public class Victor extends SafePWM implements SpeedController { * @param speed The speed value between -1.0 and 1.0 to set. */ public void set(double speed) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + /** * Get the recently set value of the PWM. * 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 a8da1f1cda..2757142aa6 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 @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; * VEX Robotics Victor SP Speed Controller */ public class VictorSP extends SafePWM implements SpeedController { - +private boolean isInverted = false; /** * Common initialization code called by all constructors. * @@ -62,7 +62,7 @@ public class VictorSP extends SafePWM implements SpeedController { * @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); + setSpeed(isInverted ? -speed: speed); Feed(); } @@ -75,10 +75,20 @@ public class VictorSP extends SafePWM implements SpeedController { * @param speed The speed value between -1.0 and 1.0 to set. */ public void set(double speed) { - setSpeed(speed); + setSpeed(isInverted ? -speed: speed); Feed(); } + /** + * Common interface for inverting direction of a speed controller + * + * @param isInverted The state of inversion true is inverted + */ + @Override + public void setInverted(boolean isInverted) { + this.isInverted = isInverted; + } + /** * Get the recently set value of the PWM. * diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index e1f647041d..2304910b9d 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java @@ -177,10 +177,10 @@ public class MotorEncoderTest extends AbstractComsSetup { pid.setSetpoint(2500); pid.enable(); - Timer.delay(5.0); + Timer.delay(10.0); pid.disable(); - assertTrue("PID loop did not reach setpoint within 5 seconds. The error was: " + pid.getError(), + assertTrue("PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(), pid.onTarget()); pid.free(); diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java new file mode 100644 index 0000000000..d699b2b628 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java @@ -0,0 +1,110 @@ +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; +import edu.wpi.first.wpilibj.test.AbstractComsSetup; + +import edu.wpi.first.wpilibj.test.TestBench; +import org.junit.AfterClass; +import org.junit.Before; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.junit.runners.Parameterized; + +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import java.util.Arrays; +import java.util.Collection; +import java.util.logging.Logger; + +/** + * Tests Inversion of motors using the SpeedController setInverted + */ +@RunWith(Parameterized.class) +public class MotorInvertingTest extends AbstractComsSetup { + static MotorEncoderFixture fixture = null; + private static final double motorspeed = 0.35; + private static final double delaytime = 0.3; + + + public MotorInvertingTest(MotorEncoderFixture afixture){ + logger.fine("Constructor with: " + afixture.getType()); + if(fixture != null && !fixture.equals(afixture)) fixture.teardown(); + fixture = afixture; + fixture.setup(); + } + @Parameterized.Parameters(name= "{index}: {0}") + public static Collection[]> generateData(){ + //logger.fine("Loading the MotorList"); + return Arrays.asList(new MotorEncoderFixture[][]{ + {TestBench.getInstance().getTalonPair()}, + {TestBench.getInstance().getVictorPair()}, + {TestBench.getInstance().getJaguarPair()} + }); + } + private static final Logger logger = Logger.getLogger(MotorInvertingTest.class.getName()); + @Override + protected Logger getClassLogger(){ + return logger; + } + @Before + public void setUp() { + // Reset the fixture elements before every test + fixture.reset(); + } + + @AfterClass + public static void tearDown() { + fixture.getMotor().setInverted(false); + // Clean up the fixture after the test + fixture.teardown(); + } + @Test + public void testInvertingPositive(){ + fixture.getMotor().setInverted(false); + fixture.getMotor().set(motorspeed); + Timer.delay(delaytime); + boolean initDirection = fixture.getEncoder().getDirection(); + fixture.getMotor().setInverted(true); + fixture.getMotor().set(motorspeed); + Timer.delay(delaytime); + assertFalse("Inverting with Positive value does not change direction",fixture.getEncoder().getDirection()==initDirection); + fixture.getMotor().set(0); + } + @Test + public void testInvertingNegative(){ + fixture.getMotor().setInverted(false); + fixture.getMotor().set(-motorspeed); + Timer.delay(delaytime); + boolean initDirection = fixture.getEncoder().getDirection(); + fixture.getMotor().setInverted(true); + fixture.getMotor().set(-motorspeed); + Timer.delay(delaytime); + assertFalse("Inverting with Negative value does not change direction",fixture.getEncoder().getDirection()==initDirection); + fixture.getMotor().set(0); + } + @Test + public void testInvertingSwitchingPosToNeg(){ + fixture.getMotor().setInverted(false); + fixture.getMotor().set(motorspeed); + Timer.delay(delaytime); + boolean initDirection = fixture.getEncoder().getDirection(); + fixture.getMotor().setInverted(true); + fixture.getMotor().set(-motorspeed); + Timer.delay(delaytime); + assertTrue("Inverting with Switching value does change direction", fixture.getEncoder().getDirection() == initDirection); + fixture.getMotor().set(0); + } + @Test + public void testInvertingSwitchingNegToPos(){ + fixture.getMotor().setInverted(false); + fixture.getMotor().set(-motorspeed); + Timer.delay(delaytime); + boolean initDirection = fixture.getEncoder().getDirection(); + fixture.getMotor().setInverted(true); + fixture.getMotor().set(motorspeed); + Timer.delay(delaytime); + assertTrue("Inverting with Switching value does change direction", fixture.getEncoder().getDirection() == initDirection); + fixture.getMotor().set(0); + } +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index 56ad8dea8f..b55ce5ddf7 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; EncoderTest.class, GyroTest.class, MotorEncoderTest.class, + MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class, diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java new file mode 100644 index 0000000000..e788c3be65 --- /dev/null +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java @@ -0,0 +1,75 @@ +package edu.wpi.first.wpilibj.can; + +import edu.wpi.first.wpilibj.CANJaguar; +import edu.wpi.first.wpilibj.Timer; +import org.junit.Test; +import static org.junit.Assert.assertTrue; +import java.util.logging.Logger; + +/** + * Created by Patrick Murphy on 3/30/15. + */ +public class CANJaguarInversionTest extends AbstractCANTest{ + private static final Logger logger = Logger.getLogger(CANJaguarInversionTest.class.getName()); + private static final double motorVoltage = 5.0; + private static final double motorPercent = 0.5; + private static final double motorSpeed = 100; + private static final double delayTime = 0.75; + private static final double speedModeDelayTime = 2.0; + @Override + protected Logger getClassLogger() { + return logger; + } + @Test + public void testInvertingVoltageMode(){ + getME().getMotor().setVoltageMode(CANJaguar.kQuadEncoder, 360); + InversionTest(motorVoltage, delayTime); + } + @Test + public void testInvertingPercentMode(){ + getME().getMotor().setPercentMode(CANJaguar.kQuadEncoder, 360); + InversionTest(motorPercent, delayTime); + } + @Test + public void testInvertingSpeedMode(){ + getME().getMotor().setSpeedMode(CANJaguar.kQuadEncoder, 360, 0.1, 0.003, 0.01); + InversionTest(motorSpeed, speedModeDelayTime); + } + /** + * Runs an inversion test + * To use set the jaguar to the proper mode(PercentVbus, voltage, speed) + * @param setpoint the speed/voltage/percent to set the motor to + * @param delayTime the amount of time to delay between starting a motor and checking the encoder + */ + private void InversionTest(double setpoint, double delayTime) { + CANJaguar jag = this.getME().getMotor(); + jag.enableControl(); + jag.setInverted(false); + jag.set(setpoint); + Timer.delay(delayTime); + double initialSpeed = jag.getSpeed(); + jag.set(0.0); + jag.setInverted(true); + jag.set(setpoint); + Timer.delay(delayTime); + jag.set(0.0); + double finalSpeed = jag.getSpeed(); + assertTrue("Inverting with Positive value does not invert direction", + Math.signum(initialSpeed)!= + Math.signum(finalSpeed) + ); + jag.set(-setpoint); + Timer.delay(delayTime); + initialSpeed = jag.getSpeed(); + jag.set(0.0); + jag.setInverted(false); + jag.set(-setpoint); + Timer.delay(delayTime); + finalSpeed = jag.getSpeed(); + jag.set(0.0); + assertTrue("Inverting with Negative value does not invert direction", + Math.signum(initialSpeed)!= + Math.signum(finalSpeed) + ); + } +} diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java index 852ee05424..63a0179159 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite; @RunWith(Suite.class) @SuiteClasses({ CANCurrentQuadEncoderModeTest.class, CANDefaultTest.class, + CANJaguarInversionTest.class, CANPercentQuadEncoderModeTest.class, CANPositionPotentiometerModeTest.class, CANPositionQuadEncoderModeTest.class, diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java index 8c34c484f5..daf5bda679 100644 --- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java +++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java @@ -124,7 +124,7 @@ public abstract class MotorEncoderFixture implements * Checks to see if the speed of the motor is within some range of a given value. * This is used instead of equals() because doubles can have inaccuracies. * @param value The value to compare against - * @param acuracy The accuracy range to check against to see if the + * @param accuracy The accuracy range to check against to see if the * @return true if the range of values between motors speed accuracy contains the 'value'. */ public boolean isMotorSpeedWithinRange(double value, double accuracy){ @@ -136,7 +136,7 @@ public abstract class MotorEncoderFixture implements public boolean reset(){ initialize(); boolean wasReset = true; - + motor.setInverted(false); motor.set(0); Timer.delay(TestBench.MOTOR_STOP_TIME); //DEFINED IN THE TestBench encoder.reset();