5106_compday511.c

#pragma config(Motor,  port1,            ,             tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           m_clawL,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           m_armL,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           m_drvR,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           m_drvL,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           m_armR,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           m_clawR,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,           ,             tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

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

#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))

void pre_auton()
{
	bStopTasksBetweenModes = true;
}

task autonomous() {
	set2Mtrs(m_claw, 127);
	wait1Msec(400);

	set2Mtrs(m_claw, 0);
	wait1Msec(10);

	set2Mtrs(m_arm, -127);
	wait1Msec(500);

	set2Mtrs(m_arm, 0);
	wait1Msec(10);

	setMtr(m_drvL, 127);
	setMtr(m_drvR, -127);
	wait1Msec(700);

	set2Mtrs(m_drv, 0);
}

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

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

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

}

// moves claw
void clawMotion(void) {

	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);
	}

}

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

task usercontrol () {

	bool armHold = false;
	short speed = 10;

	while ( true ) {
		int rJoy_rhs = +(RIGHT_VERT - RIGHT_HORIZ);
		int rJoy_lhs = -(RIGHT_VERT + RIGHT_HORIZ);

		int lJoy_bhs = LEFT_VERT;

		flatClaw(armHold);

		clawMotion();

		setMtr(m_drvL, (int) rJoy_rhs / 1.5);
		setMtr(m_drvR, (int) rJoy_lhs / 1.5);

		setMtr(m_armL, (int) lJoy_bhs / 1.5);
		setMtr(m_armR, (int) lJoy_bhs / 1.5);

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