Drivetrain#

Introduction#

The Drivetrain controls a robot’s movement, allowing it to drive forward, turn, and stop with precision.

It can also function as a smart drivetrain when configured with a Gyro Sensor or a Brain Inertial Sensor.

For the examples below, the configured drivetrain will be named Drivetrain and will be used in all subsequent examples throughout this API documentation when referring to drivetrain and smartdrive class methods.

Below is a list of all methods:

Actions – Move and turn the robot.

  • drive – Moves the drivetrain in a specified direction indefinitely.

  • driveFor – Moves the drivetrain in a specified direction for a set distance.

  • turn – Turns the drivetrain left or right indefinitely.

  • turnFor – Turns the drivetrain left or right for a set distance.

  • turnToHeading – Turns a smart drivetrain to a specified heading.

  • turnToRotation – Turns a smart drivetrain to a specified rotational value.

  • stop – Stops a drivetrain.

  • calibrateDrivetrain – Calibrates the drivetrain.

Mutators – Set default movement and turn speeds.

Getters – Return robot state and position.

  • isDone – Returns whether a drivetrain is not currently moving.

  • isMoving – Returns whether a drivetrain is currently moving.

  • heading – Returns the current heading of a smart drivetrain.

  • rotation – Returns the current rotational value of a smart drivetrain.

  • velocity – Returns the current velocity of a drivetrain.

  • current – Returns the current current of a drivetrain.

  • power – Returns the average power of the smart drivetrain.

  • torque – Returns the average torque of the drivetrain.

  • efficiency – Returns the average efficiency of the drivetrain.

  • temperature – Returns the average temperature of the drivetrain.

Constructors – Manually initialize and configure the drivetrain.

  • drivetrain – Creates a basic drivetrain.

  • smartdrive – Creates a drivetrain configured with a Gyro Sensor or Brain Inertial Sensor.

Actions#

drive#

drive moves the drivetrain in a specified direction indefinitely.

Default Usage:
Drivetrain.drive(direction);

Overload Usages:
Drivetrain.drive(direction, velocity, units);

Parameters

Description

direction

The direction in which to drive:

  • forward
  • reverse

velocity

The velocity at which the drivetrain will move as a float or integer. If the velocity is not specified, the default velocity is 50%.

units

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Drive forward, then stop
  Drivetrain.drive(forward);
  wait(2, seconds);
  Drivetrain.stop();
}

driveFor#

driveFor moves the drivetrain in a specified direction for a set distance.

Default Usage:
Drivetrain.driveFor(direction, distance, units, wait);

Overload Usages:
Drivetrain.driveFor(direction, distance, units, wait);
Drivetrain.driveFor(direction, distance, units, velocity, units_v, wait);
Drivetrain.driveFor(distance, units, wait);
Drivetrain.driveFor(distance, units, velocity, units_v, wait);

Parameters

Description

direction

The direction in which to drive:

  • forward
  • reverse

distance

The distance for the drivetrain to move as a double.

units

The unit that represents the distance:

  • mm – Millimeters
  • inches – Inches
  • cm – Centimeters

wait

Optional.

  • true (default) – The project waits until driveFor is complete before executing the next line of code.
  • false - The project starts the action and moves on to the next line of code right away, without waiting for driveFor to finish.

velocity

The velocity at which the drivetrain will move as a double. If the velocity is not specified, the default velocity is 50%.

units_v

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Drive 200 mm, then back 200 mm
  Drivetrain.driveFor(forward, 200, mm);
  Drivetrain.driveFor(reverse, 200, mm);
}

turn#

turn turns the drivetrain left or right indefinitely.

Default Usage:
Drivetrain.turn(direction);

Overload Usages:
Drivetrain.turn(direction, velocity, units);

Parameters

Description

direction

The direction in which to turn:

  • left
  • right

velocity

The velocity at which the drivetrain will turn as a double. If the velocity is not specified, the default velocity is 50%.

units

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Turn right, then left, then stop
  Drivetrain.turn(right);
  wait(2, seconds);
  Drivetrain.turn(left);
  wait(2, seconds);
  Drivetrain.stop();
}

turnFor#

