"Butterball" V1

Main

#include "main.h"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/GUI.hpp"

#include "Robot/Parts.hpp"

#include "motionProfiling.h"

#include "Robot/PID.hpp"

#include "Robot/Filter.hpp"

void initialize()

{

okapi::Logger::setDefaultLogger(std::make_shared<okapi::Logger>(

okapi::TimeUtilFactory::createDefault().getTimer(),

"/ser/sout", // Output to the PROS terminal

okapi::Logger::LogLevel::warn // Show errors and warnings

));

// Start the brain's GUI

guiInit();

// Set brakeMode for all motors------------------------------------------

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

lowerElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

upperElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

}

void disabled()

{

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

lowerElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

upperElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

}

void competition_initialize() {}

void autonomous()

{

// Reset drive encoder position

leftDriveFront.tarePosition();

rightDriveFront.tarePosition();

leftDriveBack.tarePosition();

rightDriveBack.tarePosition();

deploy.resume();

// Run the selected autonomous

selection();

// std::cout << pros::Task::get_count() << std::endl;

// PIDLoop(20,20,20,20,5);

// std::cout << pros::Task::get_count() << std::endl;

}

void opcontrol()

{

colorSort.setLedPWM(75);

bool sort{true};

double strafeValue{0};

std::uint32_t loop{0};

while (true)

{

loop = pros::millis();

strafeValue = (okapi::deadband(control.getAnalog(okapi::ControllerAnalog::leftX), 0.05, -0.05) +

okapi::deadband(control.getAnalog(okapi::ControllerAnalog::rightX), 0.05, -0.05)) /

2;

// Joystick control----------------------------------------------------

leftDriveFront.moveVoltage(

(okapi::deadband(control.getAnalog(okapi::ControllerAnalog::leftY),

-0.05, 0.05) +

strafeValue) *

12000);

leftDriveBack.moveVoltage(

(okapi::deadband(control.getAnalog(okapi::ControllerAnalog::leftY),

-0.05, 0.05) -

strafeValue) *

12000);

rightDriveFront.moveVoltage(

(okapi::deadband(control.getAnalog(okapi::ControllerAnalog::rightY),

-0.05, 0.05) -

strafeValue) *

12000);

rightDriveBack.moveVoltage(

(okapi::deadband(control.getAnalog(okapi::ControllerAnalog::rightY),

-0.05, 0.05) +

strafeValue) *

12000);

if (colorSort.getHue() >= 200 && colorSort.getProximity() > 110)

sort = (alliance == 1) ? false : true;

else if (colorSort.getHue() < 20 && colorSort.getProximity() > 110)

sort = (alliance == 1) ? true : false;

// Elevator Control----------------------------------------------------

if ((control.getDigital(okapi::ControllerDigital::L1) && sort) ||

control.getDigital(okapi::ControllerDigital::right))

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

else if ((control.getDigital(okapi::ControllerDigital::L1) && !sort) ||

control.getDigital(okapi::ControllerDigital::L2))

{

lowerElevator.moveVoltage(-12000);

upperElevator.moveVoltage(-12000);

}

else

{

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

}

// Intake Control------------------------------------------------------

if (control.getDigital(okapi::ControllerDigital::R1))

{

intake.moveVoltage(12000);

}

else if (control.getDigital(okapi::ControllerDigital::R2))

{

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

if (!rightIntakeStop.isPressed())

{

intakeRight.moveVelocity(-200);

}

else

{

intakeRight.moveVelocity(0);

}

if (!leftIntakeStop.isPressed())

{

intakeLeft.moveVelocity(-200);

}

else

{

intakeLeft.moveVelocity(0);

}

}

else

{

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.moveVelocity(0);

}

pros::Task::delay_until(&loop, 20);

}

}



Filtering

Filter.hpp

#pragma once

/**

* Filter a tower by scoring balls until a ball of the other alliance's color is detected

*/

void filter();

Filter.cpp

#include "Robot/Filter.hpp"

#include "Robot/Parts.hpp"

#include "Robot/GUI.hpp"

void filter()

{

/**

* Run Elevator when colorSort trips

* Stop intakes until elevatorDistance trips

*/

colorSort.setLedPWM(75);

// Time until the funtion will stop (regardless if the tower is completely filtered)

double timeout{3000};

// Time when the funtion started

auto time{pros::millis()};

while (pros::millis() - time < timeout)

{

// Run the intakes until the optical sensor's proximity detects a ball

while (colorSort.getProximity() < 180 && pros::millis() - time < timeout)

{

intake.moveVoltage(12000);

}

pros::delay(50);

intake.moveVoltage(0);

// If the ball inside the robot is of the opposing color, stop

if ((alliance == 2 && colorSort.getHue() < 20) ||

(alliance == 1 && colorSort.getHue() > 200))

{

break;

}

// Run the elevator until the ball exits the robot

while (elevatorDistance.get() > 100 && pros::millis() - time < timeout)

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

pros::delay(150);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

}

colorSort.setLedPWM(0);

}


GUI (brain screen graphics)

GUI.hpp

#include "main.h"

#pragma once

// Starts the brain's GUI

void guiInit();

// Runs the selected autonomous path

void selection();

// Variables for sorting balls

extern int alliance, opposingAlliance;

GUI.cpp

#include "Robot/GUI.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

// Vector containing the pointers to the buttons on the GUI

std::vector<lv_obj_t *> autonButtons{};

/**

* 0 - autonLeft

* 1 - autonRight

* 2 - autonCombo

* 3 - skills

* 4 - autonNone

*/

// Switch that controlls what balls will be sorted by the vision sensor

lv_obj_t *allianceSwitch = lv_sw_create(lv_scr_act(), NULL);

// Checkboxes for picking how many towers are attempted

