5106.c

#pragma config(Motor,  port1,           m_clawL,       tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           m_drvL,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,            ,             tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           m_armL,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           m_armR,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           m_drvR,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          m_clawR,       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()
{
	// ..........................................................................
	// 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.   */
/*---------------------------------------------------------------------------*/

#define BTN_START_ARMHOLD vexRT[Btn7R]
#define BTN_STOP_ARMHOLD vexRT[Btn7D]

#define BTN_CLW_SLUP vexRT[Btn6D]
#define BTN_CLW_FLUP vexRT[Btn6U]

#define BTN_CLW_SLDN vexRT[Btn5D]
#define BTN_CLW_FLDN vexRT[Btn5U]

#define setMtr(name, value) motor[(name)] = (value)
#define set2Mtrs(name, value) \
	setMtr(name ## L, (value)); \
	setMtr(name ## R, (value))

#define iabs(x) ((x) < 0 ? -(x) : x)

// Holds claw up
bool flatClaw(bool armHold) {

	if ( BTN_START_ARMHOLD ) {
		armHold = true;
		} else if ( BTN_STOP_ARMHOLD ) {
		armHold = false;
	}

	if (armHold) {
		set2Mtrs(m_claw, -31);
	}

	return armHold;
}

// moves claw
short clawMotion(short speed) {

	if ( BTN_CLW_FLUP )	{ // regular move up
		set2Mtrs(m_claw, 127);

	} else if ( BTN_CLW_SLUP ) {
		set2Mtrs(m_claw, 63);

	} else if ( BTN_CLW_FLDN ) { // regular move down
		set2Mtrs(m_claw, -127);

	} else if ( BTN_CLW_SLDN ) {    //slow move claw down
		set2Mtrs(m_claw, -63);

	} else {
		set2Mtrs(m_claw, 0);
	}

	return speed;
}

#define RIGHT_VERT  vexRT[Ch2]
#define RIGHT_HORIZ vexRT[Ch1]
#define LEFT_HORIZ  vexRT[Ch3]

task usercontrol () {

	int joyStickDeadZone = 10;                                     // setup the joystick Dead Zone

	nMotorPIDSpeedCtrl[m_drvL] = mtrNoReg;               // turn off PID for motors
	nMotorPIDSpeedCtrl[m_drvR] = mtrNoReg;
	nMotorPIDSpeedCtrl[m_armFL] = mtrNoReg;
	nMotorPIDSpeedCtrl[m_armFR] = mtrNoReg;
	slaveMotor(m_armFR, m_armBR);
	slaveMotor(m_armFL, m_armBL);
	int leftJoy = 0, rightJoy = 0, rArmCtrl = 0, lArmCtrl = 0;

	bool armHold = false;
	short speed = 0;

	while ( true ) {
		leftJoy = -(RIGHT_VERT + RIGHT_HORIZ);
		rightJoy = -(RIGHT_VERT - RIGHT_HORIZ);
		rArmCtrl = -(LEFT_HORIZ);
		lArmCtrl = -(LEFT_HORIZ);

		armHold = flatClaw(armHold);

		speed = clawMotion(speed);

		// check to see if left value is within dead zone.
		// if in the dead zone then zero it out
		if ( iabs(leftJoy) <= joyStickDeadZone) {
			leftJoy = 0;
		}
		if ( iabs(rightJoy) <= joyStickDeadZone) {
			rightJoy = 0;
		}
		if ( iabs(rArmCtrl) <= joyStickDeadZone) {
			rArmCtrl = 0;
		}

		if ( iabs(lArmCtrl) <= joyStickDeadZone) {
			lArmCtrl = 0;
		}

		setMtr(m_drvL, (int) rightJoy / 1.5);
		setMtr(m_drvR, (int) leftJoy / 1.5);

		setMtr(m_armLF
		set2Mtrs(m_armB, (int) rArmCtrl / 1.5);
		set2Mtrs(m_armF, (int) lArmCtrl / 1.5);

		wait1Msec(10);                                    // allow task to sleep (don't starve the other tasks).
	}
}