Flywheel_Master.c

#pragma config(Motor,  port1,           frontLeftWheel, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port2,           backLeftWheel, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           middleLeftWheel, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           middleRightWheel, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           frontRightWheel, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           backRightWheel, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           Intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           Indexer,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           Flywheel1,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          Flywheel2,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port11,          Flywheel3,     tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/

/*                                                                        */

/*      Description: Competition template for VEX EDR                   */

/*                                                                        */

/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform

#pragma platform(VEX2)

// Select Download method as "competition"

#pragma competitionControl(Competition)

//Main competition background code...do not modify!

#include "Vex_Competition_Includes.c"
#define min(a, b) (a) < (b) ? (a) : (b)
#define max(a, b) (a) < (b) ? (b) : (a)

// Global variables

const int dev = -10; // Global deviation
int con = -1; // Conventional direction
bool mat; // Are we on competition mats?
int friction = 15; // Have to account for different surface from competition mats
bool stop_intake; // For stopping ball-intake
bool holding;
bool flags = 1; // For when we're on the flags side during autonomous
bool red = 1; // For when we're on the red side during autonomous


/*---------------------------------------------------------------------------*/

/*                        Pre-Autonomous Functions                      */

/*                                                                        */

/*  You may want to perform some actions before the competition starts.   */

/*  Do them in the following function.  You must return from this function   */

/*  or the autonomous and usercontrol tasks will not be started.  This    */

/*  function is only called once after the cortex has been powered on and */

/*  not every time that the robot is disabled.                            */

/*---------------------------------------------------------------------------*/

void pre_auton () {

  // Set bStopTasksBetweenModes to false if you want to keep user created tasks

  // running between Autonomous and Driver controlled modes. You will need to

  // manage all user created tasks if set to false.

  bStopTasksBetweenModes = true;

  // Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD

  // used by the competition include file, for example, you might want

  // to display your team name on the LCD in this function.

  // bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts

  // Example: clearing encoders, setting servo positions, ...

  return;

}



// Global helper functions

// For stopping the drive
void StopDrive () {
  motor[frontLeftWheel] = motor[frontRightWheel] = motor[backLeftWheel] = motor[backRightWheel] = motor[middleLeftWheel] = motor[middleRightWheel] = 0;
}

// For stopping the shooter
void StopShoot () {
  motor[Flywheel1] = motor[Flywheel2] = 0;
  motor[Flywheel3] = 0;
}

// For stopping the claw flipper
void StopDescorer () {
  motor[Descorer] = 0;
}

// For stopping the ball intake
void StopBallIntake () {
  motor[Intake] = 0;
}

void StopBallIndexer () {
  motor[Indexer] = 0;
}

// Drive forward
void DriveF (int amount, int time) {
  if (mat) time += friction;
  motor[frontLeftWheel] = amount;
  motor[frontRightWheel] = amount;
  motor[backLeftWheel] = amount;
  motor[backRightWheel] = amount;
  motor[middleLeftWheel] = amount;
  motor[middleRightWheel] = amount;
  if (time != 0) wait1Msec(time);
}

// Drive backwards
void DriveB (int amount, int time) {
  if (mat) time += friction;
  motor[frontLeftWheel] = -amount;
  motor[frontRightWheel] = -amount;
  motor[backLeftWheel] = -amount;
  motor[backRightWheel] = -amount;
  motor[middleLeftWheel] = -amount;
  motor[middleRightWheel] = -amount;
  if (time != 0) wait1Msec(time);
}

// Turn right
void TurnR (int amount, int time) {
  if (mat) time += friction;
  motor[frontRightWheel] = amount;
  motor[middleRightWheel] = amount;
  motor[backRightWheel] = amount;
  if (time != 0) wait1Msec(time);
}

// Turn left
void TurnL (int amount, int time) {
  if (mat) time += friction;
  motor[frontLeftWheel] = amount;
  motor[middleLeftWheel] = amount;
  motor[backLeftWheel] = amount;
  if (time != 0) wait1Msec(time);
}