lv_obj_t *_3points = lv_cb_create(lv_scr_act(), NULL);

lv_obj_t *_2points = lv_cb_create(lv_scr_act(), NULL);

lv_obj_t *_1point = lv_cb_create(lv_scr_act(), NULL);

// Vector containing the pointers to the checkboxes on the GUI

std::vector<lv_obj_t *> autonCheckboxes{_3points, _2points, _1point};

// Create and assign styles to gui objects

void guiStyle()

{

// Style for a toggled button--------------------------------------------

static lv_style_t buttonSelected;

lv_style_copy(&buttonSelected, &lv_style_btn_rel);

buttonSelected.body.main_color = LV_COLOR_GREEN;

buttonSelected.body.grad_color = LV_COLOR_GREEN;

buttonSelected.body.radius = 3;

buttonSelected.body.opa = LV_OPA_MAX;

buttonSelected.body.border.color = LV_COLOR_BLACK;

buttonSelected.body.border.width = 3;

// Style for an unselected button----------------------------------------

static lv_style_t buttonDeselected;

lv_style_copy(&buttonDeselected, &lv_style_btn_pr);

buttonDeselected.body.main_color = LV_COLOR_GRAY;

buttonDeselected.body.grad_color = LV_COLOR_GRAY;

buttonDeselected.body.radius = 3;

buttonDeselected.body.opa = LV_OPA_MAX;

buttonDeselected.body.border.color = LV_COLOR_BLACK;

buttonDeselected.body.border.width = 3;

buttonDeselected.text.color = LV_COLOR_BLACK;

// Setting the styles to the buttons-------------------------------------

for (auto button : autonButtons)

{

lv_btn_set_style(button, LV_BTN_STYLE_REL, &buttonSelected);

lv_btn_set_style(button, LV_BTN_STYLE_PR, &lv_style_btn_pr);

lv_btn_set_style(button, LV_BTN_STYLE_TGL_REL, &buttonDeselected);

lv_btn_set_style(button, LV_BTN_STYLE_TGL_PR, &lv_style_btn_tgl_pr);

}

// Style for the switch with blue alliance selected----------------------

static lv_style_t switchBlueStyle;

lv_style_copy(&switchBlueStyle, &lv_style_plain);

switchBlueStyle.body.main_color = LV_COLOR_BLUE;

switchBlueStyle.body.grad_color = LV_COLOR_BLUE;

switchBlueStyle.body.radius = 125;

switchBlueStyle.body.border.color = LV_COLOR_BLACK;

switchBlueStyle.body.border.width = 2;

switchBlueStyle.body.border.opa = LV_OPA_MAX;

// Style for the switch with red alliance selected-----------------------

static lv_style_t switchRedStyle;

lv_style_copy(&switchRedStyle, &switchBlueStyle);

switchRedStyle.body.main_color = LV_COLOR_RED;

switchRedStyle.body.grad_color = LV_COLOR_RED;

// Style for the switch indicator----------------------------------------

static lv_style_t switchIndicator;

switchIndicator.body.main_color = LV_COLOR_YELLOW;

switchIndicator.body.opa = LV_OPA_TRANSP;

switchIndicator.body.radius = 150;

// Style for the switch background---------------------------------------

static lv_style_t switchBackground;

switchBackground.body.main_color = LV_COLOR_GRAY;

switchBackground.body.grad_color = LV_COLOR_WHITE;

switchBackground.body.opa = LV_OPA_60;

switchBackground.body.radius = 125;

switchBackground.body.border.color = LV_COLOR_BLACK;

switchBackground.body.border.width = 2;

// Assigning styles to the switch----------------------------------------

lv_sw_set_style(allianceSwitch, LV_SW_STYLE_BG, &switchBackground);

lv_sw_set_style(allianceSwitch, LV_SW_STYLE_INDIC, &switchIndicator);

lv_sw_set_style(allianceSwitch, LV_SW_STYLE_KNOB_OFF, &switchBlueStyle);

lv_sw_set_style(allianceSwitch, LV_SW_STYLE_KNOB_ON, &switchRedStyle);

// Styles for point checkboxes-------------------------------------------

static lv_style_t checkboxToggled, checkboxReleased, checkboxPressed;

lv_style_copy(&checkboxToggled, &lv_style_btn_tgl_pr);

lv_style_copy(&checkboxReleased, &lv_style_btn_tgl_rel);

lv_style_copy(&checkboxPressed, &lv_style_btn_pr);

// Style for the selected checkbox---------------------------------------

checkboxToggled.body.main_color = LV_COLOR_PURPLE;

checkboxToggled.body.grad_color = LV_COLOR_PURPLE;

checkboxToggled.body.radius = 5;

checkboxToggled.body.border.color = LV_COLOR_BLACK;

checkboxToggled.body.border.width = 2;

checkboxToggled.body.border.opa = LV_OPA_MAX;

// Style for the unselected checkbox-------------------------------------

checkboxReleased.body.main_color = LV_COLOR_GRAY;

checkboxReleased.body.grad_color = LV_COLOR_GRAY;

checkboxReleased.body.radius = 5;

checkboxReleased.body.border.color = LV_COLOR_BLACK;

checkboxReleased.body.border.width = 2;

checkboxReleased.body.border.opa = LV_OPA_MAX;

// Style for the actively pressed checkbox-------------------------------

checkboxPressed.body.main_color = LV_COLOR_WHITE;

checkboxPressed.body.grad_color = LV_COLOR_WHITE;

checkboxPressed.body.radius = 5;

checkboxPressed.body.border.color = LV_COLOR_BLACK;

checkboxPressed.body.border.width = 2;

checkboxToggled.body.border.opa = LV_OPA_50;

// Assigning styles to checkboxes----------------------------------------

for (auto checkbox : autonCheckboxes)

{

lv_cb_set_style(checkbox, LV_CB_STYLE_BG, &lv_style_transp);

lv_cb_set_style(checkbox, LV_CB_STYLE_BOX_REL, &checkboxReleased);

lv_cb_set_style(checkbox, LV_CB_STYLE_BOX_PR, &checkboxPressed);

lv_cb_set_style(checkbox, LV_CB_STYLE_BOX_TGL_REL, &checkboxToggled);

lv_cb_set_style(checkbox, LV_CB_STYLE_BOX_TGL_PR, &checkboxPressed);

}

}

