Two Skinky Bot III #1.c

#pragma config(Sensor, in1,    DR4Bpot,        sensorPotentiometer)
#pragma config(Sensor, in2,    Barpot,         sensorPotentiometer)
#pragma config(Sensor, in3,    Sidepot,        sensorPotentiometer)
#pragma config(Sensor, in4,    Zonepot,        sensorPotentiometer)
#pragma config(Sensor, in5,    Conepot,        sensorPotentiometer)
#pragma config(Sensor, dgtl1,  RDenc,          sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  LDenc,          sensorQuadEncoder)
#pragma config(Motor,  port2,           RD1,           tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl1)
#pragma config(Motor,  port3,           RD2,           tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl1)
#pragma config(Motor,  port4,           MG,            tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           Roller,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           Bar,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           DR4B,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           LD2,           tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl3)
#pragma config(Motor,  port9,           LD1,           tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, dgtl3)
//*!!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"

void resetsensors()
{
	SensorValue[RDenc] = 0;
	SensorValue[LDenc] = 0;
}

void driveForward(int power, int sensoramount)
{
	while (SensorValue[RDenc] < sensoramount || SensorValue[LDenc] < sensoramount)
	{
		if (SensorValue[RDenc] < sensoramount)
		{
			motor[RD1] = power;
			motor[RD2] = power;
		}
		else
		{
			motor[RD1] = 0;
			motor[RD2] = 0;
		}
		if (SensorValue[LDenc] < sensoramount)
		{
			motor[LD1] = power;
			motor[LD2] = power;
		}
		else
		{
			motor[LD1] = 0;
			motor[LD2] = 0;
		}
	}
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void driveBackwards(int power, int sensoramount)
{
	while (SensorValue[RDenc] > -sensoramount || SensorValue[LDenc] > -sensoramount)
	{
		if (SensorValue[RDenc] > -sensoramount)
		{
			motor[RD1] = -power;
			motor[RD2] = -power;
		}
		else
		{
			motor[RD1] = 0;
			motor[RD2] = 0;
		}
		if (SensorValue[LDenc] > -sensoramount)
		{
			motor[LD1] = -power;
			motor[LD2] = -power;
		}
		else
		{
			motor[LD1] = 0;
			motor[LD2] = 0;
		}
	}
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void turnLeft(int power, int sensoramount)
{
	while (SensorValue[RDenc] < sensoramount || SensorValue[LDenc] > -sensoramount)
	{
		if (SensorValue[RDenc] < sensoramount)
		{
			motor[RD1] = power;
			motor[RD2] = power;
		}
		else
		{
			motor[RD1] = 0;
			motor[RD2] = 0;
		}
		if (SensorValue[LDenc] > -sensoramount)
		{
			motor[LD1] = -power;
			motor[LD2] = -power;
		}
		else
		{
			motor[LD1] = 0;
			motor[LD2] = 0;
		}
	}
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void turnRight(int power, int sensoramount)
{
	while (SensorValue[RDenc] > -sensoramount || SensorValue[LDenc] < sensoramount)
	{
		if (SensorValue[RDenc] > -sensoramount)
		{
			motor[RD1] = -power;
			motor[RD2] = -power;
		}
		else
		{
			motor[RD1] = 0;
			motor[RD2] = 0;
		}
		if (SensorValue[LDenc] < sensoramount)
		{
			motor[LD1] = power;
			motor[LD2] = power;
		}
		else
		{
			motor[LD1] = 0;
			motor[LD2] = 0;
		}
	}
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void specialBack(int power1, int power2, int sensoramount1, int sensoramount2)
{
	while (SensorValue[RDenc] > -sensoramount1 || SensorValue[LDenc] > -sensoramount2)
	{
		if (SensorValue[RDenc] > -sensoramount1)
		{
			motor[RD1] = -power1;
			motor[RD2] = -power1;
		}
		else
		{
			motor[RD1] = 0;
			motor[RD2] = 0;
		}
		if (SensorValue[LDenc] < -sensoramount2)
		{
			motor[LD1] = -power2;
			motor[LD2] = -power2;
		}
		else
		{
			motor[LD1] = 0;
			motor[LD2] = 0;
		}
	}
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void timeForward(int power, int time)
{
	motor[RD1] = power;
	motor[RD2] = power;
	motor[LD1] = power;
	motor[LD2] = power;
	wait1Msec(time);
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void timeBackwards(int power, int time)
{
	motor[RD1] = -power;
	motor[RD2] = -power;
	motor[LD1] = -power;
	motor[LD2] = -power;
	wait1Msec(time);
	motor[RD1] = 0;
	motor[RD2] = 0;
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void timeLeft(int power, int time)
{
	motor[LD1] = power;
	motor[LD2] = power;
	wait1Msec(time);
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void timeRight(int power, int time)
{
	motor[LD1] = power;
	motor[LD2] = power;
	wait1Msec(time);
	motor[LD1] = 0;
	motor[LD2] = 0;
}

void liftMobileGoal(int power, int time)
{
	motor[MG] = power;
	wait1Msec(time);
	motor[MG] = 0;
}

void lowerMobileGoal(int power, int time)
{
	motor[MG] = -power;
	wait1Msec(time);
	motor[MG] = 0;
}

void liftDR4B(int power, int sensoramount)
{
	while(SensorValue[DR4Bpot] < sensoramount)
	{
		motor[DR4B] = power;
	}
	motor[DR4B] = 0;
}

void lowerDR4B(int power, int sensoramount)
{
	while(SensorValue[DR4Bpot] > sensoramount)
	{
		motor[DR4B] = -power;
	}
	motor[DR4B] = 0;
}

void timeLiftDR4B(int power, int time)
{
	motor[DR4B] = power;
	wait1Msec(time);
	motor[DR4B] = 0;
}

void timeLowerDR4B(int power, int time)
{
	motor[DR4B] = -power;
	wait1Msec(time);
	motor[DR4B] = 0;
}

void lift4Bar(int power, int sensoramount)
{
	while(SensorValue[Barpot] < sensoramount)
	{
		motor[Bar] = power;
	}
	motor[Bar] = 0;
}

void lower4Bar(int power, int sensoramount)
{
	while(SensorValue[Barpot] > sensoramount)
	{
		motor[Bar] = -power;
	}
	motor[Bar] = 0;
}

void timeLift4Bar(int power, int time)
{
	motor[Bar] = power;
	wait1Msec(time);
	motor[Bar] = 0;
}

void timeLower4Bar(int power, int time)
{
	motor[Bar] = -power;
	wait1Msec(time);
	motor[Bar] = 0;
}

void timeLift4Bar(int power, int time)
{
	motor[Bar] = power;
	wait1Msec(time);
	motor[Bar] = 0;
}

void timeLower4Bar(int power, int time)
{
	motor[Bar] = -power;
	wait1Msec(time);
	motor[Bar] = 0;
}

void grabCone(int power, int time)
{
	motor[Roller] = power;
	wait1Msec(time);
	motor[Roller] = 0;
}

void releaseCone(int power, int time)
{
	motor[Roller] = -power;
	wait1Msec(time);
	motor[Roller] = 0;
}

/*---------------------------------------------------------------------------*/
/*                          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()
{

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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()
//if (0 <= SensorValue[Sidepot] && 1000 >= SensorValue[Sidepot] && 0 <= SensorValue[Zonepot] && 1000 >= SensorValue[Zonepot] && 0 <= SensorValue[Conepot] && 1000 >= SensorValue[Conepot])
{


}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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()
{
	while (true)
	{
		motor[RD1] = vexRT[Ch2];
		motor[RD2] = vexRT[Ch2];
		motor[LD1] = vexRT[Ch3];
		motor[LD2] = vexRT[Ch3];

		if (vexRT[Btn5U] == 1) //4 Bar
		{
			motor[Bar] = 127;
		}
		else if (vexRT[Btn5D] == 1)
		{
			motor[Bar] = -127;
		}
		else
		{
			motor[Bar] = 0;
		}

		if (vexRT[Btn6U] == 1) //Cone Intake
		{
			motor[Roller] = 127;
		}
		else if (vexRT[Btn6D] == 1)
		{
			motor[Roller] = -127;
		}
		else
		{
			motor[Roller] = 0;
		}

		if (vexRT[Btn7U] == 1) //DR4B
		{
			motor[DR4B] = 127;
		}
		else if (vexRT[Btn7D] == 1)
		{
			motor[DR4B] = -127;
		}
		else
		{
			motor[DR4B] = 0;
		}

		if (vexRT[Btn8U] == 1) //Mobile Goal
		{
			motor[MG] = 127;
		}
		else if (vexRT[Btn8U] == 1)
		{
			motor[MG] = -127;
		}
		else
		{
			motor[MG] = 0;
		}
	}
}