Preseason Lesson5

From 1511Wookiee
Jump to navigationJump to search


The goal of this lesson is to read an encoder while a motor is turning and use it to stop the motor after turns a specific amount. Encoders are sensors that can count revolutions; this can be used to determine a distance of travel of whatever the motor is powering. Some encoders can sense direction of revolution as well as amount of rotation.

Detailed Introduction

  • Before starting this lesson, you should have completed Lesson 4 have that code available. Use it as a starting point for this lesson.
  • We will keep all existing code we did in prior lessons in place and add entirely new code for this lesson.
  • A new motor connected to a new type of motor controller, a Jaguar, will be used for this lesson.
  • The encoder we will use for this lesson is connected directly to the motor's output shaft. This is good for our testing here, but in a typical use on a robot it might be connected to a wheel or other mechanism to sense the movement of the system directly rather than the direct rotation of the motor.

Detailed Goals

  • When button 2 on the controller (the one we used in Lessons 2 and 3, not the joystick from lesson 4) is pressed, you will spin the motor for exactly one revolution.
    • We only want to start ONE revolution turn when the button is initially pressed down!
    • Put another way -- if you press and hold the button, it should do one complete revolution turn no matter how long the button is held
  • The motor to use is on a secondary board to the main test board.
    • The motor is powered by a new type of motor controller, a Jaguar.
    • The Jaguar is wired to a PWM port on the roborio. The ID for the PWM port controlling the Jaguar and motor we want is number 2
  • You can see the encoder sensor we will use mounted on top of the motor.
    • The encoder is a quadrature encoder which means it can track both direction and distance/revolutions
    • This type of encoder will register 256 counts per full revolution
    • The encoder is connected to the RoboRIO on 'two digital input ports numbered 4 and 5
  • When turning the motor to achieve the rotation, you can spin the motor at any speed you like -- this particular motor spins very slowly even at full speed.

Guided Help

Create Jaguar motor controller

  • The class you will use to talk to the Jaguar motor controller is "frc::Jaguar". It is part of WPILib and is defined in the header file frc/jaguar.h
  • Create a Jaguar object to use to control the motor in your Robot class (robot.h). Do this similar to how you created other objects in past lessons. Remember the ID of the Jaguar we want to use is 2.
    • You can name your motor controller object whatever you like; these instructions will refer to it as "jagMotor".

Create Encoder object

  • To read the encoder, we will use the "frc::Encoder" class. This is part of WPILib and is defined in frc/Encoder.h.
    • Here is the documentation for the encoder class]
    • Look at the constructor for the Encoder class. You will see that it needs at least two arguments , an integer ID representing "channel a" and "channel b". This is the numeric (int) identifiers for the two digital inputs that the encoder is connected to.
    • Supply both IDs when you create your encoder object.
    • You can name your encoder whatever you like; these instructions will refer to it as "encoder".

