Vex Transmission 0.2

#include <iostream>
#include "vex.h"

using namespace vex;

int Laxle_speed = 0;
int Raxle_speed = 0;
int Left_Bank_rpm = 0;
int Right_Bank_rpm = 0;
int Left_Bank_power = 0;
int Right_Bank_power = 0;
int gear = 1;
bool PTOtoggle = false;

vex::solenoid Solenoid[] = {vex::solenoid(A), vex::solenoid(B), vex::solenoid(C), vex::solenoid(D), vex::solenoid(E), vex::solenoid(F)};
bool solenoidStates[] = {false, false, false, false, false, false};

while(1) {
    Brain.Screen.printAt( 5, 230, "frame %3d", gif.getFrameIndex() );
    this_thread::sleep_for(10);

// Create a drive base with two motors
motor_group driveBase = motor_group(motor(PORT1), motor(PORT2));



void Gear1() {
  solenoidStates[0] = true;
  solenoidStates[1] = true;
  gear = 1;
}

void Gear2() {
  solenoidStates[1] = true;
  solenoidStates[2] = true;
  gear = 2;
}

void Gear3() {
  solenoidStates[2] = true;
  solenoidStates[3] = true;
  gear = 3;
}

void Gear4() {
  solenoidStates[3] = true;
  solenoidStates[4] = true;
  gear = 4;
}

void Aux1() {
  solenoidStates[4] = true;
  solenoidStates[5] = true;
  gear = -1;
}

void Aux2() {
  solenoidStates[5] = true;
  gear = -2;
}

void Aux3() {
  for (int i = 0; i < 6; i++) {
    solenoidStates[i] = false;
  }
  gear = -3;
}

void UpShift() {
  if (PTOtoggle == false) {
    if (gear == 1) {
      Gear2();
    } else if (gear == 2) {
      Gear3();
    } else if (gear == 3) {
      Gear4();
    }
  } else {
    if (gear == -3) {
      Aux2();
    } else if (gear == -2) {
      Aux1();
    }
  }
}

void DownShift() {
  if (PTOtoggle == false) {
    if (gear == 2) {
      Gear1();
    } else if (gear == 3) {
      Gear2();
    } else if (gear == 4) {
      Gear3();
    }
  } else {
    if (gear == -2) {
      Aux3();
    } else if (gear == -1) {
      Aux2();
    }
  }
}

int main() {
  // Initializing the VEX Brain
  vex::brain Brain;

  while (true) {
    if (Brain.Controller1.ButtonR1.pressing()) {
      UpShift();
    }

    if (Brain.Controller1.ButtonR2.pressing()) {
      DownShift();
    }

    if (Brain.Controller1.ButtonB.pressing()) {
      PTOtoggle = !PTOtoggle;
    }

    // Configure the controller
    controller Controller = controller(primary);
     vex::Gif gif("world.gif", 120, 0 );

      LeftMotor.setStopping(vex::brakeType::hold);
      RightMotor.setStopping(vex::brakeType::hold);

        // Get joystick positions
        int RightStickY = Controller.Axis2.position(percentUnits::pct);
        int RightStickX = Controller.Axis1.position(percentUnits::pct);

        // Calculate motor velocities based on joystick inputs
        double y = 0;
        double x = RightStickX;

        if (x < 0) {
            x = -x;
        }

        if (0 <= x && x < 60) {
            y = pow(1.04029, x * 1.284467);
        } else if (60 <= x && x < 80) {
            y = 0.75 * x - 25;
        } else if (80 <= x && x <= 108) {
            y = 1.0357 * x - 47.85714857;
        } else if (x > 108) {
            y = 63 + pow(1.232099, x - 108);
        } else if (x >= 128) {
            y = 128;
        }

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

        double steerX = RightStickX;
        double rightWheels = y - steerX;
        double leftWheels = y + steerX;

        // Set the motor velocities
        driveBase.setVelocity(rightWheels, percentUnits::pct);
        driveBase.setVelocity(leftWheels, percentUnits::pct);

        // Run the motors
        driveBase.spin(forward);

        task::sleep(20);
    }
  }
}