Example of how to use C++ in Arduino to make wrapper classes around our motors and sensors for the robot. Also a demonstration of how we can use algorithms and data structures from the standard C++ libraries in our code

oop_bricktronics.ino

#include <Wire.h>
#include <Encoder.h>
#include <PID_v1.h>
#include <Adafruit_MCP23017.h>
#include <BricktronicsShield.h>
#include <BricktronicsMotor.h>
#include <BricktronicsLight.h>
#include <BricktronicsButton.h>
#include <BricktronicsUltrasonic.h>

#include <system_configuration.h>
#include <utility.h>
#include <unwind-cxx.h>
#include <StandardCplusplus.h>
#include <algorithm>
#include <vector>

// This concept is called Object Oriented Design(OOP)

// This is a "class". Classes allow us to make simple wrapper functions around things that may be too specific for the user to normally need
class DifferentialDrive {
  
//    Recall how last time, we had code such as left.setFixedDrive(150) and right.setFixedDrive(-150).

//    Can you see how for a general program, little details like this aren't as important as robot.driveForward(150), where the 150 represents speed?
//    Thats what we can do with classes. Our user can write the program however they want, and they only need to care about robot.moveForward(), robot.turnLeft(), etc.

//    Your class handles the dirty work that may be repetitive, and so your user doesn't have to care about how your motors work or moving the left motor vs the right motor, etc.

  public:
//    This is the constructor. It helps us assign our left and right motors
     DifferentialDrive(const BricktronicsMotorSettings left_settings,
                      const BricktronicsMotorSettings right_settings)

//    This initializes our left and right motor using the passed in sensor values (used below, in setup() function).
    : left(left_settings),
      right(right_settings)

//      This is the code we run in our constructor. For now, we just enable our motors. 
    {
      left.begin();
      right.begin();
    }

//  When the user is done driving the robot a certain way, this makes it really easy to stop the robot.
  void stop()
  {
    left.brake();
    right.brake();
  }

//  When the user wants to drive forward at a certain speed
  void driveForward(int speed) 
  {
    left.setFixedDrive(-speed);
    right.setFixedDrive(-speed);
  }

//  When the user wants to drive backwards. Note how we've reused our code from earlier, just reversing the direction. This comes in handy
  void driveBackward(int speed)
  {
    driveForward(-speed);
  }


//  Turn left. This may seem a bit odd at first, but think it through. If the left wheel moves backwards and the right wheel moves forward
//  You end up turning left, similar to how you turn in a boat as you row.
  void turnLeft(int speed)
  {
     left.setFixedDrive(-speed);
     right.setFixedDrive(speed);
  }

//  Again, code reuse by changing the sign of our speed to change the direction of the movement
  void turnRight(int speed)
  {
    turnLeft(-speed);
  }


  private:

//    These are the variables that belong only to the single occurence of a class (called an instance)
//    Basically, every robot you create using class DifferentialDrive has its own idea of what its own left and right motors are.

    BricktronicsMotor left;
    BricktronicsMotor right;
  
};
// Class definition ends here. Now we're back to the familiar Arduino code like last time

BricktronicsButton button {BricktronicsShield::SENSOR_2};
BricktronicsUltrasonic us(BricktronicsShield::SENSOR_4);


/* Here's what our program will do:
 *  
 *  Turn left at a really slow speed
 *  Every 100 milliseconds it'll use the ultrasonic sensor to see how far away an object directly in front of it is
 *  Store these distances inside a vector/array (think of a vector like in mathematics)
 *  Find the maximum element in the vector and send it to the computer from the Arduino
 */

void setup() {
  
  BricktronicsShield::begin();
  // This is calling our constructor for the DifferentialDrive. We give it the name robot, and provide the information it needs about
  // the motors.
  DifferentialDrive robot(BricktronicsShield::MOTOR_1,
                            BricktronicsShield::MOTOR_2);
  button.begin();
  us.begin();

//  Lets us pass messages between devices. Initialize baud rate to 9600(default). Don't worry about quite what this means just yet
//  Just know that this allows the Arduino to send/receive messages from the computer at a certain rate
  Serial.begin(9600);


  /*
   * Think of a vector exactly like in math. It's just a representation of data.
   *
   * Doesn't necessarily allow all the mathematical properties like cross product and addition to work as-is,
   * but you can always write functions to do these kinds of operations.
   * 
   * <> is called chevron. Data type inside chevron determines what is the data type of elements in our data list.
   * In this case, our data type is "int", which means its all integers
   * 
   * So your vector here is named data and has space for 60 elements. ex: data = [1, 2, 3, ..., 59, 60]
   */
  std::vector<int> data(60);

//  Make the robot start turning left
  robot.turnLeft(90);
  
//  auto gets the variable based on type passed into chevrons above, and the "&" allows you to modify the element directly
//  inside the vector 
  for (auto& i : data) {
    i = us.getDistance();
    delay(100);
  }
//  Once we are done turning and populating our vector with data, stop the robot.
  robot.stop();


  /*
   * Go through list, and replace every occurence of 255 with 0
   * 
   * The sensor can detect 254 cm away at most, makes everything further away 255 automatically
   * so we replace every 255 with a 0. Now we have a list of how far away things are from us as we turn, of the things we can actually detect
   * 
   * Note how we passed in the start and end of our vector, as well as the value 255 (what we want to find) and 0 (what we want to replace it with)
   */
  std::replace(data.begin(), data.end(), 255, 0);

//  max_element takes in an iterable (something holding multiple elements in a sequence, like our vector)
//  Gives a pointer to max value. You'll understand this more later, but just understand a "pointer" as something that 
//  points to data in memory
  auto max_position = std::max_element(data.begin(), data.end());

//  dereferences max_position, so it gets the actual value from this position in memory. Derefrencing is done using the
//  "*" operator.
  auto max_value = *max_position;

//  Print max_value to serial monitor. println means "print line", so print the max value on a single line.
  Serial.println(max_value);

  /*  To see output from the serial monitor, when you have an Arduino plugged in:
   *   
   *  Look at the top right corner of the Arduino IDE. Click the magnifying glass, it'll bring up a chat box looking window.
   *  You should see your robot's max distance here
   */

}

void loop()
{
//  We have nothing happening over and over this time around, so we leave this blank
}