diff --git a/wpilibc/wpilibC++Devices/include/RobotDrive.h b/wpilibc/wpilibC++Devices/include/RobotDrive.h index e6cd031fd9..7286331754 100644 --- a/wpilibc/wpilibC++Devices/include/RobotDrive.h +++ b/wpilibc/wpilibC++Devices/include/RobotDrive.h @@ -67,6 +67,7 @@ public: void SetInvertedMotor(MotorType motor, bool isInverted); void SetSensitivity(float sensitivity); void SetMaxOutput(double maxOutput); + void SetCANJaguarSyncGroup(uint8_t syncGroup); void SetExpiration(float timeout); float GetExpiration(); @@ -93,6 +94,7 @@ protected: SpeedController *m_rearLeftMotor; SpeedController *m_rearRightMotor; MotorSafetyHelper *m_safetyHelper; + uint8_t m_syncGroup; private: int32_t GetNumMotors() diff --git a/wpilibc/wpilibC++Devices/src/RobotDrive.cpp b/wpilibc/wpilibC++Devices/src/RobotDrive.cpp index 8d44ca07e7..3eb90ff942 100644 --- a/wpilibc/wpilibC++Devices/src/RobotDrive.cpp +++ b/wpilibc/wpilibC++Devices/src/RobotDrive.cpp @@ -41,6 +41,7 @@ void RobotDrive::InitRobotDrive() { m_maxOutput = 1.0; m_safetyHelper = new MotorSafetyHelper(this); m_safetyHelper->SetSafetyEnabled(true); + m_syncGroup = 0; } /** @@ -507,14 +508,15 @@ void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float Normalize(wheelSpeeds); - uint8_t syncGroup = 0x80; + 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_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); - - CANJaguar::UpdateSyncGroup(syncGroup); + if (m_syncGroup != 0) + { + CANJaguar::UpdateSyncGroup(m_syncGroup); + } m_safetyHelper->Feed(); } @@ -556,14 +558,15 @@ void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rota Normalize(wheelSpeeds); - uint8_t syncGroup = 0x80; + 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_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); - - CANJaguar::UpdateSyncGroup(syncGroup); + if (m_syncGroup != 0) + { + CANJaguar::UpdateSyncGroup(m_syncGroup); + } m_safetyHelper->Feed(); } @@ -595,17 +598,18 @@ void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) { wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL); - uint8_t syncGroup = 0x80; - if (m_frontLeftMotor != NULL) - m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup); - m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup); + m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup); + m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup); if (m_frontRightMotor != NULL) - m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup); - m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup); + m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup); + m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup); - CANJaguar::UpdateSyncGroup(syncGroup); + if (m_syncGroup != 0) + { + CANJaguar::UpdateSyncGroup(m_syncGroup); + } m_safetyHelper->Feed(); } @@ -698,7 +702,15 @@ void RobotDrive::SetMaxOutput(double maxOutput) m_maxOutput = maxOutput; } - +/** + * Set the number of the sync group for the motor controllers. If the motor controllers are {@link CANJaguar}s, + * then they will all be added to this sync group, causing them to update their values at the same time. + * + * @param syncGroup the update group to add the motor controllers to + */ +void RobotDrive::SetCANJaguarSyncGroup(uint8_t syncGroup) { + m_syncGroup = syncGroup; +} void RobotDrive::SetExpiration(float timeout) { 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 ff3c218a37..861d899343 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 @@ -6,14 +6,13 @@ /*----------------------------------------------------------------------------*/ package edu.wpi.first.wpilibj; -import edu.wpi.first.wpilibj.can.CANNotInitializedException; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances; import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; import edu.wpi.first.wpilibj.communication.UsageReporting; /** * Utility class for handling Robot drive based on a definition of the motor configuration. - * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and + * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and * mecanum drive trains are supported. In the future other drive types like swerve might * be implemented. Motor channel numbers are supplied on creation of the class. Those are * used for either the drive function (intended for hand created drive code, such as autonomous) @@ -69,7 +68,7 @@ public class RobotDrive implements MotorSafety { protected SpeedController m_rearLeftMotor; protected SpeedController m_rearRightMotor; protected boolean m_allocatedSpeedControllers; - protected boolean m_isCANInitialized = false;//TODO: fix can + protected byte m_syncGroup = 0; protected static boolean kArcadeRatioCurve_Reported = false; protected static boolean kTank_Reported = false; protected static boolean kArcadeStandard_Reported = false; @@ -488,20 +487,13 @@ 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); - byte syncGroup = (byte)0x80; - - m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup); - m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup); - m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup); - m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup); - - if (m_isCANInitialized) { - try { - CANJaguar.updateSyncGroup(syncGroup); - } catch (CANNotInitializedException e) { - m_isCANInitialized = false; - } + if (m_syncGroup != 0) { + CANJaguar.updateSyncGroup(m_syncGroup); } if (m_safetyHelper != null) m_safetyHelper.feed(); @@ -541,19 +533,13 @@ public class RobotDrive implements MotorSafety { normalize(wheelSpeeds); - byte syncGroup = (byte)0x80; + 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_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup); - m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup); - m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup); - m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup); - - if (m_isCANInitialized) { - try { - CANJaguar.updateSyncGroup(syncGroup); - } catch (CANNotInitializedException e) { - m_isCANInitialized = false; - } + if (this.m_syncGroup != 0) { + CANJaguar.updateSyncGroup(m_syncGroup); } if (m_safetyHelper != null) m_safetyHelper.feed(); @@ -586,24 +572,18 @@ public class RobotDrive implements MotorSafety { throw new NullPointerException("Null motor provided"); } - byte syncGroup = (byte)0x80; - if (m_frontLeftMotor != null) { - m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup); + m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); } - m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup); + m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); if (m_frontRightMotor != null) { - m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup); + m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); } - m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup); + m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); - if (m_isCANInitialized) { - try { - CANJaguar.updateSyncGroup(syncGroup); - } catch (CANNotInitializedException e) { - m_isCANInitialized = false; - } + if (this.m_syncGroup != 0) { + CANJaguar.updateSyncGroup(m_syncGroup); } if (m_safetyHelper != null) m_safetyHelper.feed(); @@ -681,6 +661,15 @@ public class RobotDrive implements MotorSafety { m_maxOutput = maxOutput; } + /** + * Set the number of the sync group for the motor controllers. If the motor controllers are {@link CANJaguar}s, + * then they will all be added to this sync group, causing them to update their values at the same time. + * + * @param syncGroup the update group to add the motor controllers to + */ + public void setCANJaguarSyncGroup(byte syncGroup) { + m_syncGroup = syncGroup; + } /** * Free the speed controllers if they were allocated locally