turnFor turns the drivetrain left or right for a set distance.

Default Usage:
Drivetrain.turnFor(direction, angle, units, wait);

Overload Usages:
Drivetrain.turnFor(direction, angle, units, velocity, units_v, wait);
Drivetrain.turnFor(angle, units, wait);
Drivetrain.turnFor(angle, units, velocity, units_v, wait);

Parameters

Description

direction

The direction in which to turn:

  • left
  • right

angle

The amount of degrees the drivetrain will turn as a double.

units

The unit that represents the rotational value:

  • degrees
  • turns

velocity

The velocity at which the drivetrain will turn as a double. If the velocity is not specified, the default velocity is 50%.

units_v

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

wait

Optional.

  • true (default) – The project waits until turnFor is complete before executing the next line of code.
  • false - The project starts the action and moves on to the next line of code right away, without waiting for turnFor to finish.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Turn right then left
  Drivetrain.turnFor(right, 90, degrees);
  wait(1, seconds);
  Drivetrain.turnFor(left, 90, degrees);
}

turnToHeading#

turnToHeading turns a smart drivetrain to a specified heading.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Default Usage:
Drivetrain.turnToHeading(angle, units, wait);

Overload Usages:
Drivetrain.turnToHeading(angle, units, velocity, units_v, wait);

Parameters

Description

angle

The heading to turn the drivetrain to face as a float or integer.

units

The unit that represents the rotational value:

  • degrees
  • turns

velocity

The velocity at which the motor or motor group will spin as a float or integer. If the velocity is not specified, the default velocity is 50%.

units_v

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

wait

Optional.

  • true (default) – The project waits until turnToHeading is complete before executing the next line of code.
  • false - The project starts the action and moves on to the next line of code right away, without waiting for turnToHeading to finish.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Face the cardinal directions
  Drivetrain.turnToHeading(90, degrees);
  wait(1, seconds);
  Drivetrain.turnToHeading(180, degrees);
  wait(1, seconds);
  Drivetrain.turnToHeading(270, degrees);
  wait(1, seconds);
  Drivetrain.turnToHeading(0, degrees);
}

turnToRotation#

turnToRotation turns a smart drivetrain to a specified rotational value.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or the Brain’s Inertial Sensor.

Default Usage:
Drivetrain.turnToRotation(angle, units, wait);

Overload Usages:
Drivetrain.turnToRotation(angle, units, velocity, units_v, wait);

Parameters

Description

angle

The rotational value to turn the drivetrain to face as a float or integer.

units

The unit that represents the rotational value:

  • degrees
  • turns

velocity

The velocity at which the motor or motor group will spin as a float or integer. If the velocity is not specified, the default velocity is 50%.

units_v

The unit that represents the velocity:

  • velocityUnits::pct (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

wait

Optional.

  • true (default) – The project waits until turnToRotation is complete before executing the next line of code.
  • false - The project starts the action and moves on to the next line of code right away, without waiting for turnToRotation to finish.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Turn left, then spin clockwise to face right
  Drivetrain.turnToRotation(-90, degrees);
  wait(1, seconds);
  Drivetrain.turnToRotation(450, degrees);
}

stop#

stop stops a drivetrain.

Default Usage:
Drivetrain.stop();

Overload Usages:
Drivetrain.stop(mode);

Parameters

Description

mode

How the drivetrain will stop:

  • coast – Slows gradually to a stop.
  • brake – Stops immediately.
  • hold – Stops and resists movement using motor feedback.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Drive forward, then coast to a stop
  Drivetrain.setDriveVelocity(100, percent);
  Drivetrain.drive(forward);
  wait(2, seconds);
  Drivetrain.stop(coast);
}

calibrateDrivetrain#

calibrateDrivetrain calibrates the drivetrain.

This is a method generated by the VEXcode Robot Configuration when a Drivetrain is configured in the Devices window. It will not function outside of VEXcode IQ (2nd gen).

Note: The Brain will automatically calibrate at the start of each project.

Usage:
Drivetrain.calibrateDrivetrain();

Mutators#

setDriveVelocity#

setDriveVelocity sets the default moving velocity for a drivetrain. This velocity setting will be used for subsequent calls to any drivetrain functions if a specific velocity is not provided.

