Team 3 Stationary Autonomous

#pragma config(Motor,  port1,           leftFront,     tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           leftBack,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           rightFront,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           rightBack,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           barLift1,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           barLift2,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           Goliath,       tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void moveForward(short speed){
	motor[leftFront] = speed;
	motor[leftBack] = speed;
	motor[rightFront] = speed;
	motor[rightBack] = speed;
}

void moveBarLift(short speed){
	motor[barLift1] = speed;
	motor[barLift2] = speed;
}

void grabber(short speed){
	motor[Goliath] = speed;
}

task main()
{
	/*
				STATIONARY GOAL AUTONOMOUS
	*/

	//Keep cone in at all times
	grabber(127);

	//Move Forward
	moveForward(100);
	wait1Msec(2500);
	moveForward(0);

	//Raise the six bar arm lift
	moveBarLift(127);
	wait1Msec(1500);
	moveBarLift(0);

	//Move forward a little so the cone is above the stationary goal
	moveForward(50);
	wait1Msec(500);
	moveForward(0);

	//Release cone ontol staionary goal
	grabber(-127);
	wait1Msec(3000);
	grabber(0);

	//Move back a little
	moveForward(-127);
	wait1Msec(1000);
	moveForward(0);

	//Lower six bar lift
	moveBarLift(-127);
	wait1Msec(2500);
	moveBarLift(0);
}