// Toggles a button when pressed

lv_res_t btnToggle(lv_obj_t *btn)

{

// Loop through the autonButtons vector

for (auto button : autonButtons)

{

// If the object in the vector is the toggled button

if (button == btn)

{

// Set button's state

lv_btn_set_state(button, LV_BTN_STATE_REL);

// If the toggled button is for left or right paths, show checkboxes

if (button == autonButtons.at(0) || button == autonButtons.at(1))

{

for (auto checkbox : autonCheckboxes)

{

lv_obj_set_hidden(checkbox, false);

}

}

else

{

for (auto checkbox : autonCheckboxes)

{

lv_obj_set_hidden(checkbox, true);

}

}

}

else

{

lv_btn_set_state(button, LV_BTN_STATE_TGL_REL);

}

}

return LV_RES_OK;

}

// Toggles a checkbox when pressed

lv_res_t cbToggle(lv_obj_t *cb)

{

// Loop through the autonCheckboxes vector

for (auto checkbox : autonCheckboxes)

{

// If the object in the vector is the toggled checkbox

if (checkbox == cb)

{

lv_cb_set_checked(checkbox, true);

}

else

{

lv_cb_set_checked(checkbox, false);

}

}

return LV_RES_OK;

}

void guiInit()

{

// Create buttons and add them to the autonButtons vector

for (int i{0}; i < 5; i++)

{

autonButtons.push_back(lv_btn_create(lv_scr_act(), NULL));

}

// Set the button size---------------------------------------------------

lv_obj_set_size(autonButtons.at(0), 480 / 2, 100);

lv_obj_set_size(autonButtons.at(2), 480 / 2, 100);

lv_obj_set_size(autonButtons.at(1), 480 / 2, 100);

lv_obj_set_size(autonButtons.at(3), 480 / 4, 100);

lv_obj_set_size(autonButtons.at(4), 480 / 4, 100);

// Set the checkbox size-------------------------------------------------

for (auto cb : autonCheckboxes)

{

lv_obj_set_size(cb, 15, 15);

lv_cb_set_action(cb, cbToggle);

}

// Set button position---------------------------------------------------

lv_obj_set_pos(autonButtons.at(0), 0, 40);

lv_obj_set_pos(autonButtons.at(2), 480 / 2, 40);

lv_obj_set_pos(autonButtons.at(1), 0, 140);

lv_obj_set_pos(autonButtons.at(3), 480 / 2, 140);

lv_obj_set_pos(autonButtons.at(4), 480 * 0.75, 140);

// Set checkbox position-------------------------------------------------

lv_obj_set_pos(_3points, 150, 10);

lv_obj_set_pos(_2points, 250, 10);

lv_obj_set_pos(_1point, 350, 10);

// Set the size of the switch--------------------------------------------

lv_obj_set_size(allianceSwitch, 145, 35);

// Set the position of the switch----------------------------------------

lv_obj_set_pos(allianceSwitch, 0, 5);

// Give the buttons a label----------------------------------------------

lv_obj_t *btnLabelLeft = lv_label_create(autonButtons.at(0), NULL);

lv_obj_t *btnLabelRight = lv_label_create(autonButtons.at(1), NULL);

lv_obj_t *btnLabelCombo = lv_label_create(autonButtons.at(2), NULL);

lv_obj_t *btnLabelSkills = lv_label_create(autonButtons.at(3), NULL);

lv_obj_t *btnLabelNone = lv_label_create(autonButtons.at(4), NULL);

// Give the labels text--------------------------------------------------

lv_label_set_text(btnLabelLeft, "Left");

lv_label_set_text(btnLabelRight, "Right");

lv_label_set_text(btnLabelCombo, "Middle Goal");

lv_label_set_text(btnLabelSkills, "Skills");

lv_label_set_text(btnLabelNone, "None");

// Give the checkboxes labels--------------------------------------------

lv_cb_set_text(_3points, "3 Points");

lv_cb_set_text(_2points, "2 Points");

lv_cb_set_text(_1point, "1 Point");

// Assign a function to call when a button is pressed--------------------

for (auto button : autonButtons)

{

lv_btn_set_action(button, LV_BTN_ACTION_PR, btnToggle);

}

// Set default checkbox--------------------------------------------------

lv_cb_set_checked(_1point, true);

cbToggle(_1point);

// Set the default button------------------------------------------------

lv_btn_set_state(autonButtons.at(4), LV_BTN_STATE_TGL_PR);

btnToggle(autonButtons.at(4));

// Create and assign custom styles---------------------------------------

guiStyle();

}

int alliance{1};

int opposingAlliance{0};

void selection()

{

// Check the switch for alliance-----------------------------------------

if (lv_sw_get_state(allianceSwitch))

{

// Blue

alliance = 1;

opposingAlliance = 2;

}

else

{

// Red

alliance = 2;

opposingAlliance = 1;

}

// Run an autonomous path based on which button is selected--------------

// Check which button is selected

if (lv_btn_get_state(autonButtons.at(0)) == LV_BTN_STATE_REL)

{

if (lv_cb_is_checked(_1point))

{

cycleLeft1();

}

else if (lv_cb_is_checked(_2points))

{

cycleLeft2();

}

}

else if (lv_btn_get_state(autonButtons.at(1)) == LV_BTN_STATE_REL)

{

if (lv_cb_is_checked(_1point))

{

cycleRight1();

}

else if (lv_cb_is_checked(_2points))

{

cycleRight2();

}

}

else if (lv_btn_get_state(autonButtons.at(2)) == LV_BTN_STATE_REL)

{

cycleLeftMid();

}

else if (lv_btn_get_state(autonButtons.at(3)) == LV_BTN_STATE_REL)

{

skillsPath();

}

}


