3.1 Decoder

#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 <iostream>

float axis1 = 0.0;
float axis3 = 0.0;

#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 leftMotorA = motor(PORT14, ratio6_1, false);
motor leftMotorB = motor(PORT11, ratio6_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
motor rightMotorA = motor(PORT17, ratio6_1, true);
motor rightMotorB = motor(PORT19, ratio6_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 219.44, 295, 40, mm, 1);

motor cataMotorA = motor(PORT8, ratio36_1, false);
motor cataMotorB = motor(PORT3, ratio36_1, true);
motor_group cata = motor_group(cataMotorA, cataMotorB);

motor MotorGroup7MotorA = motor(PORT4, ratio36_1, true);
motor MotorGroup7MotorB = motor(PORT9, ratio36_1, false);
motor_group MotorGroup7 = motor_group(MotorGroup7MotorA, MotorGroup7MotorB);

digital_out DigitalOutB = digital_out(Brain.ThreeWirePort.B);
digital_out DigitalOutD = digital_out(Brain.ThreeWirePort.D);



// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  // process the controller input every 20 milliseconds
  // update the motors based on the input values
  while(true) {
    if(RemoteControlCodeEnabled) {

      // calculate the drivetrain motor velocities from the controller joystick axies
      // left = Axis3 + Axis1
      // right = Axis3 - Axis1
      double y = 0;
double x = axis3;

x = x * 1.28;

x = fabs(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);
}

y = y/1.28;

if(axis3 < 0){
    y = -y;
}
double sy = 0;
double sx = axis1;

sx = sx * 1.28;

sx = fabs(sx);

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

sy = sy/1.28;

if(axis1 < 0){
    sy = -sy;
}

y = -y;

      int drivetrainLeftSideSpeed = y + sy;
      int drivetrainRightSideSpeed = y - sy;

      // check if the value is inside of the deadband range
      if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
        // check if the left motor has already been stopped
        if (DrivetrainLNeedsToBeStopped_Controller1) {
          // stop the left drive motor
          LeftDriveSmart.stop();
          // tell the code that the left motor has been stopped
          DrivetrainLNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
        DrivetrainLNeedsToBeStopped_Controller1 = true;
      }
      // check if the value is inside of the deadband range
      if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
        // check if the right motor has already been stopped
        if (DrivetrainRNeedsToBeStopped_Controller1) {
          // stop the right drive motor
          RightDriveSmart.stop();
          // tell the code that the right motor has been stopped
          DrivetrainRNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
        DrivetrainRNeedsToBeStopped_Controller1 = true;
      }

      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainLNeedsToBeStopped_Controller1) {
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
        LeftDriveSmart.spin(forward);
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainRNeedsToBeStopped_Controller1) {
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
        RightDriveSmart.spin(forward);
      }
    }
    // wait before repeating the process
    wait(20, msec);
  }
  return 0;
}


task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

#pragma endregion VEXcode Generated Robot Configuration


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

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

float myVariable, no;