Usage:
Drivetrain.setDriveVelocity(velocity, units);

Parameters

Description

velocity

The velocity at which the drivetrain will move as a double.

units

The unit that represents the velocity:

  • percent (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Try default, slow, then fast
  Drivetrain.driveFor(forward, 150, mm);
  wait(1, seconds);
  Drivetrain.setDriveVelocity(20, percent);
  Drivetrain.driveFor(forward, 150, mm);
  wait(1, seconds);
  Drivetrain.setDriveVelocity(100, percent);
  Drivetrain.driveFor(forward, 150, mm);
}

setTurnVelocity#

setTurnVelocity sets the default turning velocity for a drivetrain. This velocity setting will be used for subsequent calls to any drivetrain functions if a specific velocity is not provided.

Usage:
Drivetrain.setTurnVelocity(velocity, units);

Parameters

Description

velocity

The velocity at which the drivetrain will turn as a double.

units

The unit that represents the velocity:

  • percent (Percent)
  • rpm (Rotations per minute)
  • dps (Degrees per second)

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Try default, slow, then fast
  Drivetrain.turnFor(right, 360, degrees);
  wait(1, seconds);
  Drivetrain.setTurnVelocity(20, percent);
  Drivetrain.turnFor(right, 360, degrees);
  wait(1, seconds);
  Drivetrain.setTurnVelocity(100, percent);
  Drivetrain.turnFor(right, 360, degrees);
}

setStopping#

setStopping sets the stopping mode for a drivetrain.

Usage:
Drivetrain.setStopping(mode);

Parameters

Description

mode

How the drivetrain will stop:

  • brake – Stops immediately.
  • coast – Slows gradually to a stop.
  • hold – Stops and resists movement using motor feedback.

setTimeout#

setTimeout sets a time limit for how long a drivetrain function will wait to reach its target. If the drivetrain cannot complete the movement within the set time, it will stop automatically and continue with the next function.

Note: The drivetrain’s time limit is used to prevent drivetrain functions that do not reach their target position from stopping the execution of the rest of the project.

Usage:
Drivetrain.setTimeout(time, units);

Parameters

Description

time

The maximum number of seconds a motor function will run before stopping and moving to the next function as an integer.

units

The unit that represents the time:

  • seconds
  • msec – Milliseconds

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Stop turning if drive took too long
  Drivetrain.setTimeout(1, seconds);
  Drivetrain.driveFor(forward, 25, inches);
  Drivetrain.turnFor(right, 90, degrees);
}

setHeading#

setHeading sets the heading of a smart drivetrain.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.setHeading(value, units);

Parameters

Description

value

The new heading as a double.

units

The unit that represents the heading:

  • degrees
  • turns

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Set 0° to current right-facing
  Drivetrain.setHeading(90, degrees);
  Drivetrain.turnToHeading(0, degrees);
}

setRotation#

setRotation sets the rotation for the smart drivetrain.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.setRotation(value, units);

Parameters

Description

value

The new rotational value as a double.

units

The unit that represents the rotation:

  • degrees
  • turns

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Spin counterclockwise for 2 turns, then face forward
  Drivetrain.setRotation(720, degrees);
  Drivetrain.turnToRotation(0, degrees);
}

setGearRatio#

setGearRatio sets the gear ratio for all motors on the Drivetrain.

Usage:
Drivetrain.setGearRatio(mode);

Parameters

Description

mode

Gear Ratio options:

  • ratio1_1 – A gear unit that is defined as 1:1 gearing.
  • ratio2_1 – A gear unit that is defined as 2:1 gearing.
  • ratio3_1 – A gear unit that is defined as 3:1 gearing.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Set 2:1 gear ratio
  Drivetrain.setGearRatio(ratio2_1);
}

setTurnThreshold#

setTurnThreshold method sets the turn threshold for the Smartdrive. The threshold value is used to determine that turns are complete. If this is too large, then turns will not be accurate. If too small, then turns may not complete.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.setTurnThreshold(t);

Parameters

Description

t

The new turn threshold in degrees. The default for a Smart Drivetrain is 1 degree.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Increase turn tolerance to 5 degrees
  Drivetrain.setTurnThreshold(5);
}

