Chapter 4: Robot Motion - Motor Control

Now that we are ready to program, we will start with declaring and controlling individual motors. Anything that interacts with the robot hardware must use the wpilib and its methods.

We do not control the motors directly. Instead, the motors are physically connected to motor controllers that are then controlled by the code. There are many types of motor controllers that FRC uses. These include Victors, Sparks, Jaguars, Talons, and others. To effectively be able to utilize the full power of motor controllers (and any object in that sense), the use of the wpilib API becomes very important. The API (linked at the bottom of every chapter for reference) contains information on how to declare and use every object in wpilib. The objects range from motor controllers and sensors to buses and inputs/outputs on the roboRIO.

As an example, we will construct and control a Victor motor controller. Looking at the API entry for Victor, located here, you can see that the Victor object takes a single argument called channel. The channel here is the port that the motor controller is connected to on the RoboRIO. Many other objects use a similar approach when being constructed since they are mostly connected to the RoboRIO ports as well. 

self.motor = wpilib.Victor(4)

Code 4.1: as discussed, the following code resides in robotInit and constructs a Victor object connected to PWM port 4 of the RoboRIO. The code to the left of the equal sign is the variable name accompanied by self to turn it into a global one that can be called in any function.

Now to be able to operate the motor, you have to set the PWM speed. PWM speed ranges from -1.0 to 1.0 specifying the speed and direction of the motors. To set the motor to full speed forward, you can do this:

self.motor.set(1)

Code 4.2: note that instead of using wpilib.Victor(4).set(1) we utilized the new variable name we had asigned earlier, simplifying the process.

Drive Train Control

Now that you know how to set up and control individual motors, you will be able to use that knowledge to combine such motors into groups that can ultimately drive a completed robot. The focus will be on Differential Drive which is a type of drive train control that works with a physical 2 or 4 wheel robot platform, which most robots utilize.

In the next example, you will construct four Victor motors, group them into the left side and right side of the robot, and create a Differential Drive object that can control each side. At first, we will have the robot drive forward autonomously, then map it to a joystick for manual operation.


from wpilib.drive import DifferentialDrive

class MyRobot(wpilib.TimedRobot):
  def robotInit(self):
      self.frontLeft = wpilib.Victor(1)
      self.rearLeft = wpilib.Victor(2)
      self.frontRight = wpilib.Victor(3)
      self.rearRight = wpilib.Victor(4)

      self.leftSide = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
      self.rightSide = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight)

      self.drive = DifferentialDrive(self.leftSide, self.rightSide)

Code 4.3: referencing the documentation for Differential Drive , we have constructed four motor controllers (as this is a robot with four motors) and using the SpeedControllerGroup method, we have mapped specific motors to a side. Finally, using the imported DifferentialDrive package, a drive group is constructed.

The foundation for driving the robot is constructed; however, the code does not know what to do with it. For this reason, you will have to specify a drive method in teleopPeriodic or autonomousPeriodic and its controls; whether that's autonomous controls or joystick is up to you.

Types of Differential Drive:

  • Tank Drive - traditional tank controls with two joystick axes controlling each side of the robot.
  • Arcade Drive - one joystick axis controlling robot movement and turns.
  • Curvature Drive - similar to Arcade Drive but with the addition of a quick turn mode.

Autonomous Driving

You can utilize the timer class in wpilib to control the robot movements in autonomous. This is the simplest way and by no means accurate but will get you started in programming autonomous controls. To do so, simply add self.timer = wpilib.Timer() to robotInit.


def autonomousInit(self):
    self.timer.reset()
    self.timer.start()

def autonomousPeriodic(self):
    if self.timer.get() < 5.0:
        self.drive.tankDrive(0.5, 0)  
    else:
        self.drive.tankDrive(0, 0)

Code 4.4: let's analyze the code above. In autonomousInit the constructed timer is called to reset and then start the timer as soon as the autonomous mode of the match is called. While autonomous is running, the robot uses the get() method of the Timer class to, you guessed it, get the match time. Using this match time, you can specify for a set amount of time for the robot to move forward at half speed using the newly created self.drive. As soon as the 5 seconds is passed, the robot will stop moving.

This method of autonomous robot motion can be modified to allow for all sorts of cruising through the field. By nesting if statements, you can create more complex robot motion such as turning with different speeds. However, be mindful that due to the alignment of the motors, negative speed might actually go forward and vice versa. For this reason, always place the robot off the ground and test wheel rotations.

Teleoperated Driving

If you wish to use a joystick to operate the robot, you can do so with the joystick class. The joystick here is a generic name for any kind of input ranging from game controllers such as an Xbox controller to flight joysticks and even a button box. To construct a joystick object, add the following code to robotInit: self.joystick = wpilib.Joystick(1). The Joystick class also requires a port number but this time, the port number is the one on your computer. You can find out which port number the joystick is connected to using the USB Devices tab in the Driver Station software.


def teleopPeriodic(self):
    self.leftAxis = self.joystick.getRawAxis(1)
    self.rightAxis = self.joystick.getRawAxis(2)

    self.drive.tankDrive(self.leftAxis, self.rightAxis)

Code 4.5: the Driver Station software maps joystick slots to specific ports as well as their buttons and axes to specific numbers. In the example above, axis 1 controls the left side of the robot and axis 2 controls the right side. Then, they are passed onto tankDrive object that is under Differential Drive so that the robot can be controlled manually using the joystick. Notice that the arguments each method takes are listed under the respective API entries that are also posted at the bottom of each chapter.

Chapter resources: WPILIB API | Anatomy of Robot | Drive Train Types | Differential Drive | Joystick | Jaguar | Spark | Talon | Victor