void Turn90L () {
  // 344 = competition mat
  TurnL(90, 314);
}

void Turn90R () {
  // 344 = competition mat
  TurnR(90, 314);
}

// For taking ball in
task BallIntake1 () {
  motor[Intake] = -100 * con;
}

// For taking ball out
task BallIntake2 () {
  motor[Intake] = 100 * con;
}

// Turns both the indexer and loader one way
task Indexer1 () {
  motor[Indexer] = 100 * -con;
}

// Turns both the indexer and loader the other way
task Indexer2 () {
  motor[Indexer] = -100 * -con;
}

// Turns both the intake and indexer and loader
task Loader1 () {
  motor[Intake] = motor[Indexer] = 100 * -con;
}

task turnFlywheelOn () {
  motor[Flywheel1] = motor[Flywheel2] = 127 * -con;
  motor[Flywheel3] = -127 * -con;
}

// For securing center and low flag points
// Starting position is facing flags
// Shoot middle flag and push low flag
void TwoFlags () {
  startTask(turnFlywheelOn);
  wait1Msec(5000);
  DriveF(100, 600); // Now prepare to hit middle flag
  StopDrive();
  // 2nd ball is much lower (right under the indexer)
  startTask(Indexer1);
  wait1Msec(3000);
  stopTask(Indexer1);
  StopBallIndexer();
  stopTask(turnFlywheelOn);
  StopBallIndexer();
  DriveF(100,1500) ; //Drive into low flag
}

// Auto shoots the balls onto the top 2 flags
void autoShoot () {
  startTask(turnFlywheelOn);
  wait1Msec(3000); // MAY NEED TO CHANGE THE TIME VALUE
  
  // Shoot 1st ball
  startTask(Indexer1);
  wait1Msec(1500);
  stopTask(Indexer1);
  StopBallIndexer();
  // wait1Msec(500); // WAIT BEFORE DRIVING FORWARD
  // MAY NEED TO CHANGE THE TIME VALUE
  DriveF(100, 500); // Now prepare to hit middle flag
  StopDrive();
  
  // 2nd ball is much lower (right under the indexer)
  startTask(Indexer1);
  wait1Msec(2500); // MAY NEED TO CHANGE THE TIME VALUE
  stopTask(Indexer1);
  StopBallIndexer();
  stopTask(turnFlywheelOn);
}

// Rotate the claw to flip the object
void DescorerUp (int amount, int time) {

  motor[Descorer] = amount * con;

  if (time != 0) wait1Msec(time);

}

void DescorerDown (int amount, int time) {

  motor[Descorer] = amount * -con;

  if (time != 0) wait1Msec(time);

}

/*---------------------------------------------------------------------------*/

/*                                                                        */

/*                            Autonomous Task                           */

/*                                                                        */

/*  This task is used to control your robot during the autonomous phase of   */

/*  a VEX Competition.                                                    */

/*                                                                        */

/*  You must modify the code to add your own robot specific commands here.   */

/*---------------------------------------------------------------------------*/

// For when we are on the side with no flags during autonomous

