Class KilloughDrive
- java.lang.Object
-
- edu.wpi.first.wpilibj.MotorSafety
-
- edu.wpi.first.wpilibj.drive.RobotDriveBase
-
- edu.wpi.first.wpilibj.drive.KilloughDrive
-
- All Implemented Interfaces:
Sendable
,AutoCloseable
public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable
A class for driving Killough drive platforms.Killough drives are triangular with one omni wheel on each corner.
Drive base diagram:
/_____\ / \ / \ \ / ---
Each drive() function provides different inverse kinematic relations for a Killough drive. The default wheel vectors are parallel to their respective opposite sides, but can be overridden. See the constructor for more information.
This library uses the NED axes convention (North-East-Down as external reference in the world frame): http://www.nuclearprojects.com/ins/images/axis_big.png.
The positive X axis points ahead, the positive Y axis points right, and the positive Z axis points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is positive.
-
-
Nested Class Summary
Nested Classes Modifier and Type Class Description static class
KilloughDrive.WheelSpeeds
-
Nested classes/interfaces inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
RobotDriveBase.MotorType
-
-
Field Summary
Fields Modifier and Type Field Description static double
kDefaultBackMotorAngle
static double
kDefaultLeftMotorAngle
static double
kDefaultRightMotorAngle
-
Fields inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
kDefaultDeadband, kDefaultMaxOutput, m_deadband, m_maxOutput
-
-
Constructor Summary
Constructors Constructor Description KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor)
Construct a Killough drive with the given motors and default motor angles.KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor, double leftMotorAngle, double rightMotorAngle, double backMotorAngle)
Construct a Killough drive with the given motors.
-
Method Summary
All Methods Instance Methods Concrete Methods Modifier and Type Method Description void
close()
void
driveCartesian(double ySpeed, double xSpeed, double zRotation)
Drive method for Killough platform.void
driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Drive method for Killough platform.KilloughDrive.WheelSpeeds
driveCartesianIK(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Cartesian inverse kinematics for Killough platform.void
drivePolar(double magnitude, double angle, double zRotation)
Drive method for Killough platform.String
getDescription()
void
initSendable(SendableBuilder builder)
Initializes thisSendable
object.void
stopMotor()
-
Methods inherited from class edu.wpi.first.wpilibj.drive.RobotDriveBase
applyDeadband, feedWatchdog, normalize, setDeadband, setMaxOutput
-
Methods inherited from class edu.wpi.first.wpilibj.MotorSafety
check, checkMotors, feed, getExpiration, isAlive, isSafetyEnabled, setExpiration, setSafetyEnabled
-
-
-
-
Field Detail
-
kDefaultLeftMotorAngle
public static final double kDefaultLeftMotorAngle
- See Also:
- Constant Field Values
-
kDefaultRightMotorAngle
public static final double kDefaultRightMotorAngle
- See Also:
- Constant Field Values
-
kDefaultBackMotorAngle
public static final double kDefaultBackMotorAngle
- See Also:
- Constant Field Values
-
-
Constructor Detail
-
KilloughDrive
public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor)
Construct a Killough drive with the given motors and default motor angles.The default motor angles make the wheels on each corner parallel to their respective opposite sides.
If a motor needs to be inverted, do so before passing it in.
- Parameters:
leftMotor
- The motor on the left corner.rightMotor
- The motor on the right corner.backMotor
- The motor on the back corner.
-
KilloughDrive
public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor, double leftMotorAngle, double rightMotorAngle, double backMotorAngle)
Construct a Killough drive with the given motors.Angles are measured in degrees clockwise from the positive X axis.
- Parameters:
leftMotor
- The motor on the left corner.rightMotor
- The motor on the right corner.backMotor
- The motor on the back corner.leftMotorAngle
- The angle of the left wheel's forward direction of travel.rightMotorAngle
- The angle of the right wheel's forward direction of travel.backMotorAngle
- The angle of the back wheel's forward direction of travel.
-
-
Method Detail
-
close
public void close()
- Specified by:
close
in interfaceAutoCloseable
-
driveCartesian
public void driveCartesian(double ySpeed, double xSpeed, double zRotation)
Drive method for Killough platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
-
driveCartesian
public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Drive method for Killough platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.gyroAngle
- The current angle reading from the gyro in degrees around the Z axis. Use this to implement field-oriented controls.
-
drivePolar
public void drivePolar(double magnitude, double angle, double zRotation)
Drive method for Killough platform.Angles are measured counter-clockwise from straight ahead. The speed at which the robot drives (translation) is independent from its angle or rotation rate.
- Parameters:
magnitude
- The robot's speed at a given angle [-1.0..1.0]. Forward is positive.angle
- The angle around the Z axis at which the robot drives in degrees [-180..180].zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.
-
driveCartesianIK
public KilloughDrive.WheelSpeeds driveCartesianIK(double ySpeed, double xSpeed, double zRotation, double gyroAngle)
Cartesian inverse kinematics for Killough platform.Angles are measured clockwise from the positive X axis. The robot's speed is independent from its angle or rotation rate.
- Parameters:
ySpeed
- The robot's speed along the Y axis [-1.0..1.0]. Right is positive.xSpeed
- The robot's speed along the X axis [-1.0..1.0]. Forward is positive.zRotation
- The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is positive.gyroAngle
- The current angle reading from the gyro in degrees around the Z axis. Use this to implement field-oriented controls.- Returns:
- Wheel speeds.
-
stopMotor
public void stopMotor()
- Specified by:
stopMotor
in classRobotDriveBase
-
getDescription
public String getDescription()
- Specified by:
getDescription
in classRobotDriveBase
-
initSendable
public void initSendable(SendableBuilder builder)
Description copied from interface:Sendable
Initializes thisSendable
object.- Specified by:
initSendable
in interfaceSendable
- Parameters:
builder
- sendable builder
-
-