1_19_18 Competition Program.c

#pragma config(Motor,  port1,           leftwheelback, tmotorVex393HighSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           leftwheelfront, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           rightwheelfront, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           DR4Bright,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           DR4Bleft,      tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           heavylifterright, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           heavylifterleft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           spinnyguy,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           endlift,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          rightwheelback, tmotorVex393HighSpeed_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"

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

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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()
{

  //AutonomousCodePlaceholderForTesting();



int turn_around = 675;
int DR4B_neutral = 0;

	//Deploy endlift so claw doesn't get stuck when raising DR4B
	//Should work (not 100% optimized)
	motor[endlift] = 75;
	wait1Msec(1000);
	motor[endlift] = 0;

	// raise up lift to lower base grabber
	//Not optimized
	motor[DR4Bright] = 100;
	motor[DR4Bleft] = 100;
	wait1Msec(750);
	motor[DR4Bright] = DR4B_neutral;
	motor[DR4Bleft] = DR4B_neutral;
//Also maybe start the heavy lift deploying in the middle of this segment



//The rest of the code is mostly from the old autonomous program; hasn't been tested recently with the new robot

	//Deploy heavy lift
	motor[heavylifterright] = -100;
	motor[heavylifterleft] = -100;
	wait1Msec(750);
	motor[heavylifterright] = 0;
	motor[heavylifterleft] = 0;


	//Drive robot to pick up mobile base
	nMotorEncoder(leftwheelback) = 0;
	while(nMotorEncoder(leftwheelback) > -500)
		{
			motor[leftwheelfront] = 75;
			motor[leftwheelback] = 75;
			motor[rightwheelfront] = 75;
			motor[rightwheelback] = 75;
		}

	motor[leftwheelfront] = 0;
	motor[leftwheelback] = 0;
	motor[rightwheelfront] = 0;
	motor[rightwheelback] = 0;


/*

	//Pick up mobile base (for real this time)
  	motor[heavylifterleft] = 120;
		motor[heavylifterright] = 120;
		wait1Msec(1000);
		motor[heavylifterleft] = 10;
		motor[heavylifterright] = 10;



		//Put cone on mobile base


	//Retract DR4B
	motor[DR4Bleft] = -50;
	motor[DR4Bright] = -50;
	wait1Msec(400);
	motor[DR4Bleft] = 0;
	motor[DR4Bright] = 0;


	//Retract claw to drop cone (in case claw is deployed already)
	motor[endlift] = 75;
	wait1Msec(250);
	motor[endlift] = 0;


	//Drop cone
	motor[spinnyguy] = -127;
	wait1Msec(500);
	motor[spinnyguy] = 0;



	//Back up more before turning
		nMotorEncoder(leftwheelback) = 0;
		while(nMotorEncoder(leftwheelback) < 600)
		{
			motor[leftwheelfront] = -127;
			motor[leftwheelback] = -127;
			motor[rightwheelfront] = -127;
			motor[rightwheelback] = -127;
		}
			motor[leftwheelfront] = 0;
			motor[leftwheelback] = 0;
			motor[rightwheelfront] = 0;
			motor[rightwheelback] = 0;



	//Turn around
		nMotorEncoder(leftwheelback) = 0;
		//nMotorEncoder(RightWheels) = 0;

		while(nMotorEncoder(leftwheelback) < turn_around)
		{
			motor[leftwheelfront] = -50;
			motor[leftwheelback] = -50;
			motor[rightwheelfront] = 50;
			motor[rightwheelback] = 50;
		}
			motor[leftwheelfront] = 0;
			motor[leftwheelback] = 0;
			motor[rightwheelfront] = 0;
			motor[rightwheelback] = 0;


	//Drive to high scoring zone
		nMotorEncoder(leftwheelback) = 0;
		//nMotorEncoder(RightWheels) = 0;

		while(nMotorEncoder(leftwheelback) > -1200)
		{
			motor[leftwheelfront] = 50;
			motor[leftwheelback] = 50;
			motor[rightwheelfront] = 50;
			motor[rightwheelback] = 50;
		}
			motor[leftwheelfront] = 0;
			motor[leftwheelback] = 0;
			motor[rightwheelfront] = 0;
			motor[rightwheelback] = 0;


	//Drop off mobile base
		motor[heavylifterleft] = -120;
		motor[heavylifterright] = -120;
		wait1Msec(1000);
		motor[heavylifterleft] = 0;
		motor[heavylifterright] = 0;


	//Backs up to give room to raise base lift
		nMotorEncoder(leftwheelback) = 0;
		while(nMotorEncoder(leftwheelback) < 200)
		{
			motor[leftwheelfront] = -127;
			motor[leftwheelback] = -127;
			motor[rightwheelfront] = -127;
			motor[rightwheelback] = -127;
		}
			motor[leftwheelfront] = 0;
			motor[leftwheelback] = 0;
			motor[rightwheelfront] = 0;
			motor[rightwheelback] = 0;

	//Raise Lift
		motor[heavylifterleft] = 120;
		motor[heavylifterright] = 120;
		wait1Msec(500);
		motor[heavylifterleft] = 0;
		motor[heavylifterright] = 0;


	//Reverse back
		nMotorEncoder(leftwheelback) = 0;
		//nMotorEncoder(RightWheels) = 0;

		while(nMotorEncoder(leftwheelback) < 720)
		{
			motor[leftwheelfront] = -127;
			motor[leftwheelback] = -127;
			motor[rightwheelfront] = -127;
			motor[rightwheelback] = -127;
		}
			motor[leftwheelfront] = 0;
			motor[leftwheelback] = 0;
			motor[rightwheelfront] = 0;
			motor[rightwheelback] = 0;

*/

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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()
{
//Code is from the 1/17/18 joystick program

  while (true)
  {
			motor[leftwheelfront] = vexRT[Ch3];
			motor[leftwheelback] = vexRT[Ch3];
			motor[rightwheelfront] = vexRT[Ch2];
			motor[rightwheelback] = vexRT[Ch2];

		// for endlift
			if (vexRT[Btn6U] == 1)
			{
				motor[endlift] = 127;
			}
			else if (vexRT[Btn6D] == 1)
			{
				motor[endlift] = -127;
			}
			else
			{
				motor[endlift] = 0;
			}

		// for spinnyguy
			if (vexRT[Btn5U] == 1)
			{
				motor[spinnyguy] = 127;
			}

			else if (vexRT[Btn5D] == 1)
			{
				motor[spinnyguy] = -127;
			}
			else
			{
				motor[spinnyguy] = 0;
			}

		// for DR4B
			if(vexRT[Btn7U] == 1)
			{
				motor[DR4Bleft] = 127;
				motor[DR4Bright] = 127;
			}
			else if (vexRT[Btn7D] == 1)
			{
				motor[DR4Bleft] = -60;
				motor[DR4Bright] = -60;
			}
			else
			{
				motor[DR4Bleft] = 0;
				motor[DR4Bright] = 0;
			}
		// for heavylifter
			if(vexRT[Btn8U] == 1)
			{
				motor[heavylifterleft] = 127;
				motor[heavylifterright] = 127;
			}
			else if (vexRT[Btn8D] == 1)

			{
				motor[heavylifterleft] = -127;
				motor[heavylifterright] = -127;
			}
			else
			{
				motor[heavylifterleft] = 0;
				motor[heavylifterright] = 0;
			}



  }
}