Drivetrain Support¶

Warning

These drivetrain models are not particularly realistic, and if you are using a tank drive style drivetrain you should use the class .TankModel instead.

Based on input from various drive motors, these helper functions simulate moving the robot in various ways. Many thanks to Ether for assistance with the motion equations.

When specifying the robot speed to the below functions, the following may help you determine the approximate speed of your robot: * Slow: 4ft/s * Typical: 5 to 7ft/s * Fast: 8 to 12ft/s

Obviously, to get the best simulation results, you should try to estimate the speed of your robot accurately. Here’s an example usage of the drivetrains:

class MyUserPhysics extends UserPhysics {

  createRobotModel(robotConfig) {
    return new this.Models.TwoMotor();
  }

  updateSim(halData, dt) {
    const dataOut = halData.out;
    const can = dataOut.CAN;

    let lMotor = can[1].value;
    let rMotor = can[2].value;

    let {rcw, fwd} = this.model.getVector(lMotor, rMotor);

    let xSpeed = fwd * Math.cos(this.robot.angle);
    let ySpeed = fwd * Math.sin(this.robot.angle);

    this.Matter.Body.setVelocity(this.robot, { x: xSpeed, y: ySpeed });
    this.Matter.Body.setAngularVelocity(this.robot, rcw);
  }
}

linearDeadzone(deadzone)¶

Real motors won’t actually move unless you give them some minimum amount of input. This computes an output speed for a motor and causes it to ‘not move’ if the input isn’t high enough. Additionally, the output is adjusted linearly to compensate.

Example: For a deadzone of 0.2:

Input of 0.0 will result in 0.0 Input of 0.2 will result in 0.0 Input of 0.3 will result in ~0.12 Input of 1.0 will result in 1.0

This returns a function that computes the deadzone. You should pass the returned function to one of the drivetrain simulation functions as the deadzone parameter.

Arguments:	deadzone (Number) –

class TwoMotorDrivetrain(xWheelbase=2, speed=5, deadzone=null)¶

Two center-mounted motors with a simple drivetrain. The motion equations are as follows:

FWD = (L+R)/2
RCW = (L-R)/W

L is forward speed of the left wheel(s), all in sync
R is forward speed of the right wheel(s), all in sync
W is wheelbase in feet

If you called “SetInvertedMotor” on any of your motors in RobotDrive, then you will need to multiply that motor’s value by -1.

Note

WPILib RobotDrive assumes that to make the robot go forward, the left motor must be set to -1, and the right to +1

Arguments:	xWheelbase (Number) – The distance in feet between right and left wheels. speed (Number) – Speed of robot in feet per second (see above) deadzone (function) – A function that adjusts the output of the motor (see `linear_deadzone()`)

TwoMotorDrivetrain.getVector(lMotor, rMotor)¶

Given motor values, retrieves the vector of (distance, speed) for your robot

Arguments:	lMotor (Number) – Left motor value (-1 to 1); -1 is forward rMotor (Number) – Right motor value (-1 to 1); 1 is forward

class FourMotorDrivetrain(xWheelbase=2, speed=5, deadzone=null)¶

Four motors, each side chained together. The motion equations are as follows:

FWD = (L+R)/2
RCW = (L-R)/W

L is forward speed of the left wheel(s), all in sync
R is forward speed of the right wheel(s), all in sync
W is wheelbase in feet

If you called “SetInvertedMotor” on any of your motors in RobotDrive, then you will need to multiply that motor’s value by -1.

Note

WPILib RobotDrive assumes that to make the robot go forward, the left motors must be set to -1, and the right to +1

Arguments:	xWheelbase (Number) – The distance in feet between right and left wheels. speed (Number) – Speed of robot in feet per second (see above) deadzone (function) – A function that adjusts the output of the motor (see `linear_deadzone()`)

FourMotorDrivetrain.getVector(lrMotor, rrMotor, lfMotor, rfMotor)¶

Arguments:	lrMotor (Number) – Left rear motor value (-1 to 1); -1 is forward rrMotor (Number) – Right rear motor value (-1 to 1); 1 is forward lfMotor (Number) – Left front motor value (-1 to 1); -1 is forward rfMotor (Number) – Right front motor value (-1 to 1); 1 is forward
Returns:	Object – Speed of robot (ft/s), clockwise rotation of robot (radians/s)

class MecanumDrivetrain(xWheelbase=2, yWheelbase=3, speed=5, deadzone=null)¶

Four motors, each with a mechanum wheel attached to it.

If you called “SetInvertedMotor” on any of your motors in RobotDrive, then you will need to multiply that motor’s value by -1.

Note

WPILib RobotDrive assumes that to make the robot go forward,

all motors are set to +1

Arguments:	xWheelbase (Number) – The distance in feet between right and left wheels. yWheelbase (Number) – The distance in feet between forward and rear wheels. speed (Number) – Speed of robot in feet per second (see above) deadzone (function) – A function that adjusts the output of the motor (see `linear_deadzone()`)

MecanumDrivetrain.getVector(lrMotor, rrnotor, lfNotor, rfMotor)¶

Given motor values, retrieves the vector of (distance, speed) for your robot

Arguments:	lrMotor (Number) – Left rear motor value (-1 to 1); 1 is forward rrnotor (Number) – Right rear motor value (-1 to 1); 1 is forward lfNotor (Number) – Left front motor value (-1 to 1); 1 is forward rfMotor (Number) – Right front motor value (-1 to 1); 1 is forward
Returns:	Object – Speed of robot in x (ft/s), Speed of robot in y (ft/s), clockwise rotation of robot (radians/s)

class FourMotorSwerveDrivetrain(xWheelbase=2, yWheelbase=2, speed=5, deadzone=null)¶

Arguments:	xWheelbase (Number) – The distance in feet between right and left wheels. yWheelbase (Number) – The distance in feet between forward and rear wheels. speed (Number) – Speed of robot in feet per second (see above) deadzone (function) – A function that adjusts the output of the motor (see `linear_deadzone()`)

FourMotorSwerveDrivetrain.getVector(lrMotor, rrMotor, lfMotor, rfMotor, lrAngle, rrAngle, lfAngle, rfAngle)¶

Four motors that can be rotated in any direction

If any motors are inverted, then you will need to multiply that motor’s value by -1.

Arguments:

lrMotor (Number) – Left rear motor value (-1 to 1); 1 is forward
rrMotor (Number) – Right rear motor value (-1 to 1); 1 is forward
lfMotor (Number) – Left front motor value (-1 to 1); 1 is forward
rfMotor (Number) – Right front motor value (-1 to 1); 1 is forward
lrAngle (Number) – Left rear motor angle in degrees (0 to 360 measured clockwise from forward position)
rrAngle (Number) – Right rear motor angle in degrees (0 to 360 measured clockwise from forward position)
lfAngle (Number) – Left front motor angle in degrees (0 to 360 measured clockwise from forward position)
rfAngle (Number) – Right front motor angle in degrees (0 to 360 measured clockwise from forward position)

Returns:

Object – Speed of robot in x (ft/s), Speed of robot in y (ft/s), clockwise rotation of robot (radians/s)