add drive pto support

[fishsticks89]

If a team is using a pto, the motors on their drive will occasionally change. Currently there is no support for this.

[ssejrog]

There are four new functions, pto_toggle(), pto_add(), pto_remove(), and pto_check(). Here is an example:

#include "main.h"

pros::Motor& intake_l = chassis.left_motors[1];
pros::Motor& intake_r = chassis.right_motors[1];
pros::ADIDigitalOut pto_intake_piston('A');
bool pto_intake_enabled = false;

void pto_intake(bool toggle) {
  pto_intake_enabled = toggle;
  chassis.pto_toggle({intake_l, intake_r}, toggle);
  pto_intake_piston.set_value(toggle);
  if (toggle) {
    intake_l.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
    intake_r.set_brake_mode(pros::E_MOTOR_BRAKE_COAST);
  }
}

void set_intake(int input) {
  if (!pto_intake_enabled) return;
  intake_l = input;
  intake_r = input;
}

int button_lock = 0;
void intake_control() {
  if (master.get_digital(DIGITAL_DOWN) && button_lock == 0) {
    pto_intake(!pto_intake_enabled);
    button_lock = 1;
  } else if (!master.get_digital(DIGITAL_DOWN)) {
    button_lock = 0;
  }

  if (master.get_digital(DIGITAL_L1))
    set_intake(127);
  else if (master.get_digital(DIGITAL_L2))
    set_intake(-127);
  else
    set_intake(0);
}

[ssejrog]

Currently in the set motor stuff in drive.cpp, if the motor is found in pto_list it breaks the for loop. This means any motor after this will not be checked.

  for (auto i : left_motors) {
    if (pto_check(i)) break;  // If the motor is in the pto list, don't do anything to the motor.
    i.set_current_limit(abs(mA));
  }

This should be changed to:

  for (auto i : left_motors) {
    if (!pto_check(i)) i.set_current_limit(abs(mA));  // If the motor is in the pto list, don't do anything to the motor.
  }