AFHSRobotics_17-18_1.2.c

#pragma config(Motor,  port1,           mL,            tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           mC,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           mAR,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           mAL,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          mR,            tmotorVex393_HBridge, openLoop)
//*!!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()
{
	motor[mL] = 127;
	motor[mR] = 127;
	wait1Msec(2000);
	motor[mL] = 0;
	motor[mR] = 0;

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

	while (true)
	{

		UserControlCodePlaceholderForTesting();
		motor[mL] = vexRT[Ch1] + vexRT[Ch2];
		motor[mR] = vexRT[Ch1] - vexRT[Ch2];
		//Tank drive system

		if(vexRT[Btn6U] == 1)
		{
			motor[mAR] = 127;
			motor[mAL] = 127;
		}
		if(vexRT[Btn6D] == 1)
		{
			motor[mAR] = -127;
			motor[mAL] = -127;
		}
		if(vexRT[Btn6D] + vexRT[Btn6U] == 0)
		{
			motor[mAR] = 0;
			motor[mAL] = 0;
		}
		//Arm up and down

		if(vexRT[Btn7L] == 1)
		{
			motor[mC] = 127;
		}
		if(vexRT[Btn7D] == 1)
		{
			motor[mC] = -127;
		}
		if(vexRT[Btn7D] + vexRT[Btn7L] == 0)
		{
			motor[mC] = 0;
		}
		//Claw control

	}
}