PID (both HPP and CPP)

PID.hpp

#include "main.h"

#pragma once

const double kp = 1000;

const double ki = 0;

const double kd = 750;

const double circ = 12.5663706144;

void PIDLoop(double lfDis,

double rfDis,

double lbDis,

double rbDis,

double errorRange);

void PIDLoopArc(double left,

double right,

double leftV,

double rightV,

double errorRange);

void PIDSingle(double lfDis,

double rfDis,

double lbDis,

double rbDis,

double errorRange);

/**

* Find the power caps for the left and right side to drive the robot in an arc

*

* @param arcDistance Distance for the robot to travel (in inches and relative to the center of the bot)

* @param theta The desired new heading for the robot

* @param turnLeft Designates if the robot is turning left or right

*

* @returns Nothing (could possibly return the motor caps)

*/

// void arc(double arcDistance, double theta, bool turnLeft);

void arc(double cordLength, double theta, bool turnLeft);

/**

* PID controller for velocity used during autonomous

* Slave motor will copy master motor

*/

class VelController

{

private:

// Pointer to the slave motor

okapi::Motor *motor;

// Pointer to the master motor

okapi::Motor *master;

// PID Gain

static const double kP, kI, kD;

// Maximum allowed output

static const double maxVoltage;

double error, lastError, cumulativeError, derivativeError;

double target, output;

uint32_t lastLoop;

// Task for independently controlling motors

pros::Task controller;

/**

* Method for updating error and output variables

*/

void update();

/**

* Send output to the slave motor

*/

void execute();

public:

/**

* Set a new master motor

* \param newTarget Pointer to the new master motor

*/

void setTarget(okapi::Motor *newTarget) { master = newTarget; }

/**

* Constructor for a VelController object

* \param controlledMotor Slave motor

* \param masterMotor Master motor

*/

VelController(okapi::Motor *controlledMotor, okapi::Motor *masterMotor);

~VelController()

{

// free(&controller);

controller.remove();

motor = nullptr;

master = nullptr;

}

};


PID.cpp

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include <iostream>

//proto types for findmax and findmin functions

//need to change something

template <size_t size>

double findMax(double (&arr)[size]);

template <size_t size>

double findMin(double (&arr)[size]);

//pid in a loop

void PIDLoop(double lfDis, double rfDis, double lbDis, double rbDis, double errorRange)

{

//tare motor position

leftDriveFront.tarePosition();

rightDriveFront.tarePosition();

leftDriveBack.tarePosition();

rightDriveBack.tarePosition();

if (lfDis != rfDis || lfDis != lbDis || lfDis != rbDis)

{

//set prototypes for variables used

double error[4] = {errorRange, errorRange, errorRange, errorRange}, integral[4], proportional[4], derrivitave[4], prevErr[4] = {0, 0, 0, 0};

double pid[4];

//breaks loop when error is out of a target range

while (findMax(error) > errorRange / 2.0 || findMin(error) < -errorRange / 2.0)

{

//get error

error[0] = lfDis - ((leftDriveFront.getPosition() / 360.0) * circ);

error[1] = rfDis - ((rightDriveFront.getPosition() / 360.0) * circ);

error[2] = lbDis - ((leftDriveBack.getPosition() / 360.0) * circ);

error[3] = rbDis - ((rightDriveBack.getPosition() / 360.0) * circ);

//calculate p i and d

for (int i = 0; i < 4; i++)

{

proportional[i] = error[i];

integral[i] += error[i];

derrivitave[i] = error[i] - prevErr[i];

pid[i] = proportional[i] * kp + integral[i] * ki + derrivitave[i] * kd;

if (pid[i] > 12000)

{

pid[i] = 12000;

}

if (pid[i] < -12000)

{

pid[i] = -12000;

}

}

//move the motors

leftDriveFront.moveVoltage(pid[0]);

rightDriveFront.moveVoltage(pid[1]);

leftDriveBack.moveVoltage(pid[2]);

rightDriveBack.moveVoltage(pid[3]);

//store previous error

for (int i = 0; i < 4; i++)

{

prevErr[i] = error[i];

}

}

}

//go straight

else

{

//set prototypes for variables used

double error[2] = {errorRange, errorRange}, integral[2], proportional[2], derrivitave[2], prevErr[2] = {0, 0};

double pid[2];

VelController vcf(&rightDriveFront, &leftDriveFront);

VelController vcb(&rightDriveBack, &leftDriveBack);

//breaks loop when error is out of a target range

while (findMax(error) > errorRange / 2.0 || findMin(error) < -errorRange / 2.0)

{

//get error

error[0] = lfDis - ((leftDriveFront.getPosition() / 360.0) * circ);

error[1] = lbDis - ((leftDriveBack.getPosition() / 360.0) * circ);

//calculate p i and d

for (int i = 0; i < 2; i++)

{

proportional[i] = error[i];

integral[i] += error[i];

derrivitave[i] = error[i] - prevErr[i];

pid[i] = proportional[i] * kp + integral[i] * ki + derrivitave[i] * kd;

if (pid[i] > 10000)

{

pid[i] = 10000;

}

if (pid[i] < -10000)

{

pid[i] = -10000;

}

}

//move the motors

leftDriveFront.moveVoltage(pid[0]);

leftDriveBack.moveVoltage(pid[1]);

//store previous error

for (int i = 0; i < 2; i++)

{

prevErr[i] = error[i];

}

}

}

leftDrive.moveVoltage(0);

rightDrive.moveVoltage(0);

}

