Preseason Lesson3

From 1511Wookiee
Jump to navigationJump to search


In Lesson 2, you made code that ran a motor when the driver station controller's stick was moved off-center and a button was held down. That setup represented a fairly common control setup and motor pattern for a low-speed free-rotating part of a robot -- think a roller bar that collects or moves game pieces around.

This lesson will build on the concepts you learned in the last lesson, but instead of an appendage that freely rotates, we will look at how to limit movement based on sensors inside the robot itself. The overall goal is to move a motor while using the controller similar to the last lesson, but we will stop that movement if a sensor it tripped (even if the controller action would otherwise cause us to move the motor).

This type of control is pretty typical of a robot appendage with bounded movement where we want to protect the motor from moving the appendage too far in either direction because it could damage itself or maybe because it would cause us to extend the appendage too far for the game rules. One concrete example of this would be an elevator that can move up or down but has limit sensors at the top and bottom of the elevator's movement range.

Detailed Introduction

  • Before starting this lesson, you should have completed Lesson 2 have that code available.
  • In this lesson we will learn how to read from digital sensors. Digital sensors are sensors that are either on or off. Common examples of Digital sensors:
    • A mechanical switch (similar to a light switch)
    • An optical beam sensor (similar to grocery store conveyor belt)
    • An optical retro-reflective sensor
  • This lesson will expand your previous use of control statements (if/else) to use more complicated conditions for making control decisions.

Detailed Goals

  • Continue controlling Motor 5 with the same input and actions from Lesson 2
  • You will add new code that uses the two large lever-arm switches on the test board to limit motion of the motor
    • The limit switch closest to the motor should limit motion in the reverse direction. If that switch is pressed, the motor should not be allowed to spin in reverse but it can spin forward if instructed to do so.
    • The limit switch furthest from the motor should limit motion in the forward direction. If that switch is pressed, the motor should not be allowed to spin forward but it can spin in reverse if instructed to do so.

Guided Help

Create DigitalInputs for the Switches

As mentioned in the introduction, the switches we are using are digital sensors, which means their value can only be interpreted by the control system as "on" or "off". Notice that on the test board, the switches are electrically wired to the Digital IO (DIO) set of pins. To read digital sensors (any sensor connected to DIO pins), we will create and use a DigitalInput object. Here is a direct link to the documentation for the DigitalInput class.

  • The process for creating our DigitalInput object is exactly the same as the process used to make our motor controller object in Lesson 1 and our Joystick object in Lesson 2!
  • The DigitalInput class is declared in the DigitalInput.h file. The path to include so that you can use this class is <frc/DigitalInput.h>
  • Similar to Joystick and any other WPILib provided class, the DigitalInput class is in the frc namespace. Once again, you can refer to it by its full name frc::DigitalInput or you use its short name (DigitalInput) if you told your program to use the entire frc namespace
  • Using the above information, construct two DigitalInput objects in your main Robot class to represent the forward and reverse limit switches:
    • If you look at the DigitalInput documentation, the constructor for DigitalInput expects an int argument that represents the channel number that identifies the DIO port on the roboRIO that our switch is wired to. This is similar to how our Joystick objects needed the numbered identifier to indicate which controller to read from, or our motor controller needed its number identifier to know which motor to control.
    • Look at the test board to determine the correct channel numbers for both switches!
    • Remember that the forward limit is the switch furthest from our motor and the reverse limit is the switch closest to our motor.
    • You can name your DigitalInput objects using any name that makes sense to you. The rest of this guided help will use the name limitSwitchFwd for the forward direction limit switch and limitSwitchRev for the reverse direction limit switch.

Reading the Current State of the Limit Switches