setTurnConstant#

setTurnConstant method sets the turn constant for the Smartdrive. The Smart Drivetrain uses a simple P controller when doing turns. This constant, generally known as kp, is the gain used in the equation that turns angular error into motor velocity.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.setTurnConstant(kp);

Parameters

Description

kp

The new turn constant in the range 0.1 - 4.0. The default is 1.0.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Increase turn gain to 2.0
  Drivetrain.setTurnConstant(2.0);
}

setTurnDirectionReverse#

setTurnDirectionReverse method sets the expected turn direction for positive heading.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.setTurnDirectionReverse(value);

Parameters

Description

value

A Boolean value to set the reversed flag to true or false.

Getters#

isDone#

isDone returns a Boolean indicating whether a drivetrain is not currently moving.

  • true – The drivetrain is not moving.

  • false – The drivetrain is moving.

Usage:
Drivetrain.isDone()

Parameters

Description

This method has no parameters.

isMoving#

isMoving returns a Boolean indicating whether a drivetrain is currently moving.

  • true – The drivetrain is moving.

  • false – The drivetrain is not moving.

Usage:
Drivetrain.isMoving()

Parameters

Description

This method has no parameters.

heading#

heading returns the current heading of a smart drivetrain as a double.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.heading(units)

Parameters

Description

units

Optional. The unit that represents the rotational value:

  • degrees (default)
  • turns

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Display heading after a long turn
  Drivetrain.turnFor(right, 450, degrees);
  Brain.Screen.print("Heading: %f", Drivetrain.heading(degrees));
}

rotation#

rotation returns the current rotational value of a smart drivetrain as a double.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.rotation(units)

Parameters

Description

units

Optional. The unit that represents the rotational value:

  • degrees (default)
  • turns

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Display rotation after a long turn
  Drivetrain.turnFor(right, 450, degrees);
  Brain.Screen.print("Rotation: %f", Drivetrain.rotation(degrees));
}

velocity#

velocity returns the current velocity of a drivetrain as a double.

Usage:
Drivetrain.velocity(units)

Parameters

Description

units

The unit that represents the velocity:

  • percent
  • rpm – Rotations per minute
  • dps – Degrees per second

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Show velocity before and during motion
  Brain.Screen.print("Resting: %f", Drivetrain.velocity(percent));
  Drivetrain.drive(forward, 100, velocityUnits::pct);
  wait(1, seconds);
  Brain.Screen.newLine();
  Brain.Screen.print("Moving: %f", Drivetrain.velocity(percent));
  Drivetrain.stop();
}

current#

current returns the drivetrain’s current as a double.

Usage:
Drivetrain.current(units)

Parameters

Description

units

Optional. The unit that represents the current:

  • percent
  • amp (default)

power#

power(units) returns the average power of the Smart Drivetrain as a double.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.power(units)

Parameters

Description

units

The only valid unit for power is watt.

torque#

torque returns the average torque of the Drivetrain as a double.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.torque(units)

Parameters

Description

units

A valid torque Unit:

  • Nm (default) – A torque unit that is measured in newton meters.
  • InLb – A torque unit that is measured in inch pounds.

efficiency#

efficiency returns the average efficiency of the Drivetrain as a double.

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Usage:
Drivetrain.efficiency(units)

Parameters

Description

units

The only valid unit for efficiency is percent.

temperature#

The temperature() method returns the average temperature of the Drivetrain as a double.

Usage:
Drivetrain.temperature(units)

Note: This method will only work with a drivetrain that has been configured with a Gyro Sensor or Brain Inertial Sensor.

Parameters

Description

units

The only valid unit for temperature is percent.

Constructors#

Drivetrain#

A drivetrain is created by using the following constructor:

The drivetrain constructor creates a drivetrain object.

Usage:
drivetrain(lm, rm, wheelTravel, trackWidth, wheelBase, unit, externalGearRatio);

Parameter

Description

lm

The name of the Left Motor or Motor Group.

rm

The name of the Right Motor or Motor Group.

wheelTravel

The circumference of the driven wheels. The default is 300 mm.