void autoWithFlags (bool red) {

  int a,b,c;//arguement variables for red and no red based on red
  // Red side
  if (red) {
      // First start intake
      startTask(BallIntake1);
      // Drive up to get the ball underneath the cap
      a = 1000;
      DriveF(100, a);
      StopDrive();
      stopTask(BallIntake1);
      b = 500;
      wait1Msec(b);
      // Drive back and rotate 90 degrees CCW
      c = 700;
      DriveB(100, c);
      StopDrive();
      Turn90L();
      StopDrive();

      /* Can alternatively use autoShoot() instead of code below */
      autoShoot();
      // Shoot the ball onto the top flag

      // Stop flywheel and drive up then stop drive

      // Turn on flywheel to shoot middle flag

      // Turn off flywheel and drive straight to toggle low flag
      DriveF(100, b);
      StopDrive();

      // Drive back and turn 90 degrees CW
      c=250;
      DriveB(100, c);
      StopDrive();
      Turn90R();
      StopDrive();

      // Flip over the cap/drive forwards simultaneously
      startTask(BallIntake2);
      DriveF(100, 300);
      StopDrive();
      stopTask(BallIntake2);

      // Drive back and turn 90 degrees CW
      DriveB(100, 250);
      StopDrive();
      Turn90R();
      StopDrive();

      // Drive forwards to get onto the platform
      DriveF(100, 200);
      StopDrive();

      // Turn 90 degrees CCW and drive onto the platform
      Turn90L();
      StopDrive();
      DriveF(100, 1000);
      StopDrive();
  }

  // Blue side (left and right are flipped)
  else {
      // First start intake
      startTask(BallIntake1);
      // Drive up to get the ball underneath the cap
      DriveF(100, a);
      StopDrive();
      stopTask(BallIntake1);
      wait1Msec(b);

      // Drive back and rotate 90 degrees CW
      DriveB(100, c);
      StopDrive();
      Turn90R();
      StopDrive();

      /* Can alternatively use autoShoot() instead of code below */
      autoShoot();
      // Shoot the ball onto the top flag

      // Stop flywheel and drive up then stop drive

      // Turn on flywheel to shoot middle flag

      // Turn off flywheel and drive straight to toggle low flag
      DriveF(100, 700);
      StopDrive();

      // Drive back and turn 90 degrees CCW
      DriveB(100, 250);
      StopDrive();
      Turn90L();
      StopDrive();

      // Flip over the cap/drive forwards simultaneously
      startTask(BallIntake2);
      DriveF(100, 300);
      StopDrive();
      stopTask(BallIntake2);

      // Drive back and turn 90 degrees CCW
      DriveB(100, 250);
      StopDrive();
      Turn90L();
      StopDrive();

      // Drive forwards to get onto the platform
      DriveF(100, 200);
      StopDrive();

      // Turn 90 degrees CW and drive onto the platform
      Turn90R();
      StopDrive();
      DriveF(100, 1000);
      StopDrive();
    }
}


// For when we are on the side with flags during autonomous
void autoNoFlags (bool red) {

  // Red side
  if (red) {
      // Drive straight to align with the opponent's cap
      DriveF(100, 100);
      StopDrive();

      // Turn 90 degrees CCW and drive forwards, while turning on ball intake, to flip over the cap
      Turn90L();
      StopDrive();
      startTask(BallIntake2);
      DriveF(100, 500);
      stopTask(BallIntake2);

      // Now drive back and turn 90 degrees CCW
      DriveB(100, 400);
      StopDrive();
      Turn90L();
      StopDrive();

      // Drive forwards and turn 90 degrees CW to align with the platform
      DriveF(100, 100);
      StopDrive();
      Turn90R();
      StopDrive();

      // Now drive forwards to get onto the platform
      DriveF(100, 1000);
      StopDrive();
  }

  // Blue side (left and right are flipped)
  else {
      // Drive straight to align with the opponent's cap
      DriveF(100, 100);
      StopDrive();

      // Turn 90 degrees CW and drive forwards, while turning on ball intake, to flip over the cap
      Turn90R();
      StopDrive();
      startTask(BallIntake2);
      DriveF(100, 500);
      stopTask(BallIntake2);

      // Now drive back and turn 90 degrees CW
      DriveB(100, 400);
      StopDrive();
      Turn90R();
      StopDrive();

      // Drive forwards and turn 90 degrees CCW to align with the platform
      DriveF(100, 100);
      StopDrive();
      Turn90L();
      StopDrive();

      // Now drive forwards to get onto the platform
      DriveF(100, 1000);
      StopDrive();
  }
}