void PIDLoopArc(double left, double right, double leftV, double rightV, double errorRange)

{

double lfDis{left}, rfDis{right}, lbDis{left}, rbDis{right};

double error[4] = {errorRange, errorRange, errorRange, errorRange}, prevErr[4], proportional[4], integral[4], derrivitave[4], pid[4]; //get defined in the loop

leftDriveFront.tarePosition();

rightDriveFront.tarePosition();

leftDriveBack.tarePosition();

rightDriveBack.tarePosition();

//breaks loop when error is out of a target range

while (findMax(error) > errorRange / 2.0 || findMin(error) < -errorRange / 2.0)

{

//calculate values from length and radius

//get error

error[0] = lfDis - ((leftDriveFront.getPosition() / 360.0) * circ);

error[1] = rfDis - ((rightDriveFront.getPosition() / 360.0) * circ);

error[2] = lbDis - ((leftDriveBack.getPosition() / 360.0) * circ);

error[3] = rbDis - ((rightDriveBack.getPosition() / 360.0) * circ);

//calculate p i and d

for (int i = 0; i < 4; i++)

{

proportional[i] = error[i];

integral[i] += error[i];

derrivitave[i] = error[i] - prevErr[i];

pid[i] = proportional[i] * kp + integral[i] * ki + derrivitave[i] * kd;

}

//cap the values

if (pid[0] > leftV)

{

pid[0] = leftV;

}

if (pid[0] < -leftV)

{

pid[0] = -leftV;

}

if (pid[1] > rightV)

{

pid[1] = rightV;

}

if (pid[1] < -rightV)

{

pid[1] = -rightV;

}

if (pid[2] > leftV)

{

pid[2] = leftV;

}

if (pid[2] < -leftV)

{

pid[2] = -leftV;

}

if (pid[3] > rightV)

{

pid[3] = rightV;

}

if (pid[3] < -rightV)

{

pid[3] = -rightV;

}

//move the motors

leftDriveFront.moveVoltage(pid[0]);

rightDriveFront.moveVoltage(pid[1]);

leftDriveBack.moveVoltage(pid[2]);

rightDriveBack.moveVoltage(pid[3]);

//store previous error

for (int i = 0; i < 4; i++)

{

prevErr[i] = error[i];

}

}

leftDrive.moveVoltage(0);

rightDrive.moveVoltage(0);

}

//function for running a single pid loop

void PIDSingle(double lfDis, double rfDis, double lbDis, double rbDis, double errorRange)

{

//tare motor position

leftDriveFront.tarePosition();

rightDriveFront.tarePosition();

leftDriveBack.tarePosition();

rightDriveBack.tarePosition();

//set prototypes for variables used

static double error[4];

double integral[4], proportional[4], derrivitave[4], prevErr[4] = {0, 0, 0, 0};

double pid[4];

//get error

error[0] = lfDis - ((leftDriveFront.getPosition() / 360.0) * circ);

error[1] = rfDis - ((rightDriveFront.getPosition() / 360.0) * circ);

error[2] = lbDis - ((leftDriveBack.getPosition() / 360.0) * circ);

error[3] = rbDis - ((rightDriveBack.getPosition() / 360.0) * circ);

//calculate p i and d

for (int i = 0; i < 4; i++)

{

proportional[i] = error[i];

integral[i] += error[i];

derrivitave[i] = error[i] - prevErr[i];

pid[i] = proportional[i] * kp + integral[i] * ki + derrivitave[i] * kd;

if (pid[i] > 12000)

{

pid[i] = 12000;

}

if (pid[i] < -12000)

{

pid[i] = -12000;

}

}

//move the motors

leftDriveFront.moveVoltage(pid[0]);

rightDriveFront.moveVoltage(pid[1]);

leftDriveBack.moveVoltage(pid[2]);

rightDriveBack.moveVoltage(pid[3]);

//store previous error

for (int i = 0; i < 4; i++)

{

prevErr[i] = error[i];

}

}

//helper to find max of an array

template <size_t size>

double findMax(double (&arr)[size])

{

double max = arr[0];

for (int i = 1; i < size; i++)

{

if (arr[i] > max)

{

max = arr[i];

}

}

return max;

}

//helper to find min of an array

template <size_t size>

double findMin(double (&arr)[size])

{

double min = arr[0];

for (int i = 1; i < size; i++)

{

if (arr[i] < min)

{

min = arr[i];

}

}

return min;

}

/* void arc(double arcDistance, double theta, bool turnLeft)

{

// bool turnLeft{true};

// double arcDistance{12}, theta{45 * PI / 180};

theta = theta * M_PI / 180;

double leftOutput{0}, rightOutput{0},

deltaLeft{0}, deltaRight{0},

radius{0}, radiusLeft{0}, radiusRight{0},

arcLeft{0}, arcRight{0};

radius = (arcDistance / theta);

radiusLeft = turnLeft ? radius - 7 : radius + 7;

radiusRight = turnLeft ? radius + 7 : radius - 7;

deltaLeft = 2 * PI * radiusLeft * (theta / (2 * PI));

deltaRight = 2 * PI * radiusRight * (theta / (2 * PI));

// leftOutput = turnLeft ? (deltaLeft * rightOutput) / deltaRight;

if (turnLeft)

{

rightOutput = 12000;

leftOutput = (deltaLeft * rightOutput) / deltaRight;

}

else

{

leftOutput = 12000;

rightOutput = (deltaRight * leftOutput) / deltaLeft;

}

arcLeft = radiusLeft * theta;

arcRight = radiusRight * theta;

// leftDrive.moveVoltage(leftOutput);

// rightDrive.moveVoltage(rightOutput);

}

*/

