New auton using PID

#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
motor Left1MotorA = motor(PORT4, ratio6_1, false);
motor Left1MotorB = motor(PORT6, ratio6_1, false);
motor_group Left1 = motor_group(Left1MotorA, Left1MotorB);

motor Left2 = motor(PORT9, ratio6_1, false);

motor Right1MotorA = motor(PORT16, ratio6_1, true);
motor Right1MotorB = motor(PORT14, ratio6_1, true);
motor_group Right1 = motor_group(Right1MotorA, Right1MotorB);

motor Right2 = motor(PORT18, ratio6_1, true);

motor Intake = motor(PORT10, ratio6_1, false);

digital_out Wings = digital_out(Brain.ThreeWirePort.G);
digital_out ratchetLock = digital_out(Brain.ThreeWirePort.B);
controller Controller1 = controller(primary);
motor Cata = motor(PORT17, ratio36_1, true);

pot Pot = pot(Brain.ThreeWirePort.B);
distance Distance1 = distance(PORT19);



// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}



// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;

#pragma endregion VEXcode Generated Robot Configuration


// Include the V5 Library
#include "vex.h"

// Allows for easier use of the VEX Library
using namespace vex;

competition Competition;

// prepare all the motors and such for operation
int whenStarted1() {
  Left1.setMaxTorque(100.0, percent);
  Left2.setMaxTorque(100.0, percent);
  Right1.setMaxTorque(100.0, percent);
  Right2.setMaxTorque(100.0, percent);
  Intake.setVelocity(100.0, percent);
  Intake.spinFor(forward, 2, turns);
  Cata.setStopping(hold);
  Cata.setVelocity(100.0, percent);
  Cata.setMaxTorque(100.0, percent);
  return 0;
}

//thread to only control printing output values
float L1 = 0.0;
float L2 = 0.0;
float L3 = 0.0;
float R1 = 0.0;
float R2 = 0.0;
float R3 = 0.0;
float cata = 0.0;
float intake = 0.0;
int ondriver_drivercontrol_1(){
  while(true){
    L1 = Left1MotorA.position(degrees);
    L2 = Left1MotorB.position(degrees);
    L3 = Left2.position(degrees);
    R1 = Right1MotorA.position(degrees);
    R2 = Right1MotorB.position(degrees);
    R3 = Right2.position(degrees);
    cata = Cata.velocity(rpm);
    intake = Intake.velocity(rpm);
    printf("%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\t%f,\n",L1,L2,L3,R1,R2,R3,cata,intake);
    wait(10, msec);
  }
  return 0;
}

//thread for cata, wings, and intake
int ondriver_drivercontrol_2(){
  while(true){
    //Intake
    if(Controller1.ButtonR1.pressing()){
      Intake.setVelocity(-100.0, percent);
      Intake.spin(forward);
      wait(100, msec);
    } else if(Controller1.ButtonR2.pressing()){
      Intake.setVelocity(100.0, percent);
      Intake.spin(forward);
      wait(100, msec);
    } else {
      Intake.stop();
      wait(100, msec);
    }
    //Wings
    if(Controller1.ButtonL1.pressing()){
      Wings.set(true);
    } else {
      Wings.set(false);
    }
    if(Controller1.ButtonUp.pressing()){
        Cata.spin(reverse);
    } else if(Controller1.ButtonDown.pressing()){
        Cata.spin(forward);
    } else {
        Cata.stop();
    }
    if(!Controller1.ButtonLeft.pressing()){
      ratchetLock.set(true);
    } else {
      ratchetLock.set(false);
    }
  }
}
//variable for setting curve values to desired axis
int SelectorReadOut = 0;

//universal output for the drive function
float driveOutput = 0.0;

//universal values for each drive factor
float axis1Output = 0.0;
float axis3Output = 0.0;


//Motor Curve function
void motorCurve(){
    //initialize variables first
    double y = 0;
    float coef = 1.28;

    int x = fabs(SelectorReadOut * coef);

    switch(x){
        case 0 ... 60:
            y = pow(1.04029, x * 1.284467);
            break;
        case 61 ... 80:
            y = 0.75 * x -25;
            break;
        case 81 ... 108:
            y = 1.0357 * x - 47.85714857;
            break;
        case 109 ... 127:
            y = 63 + pow(1.232099, x - 108);
            break;
        default:
            y = 128;
            break;
    }
    y = y / coef;

    if(SelectorReadOut < 0){
        y = -y;
    }

    driveOutput = y;
}

