James O.

for Programming Officer

#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.
controller Controller1 = controller(primary);
motor Motor1 = motor(PORT5, ratio6_1, false);

motor Motor2 = motor(PORT2, ratio6_1, false);

servo ServoNeuralNet = servo(Brain.ThreeWirePort.H);
motor GrabberArm = motor(PORT4, ratio6_1, false);

motor MotorNetExtendyThing = motor(PORT3, ratio6_1, false);

servo KeyTurn = servo(Brain.ThreeWirePort.G);


// generating and setting random seed
void initializeRandomSeed(){
  int systemTime = Brain.Timer.systemHighResolution();
  double batteryCurrent = Brain.Battery.current();
  double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);

  // Combine these values into a single integer
  int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;

  // Set the seed
  srand(seed);
}



void vexcodeInit() {

  //Initializing random seed.
  initializeRandomSeed(); 
}


// 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;

enum SelectorState {
  Idle,
  Pause,
  TurningLeft,
  TurningRight
};

SelectorState selectorState = Idle;

void makeIdle() {
  selectorState = Idle;
}

void makePaused() {
  selectorState = Pause;
}

enum DriveMode {
  Tank,
  SingleStick
};

DriveMode driveMode = SingleStick;

void showDriveMode() {
  Controller1.Screen.clearLine(1);
  Controller1.Screen.setCursor(1, 1);
  if(driveMode == Tank) {
    Controller1.Screen.print("Tank Drive Mode");
  } else if(driveMode == SingleStick) {
    Controller1.Screen.print("Single-Stick Mode");
  }
}

void swapDriveMode() {
  if(driveMode == Tank) {
    driveMode = SingleStick;
  } else if(driveMode == SingleStick) {
    driveMode = Tank;
  }
  showDriveMode();
}
 
int main() {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  Controller1.ButtonUp.pressed(swapDriveMode);
  showDriveMode();
  bool extendedArm = false;
  bool extendedFront = false;
  // Begin project code
  while(true) {
    // 1 - left, 2 - right
    double mult = 1;
    if(Controller1.ButtonL2.pressing()) {
      mult = 0.15;
      Motor1.setStopping(hold);
      Motor2.setStopping(hold);
    } else {
      Motor1.setStopping(coast);
      Motor2.setStopping(coast);
    }
    if(!Controller1.ButtonL1.pressing()) {
      if(driveMode == SingleStick) {
        Motor1.spin(forward, ((Controller1.Axis4.position() + Controller1.Axis3.position()) / 100.0) * 12.0 * mult, volt);
        Motor2.spin(forward, ((Controller1.Axis4.position() - Controller1.Axis3.position()) / 100.0) * 12.0 * mult, volt);
      }
      if(driveMode == Tank) {
        Motor1.spin(reverse, (-Controller1.Axis3.position() / 100.0) * 12.0 * mult, volt);
        Motor2.spin(reverse, (Controller1.Axis2.position() / 100.0) * 12.0 * mult, volt);
      }
    } else {
      if(driveMode == SingleStick) {
        Motor1.spin(forward, ((100 + Controller1.Axis4.position() * 2) / 100.0) * 12.0 * mult, volt);
        Motor2.spin(forward, ((-100 + Controller1.Axis4.position() * 2) / 100.0) * 12.0 * mult, volt);
      }
      if(driveMode == Tank) {
        Motor1.spin(reverse, ((-100 - Controller1.Axis3.position()) / 100.0) * 12.0 * mult, volt);
        Motor2.spin(reverse, ((100 + Controller1.Axis2.position()) / 100.0) * 12.0 * mult, volt);
      }
    }
    if(Controller1.ButtonDown.pressing()) {
      ServoNeuralNet.setPosition(-50, degrees);
    } else {
      ServoNeuralNet.setPosition(50, degrees);
    }
    if (Controller1.ButtonR1.pressing()) {
      extendedArm = true;
    }
    if (Controller1.ButtonR2.pressing()) {
      extendedFront = true;
    }
    MotorNetExtendyThing.spin(reverse, extendedFront ? 12.0 : 0.0, volt);
    GrabberArm.spin(forward, extendedArm ? 12.0 : 0.0, volt);
    if(selectorState == Idle) {
      if(Controller1.ButtonLeft.pressing()) {
        selectorState = TurningLeft;
        Brain.Timer.event(makePaused, 291);
        Brain.Timer.event(makeIdle, 600);
      }
      if(Controller1.ButtonRight.pressing()) {
        selectorState = TurningRight;
        Brain.Timer.event(makePaused, 291);
        Brain.Timer.event(makeIdle, 600);
      }
    }

    wait(50, msec);
  }
}