void arc(double cordLength, double theta, bool turnLeft)

{

double leftOutput{0}, rightOutput{0},

deltaLeft{0}, deltaRight{0},

radius{0}, radiusLeft{0}, radiusRight{0};

theta = theta * M_PI / 180;

radius = ((cordLength / 2) / std::sin(theta / 2));

radiusLeft = turnLeft ? radius - 7 : radius + 7;

radiusRight = turnLeft ? radius + 7 : radius - 7;

deltaLeft = theta * radiusLeft;

deltaRight = theta * radiusRight;

if (turnLeft)

{

rightOutput = 12000;

leftOutput = (deltaLeft * rightOutput) / deltaRight;

}

else

{

leftOutput = 12000;

rightOutput = (deltaRight * leftOutput) / deltaLeft;

}

PIDLoopArc(deltaLeft, deltaRight, leftOutput, rightOutput, 3);

}

const double VelController::kP{150}, VelController::kI{50}, VelController::kD{50},

VelController::maxVoltage{12000};

void VelController::update()

{

error = master->getActualVelocity() - motor->getActualVelocity();

cumulativeError += error;

derivativeError = (error - lastError) / (pros::millis() - lastLoop);

lastError = error;

lastLoop = pros::millis();

// output = kP * error + kI * cumulativeError + kD * derivativeError;

output = kP * error + kI * cumulativeError + kD * derivativeError + master->getActualVelocity();

}

void VelController::execute()

{

// Limit output to slave motor

if (std::abs(output) > maxVoltage)

{

if (output > 0)

motor->moveVoltage(maxVoltage);

else if (output < 0)

motor->moveVoltage(-maxVoltage);

}

else

motor->moveVoltage(output);

}

VelController::VelController(okapi::Motor *controlledMotor, okapi::Motor *masterMotor) : motor{controlledMotor},

master{masterMotor},

error{0},

cumulativeError{0},

derivativeError{0},

target{0},

output{0},

controller{

[=] {

uint32_t loop{0};

while (true)

{

loop = pros::millis();

update();

execute();

pros::Task::delay_until(&loop, 20);

}

}}

{

}



Parts

Parts.hpp

#include "main.h"

#pragma once

extern okapi::Motor intakeLeft, intakeRight, leftDriveFront, rightDriveFront,

leftDriveBack, rightDriveBack, lowerElevator, upperElevator;

extern okapi::MotorGroup intake, leftDrive, rightDrive;

extern okapi::Controller control;

extern okapi::IMU inertialSensor;

extern okapi::DistanceSensor elevatorDistance;

extern okapi::ADIButton leftIntakeStop, rightIntakeStop;

extern okapi::OpticalSensor colorSort;

Parts.cpp

#include "Robot/Parts.hpp"

// Intake motor on the left side of the robot