Tracking if we are mid-rotation or not

  • We will need to keep track of if we are in the midst of performing a single rotation or not since we are not requiring a consistent user command throughout the motion (not requiring a button to be held down or other input we can constantly look at.
    • This is called keeping track of state
    • We can use a bool variable to do this since we have only two states to track -- we are either rotating (true) or not (false)
    • However, we cannot use a local bool variable for this! Recall that Local variables only keep their value during the code block in which they are declared; when the block is exited, the variable ceases to exist and loses whatever value it had!
  • We can use a variable in our class instead!
    • This is just a variable that is declared as part of our class definition, just like our motor controllers and our joystick objects.
    • Class variables keep their value as long as the object of the class exists.
    • Our class is the Robot class; our Robot object will exist for the entire run of our program so any variable we put in it will work and keep its current value for the life of the program running
  • Declare a bool variable in your Robot class to hold the state of rotating or not. You can name it whatever you like; these instructions will use the name "rotationRunning"
    • When declaring simple types, you can supply a single argument to give the initial value for the variable
    • Since we will not be rotating when the program starts up, we should initialize to false

Seeing if a button was just pressed on the controller

  • We will be starting our 1-revolution spin when the button on the controller is pressed down.
  • We do not want to be constantly restarting our motion if the button is held down! This means that we cannot simply use GetRawButton() like in earlier lessons as it does not differentiate between an initial press and the button being held down.
  • The Joystick class we are using has a different method - GetRawButtonPressed(int button) - that can be used to see if a button has gone from not pressed to pressed since the last time it was called!
    • This method returns bool - true if the button has just been pressed, false if not.
    • You will use this in combination with appropriate if statements to decide to start a revolution or not, but read the next section before doing this

Using the Encoder

  • The encoder simply increments a count as the encoder's shaft is turned (by the motor in our setup).
  • The count starts at 0 and goes up by 1 every 1/256th of a revolution - so a full revolution will yield a count of 256.
  • Look again at the Encoder documentation for the following notable methods:
    • Reset() is a method that will cause the internal count of the Encoder object to reset back to zero
    • Get() is a method that will return an int that is the current internal count of the encoder.

Putting it all together

  • Begin by looking at the controller. If the button is newly pressed (see above section), you will start a new rotation!
    • Set your rotation state to indicate you are in a rotation
    • Zero our encoder so we are starting a fresh revolution!
  • If you are in a rotation, you will want to check if the rotation has been completed! Remember this is when a count of 256 or more is achieved. Use if statements to decide!
    • When the rotation is done, set your state to indicate you are no longer rotating
  • Control the motor based on if you are mid-rotation
    • The previous 2 steps have set up an accurate state of if you are working on a rotation or not.
    • You can use that state to set the output to the motor to either run or not


  • Enable the robot - the motor should not run at all
  • Press button 2 - the motor should turn on then off and stay off after a single revolution
  • Now press and hold down button 2. The motor should do the same thing as the prior test - turn once and stop.
  • Now repeatedly tap the button over and over. The motor should start a rotation and continue running forever as long as you are tapping the button (since each tap restarts the revolution)


This is one possible solution! There are many ways to accomplish the goal(s) - this is just one way!


// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <frc/TimedRobot.h>
#include <ctre/Phoenix.h>
#include <frc/Joystick.h>
#include <frc/DigitalInput.h>
#include <frc/Jaguar.h>
#include <frc/Encoder.h>

using namespace ctre::phoenix::motorcontrol::can;
using namespace frc;

class Robot : public frc::TimedRobot {
  // parts of a robot "appendage"
  TalonSRX motor{5};
  Joystick controller{0};
  DigitalInput limitSwitchFwd{1};
  DigitalInput limitSwitchRev{0};

  // parts of our robot's drive system
  TalonSRX driveMotor{3};
  Joystick driveJoystick{1};

  // parts of our robot's positioning mechanism
  Jaguar jagMotor{2};
  Encoder encoder{4,5};
  bool rotationRunning{false};

  void RobotInit() override;
  void RobotPeriodic() override;

  void AutonomousInit() override;
  void AutonomousPeriodic() override;

  void TeleopInit() override;
  void TeleopPeriodic() override;

  void DisabledInit() override;
  void DisabledPeriodic() override;

  void TestInit() override;
  void TestPeriodic() override;


// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "Robot.h"

void Robot::RobotInit() {}
void Robot::RobotPeriodic() {}

void Robot::AutonomousInit() {}
void Robot::AutonomousPeriodic() {}

void Robot::TeleopInit() {}
void Robot::TeleopPeriodic() {
  // "appendage" control
  bool buttonOnePressed = false;
  buttonOnePressed = controller.GetRawButton(1);
  double leftYAxis = 0;
  // controller Y axis is inverted - forward is negative, reverse is positive
  // so flip it
  leftYAxis = -1 * controller.GetRawAxis(1);

  bool fwdLimitTripped = false;
  bool revLimitTripped = false;

  fwdLimitTripped = limitSwitchFwd.Get();
  revLimitTripped = limitSwitchRev.Get();

  if (buttonOnePressed) {
    if (leftYAxis > 0.33 && fwdLimitTripped != true) {
      motor.Set(ControlMode::PercentOutput, 0.25);
    } else if (leftYAxis < -0.33 && !revLimitTripped) {
      motor.Set(ControlMode::PercentOutput, -0.25);
    } else {
      motor.Set(ControlMode::PercentOutput, 0);
  } else {
    motor.Set(ControlMode::PercentOutput, 0);

  // "drive" control
  double driveYAxis = 0;
  driveYAxis = -1 * driveJoystick.GetY();

  if (driveYAxis > 0.25) {
    // Change from (0.25, 1] to (0, 0.75]
    driveYAxis -= 0.25;
    // Now normalize (linearly scale to be full range between 0 and 1)
    driveYAxis = driveYAxis / 0.75;
    driveMotor.Set(ControlMode::PercentOutput, driveYAxis);
  } else if (driveYAxis < -0.25) {
    // Similar to above case, but in negative space so reverse signs as needed
    // Change from (-0.25, -1] to (0, -0.75]
    driveYAxis += 0.25;
    // Now normalize (linearly scale to be full range between 0 and -1)
    driveYAxis = driveYAxis / 0.75;
    driveMotor.Set(ControlMode::PercentOutput, driveYAxis);
  } else {
    driveMotor.Set(ControlMode::PercentOutput, 0);

  // Control for our precisely positioned appendage
  // See if user is commanding a fresh rotation
  if (controller.GetRawButtonPressed(2)) {
    // A fresh press, (re-)start the motion
    rotationRunning = true;
    // reset encoder to zero out count

  // See if any in-progress rotation is complete
  if (encoder.Get() >= 256) {
    // rotation complete
    rotationRunning = false;

  // Now use our internal state to control the motor to either work
  // on the rotation (go) or not (stop)
  if (rotationRunning)


void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}

void Robot::TestInit() {
void Robot::TestPeriodic() {}

int main() {
  return frc::StartRobot<Robot>();