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.0This 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 centermounted motors with a simple drivetrain. The motion equations are as follows:
FWD = (L+R)/2 RCW = (LR)/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 = (LR)/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)