Similar to how we would read in the current state of our gamepad buttons, we want to read in the current state of our limit switches to see if they are tripped (pressed) or not.

  • Look at the documentation for the DigitalInput class
    • Here you can see there is a member method (a function that is part of a class that lets us interact with it) named Get()
    • Get() is declared as:
      bool Get();
      • This means that Get() takes no arguments (we don't need to tell it anything additional) and it returns a bool
      • This probably is what we would expect -- Digital sensors can only have one of two values - "on" or "off. So "bool" is a good data type for this, since it also can only have one of two values - "true" or "false"
      • Does "true" necessarily mean our sensor is "on" or our switch is "tripped"?? Not necessarily! This depends on how the sensor is electrically wired to the roboRIO!.
      • The test board limit switches are wired such that the DigitalInput will return a true value if the switch is pressed and a false value if it is not pressed
    • How do we call a member function? You already know! You've been doing it all along with your motor controller and your controller (Joystick) objects!
      • We've used motor.Set(...) to set the speed and direction of our motor - Here, Set() is a member function of the TalonSRX class!
      • We've also used controller.GetRawButton(...) and controller.GetRawAxis(...) to read various parts of our gamepad - here GetRawButton() and GetRawAxis() are member functions of the Joystick class.
      • So, to call a member function on an object, we use the name of the object, followed by a period, followed by the name of the function, followed by a set of parenthesis. In the parenthesis we place any arguments, or additional inputs, that the member function needs.
  • Using the information above, and your knowledge from Lesson 2, do the following to read the value of the forward limit switch:
    • Declare a local variable to represent the current state of the forward limit switch. Use any name you want, but these instructions will use the name fwdLimitTripped
    • Assign this new variable the result of calling the Get() function on our robot's forward limit switch object
  • Now repeat the above, but for the reverse limit switch. The rest of these instructions will call this variable revLimitTripped

Add Extra Conditions to Control the Motor

In the last lesson, we based our choices on running the motor solely on the controller values. Now we will factor in the current limit switch states into those decisions.

  • We can change our "if" statements to consider multiple conditions when making decisions.
    • Let's assume I have two variables - one bool named "buttonFivePressed" and one double named "rightYAxis". Here is an example of how I could tell my code "if buttonFivePressed and rightYAxis is greater than 0, then run the motor at full speed":
      if (buttonFivePressed && rightYAxis > 0) {
      motor.Set(ControlMode::PercentOutput, 1.0);
    • As you can see in the example, you place all of your conditions inside of the parenthesis after "if" and you can combine them with an "and" style combination using two ampersands "&&". It is important to use TWO ampersands! One ampersand means something entirely different that we will not be covering directly.
    • We can also combine two conditions using an "or" relationship. Using the same assumed setup as above, here is how we would say "if buttonFivePressed or rightYAxis is greater than 0, then run the motor at full speed":
      if (buttonFivePressed || rightYAxis > 0) {
      motor.Set(ControlMode::PercentOutput, 1.0);
    • To combine using an "or" relationship, use two "vertical pipe" characters: ||
    • You can combine any logical conditions in this way, you can combine more than two conditions, and you can use nested parenthesis to group conditions. Below is a hypothetical example of a very complicated compound condition. You will not need anything this complicated to complete this lesson, but it is mentioned here to show you how you can do more complicated combinations of conditions:
      if ((buttonFivePressed && buttonSixPressed) || (buttonSevenPressed && rightYAxis > 0)) {
      motor.Set(ControlMode::PercentOutput, 1.0);
  • What can be a condition? Any expression that evaluates to a boolean (bool, true or false!) result. This can be a simple variable value (such as our buttonOnePressed bool variable), the return of a function call that returns a bool or another type that can be converted to a boolean, numeric comparison result or a logical comparison. Here are some common examples:
    • Equality comparison: a == b Note that you use TWO equal signs!
    • Inequality comparison: a != b
    • "Not": !a - This inverts the boolean value of the expression that follows it
    • Numeric comparisons:
      • Less than: a < b
      • Less or equal to than: a <= b
      • Greater than: a > b
      • Greater than or equal to: a >= b

Using the above information along with the value of the variables you created earlier that hold the current state of the limit switches, you can change your existing "if" statements to achieve the required goals.


When testing your code, verify the following items. You will need the help of a friend or a mentor to do this safely:

  1. Press button 1 and move left stick forward - motor should turn forward
  2. While continuing to press those controller buttons, trip the switch farthest from the motor - the motor should stop turning
  3. Release limit switch - the motor should resume
  4. Continue to hold the controller button and stick as above and trip the 2nd limit switch (closest to the motor) - the motor should still turn forward
  5. Now change your controller commands - with button 1 pressed, pull back on the left controller stick - motor should turn in reverse
  6. Continue hold the controller inputs and trip the limit switch closest to the motor - the motor should stop turning
  7. Release closest limit switch and trip the furthest limit switch - the motor should be turning in reverse
  8. Release everything (switches, trigger and joystick) - motor should be stopped
  9. Now trip the closest switch, then press button 1 and move left controller stick down - the motor should not be turning
  10. And finally, while continuing the above setup (holding controller in reverse/button down and closest switch tripped), trip the furthest switch - motor should still not turn.


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>

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

class Robot : public frc::TimedRobot {
  TalonSRX motor{5};
  Joystick controller{0};
  DigitalInput limitSwitchFwd{1};
  DigitalInput limitSwitchRev{0};

  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() {
  bool buttonOnePressed = false;
  buttonOnePressed = controller.GetRawButton(1);
  double leftYAxis = 0;
  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);

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

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

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