AHSRoboticsProgram.c

#pragma config(Motor,  port2,           wheelLeft,     tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           leftClawServo, tmotorServoStandard, openLoop)
#pragma config(Motor,  port4,           motorLift,     tmotorVex269_MC29, openLoop)
#pragma config(Motor,  port5,           rightClawServo, tmotorServoStandard, openLoop, reversed)
#pragma config(Motor,  port7,           mannyGrabber,  tmotorServoStandard, openLoop)
#pragma config(Motor,  port8,           catapultMotor, tmotorVex269_MC29, openLoop)
#pragma config(Motor,  port9,           wheelRight,    tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define ARM_LEFT_LOCK_POSITION 0
#define ARM_LEFT_UNLOCK_POSITION 127

#define ARM_RIGHT_LOCK_POSITION 40
#define ARM_RIGHT_UNLOCK_POSITION 127

#define CLAW_UNLOCK_POSITION 64
#define CLAW_LOCK_POSITION -127

typedef struct ControllerSettings
{
	int CatapultPrep;
	int CatapultRelease;
	int GrabberToggle;
	int LiftButtonUp;
	int LiftButtonDown;
	int ArmLockButton;
	int DeadBand;
} ControllerSettings;

static ControllerSettings Settings;

bool bArmsAreExtended;
bool bGrabberIsDown;
bool bArmsToggledLastFrame;

void UpdateServos();
void UpdateWheels();
void UpdateCatapult();
void UpdateGrabber();
void UpdateLift();
void InitSettings();
void InitMotors();
int AbsoluteValue(int value);


task main()
{
	// Declare variables
	bArmsAreExtended = false;
	bArmsToggledLastFrame = false;
	InitSettings();
	InitMotors();

	while (true)
	{
		UpdateServos(); // Run the Code contained within "UpdateServos()"
		UpdateCatapult(); // Run the code contiained within "UpdateCatapult()"
		UpdateGrabber();
		UpdateLift();
		UpdateWheels(); // Run the Code contained within "UpdateWheels()"
		sleep(200);
	}
}


// Function definitions
void UpdateServos()
{
	bool bDirty = false;
	if (vexRT[Settings.ArmLockButton]) // if Button 8R is pressed
	{
		bArmsAreExtended = !bArmsAreExtended; // Toggle the arm extension
		bArmsToggledLastFrame = true;
		bDirty = true;
	}

	if (!bDirty)
	{
		return;
	}

	if (bArmsAreExtended) // If the launcher is in the locked state
	{
		if (motor[rightClawServo] != ARM_RIGHT_LOCK_POSITION) // If it is not already in the lock position
		{
			motor[rightClawServo] = ARM_RIGHT_LOCK_POSITION; // Set it to the lock position
		}
		if (motor[leftClawServo] != ARM_LEFT_LOCK_POSITION) // If it is not already in the lock position
		{
			motor[leftClawServo] = ARM_LEFT_LOCK_POSITION; // Set it to the lock position
		}
	}
	else // Otherwise it isn't in the lock state
	{

		if (motor[rightClawServo] != ARM_RIGHT_UNLOCK_POSITION) // If it is not already in the lock position
		{
			motor[rightClawServo] = ARM_RIGHT_UNLOCK_POSITION; // Set it to the lock position
		}
		if (motor[leftClawServo] != ARM_LEFT_UNLOCK_POSITION) // If it is not already in the lock position
		{
			motor[leftClawServo] = ARM_LEFT_UNLOCK_POSITION; // Set it to the lock position
		}
	}
}

void UpdateWheels()
{
	int channel2 = vexRT[Ch2];
	int channel3 = vexRT[Ch3];

	if (AbsoluteValue(channel2) > Settings.DeadBand)
	{
		motor[wheelRight] = channel2; // Set the right motor equal to the right joystick h
	}
	else
	{
		motor[wheelRight] = 0;
	}
	if (AbsoluteValue(channel3) > Settings.DeadBand)
	{
		motor[wheelLeft] = channel3; // Set the left motor equal to the left joystick pitch
	}
	else
	{
		motor[wheelLeft] = 0;
	}
}

void UpdateCatapult()
{
	if (vexRT[Settings.CatapultPrep])
	{
		motor[catapultMotor] = 50;
	}
	if (vexRT[Settings.CatapultRelease])
	{
		motor[catapultMotor] = -50;
	}

	if (!vexRT[Settings.CatapultPrep] && !vexRT[Settings.CatapultRelease])
	{
		motor[catapultMotor] = 0;
	}
}

void UpdateGrabber()
{
	bool bDirty = false;
	if (vexRT[Settings.GrabberToggle])
	{
		bGrabberIsDown = !bGrabberIsDown;
		bDirty = true;
	}

	if (!bDirty)
	{
		return;
	}


	bArmsToggledLastFrame = (bool)Settings.GrabberToggle;

	if (bGrabberIsDown)
	{
		motor[mannyGrabber] = CLAW_UNLOCK_POSITION;
	}
	else
	{
		motor[mannyGrabber] = CLAW_LOCK_POSITION;
	}
}

void UpdateLift()
{
	if (vexRT[Settings.LiftButtonUp])
	{
		motor[motorLift] = 100;
	}
	if (vexRT[Settings.LiftButtonDown])
	{
		motor[motorLift] = -100;
	}

	if (!vexRT[Settings.LiftButtonUp] && !vexRT[Settings.LiftButtonDown])
	{
		motor[motorLift] = 0;
	}
}

void InitSettings()
{
	Settings.ArmLockButton = Btn8R;
	Settings.CatapultPrep = Btn8U;
	Settings.CatapultRelease = Btn8D;
	Settings.GrabberToggle = Btn6U;
	Settings.LiftButtonUp = Btn5U;
	Settings.LiftButtonDown = Btn5D;
	Settings.DeadBand = 25;
}

void InitMotors()
{
	motor[wheelLeft] = 0;
	motor[leftClawServo] = ARM_LEFT_UNLOCK_POSITION;
	motor[motorLift] = 0;
	motor[rightClawServo] = ARM_RIGHT_UNLOCK_POSITION;
	motor[mannyGrabber] = CLAW_UNLOCK_POSITION;
	motor[catapultMotor] = 0;
	motor[wheelRight] = 0;
}

int AbsoluteValue(int value)
{
	if (value < 0)
	{
		return value * -1;
	}
	return value;
}