/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        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"

/*---------------------------------------------------------------------------*/
/*                          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 setdrive(int leftvalue, int rightvalue){
	motor[port1] = leftdrive;
  motor[port2] = leftdrive;
  motor[port9] = rightdrive;
  motor[port10] = rightdrive;
}
*/
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, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{
  // ..........................................................................
  // Insert user code here.
  // ..........................................................................

  // Remove this function call once you have "real" code.
  AutonomousCodePlaceholderForTesting();
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
  // User control code here, inside the loop
	int leftdrive;
	int rightdrive;

  while (true)
  {
  	int stickleft = getJoystickValue(Ch3);
  	int stickright = getJoystickValue(Ch2);
    leftdrive = stickleft * stickleft * stickleft / 16129;
    rightdrive = -1 * stickright * stickright * stickright / 16129;
    motor[port1] = leftdrive;
    motor[port2] = leftdrive;
    motor[port9] = rightdrive;
    motor[port10] = rightdrive;

    if(getJoystickValue(Btn6U) == 1){
    	motor[port3] = 127;
    	motor[port8] = -127;
    }
    else if(getJoystickValue(Btn6D) == 1){
    	motor[port3] = -127;
    	motor[port8] = 127;
    }
    else{
    	motor[port3] = 0;
    	motor[port8] = 0;
    }

    if(getJoystickValue(Btn5U) == 1){
    	motor[port4] = 127;
    	motor[port7] = -127;
    }
    else if(getJoystickValue(Btn5D) == 1){
    	motor[port4] = -127;
    	motor[port7] = 127;
    }
    else{
    	motor[port4] = 0;
    	motor[port7] = 0;
    }

    if(getJoystickValue(Btn7U) == 1) {motor[port5] = 127;}
    else if(getJoystickValue(Btn7L) == 1) {motor[port5] = -127;}
    else {motor[port5] = 0;}
  }
}