trackWidth

The track width of the drivetrain. The default is 320 mm.

wheelBase

The wheel base of the Drivetrain. The default is 320 mm.

unit

A valid distanceUnit for the units that wheelTravel, trackWidth and wheelBase are specified in. The default is MM.

externalGearRatio

The gear ratio used to compensate drive distances if gearing is used.

Motors and/or Motor Groups must be created first before they can be used to create an object with the drivetrain Class constructor.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Make left and right drive motors
  motor leftMotor = motor(PORT1, false);
  motor rightMotor = motor(PORT2, true);

  // Build a 2-motor drivetrain with those motors
  drivetrain Drivetrain = drivetrain(leftMotor, rightMotor,
                                     259.34, 320, 40, mm, 1);
}

If making a 4-Motor Drivetrain, you need to create the Motors separately before grouping them into a Motor Group.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Make two motor groups for left and right motors
  motor leftA = motor(PORT1, false);
  motor leftB = motor(PORT2, false);
  motor_group leftGroup = motor_group(leftA, leftB);

  motor rightA = motor(PORT3, true);
  motor rightB = motor(PORT4, true);
  motor_group rightGroup = motor_group(rightA, rightB);

  // Build a 4-motor drivetrain with those motors
  drivetrain Drivetrain = drivetrain(leftGroup, rightGroup, 259.34, 320, 40, mm, 1);
}

This Drivetrain object will be used in all subsequent examples throughout this API documentation when referring to drivetrain class methods.

To incorporate an Inertial Sensor or Gyro Sensor for enhanced turning functionality in your Drivetrain, you can include it when creating an object using the smartdrive Class constructor.

Smartdrive#

A Smart Drivetrain is created by using the following constructor:

The smartdrive constructor creates a smartdrive object. This object can do everything a drivetrain can do but has additional methods that include use of an Inertial Sensor or Gyro in order to make more accurate turns.

Usage:
smartdrive(lm, rm, guido, wheelTravel, trackWidth, wheelBase, unit, externalGearRatio);

Parameter

Description

lm

The name of the Left Motor or Motor Group.

rm

The name of the Right Motor or Motor Group.

guido

The name of the device you are using to detect the angle or heading of the robot. This can be any Inertial Sensor, or Gyro Sensor.

wheelTravel

The circumference of the driven wheels. The default is 300 mm.

trackWidth

The track width of the Smart Drivetrain. The default is 320 mm.

wheelBase

The wheel base of the Smart Drivetrain. The default is 320 mm.

unit

A valid distanceUnit for the units that wheelTravel, trackWidth and wheelBase are specified in. The default is mm.

externalGearRatio

The gear ratio used to compensate drive distances if gearing is used.

Motors and/or Motor Groups must be created first before they can be used to create an object with the smartdrive Class constructor.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Create two motors
  motor leftMotor = motor(PORT1, false);
  motor rightMotor = motor(PORT2, true);

  // Create an Inertial sensor
  inertial Inertial = inertial(PORT3);

  // Build a 2-motor smartdrive with the Inertial Sensor
  smartdrive Smartdrive = smartdrive(leftMotor, rightMotor, Inertial, 259.34, 320, 40, mm, 1);
}

If making a 4-Motor Smart Drivetrain, you need to create the Motors separately before grouping them into a Motor Group.

int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // Make two motor groups for left and right motors
  motor leftA = motor(PORT1, false);
  motor leftB = motor(PORT2, false);
  motor_group leftGroup = motor_group(leftA, leftB);

  motor rightA = motor(PORT3, true);
  motor rightB = motor(PORT4, true);
  motor_group rightGroup = motor_group(rightA, rightB);

  // Create an Inertial sensor
  inertial Inertial = inertial(PORT5);

  // Build a 4-motor smartdrive with the Inertial Sensor
  smartdrive Smartdrive = smartdrive(leftGroup, rightGroup, Inertial, 259.34, 320, 40, mm, 1);
}

This Smartdrive object will be used in all subsequent examples throughout this API documentation when referring to smartdrive class methods.

To create an object without an Inertial Sensor or Gyro Sensor functionality, instead create an object using the drivetrain Class constructor.