int MotorCurve(){
    while(true){
        //get and set the appropriate values for curves function
        SelectorReadOut = Controller1.Axis3.position();
        motorCurve();
        axis3Output = driveOutput;
        SelectorReadOut = Controller1.Axis1.position();
        motorCurve();
        axis1Output = driveOutput;

        //set deadband for controller
        if(Controller1.Axis3.position() < 5 && Controller1.Axis3.position() > -5){
            axis3Output = 0.0;
        }
        if(Controller1.Axis1.position() < 5 && Controller1.Axis1.position() > -5){
            axis1Output = 0.0;
        }
        //wait so that the processor doesn’t get overloaded
        wait(5.0, msec);
    }
    return 0;
}

//DriveTrain thread
int DriveTrain(){
    while(true){
        //sets the drive based on motor curve outputs

        axis1Output = axis1Output * .87;

        if(Controller1.ButtonL1.pressing()){
            axis3Output = - axis3Output;
        }

        int leftDrive = axis3Output + axis1Output;
        int rightDrive = axis3Output - axis1Output;

        Left1.setVelocity(leftDrive, percent);
        Left2.setVelocity(leftDrive, percent);
        Right1.setVelocity(rightDrive, percent);
        Right2.setVelocity(rightDrive, percent);

        Left1.spin(forward);
        Left2.spin(forward);
        Right1.spin(forward);
        Right2.spin(forward);

        //wait so that the processor doesn’t get overloaded
        wait(5, msec);
    }
    return 0;
}


//Auton portion begins

//global list for threads to pull numbers from
float values[] = {};


//the 8 threads to decipher the array

  float pGain = 0.3;
  float iGain = 0.2;
  float dGain = 0.1;


int r1(){
  int math = -5;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Right1MotorA.position(degrees);
    wait(10, msec);
    float r2 = Right1MotorA.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Right1MotorA.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int r2(){
  int math = -4;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Right1MotorB.position(degrees);
    wait(10, msec);
    float r2 = Right1MotorB.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Right1MotorB.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int r3(){
  int math = -3;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Right2.position(degrees);
    wait(10, msec);
    float r2 = Right2.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Right2.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int l1(){
  int math = -8;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Left1MotorA.position(degrees);
    wait(10, msec);
    float r2 = Left1MotorA.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Left1MotorA.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int l2(){
  int math = -7;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Left1MotorB.position(degrees);
    wait(10, msec);
    float r2 = Left1MotorB.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Left1MotorB.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int l3(){
  int math = -6;
  float errorSum = 0.0;
  while(true){
    float target = values[math];
    math = math + 8;
    float r1 = Left2.position(degrees);
    wait(10, msec);
    float r2 = Left2.position(degrees);
    float error = target - r2;
    errorSum = error + errorSum;
    float deltaV = (r2 - r1) / 0.01;
    Left2.setVelocity((error * pGain) + (errorSum * iGain) + (deltaV * dGain), percent);
  }
  return 0;
}

int cataAuton(){
  int math = -2;
  while(true){
    math = math + 8;
    Cata.setVelocity(values[math], rpm);
    wait(10, msec);
  }
  return 0;
}

int intakeAuton(){
  int math = -1;
  Intake.spin(forward);
  while(true){
    math = math + 8;
    Intake.setVelocity(values[math], rpm);
    wait(10, msec);
  }
  return 0;
}

// things to do when auton has been started
int onauton_autonomous_0() {
  Right1.setMaxTorque(100, percent);
  Right2.setMaxTorque(100, percent);
  Left1.setMaxTorque(100, percent);
  Left2.setMaxTorque(100, percent);
  Right1.spin(reverse);
  Right2.spin(reverse);
  Left1.spin(forward);
  Left2.spin(forward);
  Intake.spinFor(reverse, 1, turns);
  Cata.spinFor(forward, 180, degrees);
  return 0;
}

void VEXcode_driver_task() {
  // Start the driver control tasks
  vex::task drive0(DriveTrain);
  vex::task drive3(MotorCurve);
  vex::task drive1(ondriver_drivercontrol_1);
    //^enable for auton recording^
  vex::task drive2(ondriver_drivercontrol_2);
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  drive0.stop();
  //drive1.stop();
  drive2.stop();
  drive3.stop();
  return;
}

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  vex::task auto1(r1);
  vex::task auto2(r2);
  vex::task auto3(r3);
  vex::task auto4(l1);
  vex::task auto5(l2);
  vex::task auto6(l3);
  vex::task auto7(cataAuton);
  vex::task auto8(intakeAuton);
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  auto0.stop();
  auto1.stop();
  auto2.stop();
  auto3.stop();
  auto4.stop();
  auto5.stop();
  auto6.stop();
  auto7.stop();
  auto8.stop();
  return;
}

int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

  wait(15, msec);
  // post event registration

  // set default print color to black
  printf("\033[30m");

  // wait for rotation sensor to fully initialize
  wait(30, msec);

  whenStarted1();
}