#include "robot-config.

h"

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

/* */

/* Description: Competition template for VCS VEX V5 */

/* */

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

//Creates a competition object that allows access to Competition methods.

vex::competition Competition;

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

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

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

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

void autonomous( void ) {

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

/* */

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

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

void usercontrol( void ) {

// User control code here, inside the loop

while (1){

// This is the main execution loop for the user control program.

// Each time through the loop your program should update motor + servo

// values based on feedback from the joysticks.

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////

//drive

Left.spin(vex::directionType::fwd, Controller1.Axis3.value(), vex::velocityUnits::pct);

Right.spin(vex::directionType::fwd, Controller1.Axis2.value(), vex::velocityUnits::pct);

Strafe.spin(vex::directionType::fwd, (Controller1.Axis4.value()+Controller1.Axis1.value())/2,
vex::velocityUnits::pct);

///////////////////////////////////////////////////////////////////////////////////////////////////////////

//intake

if(Controller1.ButtonX.pressing()){

Intake.stop(brakeType::hold);

else if(Controller1.ButtonB.pressing()){

Intake.spin(directionType::rev, 60,vex::velocityUnits::pct);

else{

Intake.spin(directionType::fwd, 75,vex::velocityUnits::pct);

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
///

//Cube claw release

if(Controller1.ButtonR1.pressing()){

Cube.spin(directionType::fwd, 50,vex::velocityUnits::pct);

else{

Cube.stop(brakeType::coast);

}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
////

//Extender

if(Controller1.ButtonUp.pressing()){

Extend.spin(directionType::fwd, 50,vex::velocityUnits::pct);

else if(Controller1.ButtonDown.pressing()){

Extend.spin(directionType::rev, 60,vex::velocityUnits::pct);

else{

Extend.stop(brakeType::hold);

//////////////////////////////////////////////////////////////////////////////////////////////////////////////

//lift

if(Controller1.ButtonL1.pressing()){

Rlift.spin(directionType::fwd, 100,vex::velocityUnits::pct);

Llift.spin(directionType::fwd, 100,vex::velocityUnits::pct);

else if(Controller1.ButtonL2.pressing()){

Rlift.spin(directionType::rev, 100,vex::velocityUnits::pct);

Llift.spin(directionType::rev, 100,vex::velocityUnits::pct);

else{

Rlift.stop(brakeType::hold);

Llift.stop(brakeType::hold);

}
 vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.

//

// Main will set up the competition functions and callbacks.

//

int main() {

//Run the pre-autonomous function.

pre_auton();

//Set up callbacks for autonomous and driver control periods.

Competition.autonomous( autonomous );

Competition.drivercontrol( usercontrol );

//Prevent main from exiting with an infinite loop.

while(1) {

vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.