// "when started" hat block
int whenStarted1() {
  Drivetrain.setDriveVelocity(600.0, rpm);
  Drivetrain.setTurnVelocity(100.0, percent);
  MotorGroup7.setMaxTorque(100, percent);
  MotorGroup7.setVelocity(100, percent);
  cata.setMaxTorque(100, percent);
  cata.setVelocity(100, percent);
  bool wingState = false;
  bool digitalB1 = false;
  bool digitalB2 = false;
  bool pl = false;
  float catapos = 0;
  int turng = 0;

//array read code

  printf("\033[30m");

//array setup
float values[] = {};

//printf("%fn", values[0]);



//int code
int ck = 0;

//the y-int for the algebra
int mathA = -10;
int mathB = -9;
int mathL1 = -8;
int mathR1 = -7;
int mathU = -6;
int mathD = -5;
int mathR = -4;
int mathL = -3;
int math3 = -2;
int math1 = -1;

float outA = 0.0;
float outB = 0.0;
float outL1 = 0.0;
float outR1 = 0.0;
float outU = 0.0;
float outD = 0.0;
float outR = 0.0;
float outL = 0.0;
float out3 = 0.0;
float out1 = 0.0;

bool buttonA = false;
bool buttonB = false;
bool buttonL1 = false;
bool buttonR1 = false;
bool buttonUp = false;
bool buttonDown = false;
bool buttonRight = false;
bool buttonLeft = false;

  while (true) {

    if(ck >= 44){
        break;
    }

    //regular code goes here
    //make sure to replace Controller1.ButtonA.pressing() with “buttonA”…

    //math at the start before reading the values
    mathA = mathA + 10;
    mathB = mathB + 10;
    mathL1 = mathL1 + 10;
    mathR1 = mathR1 + 10;
    mathU = mathU + 10;
    mathD = mathD + 10;
    mathR = mathR + 10;
    mathL = mathL + 10;
    math3 = math3 + 10;
    math1 = math1 + 10;

    //read the values off of the array and set the values accordingly
    outA = values[mathA];
    outB = values[mathB];
    outL1 = values[mathL1];
    outR1 = values[mathR1];
    outU =  values[mathU];
    outD = values[mathD];
    outR = values[mathR];
    outL = values[mathL];
    out3 = values[math3];
    out1 = values[math1];

    //do stuff on the controller
    if(outA == 1){
        buttonA = true;
    } else {
        buttonA = false;
    }
    if(outB == 1){
        buttonB = true;
    } else {
        buttonB = false;
    }
    if(outL1 == 1){
        buttonL1 = true;
    } else {
        buttonL1 = false;
    }
    if(outR1 == 1){
        buttonR1 = true;
    } else {
        buttonR1 = false;
    }
    if(outU == 1){
        buttonUp = true;
    } else {
        buttonUp = false;
    }
    if(outD == 1){
        buttonDown = true;
    } else {
        buttonDown = false;
    }
    if(outL == 1){
        buttonLeft = true;
    } else {
        buttonLeft = false;
    }
    float axis3 = out3;
    float axis1 = out1;
              double y = 0;
double x = axis3;

x = x * 1.28;

x = fabs(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);
}

y = y/1.28;

if(axis3 < 0){
    y = -y;
}
double sy = 0;
double sx = axis1;

sx = sx * 1.28;

sx = fabs(sx);

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

sy = sy/1.28;

if(axis1 < 0){
    sy = -sy;
}

y = -y;

      RightDriveSmart.setVelocity(y - sy, percent);
      LeftDriveSmart.setVelocity(y + sy, percent);
      RightDriveSmart.spin(forward);
      LeftDriveSmart.spin(forward);

    ++ck;
  axis3 = out3;
  axis1 = out1;
    cata.setStopping(hold);
    cata.stop();
    if(buttonR1 == true){
      if(axis3 < -15.0){
        MotorGroup7.spin(reverse);
      } else {
        MotorGroup7.spin(forward);
      }
    } else {
      MotorGroup7.spin(forward);
    }
    turng = floor(cata.position(turns));
    if(buttonDown == true){
      cata.spin(reverse);
    }
    if(buttonUp == true){
      cata.spin(forward);
    }
    if(catapos <= 60 || catapos >= 330){
      digitalB2 = false;
    } else {
      digitalB2 = true;
    }
    if(buttonA == true){
      cata.spinFor(forward, 360, degrees);
      digitalB1 = true;
      wait(250, msec);
    } else{
      digitalB1 = false;
    }
    if(buttonL1 == true){
      wingState = !wingState;
    }
    if (wingState == true){
      DigitalOutD.set(true);
      wait(150, msec);
    } else{
      DigitalOutD.set(false);
      wait(150, msec);
    }
    if(buttonLeft == true){
      pl = !pl;
      wait(150, msec);
    }
    catapos = cata.position(degrees) - (turng * 360);
    //if(pl == true){
      //DigitalOutB.set(true);
    //} else if((digitalB1 == true || digitalB2 == true)||(digitalB1 == true & digitalB2 == true)){
        //DigitalOutB.set(true);
      //} else {
        //DigitalOutB.set(false);
      //}
     if(Controller1.ButtonY.pressing()){
         DigitalOutB.set(true);
     }else{
         DigitalOutB.set(false);
     }
     LeftDriveSmart.spin(forward);
     printf("%f\t", outA);
     printf("%f\t", outB);
     printf("%f\t", outL1);
     printf("%f\t", outR1);
     printf("%f\t", outU);
     printf("%f\t", outD);
     printf("%f\t", outL);
     printf("%f\t", outR);
     printf("%f\t", out1);
     printf("%f\n", out3);

    wait(5, msec);
  }

  return 0;
}


int main() {
  // post event registration

  // set default print color to black

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

  whenStarted1();
}