okapi::Motor intakeLeft(15, false, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Intake motor on the right side of the robot

okapi::Motor intakeRight(14, true, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Drive motor on the left side, in the front

okapi::Motor leftDriveFront(17, false, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Drive motor on the right side, in the front

okapi::Motor rightDriveFront(19, true, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Drive motor in the back, on the left side

okapi::Motor leftDriveBack(18, false, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Drive motor on the right side, in the back

okapi::Motor rightDriveBack(20, true, okapi::AbstractMotor::gearset::green,

okapi::AbstractMotor::encoderUnits::degrees);

// Motor that controlls the snail

okapi::Motor lowerElevator(10, true, okapi::AbstractMotor::gearset::blue,

okapi::AbstractMotor::encoderUnits::degrees);

// Motor that controlls the upperElevator and launches the ball into the goal

okapi::Motor upperElevator(9, false, okapi::AbstractMotor::gearset::blue,

okapi::AbstractMotor::encoderUnits::degrees);

// Motor group that controlls the intake

okapi::MotorGroup intake({intakeLeft, intakeRight});

// Motor group that controls the drive motors on the left side

okapi::MotorGroup leftDrive({leftDriveFront, leftDriveBack});

// Motor group that controlls the drive motors on the right side

okapi::MotorGroup rightDrive({rightDriveFront, rightDriveBack});

// The controller

okapi::Controller control(okapi::ControllerId::master);

// Distance Sensor for scoring during autonomous

okapi::DistanceSensor elevatorDistance(7);

// ADI button for stopping intakes when opening

okapi::ADIButton leftIntakeStop('A', false), rightIntakeStop('B', false);

okapi::OpticalSensor colorSort(6);


Left 1 Tower

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Filter.hpp"

void cycleLeft1()

{

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{

pros::delay(20);

}

//deploy

upperElevator.moveVoltage(12000);

pros::delay(750);

upperElevator.moveVoltage(0);

//set brake mode

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//move forward

PIDLoop(38,38,38,38,5);

//rotate

PIDLoop(-21,21,-21,21,10);

//start cycle

intake.moveVelocity(200);

//move forward

PIDLoop(24,24,24,24,5);

filter();

//backup

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-15,-15,-15,-15,5);

}


Left 2 Tower

Left 2 Tower

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Filter.hpp"

void cycleLeft2()

{

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{

pros::delay(20);

}

//deploy

upperElevator.moveVoltage(12000);

pros::delay(750);

upperElevator.moveVoltage(0);

//set brake mode

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//move forward

PIDLoop(38,38,38,38,5);

//rotate

PIDLoop(-21,21,-21,21,10);

//start cycle

intake.moveVelocity(200);

//move forward

PIDLoop(24,24,24,24,5);

filter();

//backup

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-15,-15,-15,-15,5);

//rotate to look straight

PIDLoop(-35,35,-35,35,10);

//run elevator down so it spits out ball

intake.moveVoltage(12000);

upperElevator.moveVoltage(-12000);

lowerElevator.moveVoltage(-12000);

//move to 2nd cycle goal

PIDLoop(95,95,95,95,5);

intake.moveVoltage(0);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

//rotate to look at goal

PIDLoop(15,-15,15,-15,10);

//move to second goal

PIDLoop(25,25,25,25,7);

//filter final goal

filter();

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-15,-15,-15,-15,5);}


Center Tower (Old and New autons)

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include "Robot/GUI.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Filter.hpp"

void oldMid()

{

//write tons of code and its really beautiful

//deploy

int time = pros::millis();

upperElevator.moveVoltage(12000);

pros::delay(500);

upperElevator.moveVoltage(0);

//set brake mode

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//move forward

PIDLoop(40, 40, 40, 40, 5);

//rotate

PIDLoop(-20, 20, -20, 20, 10);

//start cycle

intake.moveVelocity(200);

//move forward

PIDLoop(22, 22, 22, 22, 5);

filter();

//take red

//get distance of the next blue

colorSort.setLedPWM(75);

while (elevatorDistance.get() > 100)

{

lowerElevator.moveVoltage(7000);

upperElevator.moveVoltage(7000);

}

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

//intake next red

while (colorSort.getProximity() < 50)

{

intake.moveVoltage(12000);

}

//open intake

intake.moveVoltage(-12000);

pros::delay(100);

intake.moveVoltage(0);

//shoot blue

pros::Task purge([&purge] {

while (elevatorDistance.get() < 100)

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

pros::delay(150);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

purge.suspend();

});

//move and set ball

PIDLoop(-37, -37, -37, -37, 5);

pros::Task resetBall(indexBall);

intake.moveVoltage(12000);

PIDLoop(35, -35, 35, -35, 10);

while (elevatorDistance.get() > 100)

{

pros::delay(10);

}

intake.moveVoltage(0);

//move intakes

while (!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

//move forward

PIDLoop(21, 21, 21, 21, 5);

while (colorSort.getProximity() < 180)

{

intake.moveVoltage(12000);

}

intake.moveVoltage(0);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

//open right to knock the ball into the middle goal

while (!rightIntakeStop.isPressed())

{

intakeRight.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

//strafe into ball

PIDLoop(35, -35, -35, 35, 15);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

// backup

PIDLoop(-12, -12, -12, -12, 5);

// strafe

PIDLoop(23.5, -23.5, -23.5, 23.5, 17);

// move forward to middle goal

while (!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(8.5, 8.5, 8.5, 8.5, 4);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.moveVoltage(600);

// shoot

while (colorSort.getProximity() > 50)

{

intake.moveVoltage(6000);

}

intake.moveVoltage(0);

while (elevatorDistance.get() < 100)

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

pros::delay(150);

lowerElevator.moveVoltage(-600);

upperElevator.moveVoltage(-600);

//delay between balls

pros::delay(150);

while (elevatorDistance.get() > 60)

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

pros::delay(150);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

//back out of goal

while (!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

//delay

pros::delay(100);

if (pros::millis() - time > 14)

{

PIDLoop(-20, -20, -20, -20, 10);

}

//end

}

//new mid

void cycleLeftMid()

{

upperElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

lowerElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{

pros::delay(20);

}

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

pros::delay(100);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

pros::delay(100);

//set preload

indexBall();

//move intake

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(-600);

//move to first ball

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

PIDLoop(56,56,56,56,4);

pros::delay(20);

double timout = pros::millis();

while(colorSort.getProximity() < 100 && timout + 1500 > pros::millis())

{

intake.moveVoltage(12000);

}

intake.moveVoltage(0);

//open intake

while(!rightIntakeStop.isPressed())

{

intakeRight.moveVoltage(-12000);

}

intakeRight.moveVoltage(-600);

intakeLeft.moveVoltage(0);

//strafe into middle goal

PIDLoop(35,-35,-35,35,15);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

//backup

PIDLoop(-12,-12,-12,-12,5);

//strafe

PIDLoop(24,-24,-24,24,15);

while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(-200);

//move forward

PIDLoop(10,10,10,10,5);

//run elevator

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

double time = pros::millis();

while(elevatorDistance.get() < 150 && time + 2000 > pros::millis())

{

pros::delay(20);

}

pros::delay(100);

while(colorSort.getProximity() < 100 && time + 2000 > pros::millis())

{

intake.moveVoltage(12000);

}

intake.moveVoltage(0);

while(elevatorDistance.get() > 100 && time + 2000 > pros::millis())

{

pros::delay(20);

}

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

while(elevatorDistance.get() < 150 && time + 2000 > pros::millis())

{

pros::delay(20);

}

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

//open intakes

while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(-200);

pros::delay(100);

//go to corner goal (remove this if you want to cycle mid home row)

//bckup

// PIDLoop(-12,-12,-12,-12,5);

PIDLoop(-8,-8,-8,-8,5);

//turn

PIDLoop(-31.75,31.75,-31.75,31.75,5);

//move to final goal

PIDLoop(70,70,70,70,5);

filter();

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(0);

PIDLoop(-10,-10,-10,-10,5);

/*

this cycles middle goal as well but is out of time

*/

// // backup

// PIDLoop(-36,-36,-36,-36,5);

// //turn around

// PIDLoop(44,-44,44,-44,5);

// //move to mid home row

// PIDLoop(10,10,10,10,5);

// filter();

// //backup

// //purge

// upperElevator.moveVoltage(-12000);

// lowerElevator.moveVoltage(-12000);

// intake.moveVoltage(6000);

// PIDLoop(-35,-35,-35,-35,5);

// upperElevator.moveVoltage(0);

// lowerElevator.moveVoltage(0);

// //turn towards final goal

// PIDLoop(14,-14,14,-14,5);

// //move to final goal and purge

// // intake.moveVoltage(12000);

// while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

// {

// intake.moveVoltage(-12000);

// }

// intake.moveVoltage(-200);

// PIDLoop(69,69,69,69,5);

// intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

// filter();

// //

// lowerElevator.moveVoltage(-12000);

// upperElevator.moveVoltage(-12000);

// pros::delay(10);

// while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

// {

// intake.moveVoltage(-12000);

// }

// intake.moveVoltage(-200);

// PIDLoop(-12,-12,-12,-12,5);

// intakeLeft.moveVoltage(0);

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

lowerElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

upperElevator.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

}


Right 1 tower

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Filter.hpp"

void cycleRight1()

{

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{

pros::delay(20);

}

//deploy

upperElevator.moveVoltage(12000);

pros::delay(750);

upperElevator.moveVoltage(0);

//set brake mode

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//move forward

PIDLoop(37,37,37,37,5);

//rotate

PIDLoop(18,-18,17.5,-17.5,5);

//start cycle

intake.moveVelocity(200);

//move forward

PIDLoop(22,22,22,22,5);

filter();

//backup

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-20,-20,-20,-20,5);

}


Right 2 Tower

#include "Robot/PID.hpp"

#include "Robot/Parts.hpp"

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Filter.hpp"

void cycleRight2()

{

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{pros::delay(20);}

//deploy

upperElevator.moveVoltage(12000);

pros::delay(750);

upperElevator.moveVoltage(0);

//set brake mode

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//move forward

PIDLoop(37,37,37,37,5);

//rotate

PIDLoop(18,-18,17.5,-17.5,10);

//start cycle

intake.moveVelocity(200);

//move forward

PIDLoop(22,22,22,22,5);

filter();

//backup

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-20,-20,-20,-20,5);

//rotate to look straight

PIDLoop(36,-36,36,-36,10);

//run elevator down so it spits out ball

intake.moveVoltage(12000);

upperElevator.moveVoltage(-12000);

lowerElevator.moveVoltage(-12000);

//move to 2nd cycle goal

PIDLoop(90,90,90,90,5);

intake.moveVoltage(0);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

//rotate to look at goal

PIDLoop(-18,18,-18,18,10);

//move to second goal

intake.moveVoltage(12000);

PIDLoop(25.75,25.75,25.75,25.75,5);

//filter final goal

filter();

while(!leftIntakeStop.isPressed() && !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

PIDLoop(-15,-15,-15,-15,5);

}

Skills auton

#include "Robot/Autonomous/AutonomousPaths.hpp"

#include "Robot/Parts.hpp"

#include "Robot/Filter.hpp"

#include "Robot/PID.hpp"

void skillsPath()

{

leftDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

rightDrive.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

//deploy

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

while(deploy.get_state() != pros::E_TASK_STATE_SUSPENDED)

{

pros::delay(20);

}

pros::delay(50);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

//grab first ball

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(0);

PIDLoop(35,35,35,35,5);

intake.moveVoltage(12000);

//turn and move towards first goal

PIDLoop(-20,20,-20,20,10);

intake.moveVoltage(0);

PIDLoop(22,22,22,22,5);

//KOBE (shoots ball)

while(elevatorDistance.get() > 100)

{

lowerElevator.moveVoltage(12000);

upperElevator.moveVoltage(12000);

}

pros::delay(150);

lowerElevator.moveVoltage(0);

upperElevator.moveVoltage(0);

PIDLoop(-45,-45,-45,-45,5);

//start of second goal movement and third ball grab

//rotate to face second ball

PIDLoop(37,-37,37,-37,10);

lowerElevator.moveVoltage(600);

upperElevator.moveVoltage(600);

//open intakes to grab second ball

while(!leftIntakeStop.isPressed() || !rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

intake.moveVoltage(0);

//move forward and grab second ball

PIDLoop(28,28,28,28,5);

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::coast);

intake.moveVoltage(12000);

//turn to look at second goal

PIDLoop(-28,28,-28,28,10);

intake.moveVoltage(12000);

while(colorSort.getProximity() < 100)

{

pros::delay(20);

}

intake.moveVoltage(0);

indexBall();

//open intakes

// intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

// while (!leftIntakeStop.isPressed() ||!rightIntakeStop.isPressed())

// {

// intake.moveVoltage(-12000);

// }

intake.moveVoltage(12000);

//move to midle left goal and grab third ball

PIDLoop(37,37,37,37,5);

while(elevatorDistance.get() <= 100)

{

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

}

pros::delay(150);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

//start of corner goal

//open intakes

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

while (!leftIntakeStop.isPressed() ||!rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(6000);

//backup

PIDLoop(-21,-21,-21,-21,5);

intake.moveVoltage(12000);

indexBall();

//turn to face corner goal

PIDLoop(18,-18,18,-18,5);

//move to corner goal

PIDLoop(56,56,56,56,5);

intake.moveVoltage(0);

while(elevatorDistance.get() < 100)

{

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

}

pros::delay(150);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

//back off

PIDLoop(-20,-20,-20,-20,5);

//turn to grab middle ball

PIDLoop(25,-25,25,-25,10);

//open intakes

intake.setBrakeMode(okapi::AbstractMotor::brakeMode::hold);

while (!leftIntakeStop.isPressed() ||!rightIntakeStop.isPressed())

{

intake.moveVoltage(-12000);

}

intake.moveVoltage(0);

//move to middle back goal

PIDLoop(45,45,45,45,5);

intake.moveVoltage(12000);

indexBall();

while(elevatorDistance.get() < 100)

{

upperElevator.moveVoltage(12000);

lowerElevator.moveVoltage(12000);

}

pros::delay(200);

upperElevator.moveVoltage(0);

lowerElevator.moveVoltage(0);

intake.moveVoltage(0);