// Used after all 3 flags are toggled - goes back and flips the opponent's cap
void autoCapFlipper () {
    if (red) {
      // Drive back and turn 90 degrees CW
      DriveB(100, 500); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      Turn90R();
      StopDrive();
      
      // Flip over the cap/drive forwards simultaneously
      startTask(BallIntake2);
      DriveF(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      stopTask(BallIntake2);
    }
    else {
      // Drive back and turn 90 degrees CCW
      DriveB(100, 500); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      Turn90L();
      StopDrive();

      // Flip over the cap/drive forwards simultaneously
      startTask(BallIntake2);
      DriveF(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      stopTask(BallIntake2);
    }
}

void ThreeFlags () {
    if (red) {
      // First start intake
      startTask(BallIntake1);
      // Drive up to get the ball underneath the cap
      DriveF(100, 1500); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      stopTask(BallIntake1);
      // wait1Msec(500);
      
      // Drive back and rotate 90 degrees CCW
      DriveB(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      Turn90L();
      StopDrive();

      /* Can alternatively use autoShoot() instead of code below */
      autoShoot();
      // Shoot the ball onto the top flag

      // Stop flywheel and drive up then stop drive

      // Turn on flywheel to shoot middle flag

      // Turn off flywheel and drive straight to toggle low flag
      DriveF(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      
      // Now flip over opponent's cap
      autoCapFlipper();
    }
    else {
      // First start intake
      startTask(BallIntake1);
      // Drive up to get the ball underneath the cap
      DriveF(100, 1500); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      stopTask(BallIntake1);
      // wait1Msec(500);
      
      // Drive back and rotate 90 degrees CW
      DriveB(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      Turn90R();
      StopDrive();

      /* Can alternatively use autoShoot() instead of code below */
      autoShoot();
      // Shoot the ball onto the top flag

      // Stop flywheel and drive up then stop drive

      // Turn on flywheel to shoot middle flag

      // Turn off flywheel and drive straight to toggle low flag
      DriveF(100, 1000); // MAY NEED TO CHANGE THE TIME VALUE
      StopDrive();
      
      // Now flip over opponent's cap
      autoCapFlipper();
    }
}

task autonomous () {
  // ..........................................................................
  // Insert user code here.
  // ..........................................................................
  // Still needs TESTING
  if (flags) {
    // TwoFlags();
    ThreeFlags();
  }
  else {
    autoNoFlags(red);
  }
}

/*---------------------------------------------------------------------------*/

/*                                                                        */

/*                            User Control Task                         */

/*                                                                        */

/*  This task is used to control your robot during the user control phase of */

/*  a VEX Competition.                                                    */

/*                                                                        */

/*  You must modify the code to add your own robot specific commands here.   */

/*---------------------------------------------------------------------------*/


int facing = 1;

task usercontrol () {
  // User control code here, inside the loop
  while (1) {
    /*
    // FORWARD AND BACKWARD
    int tmp = 0;
    double backDev;
    if (vexRT[Ch2] > 20) {
      tmp = min(vexRT[Ch2], 110);
      backDev = 1;
    }
    else if (vexRT[Ch2] < -20) {
      tmp = max(vexRT[Ch2], -110);
      backDev = 0.978;
    }
    motor[DriveLeft1] = tmp * con*backDev;
    motor[DriveRight1] = 0.9*tmp * con*backDev;
    motor[DriveRight2] = 0.9*tmp * con*backDev;
    motor[DriveLeft2] = tmp * con*backDev;

    // Left axle turn
    if (vexRT[Btn5U]) {
      motor[DriveLeft1] = vexRT[Btn5U] * 90 * con;
      motor[DriveRight1] = vexRT[Btn5U] * -90 * con;
      motor[DriveRight2] = vexRT[Btn5U] * -90 * con;
      motor[DriveLeft2] = vexRT[Btn5U] * 90 * con;
    }
    // Right axle turn
    else if (vexRT[Btn6U]) {
      motor[DriveLeft1] = vexRT[Btn6U] * 90 * -con;
      motor[DriveRight1] = vexRT[Btn6U] * -90 * -con;
      motor[DriveRight2] = vexRT[Btn6U] * -90 * -con;
      motor[DriveLeft2] = vexRT[Btn6U] * 90 * -con;
    }

    // Turn left sensitive
    if (vexRT[Btn7L]) {
      motor[DriveLeft1] = 40 * con;
      motor[DriveRight1] = 0;
      motor[DriveRight2] = 0;
      motor[DriveLeft2] = 40 * con;
    }

    // Turn right sensitive
    else if (vexRT[Btn7R]) {
      motor[DriveLeft1] = 0;
      motor[DriveRight1] = -40 * -con;
      motor[DriveRight2] = -40 * -con;
      motor[DriveLeft2] = 0;
    }
    */

    if (facing == 1) {
      if (vexRT[Ch2] && vexRT[Ch3]) //WHEELS CONTROL (JOYSTICKS){
        motor[frontLeftWheel] = vexRT[Ch3];
        motor[frontRightWheel] = vexRT[Ch2];
        motor[backLeftWheel] = vexRT[Ch3];
        motor[backRightWheel] = vexRT[Ch2];
        motor[middleLeftWheel] = vexRT[Ch3];
        motor[middleRightWheel] = vexRT[Ch2];
      }
      else if (vexRT[Ch3]) //LEFT SIDE
      {
        motor[frontLeftWheel] = vexRT[Ch3];
        motor[middleLeftWheel] = vexRT[Ch3];
        motor[backLeftWheel] = vexRT[Ch3];
      }
      else if (vexRT[Ch2]) //RIGHT SIDE
      {
        motor[frontRightWheel] = vexRT[Ch2];
        motor[middleRightWheel] = vexRT[Ch3];
        motor[backRightWheel] = vexRT[Ch2];
      }
      else {
        StopDrive();
      }
    } else if (facing == 2)
    {
      if (vexRT[Ch2] && vexRT[Ch3]) //WHEELS CONTROL (JOYSTICKS)
      {
        motor[frontLeftWheel] = -vexRT[Ch3];
        motor[frontRightWheel] = -vexRT[Ch2];
        motor[backLeftWheel] = -vexRT[Ch3];
        motor[backRightWheel] = -vexRT[Ch2];
        motor[middleLeftWheel] = -vexRT[Ch3];
        motor[middleRightWheel] = -vexRT[Ch2];
      }
      else if (vexRT[Ch3]) //LEFT SIDE
      {
        motor[frontLeftWheel] = vexRT[Ch3];
        motor[middleLeftWheel] = vexRT[Ch3];
        motor[backLeftWheel] = vexRT[Ch3];
      }
      else if (vexRT[Ch2]) //RIGHT SIDE
      {
        motor[frontRightWheel] = vexRT[Ch2];
        motor[middleRightWheel] = vexRT[Ch3];
        motor[backRightWheel] = vexRT[Ch2];
      }
      else {
        StopDrive();
      }
    }


    // Turn on the flywheel
    if (vexRT[Btn8U]) {
      int res = vexRT[Btn8U];
      motor[Flywheel1] = motor[Flywheel2] = res * 127 * -con;
      motor[Flywheel3] = res * -127 * -con;
    }
    else {
      StopShoot();
    }

    // Turn off the flywheel
    if (vexRT[Btn8D]) {
      StopShoot();
    }

    // Ball intake (CW) working
    if (vexRT[Btn6D]) {
      motor[Intake] = vexRT[Btn6D] * -100 * con;
    }

    // Ball intake (CCW) working
    else if (vexRT[Btn5D]) {
      motor[Intake] = vexRT[Btn5D] * 100 * con;
    }

    else {
      StopBallIntake();
    }

    // Implement indexer and (maybe) 3rd intake here...
    if (vexRT[Btn7U]) {
      motor[Indexer] = vexRT[Btn7U] * -100 * con;
    }
    else if (vexRT[Btn7D]) {
      motor[Indexer] = vexRT[Btn7D] * 100 * con;
    }
    else {
      StopBallIndexer();
    }

    // For auto shoot (top 2 flags)
    if (vexRT[Btn8L]) {
      autoShoot();
    }

    // For turning 90 degrees CW
    if (vexRT[Btn8R]) {
      Turn90R();
    }
    if (vexRT[Btn5U]) {
      facing = 1;
    }
    else if (vexRT[Btn6U]) {
      facing = 2;
    }
}